git.gag.com Git - fw/altos/blob - src/test/ao_flight_test.c

   1 /*
   2  * Copyright © 2009 Keith Packard <keithp@keithp.com>
   3  *
   4  * This program is free software; you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation; either version 2 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * This program is distributed in the hope that it will be useful, but
  10  * WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12  * General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License along
  15  * with this program; if not, write to the Free Software Foundation, Inc.,
  16  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  17  */
  18
  19 #define _GNU_SOURCE
  20
  21 #include <stdint.h>
  22 #include <stdbool.h>
  23 #include <stdio.h>
  24 #include <stdlib.h>
  25 #include <stddef.h>
  26 #include <string.h>
  27 #include <getopt.h>
  28 #include <math.h>
  29 #define log ao_log_data
  30
  31 #define AO_TICK_TYPE    uint32_t
  32 #define AO_TICK_SIGNED  int32_t
  33
  34 typedef int32_t pres_t;
  35 #define pres_to_altitude(p) ao_pa_to_altitude(p)
  36 #define ao_data_pres_cook(packet) ao_ms5607_convert(&packet->ms5607_raw, &packet->ms5607_cooked)
  37 #define ao_data_pres(packet) ((packet)->ms5607_cooked.pres)
  38 #define AO_ADC_MAX      4095
  39 #define AO_PYRO_BATTERY_DIV_PLUS        100
  40 #define AO_PYRO_BATTERY_DIV_MINUS       27
  41 #define AO_IGNITE_DIV_PLUS      100
  42 #define AO_IGNITE_DIV_MINUS     27
  43 #define AO_ADC_REFERENCE_DV     33
  44
  45 #define GRAVITY 9.80665
  46
  47 #define AO_HERTZ        100
  48
  49 #define HAS_ADC 1
  50 #define AO_DATA_RING    64
  51 #define ao_data_ring_next(n)    (((n) + 1) & (AO_DATA_RING - 1))
  52 #define ao_data_ring_prev(n)    (((n) - 1) & (AO_DATA_RING - 1))
  53
  54 #if 0
  55 #define AO_M_TO_HEIGHT(m)       ((int16_t) (m))
  56 #define AO_MS_TO_SPEED(ms)      ((int16_t) ((ms) * 16))
  57 #define AO_MSS_TO_ACCEL(mss)    ((int16_t) ((mss) * 16))
  58 #endif
  59
  60 #define AO_GPS_NEW_DATA         1
  61 #define AO_GPS_NEW_TRACKING     2
  62
  63 int ao_gps_new;
  64
  65 #if !defined(TELEMEGA) && !defined(TELEMETRUM_V2) && !defined(EASYMINI) && !defined(EASYMOTOR_V_2) && !defined(TELEMEGA_V4)
  66 #define TELEMETRUM_V1 1
  67 #endif
  68
  69 #if TELEMEGA
  70 #define AO_ADC_NUM_SENSE        6
  71 #define HAS_MS5607              1
  72 #define HAS_MPU6000             1
  73 #define HAS_MMA655X             1
  74 #define HAS_HMC5883             1
  75 #define HAS_BEEP                1
  76 #define HAS_BARO                1
  77 #define AO_CONFIG_MAX_SIZE      1024
  78 #define AO_MMA655X_INVERT       0
  79
  80 struct ao_adc {
  81         int16_t                 sense[AO_ADC_NUM_SENSE];
  82         int16_t                 v_batt;
  83         int16_t                 v_pbatt;
  84         int16_t                 temp;
  85 };
  86 #endif
  87
  88 #if TELEMEGA_V4
  89 #define AO_ADC_NUM_SENSE        6
  90 #define HAS_MS5607              1
  91 #define HAS_BMX160              1
  92 #define HAS_ADXL375             1
  93 #define AO_ADXL375_INVERT       1
  94 #define AO_ADXL375_AXIS         x
  95 #define HAS_BEEP                1
  96 #define HAS_BARO                1
  97 #define AO_CONFIG_MAX_SIZE      1024
  98
  99 struct ao_adc {
 100         int16_t                 sense[AO_ADC_NUM_SENSE];
 101         int16_t                 v_batt;
 102         int16_t                 v_pbatt;
 103         int16_t                 temp;
 104 };
 105
 106 #define ao_data_along(packet)   ((packet)->bmx160.acc_x)
 107 #define ao_data_across(packet)  (-(packet)->bmx160.acc_y)
 108 #define ao_data_through(packet) ((packet)->bmx160.acc_z)
 109
 110 #define ao_data_roll(packet)    ((packet)->bmx160.gyr_x)
 111 #define ao_data_pitch(packet)   (-(packet)->bmx160.gyr_y)
 112 #define ao_data_yaw(packet)     ((packet)->bmx160.gyr_z)
 113
 114 #define ao_data_mag_along(packet)       ((packet)->bmx160.mag_x)
 115 #define ao_data_mag_across(packet)      ((packet)->bmx160.mag_y)
 116 #define ao_data_mag_through(packet)     ((packet)->bmx160.mag_z)
 117
 118 #define ao_data_set_along(packet,v)     ((packet)->bmx160.acc_x = (v))
 119 #define ao_data_set_across(packet,v)    ((packet)->bmx160.acc_y = -(v))
 120 #define ao_data_set_through(packet,v)   ((packet)->bmx160.acc_z = (v))
 121
 122 #define ao_data_set_roll(packet,v)      ((packet)->bmx160.gyr_x = (v))
 123 #define ao_data_set_pitch(packet,v)     ((packet)->bmx160.gyr_y = -(v))
 124 #define ao_data_set_yaw(packet,v)       ((packet)->bmx160.gyr_z = (v))
 125
 126 #define ao_data_set_mag_along(packet,v) ((packet)->bmx160.mag_x = (v))
 127 #define ao_data_set_mag_across(packet,v)        ((packet)->bmx160.mag_y = (v))
 128 #define ao_data_set_mag_through(packet,v)       ((packet)->bmx160.mag_z = (v))
 129 #endif
 130
 131 #if TELEMETRUM_V2
 132 #define AO_ADC_NUM_SENSE        2
 133 #define HAS_MS5607              1
 134 #define HAS_MMA655X             1
 135 #define AO_MMA655X_INVERT       0
 136 #define HAS_BEEP                1
 137 #define HAS_BARO                1
 138 #define AO_CONFIG_MAX_SIZE      1024
 139
 140 struct ao_adc {
 141         int16_t                 sense_a;
 142         int16_t                 sense_m;
 143         int16_t                 v_batt;
 144         int16_t                 temp;
 145 };
 146 #endif
 147
 148 #if EASYMINI
 149 #define AO_ADC_NUM_SENSE        2
 150 #define HAS_MS5607              1
 151 #define HAS_BEEP                1
 152 #define AO_CONFIG_MAX_SIZE      1024
 153
 154 struct ao_adc {
 155         int16_t                 sense_a;
 156         int16_t                 sense_m;
 157         int16_t                 v_batt;
 158 };
 159 #endif
 160
 161 #if TELEMETRUM_V1
 162 /*
 163  * One set of samples read from the A/D converter
 164  */
 165 struct ao_adc {
 166         int16_t         accel;          /* accelerometer */
 167         int16_t         pres;           /* pressure sensor */
 168         int16_t         pres_real;      /* unclipped */
 169         int16_t         temp;           /* temperature sensor */
 170         int16_t         v_batt;         /* battery voltage */
 171         int16_t         sense_d;        /* drogue continuity sense */
 172         int16_t         sense_m;        /* main continuity sense */
 173 };
 174
 175 #ifndef HAS_ACCEL
 176 #define HAS_ACCEL 1
 177 #define HAS_ACCEL_REF 0
 178 #endif
 179 #define HAS_BARO                1
 180
 181 #endif
 182
 183 #if EASYMOTOR_V_2
 184 #define AO_ADC_NUM_SENSE        0
 185 #define HAS_ADXL375             1
 186 #define HAS_BEEP                1
 187 #define AO_CONFIG_MAX_SIZE      1024
 188 #define USE_ADXL375_IMU         1
 189 #define AO_ADXL375_INVERT       0
 190 #define HAS_IMU                 1
 191 #define AO_ADXL375_AXIS         x
 192 #define AO_ADXL375_ACROSS_AXIS  y
 193 #define AO_ADXL375_THROUGH_AXIS z
 194
 195 struct ao_adc {
 196         int16_t                 pressure;
 197         int16_t                 v_batt;
 198 };
 199
 200 #endif
 201
 202 #define const
 203
 204 #define HAS_FLIGHT 1
 205 #define HAS_IGNITE 1
 206 #define HAS_USB 1
 207 #define HAS_GPS 1
 208
 209 AO_TICK_TYPE
 210 ao_time(void);
 211
 212 void
 213 ao_dump_state(void);
 214
 215 #define ao_tick_count   (ao_time())
 216 #define ao_wakeup(wchan) ao_dump_state()
 217
 218 enum ao_igniter_status {
 219         ao_igniter_unknown,     /* unknown status (ambiguous voltage) */
 220         ao_igniter_ready,       /* continuity detected */
 221         ao_igniter_active,      /* igniter firing */
 222         ao_igniter_open,        /* open circuit detected */
 223 };
 224
 225 #include <ao_data.h>
 226 #include <ao_log.h>
 227 #include <ao_telemetry.h>
 228 #include <ao_sample.h>
 229
 230 #if TELEMEGA || TELEMEGA_V4
 231 int ao_gps_count;
 232 struct ao_telemetry_location ao_gps_first;
 233 struct ao_telemetry_location ao_gps_prev;
 234 struct ao_telemetry_location ao_gps_static;
 235
 236 struct ao_telemetry_satellite ao_gps_tracking;
 237
 238 static inline double sqr(double a) { return a * a; }
 239
 240 void
 241 cc_great_circle (double start_lat, double start_lon,
 242                  double end_lat, double end_lon,
 243                  double *dist, double *bearing)
 244 {
 245         const double rad = M_PI / 180;
 246         const double earth_radius = 6371.2 * 1000;      /* in meters */
 247         double lat1 = rad * start_lat;
 248         double lon1 = rad * -start_lon;
 249         double lat2 = rad * end_lat;
 250         double lon2 = rad * -end_lon;
 251
 252 //      double d_lat = lat2 - lat1;
 253         double d_lon = lon2 - lon1;
 254
 255         /* From http://en.wikipedia.org/wiki/Great-circle_distance */
 256         double vdn = sqrt(sqr(cos(lat2) * sin(d_lon)) +
 257                           sqr(cos(lat1) * sin(lat2) -
 258                               sin(lat1) * cos(lat2) * cos(d_lon)));
 259         double vdd = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(d_lon);
 260         double d = atan2(vdn,vdd);
 261         double course;
 262
 263         if (cos(lat1) < 1e-20) {
 264                 if (lat1 > 0)
 265                         course = M_PI;
 266                 else
 267                         course = -M_PI;
 268         } else {
 269                 if (d < 1e-10)
 270                         course = 0;
 271                 else
 272                         course = acos((sin(lat2)-sin(lat1)*cos(d)) /
 273                                       (sin(d)*cos(lat1)));
 274                 if (sin(lon2-lon1) > 0)
 275                         course = 2 * M_PI-course;
 276         }
 277         *dist = d * earth_radius;
 278         *bearing = course * 180/M_PI;
 279 }
 280
 281 double
 282 ao_distance_from_pad(void)
 283 {
 284         double  dist, bearing;
 285         if (!ao_gps_count)
 286                 return 0;
 287
 288         cc_great_circle(ao_gps_first.latitude / 1e7,
 289                         ao_gps_first.longitude / 1e7,
 290                         ao_gps_static.latitude / 1e7,
 291                         ao_gps_static.longitude / 1e7,
 292                         &dist, &bearing);
 293         return dist;
 294 }
 295
 296 double
 297 ao_gps_angle(void)
 298 {
 299         double  dist, bearing;
 300         double  height;
 301         double  angle;
 302
 303         if (ao_gps_count < 2)
 304                 return 0;
 305
 306         cc_great_circle(ao_gps_prev.latitude / 1e7,
 307                         ao_gps_prev.longitude / 1e7,
 308                         ao_gps_static.latitude / 1e7,
 309                         ao_gps_static.longitude / 1e7,
 310                         &dist, &bearing);
 311         height = AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_static) - AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_prev);
 312
 313         angle = atan2(dist, height);
 314         return angle * 180/M_PI;
 315 }
 316 #endif
 317
 318 #define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
 319 #define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
 320 #define from_fix(x)     ((x) >> 16)
 321
 322 #define ACCEL_NOSE_UP   (ao_accel_2g >> 2)
 323
 324 extern enum ao_flight_state ao_flight_state;
 325
 326 #define false 0
 327 #define true 1
 328
 329 volatile struct ao_data ao_data_ring[AO_DATA_RING];
 330 volatile uint8_t ao_data_head;
 331 int     ao_summary = 0;
 332
 333 #define ao_led_on(l)
 334 #define ao_led_off(l)
 335 #define ao_timer_set_adc_interval(i)
 336 #define ao_cmd_register(c)
 337 #define ao_usb_disable()
 338 #define ao_telemetry_set_interval(x)
 339 #define ao_rdf_set(rdf)
 340 #define ao_packet_slave_start()
 341 #define ao_packet_slave_stop()
 342 #define flush()
 343
 344 enum ao_igniter {
 345         ao_igniter_drogue = 0,
 346         ao_igniter_main = 1
 347 };
 348
 349 struct ao_data ao_data_static;
 350
 351 int     drogue_height;
 352 double  drogue_time;
 353 int     main_height;
 354 double  main_time;
 355
 356 uint32_t        tick_offset;
 357
 358 static ao_k_t   ao_k_height;
 359 static double   simple_speed;
 360
 361 AO_TICK_TYPE
 362 ao_time(void)
 363 {
 364         return ao_data_static.tick;
 365 }
 366
 367 void
 368 ao_delay(int16_t interval)
 369 {
 370         return;
 371 }
 372
 373 void
 374 ao_ignite(enum ao_igniter igniter)
 375 {
 376         double time = (double) (ao_data_static.tick + tick_offset) / 100;
 377
 378         if (igniter == ao_igniter_drogue) {
 379                 drogue_time = time;
 380                 drogue_height = ao_k_height >> 16;
 381         } else {
 382                 main_time = time;
 383                 main_height = ao_k_height >> 16;
 384         }
 385 }
 386
 387 struct ao_task {
 388         int dummy;
 389 };
 390
 391 #define ao_add_task(t,f,n) ((void) (t))
 392
 393 #define ao_log_start()
 394 #define ao_log_stop()
 395
 396 #define AO_MS_TO_TICKS(ms)      ((ms) / 10)
 397 #define AO_SEC_TO_TICKS(s)      ((s) * 100)
 398
 399 #define AO_FLIGHT_TEST  1
 400
 401 int     ao_flight_debug;
 402
 403 struct ao_eeprom        *eeprom;
 404 uint32_t                eeprom_offset;
 405
 406 FILE *emulator_in;
 407 char *emulator_app;
 408 char *emulator_name;
 409 char *emulator_info;
 410 double emulator_error_max = 4;
 411 double emulator_height_error_max = 20;  /* noise in the baro sensor */
 412
 413 void
 414 ao_sleep(void *wchan);
 415
 416 const char * const ao_state_names[] = {
 417         "startup", "idle", "pad", "boost", "fast",
 418         "coast", "drogue", "main", "landed", "invalid"
 419 };
 420
 421 struct ao_cmds {
 422         void            (*func)(void);
 423         const char      *help;
 424 };
 425
 426 #define AO_NEED_ALTITUDE_TO_PRES 1
 427 #if TELEMEGA || TELEMETRUM_V2 || EASYMINI || TELEMEGA_V4
 428 #include "ao_convert_pa.c"
 429 #include <ao_ms5607.h>
 430 struct ao_ms5607_prom   ao_ms5607_prom;
 431 #include "ao_ms5607_convert.c"
 432 #if TELEMEGA || TELEMEGA_V4
 433 #define AO_PYRO_NUM     4
 434 #include <ao_pyro.h>
 435 #endif
 436 #else
 437 #include "ao_convert.c"
 438 #endif
 439
 440 #include <ao_config.h>
 441 #include <ao_fake_flight.h>
 442 #include <ao_eeprom_read.h>
 443 #include <ao_log.h>
 444
 445 #define ao_config_get()
 446
 447 struct ao_config ao_config;
 448
 449 extern int16_t ao_ground_accel, ao_flight_accel;
 450 extern int16_t ao_accel_2g;
 451
 452 typedef int16_t accel_t;
 453
 454 uint16_t        ao_serial_number;
 455 uint16_t        ao_flight_number;
 456
 457 extern AO_TICK_TYPE     ao_sample_tick;
 458
 459 #if HAS_BARO
 460 extern alt_t    ao_sample_height;
 461 #endif
 462 extern accel_t  ao_sample_accel;
 463 extern int32_t  ao_accel_scale;
 464 #if HAS_BARO
 465 extern alt_t    ao_ground_height;
 466 extern alt_t    ao_sample_alt;
 467 #endif
 468
 469 double ao_sample_qangle;
 470
 471 AO_TICK_TYPE    ao_sample_prev_tick;
 472 AO_TICK_TYPE    prev_tick;
 473 AO_TICK_TYPE    start_tick;
 474
 475
 476 #include "ao_kalman.c"
 477 #include "ao_sqrt.c"
 478 #include "ao_sample.c"
 479 #include "ao_flight.c"
 480 #include "ao_data.c"
 481 #if TELEMEGA || TELEMEGA_V4
 482 #define AO_PYRO_NUM     4
 483
 484 #define AO_PYRO_0       0
 485 #define AO_PYRO_1       1
 486 #define AO_PYRO_2       2
 487 #define AO_PYRO_3       3
 488
 489 #define PYRO_DBG        1
 490
 491 static void
 492 ao_pyro_pin_set(uint8_t pin, uint8_t value)
 493 {
 494 }
 495
 496 #include "ao_pyro.c"
 497 #endif
 498 #include "ao_eeprom_read.c"
 499 #include "ao_eeprom_read_old.c"
 500
 501 #define to_double(f)    ((f) / 65536.0)
 502
 503 static int      ao_records_read = 0;
 504 static int      ao_eof_read = 0;
 505 #if !EASYMINI
 506 static int      ao_flight_ground_accel;
 507 #endif
 508 static int      ao_flight_started = 0;
 509 static int      ao_test_max_height;
 510 static double   ao_test_max_height_time;
 511 static int      ao_test_main_height;
 512 static double   ao_test_main_height_time;
 513 static double   ao_test_landed_time;
 514 static double   ao_test_landed_height;
 515 static double   ao_test_landed_time;
 516 static int      landed_set;
 517 static double   landed_time;
 518 static double   landed_height;
 519 #if AO_PYRO_NUM
 520 static uint16_t pyros_fired;
 521 #endif
 522
 523 #if HAS_MPU6000
 524 static struct ao_mpu6000_sample ao_ground_mpu6000;
 525 #endif
 526 #if HAS_BMX160
 527 static struct ao_bmx160_sample  ao_ground_bmx160;
 528 #endif
 529
 530 void
 531 ao_test_exit(void)
 532 {
 533         double  drogue_error;
 534         double  main_error;
 535         double  landed_error;
 536         double  landed_time_error;
 537
 538         if (!ao_test_main_height_time) {
 539                 ao_test_main_height_time = ao_test_max_height_time;
 540                 ao_test_main_height = ao_test_max_height;
 541         }
 542         drogue_error = fabs(ao_test_max_height_time - drogue_time);
 543         main_error = fabs(ao_test_main_height_time - main_time);
 544         landed_error = fabs(ao_test_landed_height - landed_height);
 545         landed_time_error = ao_test_landed_time - landed_time;
 546         if (drogue_error > emulator_error_max || main_error > emulator_error_max) {
 547                 printf ("%s %s\n",
 548                         emulator_app, emulator_name);
 549                 if (emulator_info)
 550                         printf ("\t%s\n", emulator_info);
 551                 printf ("\tApogee error %g\n", drogue_error);
 552                 printf ("\tMain error %g\n", main_error);
 553                 printf ("\tLanded height error %g\n", landed_error);
 554                 printf ("\tLanded time error %g\n", landed_time_error);
 555                 printf ("\tActual: apogee: %d at %7.2f main: %d at %7.2f landed %7.2f at %7.2f\n",
 556                         ao_test_max_height, ao_test_max_height_time,
 557                         ao_test_main_height, ao_test_main_height_time,
 558                         ao_test_landed_height, ao_test_landed_time);
 559                 printf ("\tComputed: apogee: %d at %7.2f main: %d at %7.2f landed %7.2f at %7.2f\n",
 560                         drogue_height, drogue_time, main_height, main_time,
 561                         landed_height, landed_time);
 562                 exit (1);
 563         }
 564         exit(0);
 565 }
 566
 567 #if TELEMEGA || TELEMEGA_V4
 568 struct ao_azel {
 569         int     az;
 570         int     el;
 571 };
 572
 573 static void
 574 azel (struct ao_azel *r, struct ao_quaternion *q)
 575 {
 576         double  v;
 577
 578         r->az = floor (atan2(q->y, q->x) * 180/M_PI + 0.5);
 579         v = sqrt (q->x*q->x + q->y*q->y);
 580         r->el = floor (atan2(q->z, v) * 180/M_PI + 0.5);
 581 }
 582 #endif
 583
 584 void
 585 ao_insert(void)
 586 {
 587         double  time;
 588
 589         ao_data_ring[ao_data_head] = ao_data_static;
 590         if (ao_flight_state != ao_flight_startup) {
 591 #if HAS_ACCEL
 592                 double  accel = ((ao_flight_ground_accel - ao_data_accel(&ao_data_static)) * GRAVITY * 2.0) /
 593                         (ao_config.accel_minus_g - ao_config.accel_plus_g);
 594 #else
 595                 double  accel = 0.0;
 596 #endif
 597
 598                 (void) accel;
 599                 if (!start_tick)
 600                         start_tick = ao_data_static.tick;
 601                 if ((AO_TICK_SIGNED) (prev_tick - ao_data_static.tick) > 0x400)
 602                         tick_offset += 65536;
 603                 if (prev_tick) {
 604                         int ticks = ao_data_static.tick - prev_tick;
 605                         if (ticks < 0)
 606                                 ticks += 65536;
 607                         simple_speed += accel * ticks / 100.0;
 608                 }
 609                 prev_tick = ao_data_static.tick;
 610                 time = (double) (ao_data_static.tick + tick_offset - start_tick) / 100;
 611
 612                 double height = 0;
 613 #if HAS_BARO
 614 #if TELEMEGA || TELEMETRUM_V2 || EASYMINI || TELEMEGA_V4
 615                 ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked);
 616                 height = ao_pa_to_altitude(ao_data_static.ms5607_cooked.pres) - ao_ground_height;
 617
 618                 /* Hack to skip baro spike at accidental drogue charge
 619                  * firing in 2015-09-26-serial-2093-flight-0012.eeprom
 620                  * so we can test the kalman filter with this data. Just
 621                  * keep reporting the same baro value across the pressure spike
 622                  */
 623                 {
 624                         static struct ao_ms5607_sample save;
 625                         if (ao_serial_number == 2093 && ao_flight_number == 12 && 32.5 < time && time < 33.7) {
 626                                 ao_data_ring[ao_data_head].ms5607_raw = save;
 627                         } else {
 628                                 save = ao_data_static.ms5607_raw;
 629                         }
 630                 }
 631 #else
 632                 height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height;
 633 #endif
 634 #endif
 635
 636                 if (ao_test_max_height < height) {
 637                         ao_test_max_height = height;
 638                         ao_test_max_height_time = time;
 639                         ao_test_landed_height = height;
 640                         ao_test_landed_time = time;
 641                 }
 642                 if (height > ao_config.main_deploy) {
 643                         ao_test_main_height_time = time;
 644                         ao_test_main_height = height;
 645                 }
 646
 647                 if (ao_test_landed_height > height) {
 648                         ao_test_landed_height = height;
 649                         ao_test_landed_time = time;
 650                 }
 651
 652                 if (ao_flight_state == ao_flight_landed && !landed_set) {
 653                         landed_set = 1;
 654                         landed_time = time;
 655                         landed_height = height;
 656                 }
 657
 658                 if (!ao_summary) {
 659 #if TELEMEGA || TELEMEGA_V4
 660                         static struct ao_quaternion     ao_ground_mag;
 661                         static int                      ao_ground_mag_set;
 662
 663                         if (!ao_ground_mag_set) {
 664                                 ao_quaternion_init_vector (&ao_ground_mag,
 665                                                            ao_data_mag_across(&ao_data_static),
 666                                                            ao_data_mag_through(&ao_data_static),
 667                                                            ao_data_mag_along(&ao_data_static));
 668                                 ao_quaternion_normalize(&ao_ground_mag, &ao_ground_mag);
 669                                 ao_quaternion_rotate(&ao_ground_mag, &ao_ground_mag, &ao_rotation);
 670                                 ao_ground_mag_set = 1;
 671                         }
 672
 673                         struct ao_quaternion            ao_mag, ao_mag_rot;
 674
 675                         ao_quaternion_init_vector(&ao_mag,
 676                                                   ao_data_mag_across(&ao_data_static),
 677                                                   ao_data_mag_through(&ao_data_static),
 678                                                   ao_data_mag_along(&ao_data_static));
 679
 680                         ao_quaternion_normalize(&ao_mag, &ao_mag);
 681                         ao_quaternion_rotate(&ao_mag_rot, &ao_mag, &ao_rotation);
 682
 683                         float                           ao_dot;
 684                         int                             ao_mag_angle;
 685
 686                         ao_dot = ao_quaternion_dot(&ao_mag_rot, &ao_ground_mag);
 687
 688                         struct ao_azel                  ground_azel, mag_azel, rot_azel;
 689
 690                         azel(&ground_azel, &ao_ground_mag);
 691                         azel(&mag_azel, &ao_mag);
 692                         azel(&rot_azel, &ao_mag_rot);
 693
 694                         ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5);
 695
 696                         (void) ao_mag_angle;
 697
 698                         static struct ao_quaternion     ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 };
 699                         struct ao_quaternion            ao_out;
 700
 701                         ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation);
 702
 703 #if 0
 704                         int     out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI);
 705
 706                         printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n",
 707                                 time,
 708                                 ao_state_names[ao_flight_state],
 709                                 ao_k_height / 65536.0,
 710                                 ao_sample_orient, out,
 711                                 mag_azel.el,
 712                                 mag_azel.az);
 713 #endif
 714 #if 0
 715                         printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d dist %12.2f gps_tilt %4d gps_sats %2d\n",
 716                                 time,
 717                                 ao_state_names[ao_flight_state],
 718                                 ao_k_height / 65536.0,
 719                                 ao_sample_orient, out,
 720                                 ao_distance_from_pad(),
 721                                 (int) floor (ao_gps_angle() + 0.5),
 722                                 (ao_gps_static.flags & 0xf) * 10);
 723
 724 #endif
 725 #if 0
 726                         printf ("\t\tstate %-8.8s ground az: %4d el %4d mag az %4d el %4d rot az %4d el %4d el_diff %4d az_diff %4d angle %4d tilt %4d ground %8.5f %8.5f %8.5f cur %8.5f %8.5f %8.5f rot %8.5f %8.5f %8.5f\n",
 727                                 ao_state_names[ao_flight_state],
 728                                 ground_azel.az, ground_azel.el,
 729                                 mag_azel.az, mag_azel.el,
 730                                 rot_azel.az, rot_azel.el,
 731                                 ground_azel.el - rot_azel.el,
 732                                 ground_azel.az - rot_azel.az,
 733                                 ao_mag_angle,
 734                                 ao_sample_orient,
 735                                 ao_ground_mag.x,
 736                                 ao_ground_mag.y,
 737                                 ao_ground_mag.z,
 738                                 ao_mag.x,
 739                                 ao_mag.y,
 740                                 ao_mag.z,
 741                                 ao_mag_rot.x,
 742                                 ao_mag_rot.y,
 743                                 ao_mag_rot.z);
 744 #endif
 745 #endif
 746
 747 #if 1
 748                         printf("%7.2f height %8.2f accel %8.3f accel_speed %8.3f "
 749                                "state %d k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d"
 750 #if TELEMEGA || TELEMEGA_V4
 751                                " angle %5d "
 752                                "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f mag_x %8d mag_y %8d, mag_z %8d mag_angle %4d "
 753                                "avg_accel %8.3f pyro %d inhibited %d"
 754 #endif
 755                                "\n",
 756                                time,
 757                                height,
 758                                accel,
 759                                simple_speed > -100.0 ? simple_speed : -100.0,
 760                                ao_flight_state * 10,
 761                                ao_k_height / 65536.0,
 762                                ao_k_speed / 65536.0 / 16.0,
 763                                ao_k_accel / 65536.0 / 16.0,
 764                                ao_avg_height,
 765                                drogue_height,
 766                                main_height,
 767                                ao_error_h_sq_avg
 768 #if TELEMEGA
 769                                , ao_sample_orient,
 770
 771                                ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
 772                                ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
 773                                ao_mpu6000_accel(ao_data_static.mpu6000.accel_z),
 774                                ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x),
 775                                ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y),
 776                                ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z),
 777                                ao_data_static.hmc5883.x,
 778                                ao_data_static.hmc5883.y,
 779                                ao_data_static.hmc5883.z,
 780                                ao_mag_angle,
 781                                ao_coast_avg_accel / 16.0,
 782                                ao_pyro_fired * 10,
 783                                ao_pyro_inhibited * 10
 784 #endif
 785 #if TELEMEGA_V4
 786                                , ao_sample_orient,
 787
 788                                ao_bmx160_accel(ao_data_static.bmx160.acc_x),
 789                                ao_bmx160_accel(ao_data_static.bmx160.acc_y),
 790                                ao_bmx160_accel(ao_data_static.bmx160.acc_z),
 791                                ao_bmx160_gyro(ao_data_static.bmx160.gyr_x - ao_ground_bmx160.gyr_x),
 792                                ao_bmx160_gyro(ao_data_static.bmx160.gyr_y - ao_ground_bmx160.gyr_y),
 793                                ao_bmx160_gyro(ao_data_static.bmx160.gyr_z - ao_ground_bmx160.gyr_z),
 794                                ao_data_static.bmx160.mag_x,
 795                                ao_data_static.bmx160.mag_y,
 796                                ao_data_static.bmx160.mag_z,
 797                                ao_mag_angle,
 798                                ao_coast_avg_accel / 16.0,
 799                                ao_pyro_fired * 10,
 800                                ao_pyro_inhibited * 10
 801 #endif
 802                                 );
 803 #endif
 804
 805 //                      if (ao_flight_state == ao_flight_landed)
 806 //                              ao_test_exit();
 807                 }
 808         }
 809         ao_data_head = ao_data_ring_next(ao_data_head);
 810 }
 811
 812
 813 uint16_t
 814 uint16(uint8_t *bytes, int off)
 815 {
 816         return (uint16_t) bytes[off] | (((uint16_t) bytes[off+1]) << 8);
 817 }
 818
 819 int16_t
 820 int16(uint8_t *bytes, int off)
 821 {
 822         return (int16_t) uint16(bytes, off);
 823 }
 824
 825 uint32_t
 826 uint32(uint8_t *bytes, int off)
 827 {
 828         return (uint32_t) bytes[off] | (((uint32_t) bytes[off+1]) << 8) |
 829                 (((uint32_t) bytes[off+2]) << 16) |
 830                 (((uint32_t) bytes[off+3]) << 24);
 831 }
 832
 833 int32_t
 834 int32(uint8_t *bytes, int off)
 835 {
 836         return (int32_t) uint32(bytes, off);
 837 }
 838
 839 uint32_t
 840 uint24(uint8_t *bytes, int off)
 841 {
 842         return (uint32_t) bytes[off] | (((uint32_t) bytes[off+1]) << 8) |
 843                 (((uint32_t) bytes[off+2]) << 16);
 844 }
 845
 846 int32_t
 847 int24(uint8_t *bytes, int off)
 848 {
 849         return (int32_t) uint24(bytes, off);
 850 }
 851
 852 static int log_format;
 853
 854 void
 855 ao_sleep(void *wchan)
 856 {
 857         if (wchan == &ao_data_head) {
 858 #if TELEMEGA || TELEMEGA_V4
 859                 if (ao_flight_state >= ao_flight_boost && ao_flight_state < ao_flight_landed)
 860                         ao_pyro_check();
 861 #endif
 862                 for (;;) {
 863                         if (ao_records_read > 2 && ao_flight_state == ao_flight_startup)
 864                         {
 865
 866 #if TELEMEGA
 867                                 ao_data_static.mpu6000 = ao_ground_mpu6000;
 868 #endif
 869 #if TELEMEGA_V4
 870                                 ao_data_static.bmx160 = ao_ground_bmx160;
 871 #endif
 872 #if TELEMETRUM_V1
 873                                 ao_data_static.adc.accel = ao_flight_ground_accel;
 874 #endif
 875 #if EASYMOTOR_V_2
 876                                 ao_data_static.adxl375.AO_ADXL375_AXIS = ao_flight_ground_accel;
 877 #endif
 878
 879                                 ao_insert();
 880                                 return;
 881                         }
 882
 883                         if (eeprom) {
 884 #if TELEMEGA || EASYMOTOR_V_2 || TELEMEGA_V4
 885                                 struct ao_log_mega      *log_mega;
 886 #endif
 887 #if EASYMOTOR_V_2
 888                                 struct ao_log_motor     *log_motor;
 889 #endif
 890 #if TELEMETRUM_V2
 891                                 struct ao_log_metrum    *log_metrum;
 892 #endif
 893 #if EASYMINI
 894                                 struct ao_log_mini      *log_mini;
 895 #endif
 896 #if TELEMETRUM_V1
 897                                 struct ao_log_record    *log_record;
 898 #endif
 899
 900                                 if (eeprom_offset >= eeprom->len) {
 901                                         if (++ao_eof_read >= 1000)
 902                                                 if (!ao_summary)
 903                                                         printf ("no more data, exiting simulation\n");
 904                                         ao_test_exit();
 905                                         ao_data_static.tick += 10;
 906                                         ao_insert();
 907                                         return;
 908                                 }
 909                                 switch (eeprom->log_format) {
 910 #if TELEMEGA
 911                                 case AO_LOG_FORMAT_TELEMEGA_OLD:
 912                                 case AO_LOG_FORMAT_TELEMEGA:
 913                                         log_mega = (struct ao_log_mega *) &eeprom->data[eeprom_offset];
 914                                         eeprom_offset += sizeof (*log_mega);
 915                                         switch (log_mega->type) {
 916                                         case AO_LOG_FLIGHT:
 917                                                 ao_flight_number = log_mega->u.flight.flight;
 918                                                 ao_flight_ground_accel = log_mega->u.flight.ground_accel;
 919                                                 ao_flight_started = 1;
 920                                                 ao_ground_pres = log_mega->u.flight.ground_pres;
 921                                                 ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
 922                                                 ao_ground_accel_along = log_mega->u.flight.ground_accel_along;
 923                                                 ao_ground_accel_across = log_mega->u.flight.ground_accel_across;
 924                                                 ao_ground_accel_through = log_mega->u.flight.ground_accel_through;
 925                                                 ao_ground_roll = log_mega->u.flight.ground_roll;
 926                                                 ao_ground_pitch = log_mega->u.flight.ground_pitch;
 927                                                 ao_ground_yaw = log_mega->u.flight.ground_yaw;
 928                                                 ao_ground_mpu6000.accel_x = ao_ground_accel_across;
 929                                                 ao_ground_mpu6000.accel_y = ao_ground_accel_along;
 930                                                 ao_ground_mpu6000.accel_z = ao_ground_accel_through;
 931                                                 ao_ground_mpu6000.gyro_x = ao_ground_pitch >> 9;
 932                                                 ao_ground_mpu6000.gyro_y = ao_ground_roll >> 9;
 933                                                 ao_ground_mpu6000.gyro_z = ao_ground_yaw >> 9;
 934                                                 break;
 935                                         case AO_LOG_STATE:
 936                                                 break;
 937                                         case AO_LOG_SENSOR:
 938                                                 ao_data_static.tick = log_mega->tick;
 939                                                 ao_data_static.ms5607_raw.pres = log_mega->u.sensor.pres;
 940                                                 ao_data_static.ms5607_raw.temp = log_mega->u.sensor.temp;
 941                                                 ao_data_static.mpu6000.accel_x = log_mega->u.sensor.accel_x;
 942                                                 ao_data_static.mpu6000.accel_y = log_mega->u.sensor.accel_y;
 943                                                 ao_data_static.mpu6000.accel_z = log_mega->u.sensor.accel_z;
 944                                                 ao_data_static.mpu6000.gyro_x = log_mega->u.sensor.gyro_x;
 945                                                 ao_data_static.mpu6000.gyro_y = log_mega->u.sensor.gyro_y;
 946                                                 ao_data_static.mpu6000.gyro_z = log_mega->u.sensor.gyro_z;
 947                                                 ao_data_static.hmc5883.x = log_mega->u.sensor.mag_x;
 948                                                 ao_data_static.hmc5883.y = log_mega->u.sensor.mag_y;
 949                                                 ao_data_static.hmc5883.z = log_mega->u.sensor.mag_z;
 950                                                 ao_data_static.mma655x = log_mega->u.sensor.accel;
 951                                                 if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
 952                                                         ao_data_static.mma655x = ao_data_accel_invert(ao_data_static.mma655x);
 953                                                 ao_records_read++;
 954                                                 ao_insert();
 955                                                 return;
 956                                         case AO_LOG_TEMP_VOLT:
 957                                                 if (pyros_fired != log_mega->u.volt.pyro) {
 958                                                         printf("pyro changed %x -> %x\n", pyros_fired, log_mega->u.volt.pyro);
 959                                                         pyros_fired = log_mega->u.volt.pyro;
 960                                                 }
 961                                                 break;
 962                                         case AO_LOG_GPS_TIME:
 963                                                 ao_gps_prev = ao_gps_static;
 964                                                 ao_gps_static.tick = log_mega->tick;
 965                                                 ao_gps_static.latitude = log_mega->u.gps.latitude;
 966                                                 ao_gps_static.longitude = log_mega->u.gps.longitude;
 967                                                 {
 968                                                         int16_t altitude_low = log_mega->u.gps.altitude_low;
 969                                                         int16_t altitude_high = log_mega->u.gps.altitude_high;
 970                                                         int32_t altitude = altitude_low | ((int32_t) altitude_high << 16);
 971
 972                                                         AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_static, altitude);
 973                                                 }
 974                                                 ao_gps_static.flags = log_mega->u.gps.flags;
 975                                                 if (!ao_gps_count)
 976                                                         ao_gps_first = ao_gps_static;
 977                                                 ao_gps_count++;
 978                                                 break;
 979                                         case AO_LOG_GPS_SAT:
 980                                                 break;
 981                                         }
 982                                         break;
 983 #endif
 984 #if TELEMEGA_V4
 985                                 case AO_LOG_FORMAT_TELEMEGA_4:
 986                                         log_mega = (struct ao_log_mega *) &eeprom->data[eeprom_offset];
 987                                         eeprom_offset += sizeof (*log_mega);
 988                                         switch (log_mega->type) {
 989                                         case AO_LOG_FLIGHT:
 990                                                 ao_flight_number = log_mega->u.flight.flight;
 991                                                 ao_flight_ground_accel = log_mega->u.flight.ground_accel;
 992                                                 ao_flight_started = 1;
 993                                                 ao_ground_pres = log_mega->u.flight.ground_pres;
 994                                                 ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
 995                                                 ao_ground_accel_along = log_mega->u.flight.ground_accel_along;
 996                                                 ao_ground_accel_across = log_mega->u.flight.ground_accel_across;
 997                                                 ao_ground_accel_through = log_mega->u.flight.ground_accel_through;
 998                                                 ao_ground_roll = log_mega->u.flight.ground_roll;
 999                                                 ao_ground_pitch = log_mega->u.flight.ground_pitch;
1000                                                 ao_ground_yaw = log_mega->u.flight.ground_yaw;
1001                                                 ao_ground_bmx160.acc_x = ao_ground_accel_along;
1002                                                 ao_ground_bmx160.acc_y = -ao_ground_accel_across;
1003                                                 ao_ground_bmx160.acc_z = ao_ground_accel_through;
1004                                                 ao_ground_bmx160.gyr_x = ao_ground_roll >> 9;
1005                                                 ao_ground_bmx160.gyr_y = -(ao_ground_pitch >> 9);
1006                                                 ao_ground_bmx160.gyr_z = ao_ground_yaw >> 9;
1007                                                 break;
1008                                         case AO_LOG_STATE:
1009                                                 break;
1010                                         case AO_LOG_SENSOR:
1011                                                 ao_data_static.tick = log_mega->tick;
1012                                                 ao_data_static.ms5607_raw.pres = log_mega->u.sensor.pres;
1013                                                 ao_data_static.ms5607_raw.temp = log_mega->u.sensor.temp;
1014                                                 ao_data_set_along(&ao_data_static, log_mega->u.sensor.accel_along);
1015                                                 ao_data_set_across(&ao_data_static, log_mega->u.sensor.accel_across);
1016                                                 ao_data_set_through(&ao_data_static, log_mega->u.sensor.accel_through);
1017                                                 ao_data_set_roll(&ao_data_static, log_mega->u.sensor.gyro_roll);
1018                                                 ao_data_set_pitch(&ao_data_static, log_mega->u.sensor.gyro_pitch);
1019                                                 ao_data_set_yaw(&ao_data_static, log_mega->u.sensor.gyro_yaw);
1020                                                 ao_data_set_mag_along(&ao_data_static, log_mega->u.sensor.mag_along);
1021                                                 ao_data_set_mag_across(&ao_data_static, log_mega->u.sensor.mag_across);
1022                                                 ao_data_set_mag_through(&ao_data_static, log_mega->u.sensor.mag_through);
1023 #if AO_ADXL375_INVERT
1024                                                 ao_data_static.adxl375.AO_ADXL375_AXIS = ao_data_accel_invert(log_mega->u.sensor.accel);
1025 #else
1026                                                 ao_data_static.adxl375.AO_ADXL375_AXIS = log_mega->u.sensor.accel;
1027 #endif
1028                                                 if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
1029                                                         ao_data_static.adxl375.AO_ADXL375_AXIS = ao_data_accel_invert(ao_data_static.adxl375.AO_ADXL375_AXIS);
1030                                                 ao_records_read++;
1031                                                 ao_insert();
1032                                                 return;
1033                                         case AO_LOG_TEMP_VOLT:
1034                                                 if (pyros_fired != log_mega->u.volt.pyro) {
1035                                                         printf("pyro changed %x -> %x\n", pyros_fired, log_mega->u.volt.pyro);
1036                                                         pyros_fired = log_mega->u.volt.pyro;
1037                                                 }
1038                                                 break;
1039                                         case AO_LOG_GPS_TIME:
1040                                                 ao_gps_prev = ao_gps_static;
1041                                                 ao_gps_static.tick = log_mega->tick;
1042                                                 ao_gps_static.latitude = log_mega->u.gps.latitude;
1043                                                 ao_gps_static.longitude = log_mega->u.gps.longitude;
1044                                                 {
1045                                                         int16_t altitude_low = log_mega->u.gps.altitude_low;
1046                                                         int16_t altitude_high = log_mega->u.gps.altitude_high;
1047                                                         int32_t altitude = altitude_low | ((int32_t) altitude_high << 16);
1048
1049                                                         AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_static, altitude);
1050                                                 }
1051                                                 ao_gps_static.flags = log_mega->u.gps.flags;
1052                                                 if (!ao_gps_count)
1053                                                         ao_gps_first = ao_gps_static;
1054                                                 ao_gps_count++;
1055                                                 break;
1056                                         case AO_LOG_GPS_SAT:
1057                                                 break;
1058                                         }
1059                                         break;
1060 #endif
1061 #ifdef foo_TELEMEGA_V4
1062                                 case AO_LOG_FORMAT_TELEMEGA_4:
1063                                         log_mega = (struct ao_log_mega *) &eeprom->data[eeprom_offset];
1064                                         eeprom_offset += sizeof (*log_mega);
1065                                         switch (log_mega->type) {
1066                                         case AO_LOG_FLIGHT:
1067                                                 ao_flight_number = log_mega->u.flight.flight;
1068                                                 ao_flight_ground_accel = log_mega->u.flight.ground_accel;
1069                                                 ao_flight_started = 1;
1070                                                 break;
1071                                         case AO_LOG_SENSOR:
1072                                                 ao_data_static.tick = log_mega->tick;
1073                                                 ao_data_static.adxl375.AO_ADXL375_AXIS = log_mega->u.sensor.accel;
1074                                                 ao_records_read++;
1075                                                 ao_insert();
1076                                                 return;
1077                                         }
1078                                         break;
1079 #endif
1080 #if TELEMETRUM_V2
1081                                 case AO_LOG_FORMAT_TELEMETRUM:
1082                                         log_metrum = (struct ao_log_metrum *) &eeprom->data[eeprom_offset];
1083                                         eeprom_offset += sizeof (*log_metrum);
1084                                         switch (log_metrum->type) {
1085                                         case AO_LOG_FLIGHT:
1086                                                 ao_flight_started = 1;
1087                                                 ao_flight_number = log_metrum->u.flight.flight;
1088                                                 ao_flight_ground_accel = log_metrum->u.flight.ground_accel;
1089                                                 ao_ground_pres = log_metrum->u.flight.ground_pres;
1090                                                 ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
1091                                                 break;
1092                                         case AO_LOG_SENSOR:
1093                                                 ao_data_static.tick = log_metrum->tick;
1094                                                 ao_data_static.ms5607_raw.pres = log_metrum->u.sensor.pres;
1095                                                 ao_data_static.ms5607_raw.temp = log_metrum->u.sensor.temp;
1096                                                 ao_data_static.mma655x = log_metrum->u.sensor.accel;
1097                                                 ao_records_read++;
1098                                                 ao_insert();
1099                                                 return;
1100                                         }
1101                                         break;
1102 #endif
1103 #if EASYMINI
1104                                 case AO_LOG_FORMAT_EASYMINI1:
1105                                 case AO_LOG_FORMAT_EASYMINI2:
1106                                 case AO_LOG_FORMAT_TELEMINI3:
1107                                         log_mini = (struct ao_log_mini *) &eeprom->data[eeprom_offset];
1108                                         eeprom_offset += sizeof (*log_mini);
1109                                         switch (log_mini->type) {
1110                                         case AO_LOG_FLIGHT:
1111                                                 ao_flight_started = 1;
1112                                                 ao_flight_number = log_mini->u.flight.flight;
1113                                                 ao_ground_pres = log_mini->u.flight.ground_pres;
1114                                                 ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
1115                                                 break;
1116                                         case AO_LOG_SENSOR:
1117                                                 ao_data_static.tick = log_mini->tick;
1118                                                 ao_data_static.ms5607_raw.pres = int24(log_mini->u.sensor.pres, 0);
1119                                                 ao_data_static.ms5607_raw.temp = int24(log_mini->u.sensor.temp, 0);
1120                                                 ao_records_read++;
1121                                                 ao_insert();
1122                                                 return;
1123                                         }
1124                                         break;
1125 #endif
1126 #if TELEMETRUM_V1
1127                                 case AO_LOG_FORMAT_FULL:
1128                                 case AO_LOG_FORMAT_TINY:
1129                                         log_record = (struct ao_log_record *) &eeprom->data[eeprom_offset];
1130                                         eeprom_offset += sizeof (*log_record);
1131                                         switch (log_record->type) {
1132                                         case AO_LOG_FLIGHT:
1133                                                 ao_flight_started = 1;
1134                                                 ao_flight_ground_accel = log_record->u.flight.ground_accel;
1135                                                 ao_flight_number = log_record->u.flight.flight;
1136                                                 break;
1137                                         case AO_LOG_SENSOR:
1138                                         case 'P':       /* ancient telemini */
1139                                                 ao_data_static.tick = log_record->tick;
1140                                                 ao_data_static.adc.accel = log_record->u.sensor.accel;
1141                                                 ao_data_static.adc.pres_real = log_record->u.sensor.pres;
1142                                                 ao_data_static.adc.pres = log_record->u.sensor.pres;
1143                                                 ao_records_read++;
1144                                                 ao_insert();
1145                                                 return;
1146                                         case AO_LOG_TEMP_VOLT:
1147                                                 ao_data_static.tick = log_record->tick;;
1148                                                 ao_data_static.adc.temp = log_record->u.temp_volt.temp;
1149                                                 ao_data_static.adc.v_batt = log_record->u.temp_volt.v_batt;
1150                                                 break;
1151                                         }
1152                                         break;
1153 #endif
1154 #if EASYMOTOR_V_2
1155                                 case AO_LOG_FORMAT_TELEMEGA_3:
1156                                         log_mega = (struct ao_log_mega *) &eeprom->data[eeprom_offset];
1157                                         eeprom_offset += sizeof (*log_mega);
1158                                         switch (log_mega->type) {
1159                                         case AO_LOG_FLIGHT:
1160                                                 ao_flight_number = log_mega->u.flight.flight;
1161                                                 ao_flight_ground_accel = log_mega->u.flight.ground_accel;
1162                                                 ao_flight_started = 1;
1163                                                 break;
1164                                         case AO_LOG_SENSOR:
1165                                                 ao_data_static.tick = log_mega->tick;
1166                                                 ao_data_static.adxl375.AO_ADXL375_AXIS = -log_mega->u.sensor.accel;
1167                                                 ao_records_read++;
1168                                                 ao_insert();
1169                                                 return;
1170                                         }
1171                                         break;
1172                                 case AO_LOG_FORMAT_TELEMEGA_4:
1173                                         log_mega = (struct ao_log_mega *) &eeprom->data[eeprom_offset];
1174                                         eeprom_offset += sizeof (*log_mega);
1175                                         switch (log_mega->type) {
1176                                         case AO_LOG_FLIGHT:
1177                                                 ao_flight_number = log_mega->u.flight.flight;
1178                                                 ao_flight_ground_accel = log_mega->u.flight.ground_accel;
1179                                                 ao_flight_started = 1;
1180                                                 break;
1181                                         case AO_LOG_SENSOR:
1182                                                 ao_data_static.tick = log_mega->tick;
1183                                                 ao_data_static.adxl375.AO_ADXL375_AXIS = log_mega->u.sensor.accel;
1184                                                 ao_records_read++;
1185                                                 ao_insert();
1186                                                 return;
1187                                         }
1188                                         break;
1189                                 case AO_LOG_FORMAT_EASYMOTOR:
1190                                         log_motor = (struct ao_log_motor *) &eeprom->data[eeprom_offset];
1191                                         eeprom_offset += sizeof (*log_motor);
1192                                         switch (log_motor->type) {
1193                                         case AO_LOG_FLIGHT:
1194                                                 ao_flight_number = log_motor->u.flight.flight;
1195                                                 ao_flight_ground_accel = log_motor->u.flight.ground_accel;
1196                                                 ao_flight_started = 1;
1197                                                 break;
1198                                         case AO_LOG_SENSOR:
1199                                                 ao_data_static.tick = log_motor->tick;
1200                                                 ao_data_static.adc.pressure = log_motor->u.sensor.pressure;
1201                                                 ao_data_static.adc.v_batt = log_motor->u.sensor.v_batt;
1202                                                 ao_data_static.adxl375.AO_ADXL375_AXIS = log_motor->u.sensor.accel_along;
1203                                                 ao_data_static.adxl375.AO_ADXL375_ACROSS_AXIS = log_motor->u.sensor.accel_across;
1204                                                 ao_data_static.adxl375.AO_ADXL375_THROUGH_AXIS = log_motor->u.sensor.accel_through;
1205                                                 ao_records_read++;
1206                                                 ao_insert();
1207                                                 return;
1208                                         }
1209                                         break;
1210 #endif
1211                                 default:
1212                                         printf ("invalid log format %d\n", log_format);
1213                                         ao_test_exit();
1214                                 }
1215                         }
1216                 }
1217
1218         }
1219 }
1220 #define COUNTS_PER_G 264.8
1221
1222 void
1223 ao_dump_state(void)
1224 {
1225 }
1226
1227 static const struct option options[] = {
1228         { .name = "summary", .has_arg = 0, .val = 's' },
1229         { .name = "debug", .has_arg = 0, .val = 'd' },
1230         { .name = "info", .has_arg = 1, .val = 'i' },
1231         { 0, 0, 0, 0},
1232 };
1233
1234 void run_flight_fixed(char *name, FILE *f, int summary, char *info)
1235 {
1236         emulator_name = name;
1237         emulator_in = f;
1238         emulator_info = info;
1239         ao_summary = summary;
1240
1241         if (strstr(name, ".eeprom") != NULL) {
1242                 char    c;
1243
1244                 c = getc(f);
1245                 ungetc(c, f);
1246                 if (c == '{')
1247                         eeprom = ao_eeprom_read(f);
1248                 else
1249                         eeprom = ao_eeprom_read_old(f);
1250
1251                 if (eeprom) {
1252 #if HAS_MS5607
1253                         ao_ms5607_prom = eeprom->ms5607_prom;
1254 #endif
1255                         ao_config = eeprom->config;
1256                         ao_serial_number = eeprom->serial_number;
1257                         log_format = eeprom->log_format;
1258                 }
1259         }
1260
1261         ao_flight_init();
1262         ao_flight();
1263 }
1264
1265 int
1266 main (int argc, char **argv)
1267 {
1268         int     summary = 0;
1269         int     c;
1270         int     i;
1271         char    *info = NULL;
1272
1273 #if HAS_ACCEL
1274         emulator_app="full";
1275 #else
1276         emulator_app="baro";
1277 #endif
1278         while ((c = getopt_long(argc, argv, "sdpi:", options, NULL)) != -1) {
1279                 switch (c) {
1280                 case 's':
1281                         summary = 1;
1282                         break;
1283                 case 'd':
1284                         ao_flight_debug = 1;
1285                         break;
1286                 case 'p':
1287 #if PYRO_DBG
1288                         pyro_dbg = 1;
1289 #endif
1290                         break;
1291                 case 'i':
1292                         info = optarg;
1293                         break;
1294                 }
1295         }
1296
1297         if (optind == argc)
1298                 run_flight_fixed("<stdin>", stdin, summary, info);
1299         else
1300                 for (i = optind; i < argc; i++) {
1301                         FILE    *f = fopen(argv[i], "r");
1302                         if (!f) {
1303                                 perror(argv[i]);
1304                                 continue;
1305                         }
1306                         run_flight_fixed(argv[i], f, summary, info);
1307                         fclose(f);
1308                 }
1309         exit(0);
1310 }