1 /*
2 * Copyright (c) 2015, Freescale Semiconductor, Inc.
3 * Copyright 2016-2017 NXP
4 * All rights reserved.
5 *
6 * SPDX-License-Identifier: BSD-3-Clause
7 */
8
9 /**
10 * @file pedometer.c
11 * @brief The pedometer.c file contains the interface definitions for
12 * pedometer application.
13 */
14
15 /*******************************************************************************
16 * Includes
17 ******************************************************************************/
18 #include "pedometer.h"
19
20 /*******************************************************************************
21 * Macros, globals
22 ******************************************************************************/
23 #define PEDOMETER_STATUS_ACTIVITY_BITNUM 0
24 #define PEDOMETER_STATUS_SUSPEND_BITNUM 3
25 #define PEDOMETER_STATUS_ACTCHG_BITNUM 4
26 #define PEDOMETER_STATUS_STEPCHG_BITNUM 5
27 #define PEDOMETER_STATUS_SUSPCHG_BITNUM 6
28 #define PEDOMETER_STATUS_MRGFLG_BITNUM 7
29
30 #define PEDOMETER_STATUS_ACTIVITY_MASK (7 << PEDOMETER_STATUS_ACTIVITY_BITNUM)
31 #define PEDOMETER_STATUS_SUSPEND_MASK (1 << PEDOMETER_STATUS_SUSPEND_BITNUM)
32 #define PEDOMETER_STATUS_ACTCHG_MASK (1 << PEDOMETER_STATUS_ACTCHG_BITNUM)
33 #define PEDOMETER_STATUS_STEPCHG_MASK (1 << PEDOMETER_STATUS_STEPCHG_BITNUM)
34 #define PEDOMETER_STATUS_SUSPCHG_MASK (1 << PEDOMETER_STATUS_SUSPCHG_BITNUM)
35 #define PEDOMETER_STATUS_MRGFLG_MASK (1 << PEDOMETER_STATUS_MRGFLG_BITNUM)
36
37 #define PEDOMETER_STATUS_CHG_MASK \
38 (PEDOMETER_STATUS_ACTCHG_MASK | PEDOMETER_STATUS_STEPCHG_MASK | PEDOMETER_STATUS_SUSPCHG_MASK)
39 #define SQUARED(x) ((x) * (x))
40 /* below values are the best set of value for the algorthm and computed during the algorithm definition.*/
41 /******************************************************************************
42 * Private Function Declarations
43 *****************************************************************************/
44
45 /* Update status variables */
46 static void status_update(pedometer_t *pedometer, uint32_t events, bool suspend);
47
48 /* Determine autonomous suspend state */
49 static bool suspend_compute(pedometer_t *pPedometer, ped_accel_t *pData);
50
51 /* compute the debounce. */
52 static bool debounce_count(bool dbcntm, bool condition, debounce_count_t *count, debounce_count_t threshold);
53
54 /* Cast uint32_t to uint16_t with saturate */
55 static uint16_t uint32_to_uint16(uint32_t val);
56
57 /******************************************************************************
58 * Private Variable Definitions
59 ******************************************************************************/
60
61 static const pedometer_t pedometer_default = {.status =
62 {
63 .version = 2,
64 },
65 .config = {
66 .sleepcount_threshold = 1,
67 .bits = {.config = 1},
68 .keynetik =
69 {
70 .height = 175,
71 .weight = 80,
72 .filtersteps = PEDO_FILTER_STEPS_DEFAULT,
73 .bits =
74 {
75 .filtertime = PEDO_FILTER_TIME_DEFAULT,
76 },
77 .speedperiod = PEDO_SPEED_PERIOD_DEFAULT,
78 .stepthreshold = PEDO_STEP_THRESHOLD_DEFAULT,
79 },
80 .stepcoalesce = 1,
81 .oneG = PEDO_ONEG_8G,
82 .frequency = PEDO_FREQHZ_DEFAULT,
83 }};
84
85 /*******************************************************************************
86 * Code
87 ******************************************************************************/
88
89 /*
90 ** ===================================================================
91 ** Method : pedometer_init
92 ** brief : The interface function initialize the pedometer.
93 ** params[in] : pedometer_t *pPedometer, handle to the pedometer.
94 **
95 ** ===================================================================
96 */
pedometer_init(pedometer_t * pPedometer)97 void pedometer_init(pedometer_t *pPedometer)
98 {
99 *pPedometer = pedometer_default;
100 KeynetikInitialize(pPedometer->config.oneG, pPedometer->config.frequency, &pPedometer->config.keynetik);
101 pPedometer->private.stepchg_stepcount = 0;
102 }
103
104 /*** ===================================================================
105 ** Method : pedometer_configure
106 ** brief : The interface function to configure the pedometer
107 ** params[in] : pedometer_t *pPedometer, handle to the pedometer.
108 ** params[in] : pedometer_config_t configuration parameter.
109 ** ===================================================================
110 */
pedometer_configure(pedometer_t * pPedometer,const pedometer_config_t * pConfig)111 void pedometer_configure(pedometer_t *pPedometer, const pedometer_config_t *pConfig)
112 {
113 pPedometer->config = *pConfig;
114 KeynetikInitialize(pPedometer->config.oneG, pPedometer->config.frequency, &pPedometer->config.keynetik);
115 pPedometer->private.stepchg_stepcount = 0;
116 }
117 /*
118 ** ===================================================================
119 ** Method : pedometer_run
120 ** brief : The interface function excutes the pedometer algorithm.
121 ** params[in] : pedometer_t *pPedometer, handle to the pedometer.
122 ** params[in] : ped_accel_t data acceleration data.
123 ** ===================================================================
124 */
pedometer_run(pedometer_t * pPedometer,ped_accel_t * pData)125 int32_t pedometer_run(pedometer_t *pPedometer, ped_accel_t *pData)
126 {
127 uint32_t events = 0;
128 /* if suspend, don't execute the alogrithm and save the power.*/
129 bool suspend = suspend_compute(pPedometer, pData);
130 if (!suspend)
131 {
132 events = KeynetikHandleIncomingEvent(pData->accel[0], pData->accel[1], pData->accel[2]);
133 }
134 status_update(pPedometer, events, suspend);
135 return events;
136 }
137 /*
138 ** ===================================================================
139 ** Method : status_update
140 ** brief : update the status of the pedometer output
141 ** params[in] : pedometer_t *pPedometer, handle to the pedometer.
142 ** params[in] : uint32_t events kynetic event
143 ** params[in] : bool suspend suspend flag
144 ** ===================================================================
145 */
status_update(pedometer_t * pPedometer,uint32_t events,bool suspend)146 static void status_update(pedometer_t *pPedometer, uint32_t events, bool suspend)
147 {
148 bool activity_stable =
149 debounce_count(pPedometer->config.bits.activity_dbcntm, !(events & KEYNETIK_ACTIVITYCHANGED),
150 &pPedometer->private.activitycount, pPedometer->config.activitycount_threshold);
151
152 activitylevel_t activity = activity_stable ? keynetikActivityLevel : pPedometer->status.status.bits.activity;
153 uint16_t stepcount = uint32_to_uint16(keynetikStepCount);
154 uint8_t newstatus = (activity << PEDOMETER_STATUS_ACTIVITY_BITNUM) & PEDOMETER_STATUS_ACTIVITY_MASK;
155
156 if ((newstatus ^ pPedometer->status.status.byte) & PEDOMETER_STATUS_ACTIVITY_MASK)
157 newstatus |= PEDOMETER_STATUS_ACTCHG_MASK;
158
159 if (pPedometer->config.stepcoalesce &&
160 (stepcount - pPedometer->private.stepchg_stepcount >= pPedometer->config.stepcoalesce))
161 {
162 pPedometer->private.stepchg_stepcount = stepcount;
163 newstatus |= PEDOMETER_STATUS_STEPCHG_MASK;
164 }
165
166 if (suspend)
167 newstatus |= PEDOMETER_STATUS_SUSPEND_MASK;
168
169 if ((newstatus ^ pPedometer->status.status.byte) & PEDOMETER_STATUS_SUSPEND_MASK)
170 newstatus |= PEDOMETER_STATUS_SUSPCHG_MASK;
171
172 if (newstatus & PEDOMETER_STATUS_CHG_MASK)
173 newstatus |= PEDOMETER_STATUS_MRGFLG_MASK;
174 /* Update the status such stepcount, distance, speed etc..*/
175 pPedometer->status.stepcount = uint32_to_uint16(keynetikStepCount);
176 pPedometer->status.distance = uint32_to_uint16(keynetikDistance);
177 pPedometer->status.speed = keynetikSpeed;
178 pPedometer->status.calories = uint32_to_uint16(keynetikCalories);
179 pPedometer->status.status.byte = newstatus;
180 }
181 /*
182 ** ===================================================================
183 ** Method : suspend_compute
184 ** brief : compute the suspend status.
185 ** params[in] : pedometer_t *pPedometer, handle to the pedometer.
186 ** params[in] : ped_accel_t data acceleration data.
187 ** ===================================================================
188 */
suspend_compute(pedometer_t * pPedometer,ped_accel_t * pData)189 static bool suspend_compute(pedometer_t *pPedometer, ped_accel_t *pData)
190 {
191 /* compute the magnitude*/
192 uint32_t mag2 = SQUARED(pData->accel[0]) + SQUARED(pData->accel[1]) + SQUARED(pData->accel[2]);
193 bool stationary =
194 (mag2 > SQUARED(pPedometer->config.sleepminimum)) && (mag2 < SQUARED(pPedometer->config.sleepmaximum));
195 bool suspend = debounce_count(pPedometer->config.bits.sleep_dbcntm, stationary, &pPedometer->status.sleepcount,
196 pPedometer->config.sleepcount_threshold);
197 return (suspend);
198 }
199 /*
200 ** ===================================================================
201 ** Method : uint32_to_uint16
202 ** brief : conversion function from 32 to 16.
203 ** params[in] : uint32_t x, imput
204 ** params[in] : ped_accel_data data acceleration data.
205 ** ===================================================================
206 */
uint32_to_uint16(uint32_t val)207 uint16_t uint32_to_uint16(uint32_t val)
208 {
209 if (val > 0x0000FFFF)
210 return (0xFFFF);
211 return ((uint16_t)val);
212 }
213 /*
214 ** ===================================================================
215 ** Method : debounce_count
216 ** brief : compute the debouct status.
217 ** params[in] : bool dbcntm,
218 ** params[in] : bool condition
219 ** params[in] : debounce_count_t *count,
220 ** params[in] : debounce_count_t threshold, debouce threshold
221 ** ===================================================================
222 */
debounce_count(bool dbcntm,bool condition,debounce_count_t * count,debounce_count_t threshold)223 static bool debounce_count(bool dbcntm, bool condition, debounce_count_t *count, debounce_count_t threshold)
224 {
225 if (condition)
226 {
227 /* increment count up to, but not above the threshold */
228 *count = (*count + 1) < threshold ? *count + 1 : threshold;
229 }
230 else
231 {
232 if (dbcntm)
233 *count = 0;
234 else
235 /* decrement count down to, but not below zero */
236 *count = (*count - 1) > 0 ? *count - 1 : 0;
237 }
238 return ((bool)(*count >= threshold));
239 }
240
