beep when GPS ready

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

diff --git a/rc_proto.c b/rc_proto.c
index 4c09ac3..1c1a29f 100644 (file)
--- a/rc_proto.c
+++ b/rc_proto.c
@@ -269,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;
@@ -278,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;
@@ -612,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;
@@ -790,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);
        }