first commit

.gitignore

@@ -0,0 +1,3 @@
+demo/demo
+*.o
+libjump.so

Makefile

@@ -0,0 +1,37 @@
+# ANSI C conformance
+CFLAGS += -ansi -pedantic-errors -Werror=pedantic -fno-asm -fno-common
+
+# Optimization
+CFLAGS += -O3 -fipa-pta
+
+# Additional warnings
+CFLAGS += -Wall -Wextra
+
+# To build shared object
+CFLAGS += -fPIC
+
+OBJFILES := schmidt_trigger.o ringbuf.o monotonic_queue.o
+OBJFILES += jump_rope_count_device.o
+
+all: libjump.so demo/demo
+
+libjump.so: $(OBJFILES)
+	cc -o $@ -shared $^
+
+schmidt_trigger.o: schmidt_trigger.c schmidt_trigger.h
+
+ringbuf.o: ringbuf.c ringbuf.h
+
+monotonic_queue.o: monotonic_queue.c monotonic_queue.h
+
+jump_rope_count_device.o: jump_rope_count_device.c \
+	jump_rope_count_device.h monotonic_queue.h ringbuf.h schmidt_trigger.h
+
+.PHONY: clean
+
+clean:
+	rm -v -f $(OBJFILES) libjump.so
+	make -C demo clean
+
+demo/demo: libjump.so
+	make -C demo demo

demo/Makefile

@@ -0,0 +1,12 @@
+LDFLAGS=-L.. -Wl,-rpath -Wl,'$${ORIGIN}/..' -ljump -lm
+CFLAGS=-I..
+
+demo: main.o
+	cc -o $@ $(LDFLAGS) $^
+
+main.o: main.c
+
+.PHONY: clean
+
+clean:
+	rm -vf demo

demo/main.c

@@ -0,0 +1,50 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+#include <jump_rope_count_device.h>
+
+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;
+
+	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 * 1;
+	cfg.cy_crit = 250;
+	cfg.cy_suppress_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: */

jump_rope_count_device.c

@@ -0,0 +1,146 @@
+#include <math.h>
+
+#include "jump_rope_count_device.h"
+#include "schmidt_trigger.h"
+
+#if !defined(__STDC_VERSION__) || __STDC_VERSION__ < 199901L
+
+/* Before C99, sqrtf is not avaliable.  */
+float _sqrtf(float x)
+{
+	return (float)sqrt(x);
+}
+#define sqrtf _sqrtf
+
+/* Before C99, INFINITY is not avaliable.  */
+#if defined __GNUC__ && (__GNUC__ > 3 || \
+			 (__GNUC__ == 3 && __GNUC_MINOR >= 3))
+#define INFINITY (__builtin_inff())
+#else
+#define INFINITY 1e10000f
+#endif
+
+#endif
+
+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->cy_suppress = 0;
+
+	return 0;
+}
+
+static float hypot3f(float x, float y, float z)
+{
+	return sqrtf(x*x + y*y + z*z);
+}
+
+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;
+
+	ringbuf_push(&dev->rbuf_g, g);
+	ringbuf_push(&dev->rbuf_a, a);
+
+	schmidt_trig(&dev->trig_g, ringbuf_stdev(&dev->rbuf_g));
+	schmidt_trig(&dev->trig_a, ringbuf_stdev(&dev->rbuf_a));
+
+	ret = monotonic_queue_push(&dev->mq_min_cy, packet->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, packet->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) == 1 &&
+	    dev->cy_suppress == 0)
+	{
+		*result = RESULT_TRIGGERED;
+		goto out;
+	}
+
+	*result = RESULT_NONE;
+out:
+	if (dev->cy_suppress > 0)
+		dev->cy_suppress--;
+	return 0;
+}
+/* vim: set ts=8 sw=8 sts=8 noet: */

jump_rope_count_device.h

@@ -0,0 +1,43 @@
+#ifndef _JUMP_ROPE_COUNT_DEVICE_H
+#define _JUMP_ROPE_COUNT_DEVICE_H 1
+
+#include "schmidt_trigger.h"
+#include "monotonic_queue.h"
+#include "ringbuf.h"
+
+struct jump_rope_count_config
+{
+	float lg, hg, la, ha, lgz, hgz, a_g_window;
+	int cy_window, cy_crit, cy_suppress_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;
+	int cy_suppress;
+};
+
+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);
+
+#endif
+
+/* vim: set ts=8 sw=8 sts=8 noet: */

monotonic_queue.c

@@ -0,0 +1,57 @@
+#include "monotonic_queue.h"
+
+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;
+}
+
+/* vim: set ts=8 sw=8 sts=8 noet: */

monotonic_queue.h

@@ -0,0 +1,25 @@
+#ifndef _JUMP_MONOTONIC_QUEUE_H
+#define _JUMP_MONOTONIC_QUEUE_H 1
+
+#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);
+
+#endif
+
+/* vim: set ts=8 sw=8 sts=8 noet: */

ringbuf.c

@@ -0,0 +1,64 @@
+#include <math.h>
+#include "ringbuf.h"
+
+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));
+}
+
+/* vim: set ts=8 sw=8 sts=8 noet: */

ringbuf.h

@@ -0,0 +1,25 @@
+#ifndef _JUMP_RINGBUF_H
+#define _JUMP_RINGBUF_H 1
+
+#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);
+
+#endif
+
+/* vim: set ts=8 sw=8 sts=8 noet: */

schmidt_trigger.c

@@ -0,0 +1,31 @@
+#include "schmidt_trigger.h"
+
+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;
+}
+
+/* vim: set ts=8 sw=8 sts=8 noet: */

schmidt_trigger.h

@@ -0,0 +1,17 @@
+#ifndef _JUMP_SCHMIDT_TRIGGER_H
+#define _JUMP_SCHMIDT_TRIGGER_H 1
+
+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);
+
+#endif
+
+/* vim: set ts=8 sw=8 sts=8 noet: */