gather latency stats

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

diff --git a/rc_proto.c b/rc_proto.c
index cc6cce4..d8c4727 100644 (file)
--- a/rc_proto.c
+++ b/rc_proto.c
@@ -59,7 +59,7 @@
 /* time before switching into bypass mode when no servo command received */
 #define AUTO_BYPASS_TIME_MS 500
 
-/* rc_proto statistics */
+/* 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 */
@@ -239,6 +240,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;
@@ -614,9 +616,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;
                }
 
@@ -758,6 +763,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 +783,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 (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)