ao-tools: Support MM telemetry packets in ao-telem

Parse the new packet formats

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/ao-tools/ao-telem/ao-telem.c b/ao-tools/ao-telem/ao-telem.c
index 384b2fc835253950678898e05a76ff703edfee39..e7fc8e268712c5251aa1cbf126d6c13b76b7efe6 100644 (file)
--- a/ao-tools/ao-telem/ao-telem.c
+++ b/ao-tools/ao-telem/ao-telem.c
@@ -82,7 +82,6 @@ main (int argc, char **argv)
                                case AO_TELEMETRY_SENSOR_TELEMETRUM:
                                case AO_TELEMETRY_SENSOR_TELEMINI:
                                case AO_TELEMETRY_SENSOR_TELENANO:
-                               case AO_TELEMETRY_SENSOR_MEGAMETRUM:
                                        printf ("state %1d accel %5d pres %5d ",
                                                telem.sensor.state, telem.sensor.accel, telem.sensor.pres);
                                        printf ("accel %6.2f speed %6.2f height %5d ",
@@ -152,6 +151,43 @@ main (int argc, char **argv)
                                                        telem.satellite.sats[c].c_n_1);
                                        }
                                        printf ("\n");
+                                       break;
+                               case AO_TELEMETRY_MEGA_SENSOR:
+                                       printf ("accel %5d pres %9d temp %5d accel_x %5d accel_y %5d accel_z %5d gyro_x %5d gyro_y %5d gyro_z %5d mag_x %5d mag_y %5d mag_z %5d\n",
+                                               telem.mega_sensor.accel,
+                                               telem.mega_sensor.pres,
+                                               telem.mega_sensor.temp,
+                                               telem.mega_sensor.accel_x,
+                                               telem.mega_sensor.accel_y,
+                                               telem.mega_sensor.accel_z,
+                                               telem.mega_sensor.gyro_x,
+                                               telem.mega_sensor.gyro_y,
+                                               telem.mega_sensor.gyro_z,
+                                               telem.mega_sensor.mag_x,
+                                               telem.mega_sensor.mag_y,
+                                               telem.mega_sensor.mag_z);
+                                       break;
+                               case AO_TELEMETRY_MEGA_DATA:
+                                       printf ("state %1d v_batt %5d v_pyro %5d ",
+                                               telem.mega_data.state,
+                                               telem.mega_data.v_batt,
+                                               telem.mega_data.v_pyro);
+                                       for (c = 0; c < 6; c++)
+                                               printf ("s%1d %5d ", c,
+                                                       telem.mega_data.sense[c] |
+                                                       (telem.mega_data.sense[c] << 8));
+                                       
+                                       printf ("ground_pres %5d ground_accel %5d accel_plus %5d accel_minus %5d ",
+                                               telem.mega_data.ground_pres,
+                                               telem.mega_data.ground_accel,
+                                               telem.mega_data.accel_plus_g,
+                                               telem.mega_data.accel_minus_g);
+
+                                       printf ("accel %6.2f speed %6.2f height %5d\n",
+                                               telem.mega_data.acceleration / 16.0,
+                                               telem.mega_data.speed / 16.0,
+                                               telem.mega_data.height);
+
                                        break;
                                default:
                                        printf("\n");

diff --git a/ao-tools/lib/cc-telemetry.h b/ao-tools/lib/cc-telemetry.h
index 4e064a97a8bdba7ea448426f57d3b70d71a4095e..e849cd3b66e1c33422b6e7bc0251436e8490a99e 100644 (file)
--- a/ao-tools/lib/cc-telemetry.h
+++ b/ao-tools/lib/cc-telemetry.h
@@ -37,7 +37,6 @@ struct ao_telemetry_generic {
 #define AO_TELEMETRY_SENSOR_TELEMETRUM 0x01
 #define AO_TELEMETRY_SENSOR_TELEMINI   0x02
 #define AO_TELEMETRY_SENSOR_TELENANO   0x03
-#define AO_TELEMETRY_SENSOR_MEGAMETRUM 0x08
 
 struct ao_telemetry_sensor {
        uint16_t        serial;         /*  0 */
@@ -151,6 +150,57 @@ struct ao_telemetry_companion {
        /* 32 */
 };
        
+#define AO_TELEMETRY_MEGA_SENSOR       0x08
+
+struct ao_telemetry_mega_sensor {
+       uint16_t        serial;         /*  0 */
+       uint16_t        tick;           /*  2 */
+       uint8_t         type;           /*  4 */
+
+       uint8_t         pad5;           /*  5 */
+       int16_t         accel;          /*  6 Z axis */
+
+       int32_t         pres;           /*  8 Pa * 10 */
+       int16_t         temp;           /* 12 °C * 100 */
+
+       int16_t         accel_x;        /* 14 */
+       int16_t         accel_y;        /* 16 */
+       int16_t         accel_z;        /* 18 */
+
+       int16_t         gyro_x;         /* 20 */
+       int16_t         gyro_y;         /* 22 */
+       int16_t         gyro_z;         /* 24 */
+
+       int16_t         mag_x;          /* 26 */
+       int16_t         mag_y;          /* 28 */
+       int16_t         mag_z;          /* 30 */
+       /* 32 */
+};
+       
+#define AO_TELEMETRY_MEGA_DATA         0x09
+
+struct ao_telemetry_mega_data {
+       uint16_t        serial;         /*  0 */
+       uint16_t        tick;           /*  2 */
+       uint8_t         type;           /*  4 */
+
+       uint8_t         state;          /*  5 flight state */
+
+       int16_t         v_batt;         /*  6 battery voltage */
+       int16_t         v_pyro;         /*  8 pyro battery voltage */
+       int8_t          sense[6];       /* 10 continuity sense */
+
+       int32_t         ground_pres;    /* 16 average pres on pad */
+       int16_t         ground_accel;   /* 20 average accel on pad */
+       int16_t         accel_plus_g;   /* 22 accel calibration at +1g */
+       int16_t         accel_minus_g;  /* 24 accel calibration at -1g */
+
+       int16_t         acceleration;   /* 26 m/s² * 16 */
+       int16_t         speed;          /* 28 m/s * 16 */
+       int16_t         height;         /* 30 m */
+       /* 32 */
+};
+
 /* #define AO_SEND_ALL_BARO */
 
 #define AO_TELEMETRY_BARO              0x80
@@ -182,6 +232,8 @@ union ao_telemetry_all {
        struct ao_telemetry_location            location;
        struct ao_telemetry_satellite           satellite;
        struct ao_telemetry_companion           companion;
+       struct ao_telemetry_mega_sensor         mega_sensor;
+       struct ao_telemetry_mega_data           mega_data;
        struct ao_telemetry_baro                baro;
 };