jump/jump_rope_count_device.c

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