add standalone version, and visualizer

all.c

@@ -0,0 +1,480 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#if !defined(__STDC_VERSION__) || __STDC_VERSION__ < 199901L
+
+/* Before C99, float variants of math functions are not avaliable.  */
+static float _sqrtf(float x)
+{
+	return (float)sqrt(x);
+}
+#define sqrtf _sqrtf
+
+static float _fmodf(float x, float y)
+{
+	return (float)fmod(x, y);
+}
+#define fmodf _fmodf
+
+/* Before C99, INFINITY may be unavaliable.  */
+#ifndef INFINITY
+
+#if defined __GNUC__ && (__GNUC__ > 3 || \
+			 (__GNUC__ == 3 && __GNUC_MINOR >= 3))
+#define INFINITY (__builtin_inff())
+#else
+#define INFINITY 1e10000f
+#endif
+
+#endif /* INFINITY */
+
+#endif /* __STDC_VERSION__ < 199901L */
+
+static FILE *data_recorder = NULL;
+static const char *column_names =
+	"gx,gy,gz,ax,ay,az,cy,cy_fixed,"
+	"g,a,g_std,a_std,g_up,a_up,is_out";
+
+struct schmidt
+{
+	float high;
+	float low;
+	int value;
+};
+
+extern int schmidt_init(struct schmidt *schmidt, float low, float high);
+extern int schmidt_trig(struct schmidt *schmidt, float level);
+extern int schmidt_get(struct schmidt *schmidt);
+
+#define RINGBUF_MAX_SIZE 100
+
+struct ringbuf
+{
+	float buf[RINGBUF_MAX_SIZE];
+	float sum;
+	float sum2;
+	int cap;
+	int p;
+	int full;
+};
+
+extern int ringbuf_init(struct ringbuf *ringbuf, int cap);
+extern void ringbuf_push(struct ringbuf *ringbuf, float value);
+extern int ringbuf_size(struct ringbuf *ringbuf);
+extern float ringbuf_mean(struct ringbuf *ringbuf);
+extern float ringbuf_variance(struct ringbuf *ringbuf);
+extern float ringbuf_stdev(struct ringbuf *ringbuf);
+
+#define MONOTONIC_QUEUE_CAP 80
+
+typedef int (*monotonic_queue_cmp)(float, float);
+
+struct monotonic_queue
+{
+	monotonic_queue_cmp cmp;
+	float buf[MONOTONIC_QUEUE_CAP];
+	int h, h_min, t;
+	int len;
+};
+
+extern void monotonic_queue_init(struct monotonic_queue *mq,
+				 monotonic_queue_cmp cmp);
+extern int monotonic_queue_push(struct monotonic_queue *mq, float value);
+extern int monotonic_queue_pop(struct monotonic_queue *mq);
+extern int monotonic_queue_get_min(struct monotonic_queue *mq, float *dest);
+extern int monotonic_queue_get_len(struct monotonic_queue *mq);
+
+struct jump_rope_count_config
+{
+	float lg, hg, la, ha, lgz, hgz, a_g_window;
+	int cy_window, cy_crit, cy_suppress_time, wait_time;
+};
+
+struct jump_rope_count_device
+{
+	struct schmidt trig_g, trig_a, trig_gz;
+	struct ringbuf rbuf_g, rbuf_a;
+	struct monotonic_queue mq_min_cy, mq_max_cy;
+	int cy_window, cy_crit, cy_suppress_time, wait_time;
+	int cy_suppress, remain_time;
+	float last_cy, last_cy_fixed;
+};
+
+struct sensor_packet
+{
+	float ax, ay, az, gx, gy, gz, cy;
+};
+
+enum jump_rope_count_result
+{
+	RESULT_INACTIVE = -1,
+	RESULT_NONE = 0,
+	RESULT_TRIGGERED = 1
+};
+
+extern int jump_rope_count_device_init(struct jump_rope_count_device *dev,
+				       struct jump_rope_count_config *cfg);
+extern int process_packet(struct jump_rope_count_device *dev,
+			  struct sensor_packet *packet,
+			  enum jump_rope_count_result *result);
+
+static int less(float a, float b)
+{
+	return a < b;
+}
+
+static int greater(float a, float b)
+{
+	return a > b;
+}
+
+int jump_rope_count_device_init(struct jump_rope_count_device *dev,
+				struct jump_rope_count_config *cfg)
+{
+	int ret;
+
+	ret = schmidt_init(&dev->trig_g, cfg->lg, cfg->hg);
+	if (ret != 0)
+		return ret;
+
+	ret = schmidt_init(&dev->trig_a, cfg->la, cfg->ha);
+	if (ret != 0)
+		return ret;
+
+	ret = schmidt_init(&dev->trig_gz, cfg->lgz, cfg->hgz);
+	if (ret != 0)
+		return ret;
+
+	ret = ringbuf_init(&dev->rbuf_a, cfg->a_g_window);
+	if (ret != 0)
+		return ret;
+
+	ret = ringbuf_init(&dev->rbuf_g, cfg->a_g_window);
+	if (ret != 0)
+		return ret;
+
+	monotonic_queue_init(&dev->mq_min_cy, less);
+	monotonic_queue_init(&dev->mq_max_cy, greater);
+
+	dev->cy_window = cfg->cy_window;
+	dev->cy_crit = cfg->cy_crit;
+	dev->cy_suppress_time = cfg->cy_suppress_time;
+	dev->wait_time = cfg->wait_time;
+
+	dev->cy_suppress = 0;
+	dev->last_cy = dev->last_cy_fixed = 0;
+
+	return 0;
+}
+
+static float hypot3f(float x, float y, float z)
+{
+	return sqrtf(x*x + y*y + z*z);
+}
+
+static float angle_change(float old, float new)
+{
+	float ret = fmodf(new - old, 360.0);
+	if (ret < -180.0)
+		ret += 360.0;
+	if (ret > 180.0)
+		ret -= 360.0;
+	return ret;
+}
+
+int process_packet(struct jump_rope_count_device *dev,
+		   struct sensor_packet *packet,
+		   enum jump_rope_count_result *result)
+{
+	int ret = 0;
+	float min, max;
+
+	float g = hypot3f(packet->gx, packet->gy, packet->gz);
+	float a = hypot3f(packet->ax, packet->ay, packet->az);
+	float gz = packet->gz;
+	float cy = dev->last_cy_fixed + angle_change(dev->last_cy,
+						     packet->cy);
+	float std_g, std_a;
+
+	dev->last_cy_fixed = cy;
+	dev->last_cy = packet->cy;
+
+	ringbuf_push(&dev->rbuf_g, g);
+	ringbuf_push(&dev->rbuf_a, a);
+
+	std_g = ringbuf_stdev(&dev->rbuf_g);
+	std_a = ringbuf_stdev(&dev->rbuf_a);
+	schmidt_trig(&dev->trig_g, std_g);
+	schmidt_trig(&dev->trig_a, std_a);
+
+	ret = monotonic_queue_push(&dev->mq_min_cy, cy);
+	if (ret != 0)
+		return ret;
+
+	if (monotonic_queue_get_len(&dev->mq_min_cy) > dev->cy_window)
+		ret = monotonic_queue_pop(&dev->mq_min_cy);
+	if (ret != 0)
+		return ret;
+
+	ret = monotonic_queue_push(&dev->mq_max_cy, cy);
+	if (ret != 0)
+		return ret;
+
+	if (monotonic_queue_get_len(&dev->mq_max_cy) > dev->cy_window)
+		ret = monotonic_queue_pop(&dev->mq_max_cy);
+	if (ret != 0)
+		return ret;
+
+	ret = monotonic_queue_get_min(&dev->mq_min_cy, &min);
+	if (ret != 0)
+		return ret;
+
+	ret = monotonic_queue_get_min(&dev->mq_max_cy, &max);
+	if (ret != 0)
+		return ret;
+
+	if (max - min > dev->cy_crit)
+		dev->cy_suppress = dev->cy_suppress_time;
+
+	if (schmidt_get(&dev->trig_g) == 0 ||
+	    schmidt_get(&dev->trig_a) == 0 ||
+	    dev->cy_suppress > 0)
+	{
+		schmidt_trig(&dev->trig_gz, -INFINITY);
+		*result = RESULT_INACTIVE;
+		goto out;
+	}
+
+	if (schmidt_trig(&dev->trig_gz, gz) == 1 &&
+	    schmidt_get(&dev->trig_gz) == 0 &&
+	    dev->cy_suppress == 0)
+	{
+		dev->remain_time = dev->wait_time;
+		*result = RESULT_TRIGGERED;
+		goto out;
+	}
+
+	if (dev->remain_time)
+		dev->remain_time--;
+
+	*result = (dev->remain_time > 0 ? RESULT_NONE : RESULT_INACTIVE);
+out:
+	if (dev->cy_suppress > 0)
+		dev->cy_suppress--;
+	if (data_recorder == NULL)
+		return 0;
+
+	/* column names:
+	"gx,gy,gz,ax,ay,az,cy,cy_fixed,"
+	"g,a,g_std,a_std,g_up,a_up,is_out" */
+	fprintf(data_recorder, "%.7f,%.7f,%.7f,",
+		packet->gx, packet->gy, packet->gz);
+	fprintf(data_recorder, "%.7f,%.7f,%.7f,",
+		packet->ax, packet->ay, packet->az);
+	fprintf(data_recorder, "%.7f,%.7f,",
+		dev->last_cy, dev->last_cy_fixed);
+	fprintf(data_recorder, "%.7f,%.7f,%.7f,%.7f,",
+		g, a, std_g, std_a);
+	fprintf(data_recorder, "%d,%d,%d\n",
+		schmidt_get(&dev->trig_g),
+		schmidt_get(&dev->trig_a),
+		*result == RESULT_TRIGGERED);
+
+	return 0;
+}
+
+void monotonic_queue_init(struct monotonic_queue *mq,
+			  monotonic_queue_cmp cmp)
+{
+	mq->h = mq->h_min = mq->t = mq->len = 0;
+	mq->cmp = cmp;
+}
+
+int monotonic_queue_push(struct monotonic_queue *mq, float value)
+{
+	int t1;
+
+	while (mq->h_min != mq->t && mq->cmp(value, mq->buf[mq->h_min]))
+		mq->h_min = (mq->h_min + 1) % MONOTONIC_QUEUE_CAP;
+
+	t1 = (mq->t + 1) % MONOTONIC_QUEUE_CAP;
+	if (t1 == mq->h)
+		/* over flow */
+		return -1;
+
+	mq->len++;
+	mq->buf[mq->t] = value;
+	mq->t = t1;
+	return 0;
+}
+
+int monotonic_queue_pop(struct monotonic_queue *mq)
+{
+	if (mq->h == mq->t)
+		/* empty */
+		return -1;
+
+	mq->len--;
+	if (mq->h == mq->h_min)
+		mq->h_min = (mq->h_min + 1) % MONOTONIC_QUEUE_CAP;
+	mq->h = (mq->h + 1) % MONOTONIC_QUEUE_CAP;
+
+	return 0;
+}
+
+int monotonic_queue_get_min(struct monotonic_queue *mq, float *dest)
+{
+	if (mq->h == mq->t)
+		/* empty */
+		return -1;
+
+	*dest = mq->buf[mq->h_min];
+	return 0;
+}
+
+int monotonic_queue_get_len(struct monotonic_queue *mq)
+{
+	return mq->len;
+}
+
+int ringbuf_init(struct ringbuf *ringbuf, int cap)
+{
+	if (cap > RINGBUF_MAX_SIZE)
+		return -1;
+
+	ringbuf->p = 0;
+	ringbuf->full = 0;
+	ringbuf->sum = ringbuf->sum2 = 0.0;
+	ringbuf->cap = cap;
+
+	return 0;
+}
+
+void ringbuf_push(struct ringbuf *ringbuf, float value)
+{
+	if (ringbuf->full) {
+		float old = ringbuf->buf[ringbuf->p];
+		ringbuf->sum -= old;
+		ringbuf->sum2 -= old * old;
+	}
+
+	ringbuf->sum += value;
+	ringbuf->sum2 += value * value;
+
+	ringbuf->buf[ringbuf->p++] = value;
+	if (ringbuf->p == ringbuf->cap) {
+		ringbuf->p = 0;
+		ringbuf->full = 1;
+	}
+}
+
+int ringbuf_size(struct ringbuf *ringbuf)
+{
+	return ringbuf->full ? ringbuf->cap : ringbuf->p;
+}
+
+float ringbuf_mean(struct ringbuf *ringbuf)
+{
+	int sz = ringbuf_size(ringbuf);
+	if (sz == 0)
+		return 0;
+	return ringbuf->sum / sz;
+}
+
+float ringbuf_variance(struct ringbuf *ringbuf)
+{
+	float mean;
+	int sz = ringbuf_size(ringbuf);
+	if (sz == 0)
+		return 0;
+
+	mean = ringbuf_mean(ringbuf);
+	return ringbuf->sum2 / sz - mean * mean;
+}
+
+float ringbuf_stdev(struct ringbuf *ringbuf)
+{
+	return sqrt(ringbuf_variance(ringbuf));
+}
+
+int schmidt_init(struct schmidt *schmidt, float low, float high)
+{
+	if (low > high)
+		return -1;
+
+	schmidt->high = high;
+	schmidt->low = low;
+	schmidt->value = 0;
+
+	return 0;
+}
+
+int schmidt_trig(struct schmidt *schmidt, float level)
+{
+	if ((schmidt->value == 1 && level <= schmidt->low) ||
+	    (schmidt->value == 0 && level >= schmidt->high)) {
+		schmidt->value ^= 1;
+		return 1;
+	}
+
+	return 0;
+}
+
+int schmidt_get(struct schmidt *schmidt)
+{
+	return schmidt->value;
+}
+
+const int fs = 50;
+
+int main()
+{
+	struct jump_rope_count_config cfg;
+	struct jump_rope_count_device dev;
+	struct sensor_packet packet;
+	int count = 0, ret;
+	enum jump_rope_count_result result;
+
+	data_recorder = fopen("data_record.csv", "w");
+	if (data_recorder == NULL)
+		fprintf(stderr, "can not open data_record.csv\n");
+	else
+		fprintf(data_recorder, "%s\n", column_names);
+
+	cfg.lg = 0.5;
+	cfg.hg = 1.5;
+	cfg.la = 4.0;
+	cfg.ha = 6.0;
+	cfg.lgz = -1.5;
+	cfg.hgz = 2;
+	cfg.a_g_window = 100;
+	cfg.cy_window = fs * 0.1;
+	cfg.cy_crit = 200;
+	cfg.cy_suppress_time = fs * 0.2;
+	cfg.wait_time = fs * 1;
+
+	if (jump_rope_count_device_init(&dev, &cfg) != 0)
+		abort();
+
+	while (scanf("%f%f%f%f%f%f%f",
+		     &packet.gx, &packet.gy, &packet.gz,
+		     &packet.ax, &packet.ay, &packet.az,
+		     &packet.cy) == 7) {
+		ret = process_packet(&dev, &packet, &result);
+		if (ret != 0)
+			abort();
+		if (result == RESULT_INACTIVE) {
+			if (count > 1)
+				printf("%d\n", count - 1);
+			count = 0;
+		} else if (result == RESULT_TRIGGERED)
+			count += 1;
+	}
+
+	if (count > 1)
+		printf("%d\n", count - 1);
+	return 0;
+}
+
+/* vim: set ts=8 sw=8 sts=8 noet: */

visualize.py

@@ -0,0 +1,40 @@
+def main():
+	import pandas as pd
+	import numpy as np
+	import matplotlib.pyplot as plt
+
+	fs = 50
+
+	dataset = pd.read_csv('data_record.csv')
+	a_std = np.array(dataset['a_std'])
+	a_up = np.array(dataset['a_up'])
+	g_std = np.array(dataset['g_std'])
+	g_up = np.array(dataset['g_up'])
+	gz = np.array(dataset['gz'])
+	is_out = np.array(dataset['is_out'])
+
+	out_idx = [i for i in range(len(is_out)) if is_out[i] == 1]
+
+	n_samples = len(gz)
+	T = n_samples / fs
+	t = np.linspace(0, T, n_samples, endpoint = False)
+
+	out_t = t[out_idx]
+	out_gz = gz[out_idx]
+
+	plt.figure(1)
+	plt.clf()
+	plt.plot(t, a_std, label = 'std variation of accelerate magnitude')
+	plt.plot(t, a_up, label = 'Schmidt trigger output for accelerate')
+	plt.plot(t, g_std, label = 'std variation of gyro magnitude')
+	plt.plot(t, g_up, label = 'Schmidt trigger output for gyro')
+	plt.plot(t, gz, label = 'gyro z')
+	plt.scatter(out_t, out_gz)
+	plt.grid(True)
+	plt.legend(loc = 'upper left')
+
+	plt.show()
+	return 0
+
+if __name__ == '__main__':
+	exit(main())