jump/all.c

#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: */