X-Git-Url: http://git.droids-corp.org/?p=protos%2Fxbee-avr.git;a=blobdiff_plain;f=rc_proto.c;h=1c1a29fec54d5714af6d5b39b154eb832b5d6af5;hp=d8c4727e6497bee7f122eb16a0d4f3b45d436dbe;hb=66e8a69f26ffb8039688651235f223428383dac9;hpb=d8ce6478238b8e6093c9c0a18325d51d6dcf7e06

diff --git a/rc_proto.c b/rc_proto.c
index d8c4727..1c1a29f 100644
--- a/rc_proto.c
+++ b/rc_proto.c
@@ -51,13 +51,13 @@
 
 #define RX_DB_THRESHOLD 65 /* mean -65 dB */
 
-/* XXX make it configurable */
-/* min time between 2 servo_send */
-#define SEND_SERVO_MIN_TIME_MS 50
-/* max time between 2 servo_send */
-#define SEND_SERVO_MAX_TIME_MS 300
-/* time before switching into bypass mode when no servo command received */
-#define AUTO_BYPASS_TIME_MS 500
+/* default values */
+struct rc_proto_timers rc_proto_timers = {
+	.send_servo_min_ms = 50,
+	.send_servo_max_ms = 300,
+	.send_power_probe_ms = 500,
+	.autobypass_ms = 500,
+};
 
 /* rc_proto statistics, accessed with sched_prio=XBEE_PRIO */
 struct rc_proto_stats_data {
@@ -149,7 +149,7 @@ static void rc_proto_rx_power_probe(int power_level)
 		(void *)(intptr_t)power_level);
 }
 
-/* called every second */
+/* called every second to decide which power should be used */
 static void compute_best_power(void)
 {
 	int8_t best_power_level = -1;
@@ -183,8 +183,13 @@ static void compute_best_power(void)
 	power_level_global = best_power_level;
 }
 
-static uint8_t get_best_power(void)
+/* return the best power level, or -1 if best power level computation is
+ * disabled. */
+static int8_t get_best_power(void)
 {
+	if ((rc_proto_flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) == 0)
+		return -1;
+
 	/* special values */
 	if (power_level_global == -1) {
 		power_level_global = -4;
@@ -264,7 +269,7 @@ int8_t rc_proto_send_echo_req(uint64_t addr, void *data, uint8_t data_len,
 
 /* send an echo message: expect a reply */
 int8_t rc_proto_send_echo_ans(uint64_t addr, void *data, uint8_t data_len,
-	int8_t power)
+	int8_t power, uint16_t timestamp)
 {
 	struct rc_proto_echo_ans hdr;
 	struct xbee_msg msg;
@@ -273,6 +278,7 @@ int8_t rc_proto_send_echo_ans(uint64_t addr, void *data, uint8_t data_len,
 
 	hdr.type = RC_PROTO_ECHO_ANS;
 	hdr.datalen = data_len;
+	hdr.timestamp = timestamp;
 
 	msg.iovlen = 2;
 	msg.iov[0].buf = &hdr;
@@ -368,7 +374,7 @@ static int8_t rc_proto_send_servos(void)
 	/* if we transmitted servos values recently, nothing to do */
 	ms = get_time_ms();
 	diff = ms - servo_tx.time;
-	if (diff < SEND_SERVO_MIN_TIME_MS)
+	if (diff < rc_proto_timers.send_servo_min_ms)
 		return 0;
 
 	/* prepare values to send */
@@ -401,7 +407,7 @@ static int8_t rc_proto_send_servos(void)
 	/* if no value changed and last message is acknowledged, don't transmit
 	 * if we already transmitted quite recently */
 	if (updated == 0 && ack == servo_tx.seq &&
-		diff < SEND_SERVO_MAX_TIME_MS)
+		diff < rc_proto_timers.send_servo_max_ms)
 		return 0;
 
 	/* ok, we need to transmit */
@@ -607,7 +613,8 @@ int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len)
 			stats.echo_req_rx++;
 
 			if (rc_proto_send_echo_ans(ntohll(recvframe->srcaddr),
-					rce->data, rce->datalen, power) < 0)
+					rce->data, rce->datalen, power,
+					rce->timestamp) < 0)
 				return -1;
 
 			return 0;
@@ -711,7 +718,7 @@ static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 	/* send power probe periodically */
 	if (rc_proto_flags & RC_PROTO_FLAGS_TX_POW_PROBE) {
 		diff = t - prev_power_probe;
-		if (diff > AUTO_BYPASS_TIME_MS) {
+		if (diff > rc_proto_timers.send_power_probe_ms) {
 			pow_probe++;
 			if (pow_probe > 4)
 				pow_probe = 0;
@@ -723,15 +730,17 @@ static void rc_proto_cb(struct callout_mgr *cm, struct callout *tim, void *arg)
 	/* on wing, auto bypass servos if no commands since some time */
 	if (rc_proto_flags & RC_PROTO_FLAGS_RX_AUTOBYPASS) {
 		diff = t - servo_rx.time;
-		if (diff > AUTO_BYPASS_TIME_MS)
+		if (diff > rc_proto_timers.autobypass_ms)
 			spi_servo_set_bypass(1);
 	}
 
 	/* send stats to peer every second */
-	diff = t - prev_compute_pow;
-	if (diff >= 1000) {
-		compute_best_power();
-		prev_compute_pow = t;
+	if (rc_proto_flags & RC_PROTO_FLAGS_COMPUTE_BEST_POW) {
+		diff = t - prev_compute_pow;
+		if (diff >= 1000) {
+			compute_best_power();
+			prev_compute_pow = t;
+		}
 	}
 
 	/* send stats to peer every second */
@@ -783,7 +792,7 @@ void rc_proto_dump_stats(void)
 	printf_P(PSTR("  servo_tx: %"PRIu32"\r\n"), peer_stats.servo_tx);
 	printf_P(PSTR("  stats_rx: %"PRIu32"\r\n"), peer_stats.stats_rx);
 	printf_P(PSTR("  stats_tx: %"PRIu32"\r\n"), peer_stats.stats_tx);
-	if (stats.echo_ans_rx != 0) {
+	if (peer_stats.echo_ans_rx != 0) {
 		printf_P(PSTR("  echo_ans_latency_ms: %"PRIu32"\r\n"),
 			peer_stats.echo_ans_latency_sum / peer_stats.echo_ans_rx);
 	}