Remove monitor/rssi functions from telemetrum load

[fw/altos] / ao_flight_test.c

diff --git a/ao_flight_test.c b/ao_flight_test.c
index aefe3da748ad2e0da07f6877d52b439eec2b4471..0b03eb1f59c3f24fb39490ee8bd3ba9017590489 100644 (file)
--- a/ao_flight_test.c
+++ b/ao_flight_test.c
@@ -108,7 +108,7 @@ const char const * const ao_state_names[] = {
 };
 
 struct ao_cmds {
-       uint8_t         cmd;
+       char            cmd;
        void            (*func)(void);
        const char      *help;
 };
@@ -163,6 +163,9 @@ ao_insert(void)
 }
 
 static int     ao_records_read = 0;
+static int     ao_eof_read = 0;
+static int     ao_flight_ground_accel;
+static int     ao_flight_started = 0;
 
 void
 ao_sleep(void *wchan)
@@ -173,24 +176,35 @@ ao_sleep(void *wchan)
                uint16_t        tick;
                uint16_t        a, b;
                int             ret;
+               char            line[1024];
 
                for (;;) {
                        if (ao_records_read > 20 && ao_flight_state == ao_flight_startup)
                        {
+                               ao_adc_static.accel = ao_flight_ground_accel;
                                ao_insert();
                                return;
                        }
 
-                       ret = fscanf(emulator_in, "%c %hx %hx %hx\n", &type, &tick, &a, &b);
-                       if (ret == EOF) {
-                               printf ("no more data, exiting simulation\n");
-                               exit(0);
+                       if (!fgets(line, sizeof (line), emulator_in)) {
+                               if (++ao_eof_read >= 1000) {
+                                       printf ("no more data, exiting simulation\n");
+                                       exit(0);
+                               }
+                               ao_adc_static.tick += 10;
+                               ao_insert();
                                return;
                        }
+                       ret = sscanf(line, "%c %hx %hx %hx", &type, &tick, &a, &b);
                        if (ret != 4)
                                continue;
+                       if (type != 'F' && !ao_flight_started)
+                               continue;
+
                        switch (type) {
                        case 'F':
+                               ao_flight_ground_accel = a;
+                               ao_flight_started = 1;
                                break;
                        case 'S':
                                break;
@@ -226,10 +240,10 @@ ao_dump_state(void)
                return;
        printf ("\t\t\t\t\t%s accel %g vel %g alt %d main %d\n",
                ao_state_names[ao_flight_state],
-               (ao_flight_accel - ao_ground_accel) / COUNTS_PER_G * GRAVITY,
+               (ao_ground_accel - ao_flight_accel) / COUNTS_PER_G * GRAVITY,
                (double) ao_flight_vel / 100 / COUNTS_PER_G * GRAVITY,
-               ao_pres_to_altitude(ao_flight_pres),
-               ao_pres_to_altitude(ao_main_pres));
+               ao_pres_to_altitude(ao_flight_pres) - ao_pres_to_altitude(ao_ground_pres),
+               ao_pres_to_altitude(ao_main_pres) - ao_pres_to_altitude(ao_ground_pres));
        if (ao_flight_state == ao_flight_landed)
                exit(0);
 }