summaryrefslogtreecommitdiff
path: root/watchapp/src/actimon.c
blob: 1002672e80ac787480f08160f4f56102d6d36e9f (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
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 */
}