*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; version 2 of the License.
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
*
* This program is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
#include <string.h>
#include <getopt.h>
#include <math.h>
+#define log ao_log_data
+
+#define GRAVITY 9.80665
#define AO_HERTZ 100
#define ao_data_ring_next(n) (((n) + 1) & (AO_DATA_RING - 1))
#define ao_data_ring_prev(n) (((n) - 1) & (AO_DATA_RING - 1))
+#if 0
#define AO_M_TO_HEIGHT(m) ((int16_t) (m))
#define AO_MS_TO_SPEED(ms) ((int16_t) ((ms) * 16))
#define AO_MSS_TO_ACCEL(mss) ((int16_t) ((mss) * 16))
+#endif
+
+#define AO_GPS_NEW_DATA 1
+#define AO_GPS_NEW_TRACKING 2
+
+int ao_gps_new;
+
+#if !defined(TELEMEGA) && !defined(TELEMETRUM_V2) && !defined(EASYMINI)
+#define TELEMETRUM_V1 1
+#endif
#if TELEMEGA
#define AO_ADC_NUM_SENSE 6
#define HAS_MS5607 1
#define HAS_MPU6000 1
#define HAS_MMA655X 1
+#define HAS_HMC5883 1
+#define HAS_BEEP 1
+#define AO_CONFIG_MAX_SIZE 1024
+#define AO_MMA655X_INVERT 0
struct ao_adc {
int16_t sense[AO_ADC_NUM_SENSE];
int16_t v_batt;
int16_t v_pbatt;
- int16_t accel_ref;
- int16_t accel;
int16_t temp;
};
-#else
+#endif
+
+#if TELEMETRUM_V2
+#define AO_ADC_NUM_SENSE 2
+#define HAS_MS5607 1
+#define HAS_MMA655X 1
+#define AO_MMA655X_INVERT 0
+#define HAS_BEEP 1
+#define AO_CONFIG_MAX_SIZE 1024
+
+struct ao_adc {
+ int16_t sense_a;
+ int16_t sense_m;
+ int16_t v_batt;
+ int16_t temp;
+};
+#endif
+
+#if EASYMINI
+#define AO_ADC_NUM_SENSE 2
+#define HAS_MS5607 1
+#define HAS_BEEP 1
+#define AO_CONFIG_MAX_SIZE 1024
+
+struct ao_adc {
+ int16_t sense_a;
+ int16_t sense_m;
+ int16_t v_batt;
+};
+#endif
+
+#if TELEMETRUM_V1
/*
* One set of samples read from the A/D converter
*/
#endif
-#define __pdata
-#define __data
-#define __xdata
-#define __code
-#define __reentrant
+#define const
#define HAS_FLIGHT 1
#define HAS_IGNITE 1
#include <ao_data.h>
#include <ao_log.h>
+#include <ao_telemetry.h>
+#include <ao_sample.h>
+
+#if TELEMEGA
+int ao_gps_count;
+struct ao_telemetry_location ao_gps_first;
+struct ao_telemetry_location ao_gps_prev;
+struct ao_telemetry_location ao_gps_static;
+
+struct ao_telemetry_satellite ao_gps_tracking;
+
+static inline double sqr(double a) { return a * a; }
+
+void
+cc_great_circle (double start_lat, double start_lon,
+ double end_lat, double end_lon,
+ double *dist, double *bearing)
+{
+ const double rad = M_PI / 180;
+ const double earth_radius = 6371.2 * 1000; /* in meters */
+ double lat1 = rad * start_lat;
+ double lon1 = rad * -start_lon;
+ double lat2 = rad * end_lat;
+ double lon2 = rad * -end_lon;
+
+// double d_lat = lat2 - lat1;
+ double d_lon = lon2 - lon1;
+
+ /* From http://en.wikipedia.org/wiki/Great-circle_distance */
+ double vdn = sqrt(sqr(cos(lat2) * sin(d_lon)) +
+ sqr(cos(lat1) * sin(lat2) -
+ sin(lat1) * cos(lat2) * cos(d_lon)));
+ double vdd = sin(lat1) * sin(lat2) + cos(lat1) * cos(lat2) * cos(d_lon);
+ double d = atan2(vdn,vdd);
+ double course;
+
+ if (cos(lat1) < 1e-20) {
+ if (lat1 > 0)
+ course = M_PI;
+ else
+ course = -M_PI;
+ } else {
+ if (d < 1e-10)
+ course = 0;
+ else
+ course = acos((sin(lat2)-sin(lat1)*cos(d)) /
+ (sin(d)*cos(lat1)));
+ if (sin(lon2-lon1) > 0)
+ course = 2 * M_PI-course;
+ }
+ *dist = d * earth_radius;
+ *bearing = course * 180/M_PI;
+}
+
+double
+ao_distance_from_pad(void)
+{
+ double dist, bearing;
+ if (!ao_gps_count)
+ return 0;
+
+ cc_great_circle(ao_gps_first.latitude / 1e7,
+ ao_gps_first.longitude / 1e7,
+ ao_gps_static.latitude / 1e7,
+ ao_gps_static.longitude / 1e7,
+ &dist, &bearing);
+ return dist;
+}
+
+double
+ao_gps_angle(void)
+{
+ double dist, bearing;
+ double height;
+ double angle;
+
+ if (ao_gps_count < 2)
+ return 0;
+
+ cc_great_circle(ao_gps_prev.latitude / 1e7,
+ ao_gps_prev.longitude / 1e7,
+ ao_gps_static.latitude / 1e7,
+ ao_gps_static.longitude / 1e7,
+ &dist, &bearing);
+ height = AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_static) - AO_TELEMETRY_LOCATION_ALTITUDE(&ao_gps_prev);
+
+ angle = atan2(dist, height);
+ return angle * 180/M_PI;
+}
+#endif
#define to_fix16(x) ((int16_t) ((x) * 65536.0 + 0.5))
#define to_fix32(x) ((int32_t) ((x) * 65536.0 + 0.5))
#define from_fix(x) ((x) >> 16)
-/*
- * Above this height, the baro sensor doesn't work
- */
-#define AO_BARO_SATURATE 13000
-#define AO_MIN_BARO_VALUE ao_altitude_to_pres(AO_BARO_SATURATE)
-
-/*
- * Above this speed, baro measurements are unreliable
- */
-#define AO_MAX_BARO_SPEED 200
-
#define ACCEL_NOSE_UP (ao_accel_2g >> 2)
extern enum ao_flight_state ao_flight_state;
-#define FALSE 0
-#define TRUE 1
+#define false 0
+#define true 1
volatile struct ao_data ao_data_ring[AO_DATA_RING];
volatile uint8_t ao_data_head;
#define ao_rdf_set(rdf)
#define ao_packet_slave_start()
#define ao_packet_slave_stop()
+#define flush()
enum ao_igniter {
ao_igniter_drogue = 0,
int tick_offset;
-static int32_t ao_k_height;
+static ao_k_t ao_k_height;
+static double simple_speed;
int16_t
ao_time(void)
#define AO_MS_TO_TICKS(ms) ((ms) / 10)
#define AO_SEC_TO_TICKS(s) ((s) * 100)
-#define AO_FLIGHT_TEST
+#define AO_FLIGHT_TEST 1
int ao_flight_debug;
+struct ao_eeprom *eeprom;
+uint32_t eeprom_offset;
+
FILE *emulator_in;
char *emulator_app;
char *emulator_name;
void
ao_sleep(void *wchan);
-const char const * const ao_state_names[] = {
+const char * const ao_state_names[] = {
"startup", "idle", "pad", "boost", "fast",
"coast", "drogue", "main", "landed", "invalid"
};
#define ao_xmemcmp(d,s,c) memcmp(d,s,c)
#define AO_NEED_ALTITUDE_TO_PRES 1
-#if TELEMEGA
+#if TELEMEGA || TELEMETRUM_V2 || EASYMINI
#include "ao_convert_pa.c"
#include <ao_ms5607.h>
-struct ao_ms5607_prom ms5607_prom;
+struct ao_ms5607_prom ao_ms5607_prom;
#include "ao_ms5607_convert.c"
+#if TELEMEGA
#define AO_PYRO_NUM 4
#include <ao_pyro.h>
+#endif
#else
#include "ao_convert.c"
#endif
-struct ao_config {
- uint16_t main_deploy;
- int16_t accel_plus_g;
- int16_t accel_minus_g;
- uint8_t pad_orientation;
- uint16_t apogee_lockout;
-#if TELEMEGA
- struct ao_pyro pyro[AO_PYRO_NUM]; /* minor version 12 */
-#endif
-};
-
-#define AO_PAD_ORIENTATION_ANTENNA_UP 0
-#define AO_PAD_ORIENTATION_ANTENNA_DOWN 1
+#include <ao_config.h>
+#include <ao_fake_flight.h>
+#include <ao_eeprom_read.h>
+#include <ao_log.h>
#define ao_config_get()
struct ao_config ao_config;
-#define DATA_TO_XDATA(x) (x)
+#define x (x)
-#define GRAVITY 9.80665
extern int16_t ao_ground_accel, ao_flight_accel;
extern int16_t ao_accel_2g;
typedef int16_t accel_t;
+uint16_t ao_serial_number;
+int16_t ao_flight_number;
+
extern uint16_t ao_sample_tick;
extern alt_t ao_sample_height;
#define AO_PYRO_2 2
#define AO_PYRO_3 3
+#define PYRO_DBG 1
+
static void
ao_pyro_pin_set(uint8_t pin, uint8_t value)
{
#include "ao_pyro.c"
#endif
+#include "ao_eeprom_read.c"
+#include "ao_eeprom_read_old.c"
#define to_double(f) ((f) / 65536.0)
static int ao_records_read = 0;
static int ao_eof_read = 0;
+#if !EASYMINI
static int ao_flight_ground_accel;
+#endif
static int ao_flight_started = 0;
static int ao_test_max_height;
static double ao_test_max_height_time;
static int landed_set;
static double landed_time;
static double landed_height;
+#if AO_PYRO_NUM
+static uint16_t pyros_fired;
+#endif
#if HAS_MPU6000
static struct ao_mpu6000_sample ao_ground_mpu6000;
exit(0);
}
-#if HAS_MPU6000
-static double
-ao_mpu6000_accel(int16_t sensor)
-{
- return sensor / 32767.0 * MPU6000_ACCEL_FULLSCALE * GRAVITY;
-}
+#ifdef TELEMEGA
+struct ao_azel {
+ int az;
+ int el;
+};
-static double
-ao_mpu6000_gyro(int32_t sensor)
+static void
+azel (struct ao_azel *r, struct ao_quaternion *q)
{
- return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE;
+ double v;
+
+ r->az = floor (atan2(q->y, q->x) * 180/M_PI + 0.5);
+ v = sqrt (q->x*q->x + q->y*q->y);
+ r->el = floor (atan2(q->z, v) * 180/M_PI + 0.5);
}
#endif
double time;
ao_data_ring[ao_data_head] = ao_data_static;
- ao_data_head = ao_data_ring_next(ao_data_head);
if (ao_flight_state != ao_flight_startup) {
#if HAS_ACCEL
- double accel = ((ao_flight_ground_accel - ao_data_accel_cook(&ao_data_static)) * GRAVITY * 2.0) /
+ double accel = ((ao_flight_ground_accel - ao_data_accel(&ao_data_static)) * GRAVITY * 2.0) /
(ao_config.accel_minus_g - ao_config.accel_plus_g);
#else
double accel = 0.0;
#endif
-#if TELEMEGA
- double height;
-
- ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked);
- height = ao_pa_to_altitude(ao_data_static.ms5607_cooked.pres) - ao_ground_height;
-#else
- double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height;
-#endif
+ (void) accel;
if (!tick_offset)
tick_offset = -ao_data_static.tick;
if ((prev_tick - ao_data_static.tick) > 0x400)
tick_offset += 65536;
+ if (prev_tick) {
+ int ticks = ao_data_static.tick - prev_tick;
+ if (ticks < 0)
+ ticks += 65536;
+ simple_speed += accel * ticks / 100.0;
+ }
prev_tick = ao_data_static.tick;
time = (double) (ao_data_static.tick + tick_offset) / 100;
+#if TELEMEGA || TELEMETRUM_V2 || EASYMINI
+ ao_ms5607_convert(&ao_data_static.ms5607_raw, &ao_data_static.ms5607_cooked);
+ double height = ao_pa_to_altitude(ao_data_static.ms5607_cooked.pres) - ao_ground_height;
+
+ /* Hack to skip baro spike at accidental drogue charge
+ * firing in 2015-09-26-serial-2093-flight-0012.eeprom
+ * so we can test the kalman filter with this data. Just
+ * keep reporting the same baro value across the pressure spike
+ */
+ {
+ static struct ao_ms5607_sample save;
+ if (ao_serial_number == 2093 && ao_flight_number == 12 && 32.5 < time && time < 33.7) {
+ ao_data_ring[ao_data_head].ms5607_raw = save;
+ } else {
+ save = ao_data_static.ms5607_raw;
+ }
+ }
+#else
+ double height = ao_pres_to_altitude(ao_data_static.adc.pres_real) - ao_ground_height;
+#endif
+
if (ao_test_max_height < height) {
ao_test_max_height = height;
ao_test_max_height_time = time;
}
if (!ao_summary) {
- printf("%7.2f height %8.2f accel %8.3f "
#if TELEMEGA
- "roll %8.3f angle %8.3f qangle %8.3f "
- "accel_x %8.3f accel_y %8.3f accel_z %8.3f gyro_x %8.3f gyro_y %8.3f gyro_z %8.3f "
+ static struct ao_quaternion ao_ground_mag;
+ static int ao_ground_mag_set;
+
+ if (!ao_ground_mag_set) {
+ ao_quaternion_init_vector (&ao_ground_mag,
+ ao_data_mag_across(&ao_data_static),
+ ao_data_mag_through(&ao_data_static),
+ ao_data_mag_along(&ao_data_static));
+ ao_quaternion_normalize(&ao_ground_mag, &ao_ground_mag);
+ ao_quaternion_rotate(&ao_ground_mag, &ao_ground_mag, &ao_rotation);
+ ao_ground_mag_set = 1;
+ }
+
+ struct ao_quaternion ao_mag, ao_mag_rot;
+
+ ao_quaternion_init_vector(&ao_mag,
+ ao_data_mag_across(&ao_data_static),
+ ao_data_mag_through(&ao_data_static),
+ ao_data_mag_along(&ao_data_static));
+
+ ao_quaternion_normalize(&ao_mag, &ao_mag);
+ ao_quaternion_rotate(&ao_mag_rot, &ao_mag, &ao_rotation);
+
+ float ao_dot;
+ int ao_mag_angle;
+
+ ao_dot = ao_quaternion_dot(&ao_mag_rot, &ao_ground_mag);
+
+ struct ao_azel ground_azel, mag_azel, rot_azel;
+
+ azel(&ground_azel, &ao_ground_mag);
+ azel(&mag_azel, &ao_mag);
+ azel(&rot_azel, &ao_mag_rot);
+
+ ao_mag_angle = floor (acos(ao_dot) * 180 / M_PI + 0.5);
+
+ (void) ao_mag_angle;
+
+ static struct ao_quaternion ao_x = { .r = 0, .x = 1, .y = 0, .z = 0 };
+ struct ao_quaternion ao_out;
+
+ ao_quaternion_rotate(&ao_out, &ao_x, &ao_rotation);
+
+#if 0
+ int out = floor (atan2(ao_out.y, ao_out.x) * 180 / M_PI);
+
+ printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d mag_tilt %4d mag_rot %4d\n",
+ time,
+ ao_state_names[ao_flight_state],
+ ao_k_height / 65536.0,
+ ao_sample_orient, out,
+ mag_azel.el,
+ mag_azel.az);
+#endif
+#if 0
+ printf ("%7.2f state %-8.8s height %8.4f tilt %4d rot %4d dist %12.2f gps_tilt %4d gps_sats %2d\n",
+ time,
+ ao_state_names[ao_flight_state],
+ ao_k_height / 65536.0,
+ ao_sample_orient, out,
+ ao_distance_from_pad(),
+ (int) floor (ao_gps_angle() + 0.5),
+ (ao_gps_static.flags & 0xf) * 10);
+
+#endif
+#if 0
+ 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",
+ ao_state_names[ao_flight_state],
+ ground_azel.az, ground_azel.el,
+ mag_azel.az, mag_azel.el,
+ rot_azel.az, rot_azel.el,
+ ground_azel.el - rot_azel.el,
+ ground_azel.az - rot_azel.az,
+ ao_mag_angle,
+ ao_sample_orient,
+ ao_ground_mag.x,
+ ao_ground_mag.y,
+ ao_ground_mag.z,
+ ao_mag.x,
+ ao_mag.y,
+ ao_mag.z,
+ ao_mag_rot.x,
+ ao_mag_rot.y,
+ ao_mag_rot.z);
+#endif
+#endif
+
+#if 1
+ printf("%7.2f height %8.2f accel %8.3f accel_speed %8.3f "
+ "state %d k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d"
+#if TELEMEGA
+ " angle %5d "
+ "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 "
#endif
- "state %-8.8s k_height %8.2f k_speed %8.3f k_accel %8.3f avg_height %5d drogue %4d main %4d error %5d\n",
+ "\n",
time,
height,
accel,
+ simple_speed > -100.0 ? simple_speed : -100.0,
+ ao_flight_state * 10,
+ ao_k_height / 65536.0,
+ ao_k_speed / 65536.0 / 16.0,
+ ao_k_accel / 65536.0 / 16.0,
+ ao_avg_height,
+ drogue_height,
+ main_height,
+ ao_error_h_sq_avg
#if TELEMEGA
- ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0,
- ao_mpu6000_gyro(ao_sample_angle) / 100.0,
- ao_sample_qangle,
+ , ao_sample_orient,
+
ao_mpu6000_accel(ao_data_static.mpu6000.accel_x),
ao_mpu6000_accel(ao_data_static.mpu6000.accel_y),
ao_mpu6000_accel(ao_data_static.mpu6000.accel_z),
ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x),
ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y),
ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z),
+ ao_data_static.hmc5883.x,
+ ao_data_static.hmc5883.y,
+ ao_data_static.hmc5883.z,
+ ao_mag_angle
#endif
- ao_state_names[ao_flight_state],
- ao_k_height / 65536.0,
- ao_k_speed / 65536.0 / 16.0,
- ao_k_accel / 65536.0 / 16.0,
- ao_avg_height,
- drogue_height,
- main_height,
- ao_error_h_sq_avg);
-
+ );
+#endif
+
// if (ao_flight_state == ao_flight_landed)
// ao_test_exit();
}
}
+ ao_data_head = ao_data_ring_next(ao_data_head);
}
-#define AO_MAX_CALLSIGN 8
-#define AO_MAX_VERSION 8
-#define AO_MAX_TELEMETRY 128
-
-struct ao_telemetry_generic {
- uint16_t serial; /* 0 */
- uint16_t tick; /* 2 */
- uint8_t type; /* 4 */
- uint8_t payload[27]; /* 5 */
- /* 32 */
-};
-
-#define AO_TELEMETRY_SENSOR_TELEMETRUM 0x01
-#define AO_TELEMETRY_SENSOR_TELEMINI 0x02
-#define AO_TELEMETRY_SENSOR_TELENANO 0x03
-
-struct ao_telemetry_sensor {
- uint16_t serial; /* 0 */
- uint16_t tick; /* 2 */
- uint8_t type; /* 4 */
-
- uint8_t state; /* 5 flight state */
- int16_t accel; /* 6 accelerometer (TM only) */
- int16_t pres; /* 8 pressure sensor */
- int16_t temp; /* 10 temperature sensor */
- int16_t v_batt; /* 12 battery voltage */
- int16_t sense_d; /* 14 drogue continuity sense (TM/Tm) */
- int16_t sense_m; /* 16 main continuity sense (TM/Tm) */
-
- int16_t acceleration; /* 18 m/s² * 16 */
- int16_t speed; /* 20 m/s * 16 */
- int16_t height; /* 22 m */
-
- int16_t ground_pres; /* 24 average pres on pad */
- int16_t ground_accel; /* 26 average accel on pad */
- int16_t accel_plus_g; /* 28 accel calibration at +1g */
- int16_t accel_minus_g; /* 30 accel calibration at -1g */
- /* 32 */
-};
-
-#define AO_TELEMETRY_CONFIGURATION 0x04
-
-struct ao_telemetry_configuration {
- uint16_t serial; /* 0 */
- uint16_t tick; /* 2 */
- uint8_t type; /* 4 */
-
- uint8_t device; /* 5 device type */
- uint16_t flight; /* 6 flight number */
- uint8_t config_major; /* 8 Config major version */
- uint8_t config_minor; /* 9 Config minor version */
- uint16_t apogee_delay; /* 10 Apogee deploy delay in seconds */
- uint16_t main_deploy; /* 12 Main deploy alt in meters */
- uint16_t flight_log_max; /* 14 Maximum flight log size in kB */
- char callsign[AO_MAX_CALLSIGN]; /* 16 Radio operator identity */
- char version[AO_MAX_VERSION]; /* 24 Software version */
- /* 32 */
-};
-
-#define AO_TELEMETRY_LOCATION 0x05
-
-#define AO_GPS_MODE_NOT_VALID 'N'
-#define AO_GPS_MODE_AUTONOMOUS 'A'
-#define AO_GPS_MODE_DIFFERENTIAL 'D'
-#define AO_GPS_MODE_ESTIMATED 'E'
-#define AO_GPS_MODE_MANUAL 'M'
-#define AO_GPS_MODE_SIMULATED 'S'
-
-struct ao_telemetry_location {
- uint16_t serial; /* 0 */
- uint16_t tick; /* 2 */
- uint8_t type; /* 4 */
-
- uint8_t flags; /* 5 Number of sats and other flags */
- int16_t altitude; /* 6 GPS reported altitude (m) */
- int32_t latitude; /* 8 latitude (degrees * 10⁷) */
- int32_t longitude; /* 12 longitude (degrees * 10⁷) */
- uint8_t year; /* 16 (- 2000) */
- uint8_t month; /* 17 (1-12) */
- uint8_t day; /* 18 (1-31) */
- uint8_t hour; /* 19 (0-23) */
- uint8_t minute; /* 20 (0-59) */
- uint8_t second; /* 21 (0-59) */
- uint8_t pdop; /* 22 (m * 5) */
- uint8_t hdop; /* 23 (m * 5) */
- uint8_t vdop; /* 24 (m * 5) */
- uint8_t mode; /* 25 */
- uint16_t ground_speed; /* 26 cm/s */
- int16_t climb_rate; /* 28 cm/s */
- uint8_t course; /* 30 degrees / 2 */
- uint8_t unused[1]; /* 31 */
- /* 32 */
-};
-
-#define AO_TELEMETRY_SATELLITE 0x06
-
-struct ao_telemetry_satellite_info {
- uint8_t svid;
- uint8_t c_n_1;
-};
-
-struct ao_telemetry_satellite {
- uint16_t serial; /* 0 */
- uint16_t tick; /* 2 */
- uint8_t type; /* 4 */
- uint8_t channels; /* 5 number of reported sats */
-
- struct ao_telemetry_satellite_info sats[12]; /* 6 */
- uint8_t unused[2]; /* 30 */
- /* 32 */
-};
-
-union ao_telemetry_all {
- struct ao_telemetry_generic generic;
- struct ao_telemetry_sensor sensor;
- struct ao_telemetry_configuration configuration;
- struct ao_telemetry_location location;
- struct ao_telemetry_satellite satellite;
-};
uint16_t
uint16(uint8_t *bytes, int off)
return (int32_t) uint32(bytes, off);
}
-static int log_format;
-
-#if TELEMEGA
-
-static double
-ao_vec_norm(double x, double y, double z)
-{
- return x*x + y*y + z*z;
-}
-
-static void
-ao_vec_normalize(double *x, double *y, double *z)
-{
- double scale = 1/sqrt(ao_vec_norm(*x, *y, *z));
-
- *x *= scale;
- *y *= scale;
- *z *= scale;
-}
-
-struct ao_quat {
- double q0, q1, q2, q3;
-};
-
-static void
-ao_quat_mul(struct ao_quat *r, struct ao_quat *a, struct ao_quat *b)
-{
- r->q0 = a->q0 * b->q0 - a->q1 * b->q1 - a->q2 * b->q2 - a->q3 * b->q3;
- r->q1 = a->q0 * b->q1 + a->q1 * b->q0 + a->q2 * b->q3 - a->q3 * b->q2;
- r->q2 = a->q0 * b->q2 - a->q1 * b->q3 + a->q2 * b->q0 + a->q3 * b->q1;
- r->q3 = a->q0 * b->q3 + a->q1 * b->q2 - a->q2 * b->q1 + a->q3 * b->q0;
-}
-
-#if 0
-static void
-ao_quat_scale(struct ao_quat *r, struct ao_quat *a, double s)
-{
- r->q0 = a->q0 * s;
- r->q1 = a->q1 * s;
- r->q2 = a->q2 * s;
- r->q3 = a->q3 * s;
-}
-#endif
-
-static void
-ao_quat_conj(struct ao_quat *r, struct ao_quat *a)
-{
- r->q0 = a->q0;
- r->q1 = -a->q1;
- r->q2 = -a->q2;
- r->q3 = -a->q3;
-}
-
-static void
-ao_quat_rot(struct ao_quat *r, struct ao_quat *a, struct ao_quat *q)
-{
- struct ao_quat t;
- struct ao_quat c;
- ao_quat_mul(&t, q, a);
- ao_quat_conj(&c, q);
- ao_quat_mul(r, &t, &c);
-}
-
-static void
-ao_quat_from_angle(struct ao_quat *r,
- double x_rad,
- double y_rad,
- double z_rad)
-{
- double angle = sqrt (x_rad * x_rad + y_rad * y_rad + z_rad * z_rad);
- double s = sin(angle/2);
- double c = cos(angle/2);
-
- r->q0 = c;
- r->q1 = x_rad * s / angle;
- r->q2 = y_rad * s / angle;
- r->q3 = z_rad * s / angle;
-}
-
-static void
-ao_quat_from_vector(struct ao_quat *r, double x, double y, double z)
-{
- ao_vec_normalize(&x, &y, &z);
- double x_rad = atan2(z, y);
- double y_rad = atan2(x, z);
- double z_rad = atan2(y, x);
-
- ao_quat_from_angle(r, x_rad, y_rad, z_rad);
-}
-
-static double
-ao_quat_norm(struct ao_quat *a)
-{
- return (a->q0 * a->q0 +
- a->q1 * a->q1 +
- a->q2 * a->q2 +
- a->q3 * a->q3);
-}
-
-static void
-ao_quat_normalize(struct ao_quat *a)
+uint32_t
+uint24(uint8_t *bytes, int off)
{
- double norm = ao_quat_norm(a);
-
- if (norm) {
- double m = 1/sqrt(norm);
-
- a->q0 *= m;
- a->q1 *= m;
- a->q2 *= m;
- a->q3 *= m;
- }
+ return (uint32_t) bytes[off] | (((uint32_t) bytes[off+1]) << 8) |
+ (((uint32_t) bytes[off+2]) << 16);
}
-static struct ao_quat ao_up, ao_current;
-static struct ao_quat ao_orient;
-static int ao_orient_tick;
-
-void
-set_orientation(double x, double y, double z, int tick)
+int32_t
+int24(uint8_t *bytes, int off)
{
- struct ao_quat t;
-
- printf ("set_orientation %g %g %g\n", x, y, z);
- ao_quat_from_vector(&ao_orient, x, y, z);
- ao_up.q1 = ao_up.q2 = 0;
- ao_up.q0 = ao_up.q3 = sqrt(2)/2;
- ao_orient_tick = tick;
-
- ao_orient.q0 = 1;
- ao_orient.q1 = 0;
- ao_orient.q2 = 0;
- ao_orient.q3 = 0;
-
- printf ("orient (%g) %g %g %g up (%g) %g %g %g\n",
- ao_orient.q0,
- ao_orient.q1,
- ao_orient.q2,
- ao_orient.q3,
- ao_up.q0,
- ao_up.q1,
- ao_up.q2,
- ao_up.q3);
-
- ao_quat_rot(&t, &ao_up, &ao_orient);
- printf ("pad orient (%g) %g %g %g\n",
- t.q0,
- t.q1,
- t.q2,
- t.q3);
-
+ return (int32_t) uint24(bytes, off);
}
-void
-update_orientation (double rate_x, double rate_y, double rate_z, int tick)
-{
- struct ao_quat q_dot;
- double lambda;
- double dt = (tick - ao_orient_tick) / 100.0;
-
- ao_orient_tick = tick;
-
-// lambda = 1 - ao_quat_norm(&ao_orient);
- lambda = 0;
-
- q_dot.q0 = -0.5 * (ao_orient.q1 * rate_x + ao_orient.q2 * rate_y + ao_orient.q3 * rate_z) + lambda * ao_orient.q0;
- q_dot.q1 = 0.5 * (ao_orient.q0 * rate_x + ao_orient.q2 * rate_z - ao_orient.q3 * rate_y) + lambda * ao_orient.q1;
- q_dot.q2 = 0.5 * (ao_orient.q0 * rate_y + ao_orient.q3 * rate_x - ao_orient.q1 * rate_z) + lambda * ao_orient.q2;
- q_dot.q3 = 0.5 * (ao_orient.q0 * rate_z + ao_orient.q1 * rate_y - ao_orient.q2 * rate_x) + lambda * ao_orient.q3;
-
-#if 0
- printf ("update_orientation %g %g %g (%g s)\n", rate_x, rate_y, rate_z, dt);
- printf ("q_dot (%g) %g %g %g\n",
- q_dot.q0,
- q_dot.q1,
- q_dot.q2,
- q_dot.q3);
-#endif
-
- ao_orient.q0 += q_dot.q0 * dt;
- ao_orient.q1 += q_dot.q1 * dt;
- ao_orient.q2 += q_dot.q2 * dt;
- ao_orient.q3 += q_dot.q3 * dt;
-
- ao_quat_normalize(&ao_orient);
-
- ao_quat_rot(&ao_current, &ao_up, &ao_orient);
-
- ao_sample_qangle = 180 / M_PI * acos(ao_current.q3 * sqrt(2));
-#if 0
- printf ("orient (%g) %g %g %g current (%g) %g %g %g\n",
- ao_orient.q0,
- ao_orient.q1,
- ao_orient.q2,
- ao_orient.q3,
- ao_current.q0,
- ao_current.q1,
- ao_current.q2,
- ao_current.q3);
-#endif
-}
-#endif
+static int log_format;
void
ao_sleep(void *wchan)
{
if (wchan == &ao_data_head) {
- char type = 0;
- uint16_t tick = 0;
- uint16_t a = 0, b = 0;
- uint8_t bytes[1024];
- union ao_telemetry_all telem;
- char line[1024];
- char *saveptr;
- char *l;
- char *words[64];
- int nword;
-
#if TELEMEGA
if (ao_flight_state >= ao_flight_boost && ao_flight_state < ao_flight_landed)
ao_pyro_check();
for (;;) {
if (ao_records_read > 2 && ao_flight_state == ao_flight_startup)
{
+
#if TELEMEGA
ao_data_static.mpu6000 = ao_ground_mpu6000;
-#else
+#endif
+#if TELEMETRUM_V1
ao_data_static.adc.accel = ao_flight_ground_accel;
#endif
+
ao_insert();
return;
}
- if (!fgets(line, sizeof (line), emulator_in)) {
- if (++ao_eof_read >= 1000) {
- if (!ao_summary)
- printf ("no more data, exiting simulation\n");
+ if (eeprom) {
+#if TELEMEGA
+ struct ao_log_mega *log_mega;
+#endif
+#if TELEMETRUM_V2
+ struct ao_log_metrum *log_metrum;
+#endif
+#if EASYMINI
+ struct ao_log_mini *log_mini;
+#endif
+#if TELEMETRUM_V1
+ struct ao_log_record *log_record;
+#endif
+
+ if (eeprom_offset >= eeprom->len) {
+ if (++ao_eof_read >= 1000)
+ if (!ao_summary)
+ printf ("no more data, exiting simulation\n");
ao_test_exit();
+ ao_data_static.tick += 10;
+ ao_insert();
+ return;
}
- ao_data_static.tick += 10;
- ao_insert();
- return;
- }
- l = line;
- for (nword = 0; nword < 64; nword++) {
- words[nword] = strtok_r(l, " \t\n", &saveptr);
- l = NULL;
- if (words[nword] == NULL)
- break;
- }
+ switch (eeprom->log_format) {
#if TELEMEGA
- if (log_format == AO_LOG_FORMAT_TELEMEGA && nword == 30 && strlen(words[0]) == 1) {
- int i;
- struct ao_ms5607_value value;
-
- type = words[0][0];
- tick = strtoul(words[1], NULL, 16);
-// printf ("%c %04x", type, tick);
- for (i = 2; i < nword; i++) {
- bytes[i - 2] = strtoul(words[i], NULL, 16);
-// printf(" %02x", bytes[i-2]);
- }
-// printf ("\n");
- switch (type) {
- case 'F':
- ao_flight_ground_accel = int16(bytes, 2);
- ao_flight_started = 1;
- ao_ground_pres = int32(bytes, 4);
- ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
+ case AO_LOG_FORMAT_TELEMEGA_OLD:
+ case AO_LOG_FORMAT_TELEMEGA:
+ log_mega = (struct ao_log_mega *) &eeprom->data[eeprom_offset];
+ eeprom_offset += sizeof (*log_mega);
+ switch (log_mega->type) {
+ case AO_LOG_FLIGHT:
+ ao_flight_number = log_mega->u.flight.flight;
+ ao_flight_ground_accel = log_mega->u.flight.ground_accel;
+ ao_flight_started = 1;
+ ao_ground_pres = log_mega->u.flight.ground_pres;
+ ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
+ ao_ground_accel_along = log_mega->u.flight.ground_accel_along;
+ ao_ground_accel_across = log_mega->u.flight.ground_accel_across;
+ ao_ground_accel_through = log_mega->u.flight.ground_accel_through;
+ ao_ground_roll = log_mega->u.flight.ground_roll;
+ ao_ground_pitch = log_mega->u.flight.ground_pitch;
+ ao_ground_yaw = log_mega->u.flight.ground_yaw;
+ ao_ground_mpu6000.accel_x = ao_ground_accel_across;
+ ao_ground_mpu6000.accel_y = ao_ground_accel_along;
+ ao_ground_mpu6000.accel_z = ao_ground_accel_through;
+ ao_ground_mpu6000.gyro_x = ao_ground_pitch >> 9;
+ ao_ground_mpu6000.gyro_y = ao_ground_roll >> 9;
+ ao_ground_mpu6000.gyro_z = ao_ground_yaw >> 9;
+ break;
+ case AO_LOG_STATE:
+ break;
+ case AO_LOG_SENSOR:
+ ao_data_static.tick = log_mega->tick;
+ ao_data_static.ms5607_raw.pres = log_mega->u.sensor.pres;
+ ao_data_static.ms5607_raw.temp = log_mega->u.sensor.temp;
+ ao_data_static.mpu6000.accel_x = log_mega->u.sensor.accel_x;
+ ao_data_static.mpu6000.accel_y = log_mega->u.sensor.accel_y;
+ ao_data_static.mpu6000.accel_z = log_mega->u.sensor.accel_z;
+ ao_data_static.mpu6000.gyro_x = log_mega->u.sensor.gyro_x;
+ ao_data_static.mpu6000.gyro_y = log_mega->u.sensor.gyro_y;
+ ao_data_static.mpu6000.gyro_z = log_mega->u.sensor.gyro_z;
+ ao_data_static.hmc5883.x = log_mega->u.sensor.mag_x;
+ ao_data_static.hmc5883.y = log_mega->u.sensor.mag_y;
+ ao_data_static.hmc5883.z = log_mega->u.sensor.mag_z;
+ ao_data_static.mma655x = log_mega->u.sensor.accel;
+ if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
+ ao_data_static.mma655x = ao_data_accel_invert(ao_data_static.mma655x);
+ ao_records_read++;
+ ao_insert();
+ return;
+ case AO_LOG_TEMP_VOLT:
+ if (pyros_fired != log_mega->u.volt.pyro) {
+ printf("pyro changed %x -> %x\n", pyros_fired, log_mega->u.volt.pyro);
+ pyros_fired = log_mega->u.volt.pyro;
+ }
+ break;
+ case AO_LOG_GPS_TIME:
+ ao_gps_prev = ao_gps_static;
+ ao_gps_static.tick = log_mega->tick;
+ ao_gps_static.latitude = log_mega->u.gps.latitude;
+ ao_gps_static.longitude = log_mega->u.gps.longitude;
+ {
+ int16_t altitude_low = log_mega->u.gps.altitude_low;
+ int16_t altitude_high = log_mega->u.gps.altitude_high;
+ int32_t altitude = altitude_low | ((int32_t) altitude_high << 16);
+
+ AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_static, altitude);
+ }
+ ao_gps_static.flags = log_mega->u.gps.flags;
+ if (!ao_gps_count)
+ ao_gps_first = ao_gps_static;
+ ao_gps_count++;
+ break;
+ case AO_LOG_GPS_SAT:
+ break;
+ }
break;
- case 'A':
- ao_data_static.tick = tick;
- ao_data_static.ms5607_raw.pres = int32(bytes, 0);
- ao_data_static.ms5607_raw.temp = int32(bytes, 4);
- ao_ms5607_convert(&ao_data_static.ms5607_raw, &value);
- ao_data_static.mpu6000.accel_x = int16(bytes, 8);
- ao_data_static.mpu6000.accel_y = -int16(bytes, 10);
- ao_data_static.mpu6000.accel_z = int16(bytes, 12);
- ao_data_static.mpu6000.gyro_x = int16(bytes, 14);
- ao_data_static.mpu6000.gyro_y = -int16(bytes, 16);
- ao_data_static.mpu6000.gyro_z = int16(bytes, 18);
-#if HAS_MMA655X
- ao_data_static.mma655x = int16(bytes, 26);
#endif
- if (ao_records_read == 0)
- ao_ground_mpu6000 = ao_data_static.mpu6000;
- else if (ao_records_read < 10) {
-#define f(f) ao_ground_mpu6000.f = ao_ground_mpu6000.f + ((ao_data_static.mpu6000.f - ao_ground_mpu6000.f) >> 2)
- f(accel_x);
- f(accel_y);
- f(accel_z);
- f(gyro_x);
- f(gyro_y);
- f(gyro_z);
-
- double accel_x = ao_mpu6000_accel(ao_ground_mpu6000.accel_x);
- double accel_y = ao_mpu6000_accel(ao_ground_mpu6000.accel_y);
- double accel_z = ao_mpu6000_accel(ao_ground_mpu6000.accel_z);
-
- /* X and Y are in the ground plane, arbitraryily picked as MPU X and Z axes
- * Z is normal to the ground, the MPU y axis
- */
- set_orientation(accel_x, accel_z, accel_y, tick);
- } else {
- double rate_x = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_x - ao_ground_mpu6000.gyro_x);
- double rate_y = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_y - ao_ground_mpu6000.gyro_y);
- double rate_z = ao_mpu6000_gyro(ao_data_static.mpu6000.gyro_z - ao_ground_mpu6000.gyro_z);
-
- update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick);
+#if TELEMETRUM_V2
+ case AO_LOG_FORMAT_TELEMETRUM:
+ log_metrum = (struct ao_log_metrum *) &eeprom->data[eeprom_offset];
+ eeprom_offset += sizeof (*log_metrum);
+ switch (log_metrum->type) {
+ case AO_LOG_FLIGHT:
+ ao_flight_started = 1;
+ ao_flight_number = log_metrum->u.flight.flight;
+ ao_flight_ground_accel = log_metrum->u.flight.ground_accel;
+ ao_ground_pres = log_metrum->u.flight.ground_pres;
+ ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
+ break;
+ case AO_LOG_SENSOR:
+ ao_data_static.tick = log_metrum->tick;
+ ao_data_static.ms5607_raw.pres = log_metrum->u.sensor.pres;
+ ao_data_static.ms5607_raw.temp = log_metrum->u.sensor.temp;
+ ao_data_static.mma655x = log_metrum->u.sensor.accel;
+ ao_records_read++;
+ ao_insert();
+ return;
}
- ao_records_read++;
- ao_insert();
- return;
- }
- continue;
- } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) {
- if (strcmp(words[1], "reserved:") == 0)
- ms5607_prom.reserved = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "sens:") == 0)
- ms5607_prom.sens = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "off:") == 0)
- ms5607_prom.off = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tcs:") == 0)
- ms5607_prom.tcs = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tco:") == 0)
- ms5607_prom.tco = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tref:") == 0)
- ms5607_prom.tref = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tempsens:") == 0)
- ms5607_prom.tempsens = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "crc:") == 0)
- ms5607_prom.crc = strtoul(words[2], NULL, 10);
- continue;
- } else if (nword >= 3 && strcmp(words[0], "Pyro") == 0) {
- int p = strtoul(words[1], NULL, 10);
- int i, j;
- struct ao_pyro *pyro = &ao_config.pyro[p];
-
- for (i = 2; i < nword; i++) {
- for (j = 0; j < NUM_PYRO_VALUES; j++)
- if (!strcmp (words[2], ao_pyro_values[j].name))
- break;
- if (j == NUM_PYRO_VALUES)
- continue;
- pyro->flags |= ao_pyro_values[j].flag;
- if (ao_pyro_values[j].offset != NO_VALUE && i + 1 < nword) {
- int16_t val = strtoul(words[++i], NULL, 10);
- *((int16_t *) ((char *) pyro + ao_pyro_values[j].offset)) = val;
+ break;
+#endif
+#if EASYMINI
+ case AO_LOG_FORMAT_EASYMINI1:
+ case AO_LOG_FORMAT_EASYMINI2:
+ case AO_LOG_FORMAT_TELEMINI3:
+ log_mini = (struct ao_log_mini *) &eeprom->data[eeprom_offset];
+ eeprom_offset += sizeof (*log_mini);
+ switch (log_mini->type) {
+ case AO_LOG_FLIGHT:
+ ao_flight_started = 1;
+ ao_flight_number = log_mini->u.flight.flight;
+ ao_ground_pres = log_mini->u.flight.ground_pres;
+ ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
+ break;
+ case AO_LOG_SENSOR:
+ ao_data_static.tick = log_mini->tick;
+ ao_data_static.ms5607_raw.pres = int24(log_mini->u.sensor.pres, 0);
+ ao_data_static.ms5607_raw.temp = int24(log_mini->u.sensor.temp, 0);
+ ao_records_read++;
+ ao_insert();
+ return;
}
- }
- }
-#else
- if (nword == 4 && log_format != AO_LOG_FORMAT_TELEMEGA) {
- type = words[0][0];
- tick = strtoul(words[1], NULL, 16);
- a = strtoul(words[2], NULL, 16);
- b = strtoul(words[3], NULL, 16);
- if (type == 'P')
- type = 'A';
- }
+ break;
#endif
- else if (nword == 2 && strcmp(words[0], "log-format") == 0) {
- log_format = strtoul(words[1], NULL, 10);
- } else if (nword >= 6 && strcmp(words[0], "Accel") == 0) {
- ao_config.accel_plus_g = atoi(words[3]);
- ao_config.accel_minus_g = atoi(words[5]);
- } else if (nword >= 4 && strcmp(words[0], "Main") == 0) {
- ao_config.main_deploy = atoi(words[2]);
- } else if (nword >= 3 && strcmp(words[0], "Apogee") == 0 &&
- strcmp(words[1], "lockout:") == 0) {
- ao_config.apogee_lockout = atoi(words[2]);
- } else if (nword >= 36 && strcmp(words[0], "CALL") == 0) {
- tick = atoi(words[10]);
- if (!ao_flight_started) {
- type = 'F';
- a = atoi(words[26]);
- ao_flight_started = 1;
- } else {
- type = 'A';
- a = atoi(words[12]);
- b = atoi(words[14]);
- }
- } else if (nword == 3 && strcmp(words[0], "BARO") == 0) {
- tick = strtol(words[1], NULL, 16);
- a = 16384 - 328;
- b = strtol(words[2], NULL, 10);
- type = 'A';
- if (!ao_flight_started) {
- ao_flight_ground_accel = 16384 - 328;
- ao_config.accel_plus_g = 16384 - 328;
- ao_config.accel_minus_g = 16384 + 328;
- ao_flight_started = 1;
- }
- } else if (nword == 2 && strcmp(words[0], "TELEM") == 0) {
- __xdata char *hex = words[1];
- char elt[3];
- int i, len;
- uint8_t sum;
-
- len = strlen(hex);
- if (len > sizeof (bytes) * 2) {
- len = sizeof (bytes)*2;
- hex[len] = '\0';
- }
- for (i = 0; i < len; i += 2) {
- elt[0] = hex[i];
- elt[1] = hex[i+1];
- elt[2] = '\0';
- bytes[i/2] = (uint8_t) strtol(elt, NULL, 16);
- }
- len = i/2;
- if (bytes[0] != len - 2) {
- printf ("bad length %d != %d\n", bytes[0], len - 2);
- continue;
- }
- sum = 0x5a;
- for (i = 1; i < len-1; i++)
- sum += bytes[i];
- if (sum != bytes[len-1]) {
- printf ("bad checksum\n");
- continue;
- }
- if ((bytes[len-2] & 0x80) == 0) {
- continue;
- }
- if (len == 36) {
- ao_xmemcpy(&telem, bytes + 1, 32);
- tick = telem.generic.tick;
- switch (telem.generic.type) {
- case AO_TELEMETRY_SENSOR_TELEMETRUM:
- case AO_TELEMETRY_SENSOR_TELEMINI:
- case AO_TELEMETRY_SENSOR_TELENANO:
- if (!ao_flight_started) {
- ao_flight_ground_accel = telem.sensor.ground_accel;
- ao_config.accel_plus_g = telem.sensor.accel_plus_g;
- ao_config.accel_minus_g = telem.sensor.accel_minus_g;
- ao_flight_started = 1;
- }
- type = 'A';
- a = telem.sensor.accel;
- b = telem.sensor.pres;
+#if TELEMETRUM_V1
+ case AO_LOG_FORMAT_FULL:
+ case AO_LOG_FORMAT_TINY:
+ log_record = (struct ao_log_record *) &eeprom->data[eeprom_offset];
+ eeprom_offset += sizeof (*log_record);
+ switch (log_record->type) {
+ case AO_LOG_FLIGHT:
+ ao_flight_started = 1;
+ ao_flight_ground_accel = log_record->u.flight.ground_accel;
+ ao_flight_number = log_record->u.flight.flight;
+ break;
+ case AO_LOG_SENSOR:
+ case 'P': /* ancient telemini */
+ ao_data_static.tick = log_record->tick;
+ ao_data_static.adc.accel = log_record->u.sensor.accel;
+ ao_data_static.adc.pres_real = log_record->u.sensor.pres;
+ ao_data_static.adc.pres = log_record->u.sensor.pres;
+ ao_records_read++;
+ ao_insert();
+ return;
+ case AO_LOG_TEMP_VOLT:
+ ao_data_static.tick = log_record->tick;;
+ ao_data_static.adc.temp = log_record->u.temp_volt.temp;
+ ao_data_static.adc.v_batt = log_record->u.temp_volt.v_batt;
break;
}
- } else if (len == 99) {
- ao_flight_started = 1;
- tick = uint16(bytes+1, 21);
- ao_flight_ground_accel = int16(bytes+1, 7);
- ao_config.accel_plus_g = int16(bytes+1, 17);
- ao_config.accel_minus_g = int16(bytes+1, 19);
- type = 'A';
- a = int16(bytes+1, 23);
- b = int16(bytes+1, 25);
- } else if (len == 98) {
- ao_flight_started = 1;
- tick = uint16(bytes+1, 20);
- ao_flight_ground_accel = int16(bytes+1, 6);
- ao_config.accel_plus_g = int16(bytes+1, 16);
- ao_config.accel_minus_g = int16(bytes+1, 18);
- type = 'A';
- a = int16(bytes+1, 22);
- b = int16(bytes+1, 24);
- } else {
- printf("unknown len %d\n", len);
- continue;
- }
- }
- if (type != 'F' && !ao_flight_started)
- continue;
-
-#if TELEMEGA
- (void) a;
- (void) b;
-#else
- switch (type) {
- case 'F':
- ao_flight_ground_accel = a;
- if (ao_config.accel_plus_g == 0) {
- ao_config.accel_plus_g = a;
- ao_config.accel_minus_g = a + 530;
+ break;
+#endif
+ default:
+ printf ("invalid log format %d\n", log_format);
+ ao_test_exit();
}
- if (ao_config.main_deploy == 0)
- ao_config.main_deploy = 250;
- ao_flight_started = 1;
- break;
- case 'S':
- break;
- case 'A':
- ao_data_static.tick = tick;
- ao_data_static.adc.accel = a;
- ao_data_static.adc.pres_real = b;
- if (b < AO_MIN_BARO_VALUE)
- b = AO_MIN_BARO_VALUE;
- ao_data_static.adc.pres = b;
- ao_records_read++;
- ao_insert();
- return;
- case 'T':
- ao_data_static.tick = tick;
- ao_data_static.adc.temp = a;
- ao_data_static.adc.v_batt = b;
- break;
- case 'D':
- case 'G':
- case 'N':
- case 'W':
- case 'H':
- break;
}
-#endif
}
}
emulator_in = f;
emulator_info = info;
ao_summary = summary;
+
+ if (strstr(name, ".eeprom") != NULL) {
+ char c;
+
+ c = getc(f);
+ ungetc(c, f);
+ if (c == '{')
+ eeprom = ao_eeprom_read(f);
+ else
+ eeprom = ao_eeprom_read_old(f);
+
+ if (eeprom) {
+#if HAS_MS5607
+ ao_ms5607_prom = eeprom->ms5607_prom;
+#endif
+ ao_config = eeprom->config;
+ ao_serial_number = eeprom->serial_number;
+ log_format = eeprom->log_format;
+ }
+ }
+
ao_flight_init();
ao_flight();
}
#else
emulator_app="baro";
#endif
- while ((c = getopt_long(argc, argv, "sdi:", options, NULL)) != -1) {
+ while ((c = getopt_long(argc, argv, "sdpi:", options, NULL)) != -1) {
switch (c) {
case 's':
summary = 1;
case 'd':
ao_flight_debug = 1;
break;
+ case 'p':
+#if PYRO_DBG
+ pyro_dbg = 1;
+#endif
+ break;
case 'i':
info = optarg;
break;