gather latency stats

diff --git a/rc_proto.c b/rc_proto.c
index 8c9166517ffc0a548250b79156604a086e10f599..d8c4727e6497bee7f122eb16a0d4f3b45d436dbe 100644 (file)
--- a/rc_proto.c
+++ b/rc_proto.c
@@ -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,10 @@ 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)