Merge branch 'valley'

all.c

@@ -34,7 +34,7 @@ static float _fmodf(float x, float y)
 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";
+	"g,a,g_std,a_std,g_up,a_up,is_out,valley";
 
 struct schmidt
 {
@@ -85,10 +85,14 @@ 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);
 
+typedef int (*jump_rope_count_valley_cmp)(float valley, float prev_valley);
+
 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, dead_zone_time;
+	int record_valley_time, prev_valley_lifetime;
+	jump_rope_count_valley_cmp valley_cmp;
 };
 
 struct jump_rope_count_device
@@ -97,8 +101,13 @@ struct jump_rope_count_device
 	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, dead_zone_time;
+	int record_valley_time, prev_valley_lifetime;
+	jump_rope_count_valley_cmp valley_cmp;
+
 	int cy_suppress, wait_remain_time, dead_zone_remain_time;
+	int recording_valley, has_prev_valley;
 	float last_cy, last_cy_fixed;
+	float valley, prev_valley;
 };
 
 struct sensor_packet
@@ -167,6 +176,18 @@ int jump_rope_count_device_init(struct jump_rope_count_device *dev,
 	dev->last_cy = dev->last_cy_fixed = 0;
 	dev->wait_remain_time = dev->dead_zone_remain_time = 0;
 
+	dev->record_valley_time = cfg->record_valley_time;
+	dev->prev_valley_lifetime = cfg->prev_valley_lifetime;
+	dev->valley_cmp = cfg->valley_cmp;
+
+	if (dev->record_valley_time < 0)
+		dev->record_valley_time = 1;
+
+	dev->has_prev_valley = 0;
+
+	/* an initial value only used for data recording */
+	dev->valley = cfg->lgz;
+
 	return 0;
 }
 
@@ -228,6 +249,27 @@ int process_packet(struct jump_rope_count_device *dev,
 	if (ret != 0)
 		return ret;
 
+	if (dev->has_prev_valley > 0)
+		dev->has_prev_valley--;
+
+	if (dev->recording_valley) {
+		if (--dev->recording_valley == 0 &&
+		    (!dev->has_prev_valley ||
+		     dev->valley_cmp(dev->valley, dev->prev_valley))) {
+			dev->has_prev_valley = dev->prev_valley_lifetime;
+			dev->prev_valley = dev->valley;
+			*result = RESULT_TRIGGERED;
+			goto out;
+		}
+
+		if (dev->recording_valley) {
+			if (dev->valley > gz)
+				dev->valley = gz;
+			*result = RESULT_NONE;
+			goto out;
+		}
+	}
+
 	ret = monotonic_queue_get_min(&dev->mq_min_cy, &min);
 	if (ret != 0)
 		return ret;
@@ -255,27 +297,30 @@ int process_packet(struct jump_rope_count_device *dev,
 	{
 		dev->wait_remain_time = dev->wait_time;
 		dev->dead_zone_remain_time = dev->dead_zone_time;
-		*result = RESULT_TRIGGERED;
+		dev->recording_valley = dev->record_valley_time;
+		dev->valley = gz;
+		*result = RESULT_NONE;
 		goto out;
 	}
 
+	*result = (dev->wait_remain_time > 0 ? RESULT_NONE :
+					       RESULT_INACTIVE);
+out:
 	if (dev->wait_remain_time)
 		dev->wait_remain_time--;
 
 	if (dev->dead_zone_remain_time)
 		dev->dead_zone_remain_time--;
 
-	*result = (dev->wait_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" */
+	"g,a,g_std,a_std,g_up,a_up,is_out,valley" */
 	fprintf(data_recorder, "%.7f,%.7f,%.7f,",
 		packet->gx, packet->gy, packet->gz);
 	fprintf(data_recorder, "%.7f,%.7f,%.7f,",
@@ -284,10 +329,11 @@ out:
 		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",
+	fprintf(data_recorder, "%d,%d,%d,%.7f\n",
 		schmidt_get(&dev->trig_g),
 		schmidt_get(&dev->trig_a),
-		*result == RESULT_TRIGGERED);
+		*result == RESULT_TRIGGERED,
+		dev->valley);
 
 	return 0;
 }
@@ -436,6 +482,11 @@ int schmidt_get(struct schmidt *schmidt)
 
 const int fs = 50;
 
+int valley_cmp_naive(float valley, float prev_valley)
+{
+	return prev_valley + 2.0 > valley;
+}
+
 int main()
 {
 	struct jump_rope_count_config cfg;
@@ -462,6 +513,9 @@ int main()
 	cfg.cy_suppress_time = fs * 0.2;
 	cfg.wait_time = fs * 1;
 	cfg.dead_zone_time = fs * 0.2;
+	cfg.record_valley_time = fs * 0.1;
+	cfg.prev_valley_lifetime = fs * 0.5;
+	cfg.valley_cmp = &valley_cmp_naive;
 
 	if (jump_rope_count_device_init(&dev, &cfg) != 0)
 		abort();