beep when GPS ready

[protos/xbee-avr.git] / rc_proto.c

diff --git a/rc_proto.c b/rc_proto.c
index cc6cce4..1c1a29f 100644 (file)
--- a/rc_proto.c
+++ b/rc_proto.c
@@ -51,15 +51,15 @@
 
 #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
-
-/* rc_proto statistics */
+/* 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 {
        uint32_t hello_rx;
        uint32_t hello_tx;
@@ -75,6 +75,7 @@ struct rc_proto_stats_data {
        uint32_t servo_tx;
        uint32_t stats_rx;
        uint32_t stats_tx;
+       uint32_t echo_ans_latency_sum;
 };
 static struct rc_proto_stats_data stats; /* local stats */
 static struct rc_proto_stats_data peer_stats; /* peer stats */
@@ -148,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;
@@ -182,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;
@@ -239,6 +245,7 @@ int8_t rc_proto_send_echo_req(uint64_t addr, void *data, uint8_t data_len,
 
        hdr.type = RC_PROTO_ECHO_REQ;
        hdr.power = power;
+       hdr.timestamp = get_time_ms();
        hdr.datalen = data_len;
 
        msg.iovlen = 2;
@@ -262,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;
@@ -271,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;
@@ -366,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 */
@@ -399,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 */
@@ -605,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;
@@ -614,9 +623,12 @@ int rc_proto_rx(struct xbee_recv_hdr *recvframe, unsigned len)
                case RC_PROTO_ECHO_ANS: {
                        struct rc_proto_echo_ans *rce =
                                (struct rc_proto_echo_ans *) recvframe->data;
+                       uint16_t diff;
 
                        NOTICE(E_USER_XBEE, "recv echo_ans len=%d", rce->datalen);
                        stats.echo_ans_rx++;
+                       diff = get_time_ms() - rce->timestamp;
+                       stats.echo_ans_latency_sum += diff;
                        return 0;
                }
 
@@ -706,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;
@@ -718,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 */
@@ -758,6 +772,10 @@ void rc_proto_dump_stats(void)
        printf_P(PSTR("  servo_tx: %"PRIu32"\r\n"), stats.servo_tx);
        printf_P(PSTR("  stats_rx: %"PRIu32"\r\n"), stats.stats_rx);
        printf_P(PSTR("  stats_tx: %"PRIu32"\r\n"), stats.stats_tx);
+       if (stats.echo_ans_rx != 0) {
+               printf_P(PSTR("  echo_ans_latency_ms: %"PRIu32"\r\n"),
+                       stats.echo_ans_latency_sum / stats.echo_ans_rx);
+       }
 
        printf_P(PSTR("rc_proto stats PEER\r\n"));
        printf_P(PSTR("  hello_tx: %"PRIu32"\r\n"), peer_stats.hello_tx);
@@ -774,6 +792,19 @@ 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 (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);
+       }
+}
+
+void rc_proto_reset_stats(void)
+{
+       uint8_t prio;
+
+       prio = callout_mgr_set_prio(&xbeeboard.intr_cm, XBEE_PRIO);
+       memset(&stats, 0, sizeof(stats));
+       callout_mgr_restore_prio(&xbeeboard.intr_cm, prio);
 }
 
 void rc_proto_dump_servos(void)