altos: Add checksum to TELEM output lines

[fw/altos] / src / ao_monitor.c

diff --git a/src/ao_monitor.c b/src/ao_monitor.c
index 248857debca265afbd35c127382e7fd20a896faf..1d083c59eff4de019647ccaf7ca565946cb975ed 100644 (file)
--- a/src/ao_monitor.c
+++ b/src/ao_monitor.c
@@ -26,6 +26,7 @@ ao_monitor(void)
 {
        __xdata char callsign[AO_MAX_CALLSIGN+1];
        __xdata union {
+               struct ao_telemetry_raw_recv    raw;
                struct ao_telemetry_orig_recv   orig;
                struct ao_telemetry_tiny_recv   tiny;
        } u;
@@ -35,6 +36,7 @@ ao_monitor(void)
 #define recv_tiny      (u.tiny)
 
        uint8_t state;
+       uint8_t sum, byte;
        int16_t rssi;
 
        for (;;) {
@@ -185,6 +187,20 @@ ao_monitor(void)
                                printf("CRC INVALID RSSI %3d\n", rssi);
                        }
                        break;
+               default:
+                       if (ao_monitoring > AO_MAX_TELEMETRY)
+                               ao_monitoring = AO_MAX_TELEMETRY;
+                       if (!ao_radio_recv(&recv_raw, ao_monitoring))
+                               continue;
+                       printf ("TELEM %02x", ao_monitoring+2);
+                       sum = 0x5a;
+                       for (state = 0; state < ao_monitoring + 2; state++) {
+                               byte = recv_raw.packet[state];
+                               sum += byte;
+                               printf("%02x", byte);
+                       }
+                       printf("%02x\n", sum);
+                       break;
                }
                ao_usb_flush();
                ao_led_toggle(ao_monitor_led);