summaryrefslogtreecommitdiff
path: root/watchapp/src/actimon.c
diff options
context:
space:
mode:
Diffstat (limited to 'watchapp/src/actimon.c')
-rw-r--r--watchapp/src/actimon.c138
1 files changed, 138 insertions, 0 deletions
diff --git a/watchapp/src/actimon.c b/watchapp/src/actimon.c
new file mode 100644
index 0000000..1002672
--- /dev/null
+++ b/watchapp/src/actimon.c
@@ -0,0 +1,138 @@
+#include <pebble.h>
+#include "actimon.h"
+
+/* Since the user just launched us, we're likely to be stepcounting */
+monitor_mode_e monitor_mode = mmSteps;
+static monitor_mode_e last_monitor_mode = mmSleep;
+uint32_t step_total_today = 0;
+
+static int8_t strunc(int16_t samp)
+{
+ return (int8_t)(samp >> 5);
+}
+
+#define iter1(N) \
+ try = root + (1 << (N)); \
+ if (n >= try << (N)) \
+ { n -= try << (N); \
+ root |= 2 << (N); \
+ }
+
+static uint32_t isqrt(uint32_t n)
+{
+ uint32_t root = 0, try;
+ iter1 (15); iter1 (14); iter1 (13); iter1 (12);
+ iter1 (11); iter1 (10); iter1 ( 9); iter1 ( 8);
+ iter1 ( 7); iter1 ( 6); iter1 ( 5); iter1 ( 4);
+ iter1 ( 3); iter1 ( 2); iter1 ( 1); iter1 ( 0);
+ return root >> 1;
+}
+
+#undef iter1
+
+static uint32_t mag(int16_t x, int16_t y, int16_t z)
+{
+ return isqrt((x*x) + (y*y) + (z*z));
+}
+
+static uint32_t runiir(uint32_t m)
+{
+ static uint32_t z1 = 0;
+ return (z1 = (((z1 << 2) - z1) + m) >> 2);
+}
+
+static uint64_t now;
+
+static uint8_t step_last_mag;
+static int16_t step_last_delta;
+static uint64_t step_last_event;
+static bool step_last_event_was_step;
+static uint16_t step_current_gap;
+
+static void init_steps()
+{
+ step_last_mag = 0;
+ step_last_delta = 0;
+ step_last_event = 0;
+ step_last_event_was_step = false;
+}
+
+static void log_step_point(uint8_t since, uint8_t mag)
+{
+ int32_t delta = mag - step_last_mag;
+ bool maybeevent = (step_last_delta > 0) && (delta <= 0);
+ step_last_mag = mag;
+ step_last_delta = delta;
+ if (maybeevent) {
+ uint32_t steptime = (uint32_t)(now - step_last_event);
+ if (steptime >= (uint32_t)(step_current_gap - 150) &&
+ steptime <= (uint32_t)(step_current_gap + 150)) {
+ /* Event detected */
+ step_total_today++;
+ if (!step_last_event_was_step)
+ step_total_today++;
+ step_last_event_was_step = true;
+ } else {
+ step_last_event_was_step = false;
+ }
+ step_last_event = now;
+ if (steptime < 2000) {
+ step_current_gap = ((step_current_gap << 3) - step_current_gap + steptime) >> 3;
+ if (step_current_gap < 425) step_current_gap = 425;
+ if (step_current_gap > 675) step_current_gap = 675;
+ }
+ }
+}
+
+static void log_sleep_point(uint8_t since, uint8_t mag)
+{
+}
+
+static void init_sleep(void)
+{
+}
+
+static void log_data_point(uint64_t when, uint8_t mag)
+{
+ uint8_t since = (uint8_t)(when-now);
+ if (when-now > 255) since = 255;
+ now = when;
+ if (monitor_mode == mmSteps)
+ log_step_point(since, mag);
+ else
+ log_sleep_point(since, mag);
+}
+
+static void accel_handler(AccelData *data, uint32_t num_samples)
+{
+ if (monitor_mode != last_monitor_mode) {
+ if (monitor_mode == mmSteps)
+ init_steps();
+ else
+ init_sleep();
+ last_monitor_mode = monitor_mode;
+ /* Shift "now" to the timestamp */
+ now = data->timestamp;
+ }
+ for (uint32_t n = 0; n < num_samples; ++n, ++data) {
+ if (data->did_vibrate) continue;
+ int16_t x = data->x, y = data->y, z = data->z;
+ int8_t tx = strunc(x), ty = strunc(y), tz = strunc(z);
+ uint32_t tmag = mag(tx,ty,tz);
+ uint8_t smooth = (uint8_t)runiir(tmag);
+ log_data_point(data->timestamp, smooth);
+ }
+}
+
+void init_activity_monitor(void)
+{
+ /* TODO: Data logging */
+ accel_data_service_subscribe(10, accel_handler);
+ accel_service_set_sampling_rate(ACCEL_SAMPLING_10HZ);
+}
+
+void fini_activity_monitor(void)
+{
+ accel_data_service_unsubscribe();
+ /* TODO: Data logging */
+}