*
* 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 <stdint.h>
#include <stdio.h>
#include <stdlib.h>
+#include <stddef.h>
#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 MEGAMETRUM
+#if TELEMEGA
#define AO_ADC_NUM_SENSE 6
#define HAS_MS5607 1
#define HAS_MPU6000 1
-#define HAS_MMA655X 0
+#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
*/
#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 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)
+{
+ return ao_data_static.tick;
+}
+
+void
+ao_delay(int16_t interval)
+{
+ return;
+}
void
ao_ignite(enum ao_igniter igniter)
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 MEGAMETRUM
+#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;
-};
-
-#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()
#define DATA_TO_XDATA(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;
extern alt_t ao_ground_height;
extern alt_t ao_sample_alt;
+double ao_sample_qangle;
+
int ao_sample_prev_tick;
uint16_t prev_tick;
+
#include "ao_kalman.c"
+#include "ao_sqrt.c"
#include "ao_sample.c"
#include "ao_flight.c"
+#if TELEMEGA
+#define AO_PYRO_NUM 4
+
+#define AO_PYRO_0 0
+#define AO_PYRO_1 1
+#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)
+{
+ printf ("set pyro %d %d\n", pin, 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;
+#endif
+
+#if HAS_ACCEL
+int ao_error_h_sq_avg;
+#endif
void
ao_test_exit(void)
exit(0);
}
+#ifdef TELEMEGA
+struct ao_azel {
+ int az;
+ int el;
+};
+
+static void
+azel (struct ao_azel *r, struct ao_quaternion *q)
+{
+ 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
+
void
ao_insert(void)
{
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) /
#else
double accel = 0.0;
#endif
-#if MEGAMETRUM
- 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;
+ simple_speed += accel * (ao_data_static.tick - prev_tick) / 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 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",
+#if TELEMEGA
+ 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 %-8.8s 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
+ "\n",
time,
height,
accel,
+ simple_speed > -100.0 ? simple_speed : -100.0,
ao_state_names[ao_flight_state],
ao_k_height / 65536.0,
ao_k_speed / 65536.0 / 16.0,
ao_avg_height,
drogue_height,
main_height,
- ao_error_h_sq_avg);
-
+ ao_error_h_sq_avg
+#if TELEMEGA
+ , 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
+ );
+#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);
}
+uint32_t
+uint24(uint8_t *bytes, int off)
+{
+ return (uint32_t) bytes[off] | (((uint32_t) bytes[off+1]) << 8) |
+ (((uint32_t) bytes[off+2]) << 16);
+}
+
+int32_t
+int24(uint8_t *bytes, int off)
+{
+ return (int32_t) uint24(bytes, off);
+}
+
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();
+#endif
for (;;) {
if (ao_records_read > 2 && ao_flight_state == ao_flight_startup)
{
-#if MEGAMETRUM
-#else
+
+#if TELEMEGA
+ ao_data_static.mpu6000 = ao_ground_mpu6000;
+#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;
- }
- l = line;
- for (nword = 0; nword < 64; nword++) {
- words[nword] = strtok_r(l, " \t\n", &saveptr);
- l = NULL;
- if (words[nword] == NULL)
- break;
- }
-#if MEGAMETRUM
- if (log_format == AO_LOG_FORMAT_MEGAMETRUM && 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);
- 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_set_accel(&ao_data_static, -int16(bytes, 10));
-// printf ("accel %d pres %d\n", ao_data_accel_cook(&ao_data_static), value.pres);
- ao_records_read++;
+ ao_data_static.tick += 10;
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 == 4 && log_format != AO_LOG_FORMAT_MEGAMETRUM) {
- 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';
- }
-#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;
+ switch (eeprom->log_format) {
+#if TELEMEGA
+ 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;
}
- type = 'A';
- a = telem.sensor.accel;
- b = telem.sensor.pres;
+ 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;
}
- } 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 MEGAMETRUM
-#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
+#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;
+ }
+ 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;
+ }
+ break;
+#endif
+#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;
+ }
+ 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;