pedometer.c

Go to the documentation of this file.

/*
 * The Clear BSD License
 * Copyright (c) 2015 - 2016, Freescale Semiconductor, Inc.
 * Copyright 2016-2017 NXP
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted (subject to the limitations in the disclaimer below) provided
 * that the following conditions are met:
 *
 * o Redistributions of source code must retain the above copyright notice, this list
 *   of conditions and the following disclaimer.
 *
 * o Redistributions in binary form must reproduce the above copyright notice, this
 *   list of conditions and the following disclaimer in the documentation and/or
 *   other materials provided with the distribution.
 *
 * o Neither the name of the copyright holder nor the names of its
 *   contributors may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
 *
 * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE.
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

/** 
 * @file pedometer.c
* @brief The pedometer.c file contains the interface definitions for
* pedometer application.
*/

/*******************************************************************************
 * Includes
 ******************************************************************************/
#include "pedometer.h"

/*******************************************************************************
 * Macros, globals
 ******************************************************************************/
#define PEDOMETER_STATUS_ACTIVITY_BITNUM 0
#define PEDOMETER_STATUS_SUSPEND_BITNUM 3
#define PEDOMETER_STATUS_ACTCHG_BITNUM 4
#define PEDOMETER_STATUS_STEPCHG_BITNUM 5
#define PEDOMETER_STATUS_SUSPCHG_BITNUM 6
#define PEDOMETER_STATUS_MRGFLG_BITNUM 7

#define PEDOMETER_STATUS_ACTIVITY_MASK (7 << PEDOMETER_STATUS_ACTIVITY_BITNUM)
#define PEDOMETER_STATUS_SUSPEND_MASK (1 << PEDOMETER_STATUS_SUSPEND_BITNUM)
#define PEDOMETER_STATUS_ACTCHG_MASK (1 << PEDOMETER_STATUS_ACTCHG_BITNUM)
#define PEDOMETER_STATUS_STEPCHG_MASK (1 << PEDOMETER_STATUS_STEPCHG_BITNUM)
#define PEDOMETER_STATUS_SUSPCHG_MASK (1 << PEDOMETER_STATUS_SUSPCHG_BITNUM)
#define PEDOMETER_STATUS_MRGFLG_MASK (1 << PEDOMETER_STATUS_MRGFLG_BITNUM)

#define PEDOMETER_STATUS_CHG_MASK \
    (PEDOMETER_STATUS_ACTCHG_MASK | PEDOMETER_STATUS_STEPCHG_MASK | PEDOMETER_STATUS_SUSPCHG_MASK)
#define SQUARED(x) ((x) * (x))
/* below values are the best set of value for the algorthm and computed during the algorithm definition.*/
/******************************************************************************
 * Private Function Declarations
 *****************************************************************************/

/* Update status variables */
static void status_update(pedometer_t *pedometer, uint32_t events, bool suspend);

/* Determine autonomous suspend state */
static bool suspend_compute(pedometer_t *pPedometer, ped_accel_t *pData);

/* compute the debounce. */
static bool debounce_count(bool dbcntm, bool condition, debounce_count_t *count, debounce_count_t threshold);

/* Cast uint32_t to uint16_t with saturate */
static uint16_t uint32_to_uint16(uint32_t val);

/******************************************************************************
 * Private Variable Definitions
 ******************************************************************************/

static const pedometer_t pedometer_default = {.status =
                                                  {
                                                      .version = 2,
                                                  },
                                              .config = {
                                                  .sleepcount_threshold = 1,
                                                  .bits = {.config = 1},
                                                  .keynetik =
                                                      {
                                                          .height = 175,
                                                          .weight = 80,
                                                          .filtersteps = PEDO_FILTER_STEPS_DEFAULT,
                                                          .bits =
                                                              {
                                                                  .filtertime = PEDO_FILTER_TIME_DEFAULT,
                                                              },
                                                          .speedperiod = PEDO_SPEED_PERIOD_DEFAULT,
                                                          .stepthreshold = PEDO_STEP_THRESHOLD_DEFAULT,
                                                      },
                                                  .stepcoalesce = 1,
                                                  .oneG = PEDO_ONEG_8G,
                                                  .frequency = PEDO_FREQHZ_DEFAULT,
                                              }};

/*******************************************************************************
 * Code
 ******************************************************************************/

/*
** ===================================================================
**     Method      :  pedometer_init
**     brief       :  The interface function initialize the pedometer.
**     params[in]  :  pedometer_t *pPedometer, handle to the pedometer.
**
** ===================================================================
*/
void pedometer_init(pedometer_t *pPedometer)
{
    *pPedometer = pedometer_default;
    KeynetikInitialize(pPedometer->config.oneG, pPedometer->config.frequency, &pPedometer->config.keynetik);
    pPedometer->private.stepchg_stepcount = 0;
}

/*** ===================================================================
**     Method      :  pedometer_configure
**     brief       :  The interface function to configure the pedometer
**     params[in]  :  pedometer_t *pPedometer, handle to the pedometer.
**     params[in]  :  pedometer_config_t configuration parameter.
** ===================================================================
*/
void pedometer_configure(pedometer_t *pPedometer, const pedometer_config_t *pConfig)
{
    pPedometer->config = *pConfig;
    KeynetikInitialize(pPedometer->config.oneG, pPedometer->config.frequency, &pPedometer->config.keynetik);
    pPedometer->private.stepchg_stepcount = 0;
}
/*
** ===================================================================
**     Method      :  pedometer_run
**     brief       :  The interface function excutes the pedometer algorithm.
**     params[in]  :  pedometer_t *pPedometer, handle to the pedometer.
**     params[in]  :  ped_accel_t data   acceleration data.
** ===================================================================
*/
int32_t pedometer_run(pedometer_t *pPedometer, ped_accel_t *pData)
{
    uint32_t events = 0;
    /* if suspend, don't execute the alogrithm and save the power.*/
    bool suspend = suspend_compute(pPedometer, pData);
    if (!suspend)
    {
        events = KeynetikHandleIncomingEvent(pData->accel[0], pData->accel[1], pData->accel[2]);
    }
    status_update(pPedometer, events, suspend);
    return events;
}
/*
** ===================================================================
**     Method      :  status_update
**     brief       :  update the status of the pedometer output
**     params[in]  :  pedometer_t *pPedometer, handle to the pedometer.
**     params[in]  :  uint32_t events    kynetic event
**     params[in]  :  bool suspend     suspend flag
** ===================================================================
*/
static void status_update(pedometer_t *pPedometer, uint32_t events, bool suspend)
{
    bool activity_stable =
        debounce_count(pPedometer->config.bits.activity_dbcntm, !(events & KEYNETIK_ACTIVITYCHANGED),
                       &pPedometer->private.activitycount, pPedometer->config.activitycount_threshold);

    activitylevel_t activity = activity_stable ? keynetikActivityLevel : pPedometer->status.status.bits.activity;
    uint16_t stepcount = uint32_to_uint16(keynetikStepCount);
    uint8_t newstatus = (activity << PEDOMETER_STATUS_ACTIVITY_BITNUM) & PEDOMETER_STATUS_ACTIVITY_MASK;

    if ((newstatus ^ pPedometer->status.status.byte) & PEDOMETER_STATUS_ACTIVITY_MASK)
        newstatus |= PEDOMETER_STATUS_ACTCHG_MASK;

    if (pPedometer->config.stepcoalesce &&
        (stepcount - pPedometer->private.stepchg_stepcount >= pPedometer->config.stepcoalesce))
    {
        pPedometer->private.stepchg_stepcount = stepcount;
        newstatus |= PEDOMETER_STATUS_STEPCHG_MASK;
    }

    if (suspend)
        newstatus |= PEDOMETER_STATUS_SUSPEND_MASK;

    if ((newstatus ^ pPedometer->status.status.byte) & PEDOMETER_STATUS_SUSPEND_MASK)
        newstatus |= PEDOMETER_STATUS_SUSPCHG_MASK;

    if (newstatus & PEDOMETER_STATUS_CHG_MASK)
        newstatus |= PEDOMETER_STATUS_MRGFLG_MASK;
    /* Update the status such stepcount, distance, speed etc..*/
    pPedometer->status.stepcount = uint32_to_uint16(keynetikStepCount);
    pPedometer->status.distance = uint32_to_uint16(keynetikDistance);
    pPedometer->status.speed = keynetikSpeed;
    pPedometer->status.calories = uint32_to_uint16(keynetikCalories);
    pPedometer->status.status.byte = newstatus;
}
/*
** ===================================================================
**     Method      :  suspend_compute
**     brief       :  compute the suspend status.
**     params[in]  :  pedometer_t *pPedometer, handle to the pedometer.
**     params[in]  :  ped_accel_t data   acceleration data.
** ===================================================================
*/
static bool suspend_compute(pedometer_t *pPedometer, ped_accel_t *pData)
{
    /* compute the magnitude*/
    uint32_t mag2 = SQUARED(pData->accel[0]) + SQUARED(pData->accel[1]) + SQUARED(pData->accel[2]);
    bool stationary =
        (mag2 > SQUARED(pPedometer->config.sleepminimum)) && (mag2 < SQUARED(pPedometer->config.sleepmaximum));
    bool suspend = debounce_count(pPedometer->config.bits.sleep_dbcntm, stationary, &pPedometer->status.sleepcount,
                                  pPedometer->config.sleepcount_threshold);
    return (suspend);
}
/*
** ===================================================================
**     Method      :  uint32_to_uint16
**     brief       :  conversion function from 32 to 16.
**     params[in]  :  uint32_t x, imput
**     params[in]  :  ped_accel_data data   acceleration data.
** ===================================================================
*/
uint16_t uint32_to_uint16(uint32_t val)
{
    if (val > 0x0000FFFF)
        return (0xFFFF);
    return ((uint16_t)val);
}
/*
** ===================================================================
**     Method      :  debounce_count
**     brief       :  compute the debouct status.
**     params[in]  :  bool dbcntm,
**     params[in]  :  bool condition
**     params[in]  :  debounce_count_t *count,
**     params[in]  :  debounce_count_t threshold, debouce threshold
** ===================================================================
*/
static bool debounce_count(bool dbcntm, bool condition, debounce_count_t *count, debounce_count_t threshold)
{
    if (condition)
    {
        /* increment count up to, but not above the threshold */
        *count = (*count + 1) < threshold ? *count + 1 : threshold;
    }
    else
    {
        if (dbcntm)
            *count = 0;
        else
            /* decrement count down to, but not below zero */
            *count = (*count - 1) > 0 ? *count - 1 : 0;
    }
    return ((bool)(*count >= threshold));
}