Send computed accel/vel/pres values over the radio

These computed values reflect what the flight computer is actually refering
to for state changes, and will be useful in debugging the flight software as
well as provide a filtered view of the data.

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

diff --git a/ao.h b/ao.h
index 76fc00ce4648a954f673e7cb289b1d8230753768..315585293e9b25a51a44b3416ddb465e7f39e50e 100644 (file)
--- a/ao.h
+++ b/ao.h
@@ -568,6 +568,7 @@ extern __pdata enum ao_flight_state ao_flight_state;
 extern __pdata uint16_t                        ao_flight_tick;
 extern __pdata int16_t                 ao_flight_accel;
 extern __pdata int16_t                 ao_flight_pres;
 extern __pdata uint16_t                        ao_flight_tick;
 extern __pdata int16_t                 ao_flight_accel;
 extern __pdata int16_t                 ao_flight_pres;

+extern __pdata int32_t                 ao_flight_vel;

 extern __pdata int16_t                 ao_ground_pres;
 extern __pdata int16_t                 ao_ground_accel;
 extern __pdata int16_t                 ao_min_pres;
 extern __pdata int16_t                 ao_ground_pres;
 extern __pdata int16_t                 ao_ground_accel;
 extern __pdata int16_t                 ao_min_pres;


@@ -723,6 +724,9 @@ ao_gps_report_init(void);
 struct ao_telemetry {
        uint8_t                 addr;
        uint8_t                 flight_state;
 struct ao_telemetry {
        uint8_t                 addr;
        uint8_t                 flight_state;

+       int16_t                 flight_accel;
+       int32_t                 flight_vel;
+       int16_t                 flight_pres;

        struct ao_adc           adc;
        struct ao_gps_data      gps;
        char                    callsign[AO_MAX_CALLSIGN];
        struct ao_adc           adc;
        struct ao_gps_data      gps;
        char                    callsign[AO_MAX_CALLSIGN];

diff --git a/ao_monitor.c b/ao_monitor.c
index 6aed581a69b1f6d71a1b597923860f31428c728e..880f257ca87d1ccfd52ec11882f48150d4f4f51b 100644 (file)
--- a/ao_monitor.c
+++ b/ao_monitor.c
@@ -41,14 +41,17 @@ ao_monitor(void)
                                recv.telemetry.addr,
                                (int) recv.rssi - 74, recv.status,
                                ao_state_names[state]);
                                recv.telemetry.addr,
                                (int) recv.rssi - 74, recv.status,
                                ao_state_names[state]);

-                       printf("%5u a: %5d p: %5d t: %5d v: %5d d: %5d m: %5d ",
+                       printf("%5u a: %5d p: %5d t: %5d v: %5d d: %5d m: %5d fa: %5d fv: %7ld fp: %5d ",

                               recv.telemetry.adc.tick,
                               recv.telemetry.adc.accel,
                               recv.telemetry.adc.pres,
                               recv.telemetry.adc.temp,
                               recv.telemetry.adc.v_batt,
                               recv.telemetry.adc.sense_d,
                               recv.telemetry.adc.tick,
                               recv.telemetry.adc.accel,
                               recv.telemetry.adc.pres,
                               recv.telemetry.adc.temp,
                               recv.telemetry.adc.v_batt,
                               recv.telemetry.adc.sense_d,

-                              recv.telemetry.adc.sense_m);
+                              recv.telemetry.adc.sense_m,
+                              recv.telemetry.flight_accel,
+                              recv.telemetry.flight_vel,
+                              recv.telemetry.flight_pres);

                        ao_gps_print(&recv.telemetry.gps);
                        ao_rssi_set((int) recv.rssi - 74);
                } else {
                        ao_gps_print(&recv.telemetry.gps);
                        ao_rssi_set((int) recv.rssi - 74);
                } else {

diff --git a/ao_telemetry.c b/ao_telemetry.c
index 5299b487bcbd95b00129b7bc4ad1e953096dfb9c..b4461e927a065811dbc5fb2cc93056b061bf456a 100644 (file)
--- a/ao_telemetry.c
+++ b/ao_telemetry.c
@@ -36,6 +36,9 @@ ao_telemetry(void)
                while (ao_telemetry_interval == 0)
                        ao_sleep(&ao_telemetry_interval);
                telemetry.flight_state = ao_flight_state;
                while (ao_telemetry_interval == 0)
                        ao_sleep(&ao_telemetry_interval);
                telemetry.flight_state = ao_flight_state;

+               telemetry.flight_accel = ao_flight_accel;
+               telemetry.flight_vel = ao_flight_vel;
+               telemetry.flight_pres = ao_flight_pres;

                ao_adc_get(&telemetry.adc);
                ao_mutex_get(&ao_gps_mutex);
                memcpy(&telemetry.gps, &ao_gps_data, sizeof (struct ao_gps_data));
                ao_adc_get(&telemetry.adc);
                ao_mutex_get(&ao_gps_mutex);
                memcpy(&telemetry.gps, &ao_gps_data, sizeof (struct ao_gps_data));

diff --git a/aoview/aoview.h b/aoview/aoview.h
index 7824491228a4eb10dfb834961ae2311df1404525..44412bec53b66ed66cb5296f8a9fdf5463678340 100644 (file)
--- a/aoview/aoview.h
+++ b/aoview/aoview.h
@@ -58,6 +58,9 @@ struct aostate {
        int     batt;
        int     drogue;
        int     main;
        int     batt;
        int     drogue;
        int     main;

+       int     flight_accel;
+       int     flight_vel;
+       int     flight_pres;

        int     nsat;
        int     locked;
        struct {
        int     nsat;
        int     locked;
        struct {

diff --git a/aoview/aoview_monitor.c b/aoview/aoview_monitor.c
index 3b3245e268866a77c1941bd26e242eecf75b68ed..b0189c60698d2777935596773f2ae1054c8f6123 100644 (file)
--- a/aoview/aoview_monitor.c
+++ b/aoview/aoview_monitor.c
@@ -82,7 +82,7 @@ aoview_monitor_parse(char *line)
                if (words[nword] == NULL)
                        break;
        }
                if (words[nword] == NULL)
                        break;
        }

-       if (nword < 26)
+       if (nword < 32)

                return;
        if (strcmp(words[0], "CALL") != 0)
                return;
                return;
        if (strcmp(words[0], "CALL") != 0)
                return;


@@ -100,13 +100,16 @@ aoview_monitor_parse(char *line)
        aoview_parse_int(&state.batt, words[18]);
        aoview_parse_int(&state.drogue, words[20]);
        aoview_parse_int(&state.main, words[22]);
        aoview_parse_int(&state.batt, words[18]);
        aoview_parse_int(&state.drogue, words[20]);
        aoview_parse_int(&state.main, words[22]);

-       aoview_parse_int(&state.nsat, words[24]);
-       if (strcmp (words[26], "unlocked") != 0 && nword >= 29) {
+       aoview_parse_int(&state.flight_accel, words[24]);
+       aoview_parse_int(&state.flight_vel, words[26]);
+       aoview_parse_int(&state.flight_pres, words[28]);
+       aoview_parse_int(&state.nsat, words[30]);
+       if (strcmp (words[32], "unlocked") != 0 && nword >= 35) {

                state.locked = 1;
                state.locked = 1;

-               sscanf(words[26], "%d:%d:%d", &state.gps_time.hour, &state.gps_time.minute, &state.gps_time.second);
-               aoview_parse_pos(&state.lat, words[27]);
-               aoview_parse_pos(&state.lon, words[28]);
-               sscanf(words[29], "%dm", &state.alt);
+               sscanf(words[32], "%d:%d:%d", &state.gps_time.hour, &state.gps_time.minute, &state.gps_time.second);
+               aoview_parse_pos(&state.lat, words[33]);
+               aoview_parse_pos(&state.lon, words[34]);
+               sscanf(words[35], "%dm", &state.alt);

        } else {
                state.locked = 0;
                state.gps_time.hour = state.gps_time.minute = state.gps_time.second = 0;
        } else {
                state.locked = 0;
                state.gps_time.hour = state.gps_time.minute = state.gps_time.second = 0;

diff --git a/aoview/aoview_state.c b/aoview/aoview_state.c
index 85ed7b5a629964442e131793d87c949981aa6dae..dda92af9a2993277ca4f62ec3bc1430e05c01d1c 100644 (file)
--- a/aoview/aoview_state.c
+++ b/aoview/aoview_state.c
@@ -20,7 +20,6 @@
 
 static int     pad_pres;
 static int     pad_accel;
 
 static int     pad_pres;
 static int     pad_accel;

-

 static int     pad_pres_total;
 static int     pad_accel_total;
 static double  pad_lat_total;
 static int     pad_pres_total;
 static int     pad_accel_total;
 static double  pad_lat_total;


@@ -30,7 +29,6 @@ static int    npad;
 static int     npad_gps;
 static int     prev_tick;
 static double  prev_accel;
 static int     npad_gps;
 static int     prev_tick;
 static double  prev_accel;

-static double  velocity;

 static double  pad_lat;
 static double  pad_lon;
 static double  pad_alt;
 static double  pad_lat;
 static double  pad_lon;
 static double  pad_alt;


@@ -80,19 +78,19 @@ aoview_state_notify(struct aostate *state)
 {
        int     altitude;
        double  accel;
 {
        int     altitude;
        double  accel;

-       double  velocity_change;

        int     ticks;
        double  dist;
        double  bearing;
        double  temp;
        int     ticks;
        double  dist;
        double  bearing;
        double  temp;

+       double  velocity;

        double  battery;
        double  drogue_sense, main_sense;
        double  max_accel;
 
        if (!strcmp(state->state, "pad")) {
                if (npad < NUM_PAD_SAMPLES) {
        double  battery;
        double  drogue_sense, main_sense;
        double  max_accel;
 
        if (!strcmp(state->state, "pad")) {
                if (npad < NUM_PAD_SAMPLES) {

-                       pad_accel_total += state->accel;
-                       pad_pres_total += state->pres;
+                       pad_accel_total += state->flight_accel;
+                       pad_pres_total += state->flight_pres;

                        if (state->locked) {
                                pad_lat_total += state->lat;
                                pad_lon_total += state->lon;
                        if (state->locked) {
                                pad_lat_total += state->lat;
                                pad_lon_total += state->lon;


@@ -100,7 +98,6 @@ aoview_state_notify(struct aostate *state)
                                npad_gps++;
                        }
                        npad++;
                                npad_gps++;
                        }
                        npad++;

-                       velocity = 0;

                }
                if (npad <= NUM_PAD_SAMPLES) {
                        pad_pres = pad_pres_total / npad;
                }
                if (npad <= NUM_PAD_SAMPLES) {
                        pad_pres = pad_pres_total / npad;


@@ -113,21 +110,19 @@ aoview_state_notify(struct aostate *state)
                }
                if (npad == NUM_PAD_SAMPLES) {
                        npad++;
                }
                if (npad == NUM_PAD_SAMPLES) {
                        npad++;

-                       velocity = 0;

                        min_pres = pad_pres;
                        min_accel = pad_accel;
                }
        }
                        min_pres = pad_pres;
                        min_accel = pad_accel;
                }
        }

-       if (state->pres < min_pres)
-               min_pres = state->pres;
-       if (state->accel < min_accel)
-               min_accel = state->accel;
-       altitude = aoview_pres_to_altitude(state->pres) - aoview_pres_to_altitude(pad_pres);
-       accel = (pad_accel - state->accel) / 264.8 *  9.80665;
-       max_accel = (pad_accel - min_accel) / 264.8 * 9.80665;
-       velocity_change = (accel + prev_accel) / 2.0;
+       if (state->flight_pres < min_pres)
+               min_pres = state->flight_pres;
+       if (state->flight_accel < min_accel)
+               min_accel = state->flight_accel;
+       altitude = aoview_pres_to_altitude(state->flight_pres) - aoview_pres_to_altitude(pad_pres);
+       accel = (pad_accel - state->flight_accel) / 27.0;
+       velocity = state->flight_vel / 2700.0;
+       max_accel = (pad_accel - min_accel) / 27.0;

        ticks = state->tick - prev_tick;
        ticks = state->tick - prev_tick;

-       velocity -= velocity_change * (ticks / 100.0);

        temp = ((state->temp / 32767.0 * 3.3) - 0.5) / 0.01;
        battery = (state->batt / 32767.0 * 5.0);
        drogue_sense = (state->drogue / 32767.0 * 15.0);
        temp = ((state->temp / 32767.0 * 3.3) - 0.5) / 0.01;
        battery = (state->batt / 32767.0 * 5.0);
        drogue_sense = (state->drogue / 32767.0 * 15.0);


@@ -186,9 +181,22 @@ aoview_state_notify(struct aostate *state)
 void
 aoview_state_new(void)
 {
 void
 aoview_state_new(void)
 {

+       pad_pres = 0;
+       pad_accel = 0;
+       pad_pres_total = 0;
+       pad_accel_total = 0;
+       pad_lat_total = 0;
+       pad_lon_total = 0;
+       pad_alt_total = 0;
+       npad = 0;
+       npad_gps = 0;
+       prev_tick = 0;
+       prev_accel = 0;
+       pad_lat = 0;
+       pad_lon = 0;
+       pad_alt = 0;

        min_pres = 32767;
        min_accel = 32767;
        min_pres = 32767;
        min_accel = 32767;

-       npad = 0;

 }
 
 void
 }
 
 void