--- /dev/null
+/*
+ * Copyright © 2017 Keith Packard <keithp@keithp.com>
+ *
+ * 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, 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
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ */
+
+#include "ao_eeprom_read.h"
+#include <json-c/json.h>
+#include <string.h>
+#include <errno.h>
+
+static struct json_object *
+ao_eeprom_read_config(FILE *file)
+{
+ char line[1024];
+ struct json_tokener *tok;
+ struct json_object *obj = NULL;
+ enum json_tokener_error err;
+
+ tok = json_tokener_new();
+ if (!tok)
+ goto fail_tok;
+
+ for (;;) {
+ if (fgets(line, sizeof(line), file) == NULL)
+ goto fail_read;
+ obj = json_tokener_parse_ex(tok, line, strlen(line));
+ err = json_tokener_get_error(tok);
+ if (err == json_tokener_success)
+ break;
+ if (err != json_tokener_continue)
+ goto fail_read;
+ }
+ json_tokener_free(tok);
+ return obj;
+fail_read:
+ json_tokener_free(tok);
+fail_tok:
+ return NULL;
+}
+
+static int
+ao_eeprom_read_byte(FILE *file)
+{
+ int byte;
+ if (fscanf(file, "%x", &byte) != 1)
+ return EOF;
+ return byte;
+}
+
+static int
+ao_eeprom_read_data(FILE *file, struct ao_eeprom *eeprom)
+{
+ uint8_t *data = NULL, *ndata;
+ int len = 0;
+ int size = 0;
+ int byte;
+
+ data = malloc(size = 64);
+ if (!data)
+ goto fail_alloc;
+ while ((byte = ao_eeprom_read_byte(file)) != EOF) {
+ if (len == size) {
+ ndata = realloc(data, size *= 2);
+ if (!ndata)
+ goto fail_realloc;
+ data = ndata;
+ }
+ data[len++] = (uint8_t) byte;
+ }
+ eeprom->data = data;
+ eeprom->len = len;
+ return 1;
+fail_realloc:
+ free(data);
+fail_alloc:
+ return 0;
+}
+
+static int
+ao_json_get_int(struct json_object *obj, const char *key, int def)
+{
+ struct json_object *value;
+ int i;
+
+ if (!json_object_object_get_ex(obj, key, &value))
+ return def;
+ errno = 0;
+ i = (int) json_object_get_int(value);
+ if (errno != 0)
+ return def;
+ return i;
+}
+
+static const char *
+ao_json_get_string(struct json_object *obj, const char *key, const char *def)
+{
+ struct json_object *value;
+ const char *str;
+
+ if (!json_object_object_get_ex(obj, key, &value))
+ return def;
+ errno = 0;
+ str = json_object_get_string(value);
+ if (errno)
+ return def;
+ if (!str)
+ return def;
+ return str;
+}
+
+#if AO_PYRO_NUM
+static int
+ao_eeprom_get_pyro(struct ao_config *config, struct json_object *obj)
+{
+ struct json_object *pyros;
+ struct json_object *pyro;
+ int i, p;
+
+ if (!json_object_object_get_ex(obj, "pyros", &pyros))
+ return 1;
+
+ if (json_object_get_type(pyros) != json_type_array)
+ return 0;
+
+ for (i = 0; i < json_object_array_length(pyros); i++) {
+ pyro = json_object_array_get_idx(pyros, i);
+ if (pyro) {
+ p = ao_json_get_int(pyro, "channel", -1);
+ if (0 <= p && p < AO_PYRO_NUM) {
+ config->pyro[p].flags = ao_json_get_int(pyro, "flags", 0);
+ config->pyro[p].accel_less = ao_json_get_int(pyro, "accel_less", 0);
+ config->pyro[p].accel_greater = ao_json_get_int(pyro, "accel_greater", 0);
+ config->pyro[p].speed_less = ao_json_get_int(pyro, "speed_less", 0);
+ config->pyro[p].speed_greater = ao_json_get_int(pyro, "speed_greater", 0);
+ config->pyro[p].height_less = ao_json_get_int(pyro, "height_less", 0);
+ config->pyro[p].height_greater = ao_json_get_int(pyro, "height_greater", 0);
+ config->pyro[p].orient_less = ao_json_get_int(pyro, "orient_less", 0);
+ config->pyro[p].orient_greater = ao_json_get_int(pyro, "orient_greater", 0);
+ config->pyro[p].time_less = ao_json_get_int(pyro, "time_less", 0);
+ config->pyro[p].time_greater = ao_json_get_int(pyro, "time_greater", 0);
+ config->pyro[p].delay = ao_json_get_int(pyro, "delay", 0);
+ config->pyro[p].state_less = ao_json_get_int(pyro, "state_less", 0);
+ config->pyro[p].state_greater_or_equal = ao_json_get_int(pyro, "state_greater_or_equal", 0);
+ config->pyro[p].motor = ao_json_get_int(pyro, "motor", 0);
+ }
+ }
+ }
+ return 1;
+}
+#endif
+
+static int
+ao_eeprom_get_ms5607(struct ao_ms5607_prom *ms5607_prom, struct json_object *obj)
+{
+ struct json_object *ms5607;
+
+ if (!json_object_object_get_ex(obj, "ms5607", &ms5607))
+ return 1;
+
+ if (json_object_get_type(ms5607) != json_type_object)
+ return 0;
+
+ ms5607_prom->reserved = ao_json_get_int(ms5607, "reserved", 0);
+ ms5607_prom->sens = ao_json_get_int(ms5607, "sens", 0);
+ ms5607_prom->off = ao_json_get_int(ms5607, "off", 0);
+ ms5607_prom->tcs = ao_json_get_int(ms5607, "tcs", 0);
+ ms5607_prom->tco = ao_json_get_int(ms5607, "tco", 0);
+ ms5607_prom->tref = ao_json_get_int(ms5607, "tref", 0);
+ ms5607_prom->tempsens = ao_json_get_int(ms5607, "tempsens", 0);
+ ms5607_prom->crc = ao_json_get_int(ms5607, "crc", 0);
+ return 1;
+}
+
+static int
+ao_eeprom_get_config(struct ao_eeprom *ao_eeprom, struct json_object *obj)
+{
+ struct ao_config *config = &ao_eeprom->config;
+ const char *s;
+
+ if (json_object_get_type(obj) != json_type_object)
+ return 0;
+
+ ao_eeprom->log_format = ao_json_get_int(obj, "log_format", 0);
+ ao_eeprom->serial_number = ao_json_get_int(obj, "serial", 0);
+
+ config->major = ao_json_get_int(obj, "config_major", 0);
+ config->minor = ao_json_get_int(obj, "config_minor", 0);
+ if (config->major == 0 || config->minor == 0)
+ return 0;
+
+ config->main_deploy = ao_json_get_int(obj, "main_deploy", 250);
+ config->accel_plus_g = ao_json_get_int(obj, "accel_cal_plus", 0);
+
+ s = ao_json_get_string(obj, "callsign", "N0CALL");
+ strncpy(config->callsign, s, sizeof(config->callsign));
+
+ config->apogee_delay = ao_json_get_int(obj, "apogee_delay", 0);
+ config->accel_minus_g = ao_json_get_int(obj, "accel_cal_minus", 0);
+ config->radio_cal = ao_json_get_int(obj, "radio_calibration", 0);
+ config->flight_log_max = ao_json_get_int(obj, "flight_log_max", 0);
+ config->ignite_mode = ao_json_get_int(obj, "ignite_mode", 0);
+ config->pad_orientation = ao_json_get_int(obj, "pad_orientation", 0);
+ config->radio_setting = ao_json_get_int(obj, "radio_setting", 0);
+ config->radio_enable = ao_json_get_int(obj, "radio_enable", 1);
+ config->frequency = ao_json_get_int(obj, "frequency", 434550);
+ config->apogee_lockout = ao_json_get_int(obj, "apogee_lockout", 0);
+#if AO_PYRO_NUM
+ if (!ao_eeprom_get_pyro(config, obj))
+ return 0;
+#endif
+ config->ignite_mode = ao_json_get_int(obj, "ignite_mode", 0);
+ config->ignite_mode = ao_json_get_int(obj, "ignite_mode", 0);
+ config->ignite_mode = ao_json_get_int(obj, "ignite_mode", 0);
+ config->ignite_mode = ao_json_get_int(obj, "ignite_mode", 0);
+ config->ignite_mode = ao_json_get_int(obj, "ignite_mode", 0);
+ config->ignite_mode = ao_json_get_int(obj, "ignite_mode", 0);
+
+ if (!ao_eeprom_get_ms5607(&ao_eeprom->ms5607_prom, obj))
+ return 0;
+
+ return 1;
+}
+
+struct ao_eeprom *
+ao_eeprom_read(FILE *file)
+{
+ struct ao_eeprom *ao_eeprom;
+ struct json_object *obj;
+ int ret;
+
+ ao_eeprom = calloc(1, sizeof (struct ao_eeprom));
+ if (!ao_eeprom)
+ goto fail_ao_eeprom;
+
+ obj = ao_eeprom_read_config(file);
+ if (!obj)
+ goto fail_config;
+
+ ret = ao_eeprom_get_config(ao_eeprom, obj);
+ json_object_put(obj);
+ if (!ret)
+ goto fail_config;
+
+ if (!ao_eeprom_read_data(file, ao_eeprom))
+ goto fail_data;
+
+ return ao_eeprom;
+fail_data:
+fail_config:
+ free(ao_eeprom);
+fail_ao_eeprom:
+ return NULL;
+}
--- /dev/null
+/*
+ * Copyright © 2017 Keith Packard <keithp@keithp.com>
+ *
+ * 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.
+ *
+ * This program is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+ * General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#include "ao_eeprom_read.h"
+#include <string.h>
+#include <stdlib.h>
+#include <errno.h>
+
+static int
+ao_eeprom_add_u8(struct ao_eeprom *ao_eeprom, uint8_t byte)
+{
+ if (ao_eeprom->len == ao_eeprom->size) {
+ uint32_t nsize = ao_eeprom->size * 2;
+ uint8_t *ndata = realloc(ao_eeprom->data, nsize);
+ if (!ndata)
+ return 0;
+ ao_eeprom->data = ndata;
+ ao_eeprom->size = nsize;
+ }
+ ao_eeprom->data[ao_eeprom->len++] = byte;
+ return 1;
+}
+
+static int
+ao_eeprom_add_u16(struct ao_eeprom *ao_eeprom, uint16_t u16)
+{
+ if (!ao_eeprom_add_u8(ao_eeprom, u16 & 0xff))
+ return 0;
+
+ return ao_eeprom_add_u8(ao_eeprom, u16 >> 8);
+}
+
+struct ao_eeprom *
+ao_eeprom_read_old(FILE *file)
+{
+ struct ao_eeprom *ao_eeprom;
+ char line[1024];
+ char *l;
+
+ ao_eeprom = calloc(1, sizeof (struct ao_eeprom));
+ if (!ao_eeprom)
+ return NULL;
+
+ ao_eeprom->log_format = -1;
+ ao_eeprom->size = 64;
+ ao_eeprom->data = malloc(ao_eeprom->size);
+ if (!ao_eeprom->data) {
+ free(ao_eeprom);
+ return NULL;
+ }
+ while (fgets(line, sizeof(line), file) != NULL) {
+ int nword;
+ char *words[64];
+ char *saveptr;
+ l = line;
+ for (nword = 0; nword < 64; nword++) {
+ words[nword] = strtok_r(l, " \t\n", &saveptr);
+ l = NULL;
+ if (words[nword] == NULL)
+ break;
+ }
+ if (((ao_eeprom->log_format == AO_LOG_FORMAT_TELEMEGA_OLD || ao_eeprom->log_format == AO_LOG_FORMAT_TELEMEGA) && nword == 30 && strlen(words[0]) == 1) ||
+ ((ao_eeprom->log_format == AO_LOG_FORMAT_EASYMINI1 || ao_eeprom->log_format == AO_LOG_FORMAT_EASYMINI2) && nword == 14 && strlen(words[0]) == 1) ||
+ (ao_eeprom->log_format == AO_LOG_FORMAT_TELEMETRUM && nword == 14 && strlen(words[0]) == 1))
+ {
+ int i;
+ uint8_t type;
+ uint16_t tick;
+
+ type = words[0][0];
+ tick = strtoul(words[1], NULL, 16);
+ ao_eeprom_add_u8(ao_eeprom, type);
+ ao_eeprom_add_u8(ao_eeprom, 0); /* checksum */
+ ao_eeprom_add_u16(ao_eeprom, tick);
+ for (i = 2; i < nword; i++)
+ ao_eeprom_add_u8(ao_eeprom, strtoul(words[i], NULL, 16));
+ }
+ else if (nword == 4 && strlen(words[0]) == 1) {
+ uint8_t type;
+ uint16_t tick, a, b;
+ 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';
+ ao_eeprom_add_u8(ao_eeprom, type);
+ ao_eeprom_add_u8(ao_eeprom, 0); /* checksum */
+ ao_eeprom_add_u16(ao_eeprom, tick);
+ ao_eeprom_add_u16(ao_eeprom, a);
+ ao_eeprom_add_u16(ao_eeprom, b);
+ }
+ else if (nword == 3 && strcmp(words[0], "ms5607") == 0) {
+ if (strcmp(words[1], "reserved:") == 0)
+ ao_eeprom->ms5607_prom.reserved = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "sens:") == 0)
+ ao_eeprom->ms5607_prom.sens = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "off:") == 0)
+ ao_eeprom->ms5607_prom.off = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "tcs:") == 0)
+ ao_eeprom->ms5607_prom.tcs = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "tco:") == 0)
+ ao_eeprom->ms5607_prom.tco = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "tref:") == 0)
+ ao_eeprom->ms5607_prom.tref = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "tempsens:") == 0)
+ ao_eeprom->ms5607_prom.tempsens = strtoul(words[2], NULL, 10);
+ else if (strcmp(words[1], "crc:") == 0)
+ ao_eeprom->ms5607_prom.crc = strtoul(words[2], NULL, 10);
+ continue;
+ }
+#if AO_NUM_PYRO
+ else if (nword >= 3 && strcmp(words[0], "Pyro") == 0) {
+ int p = strtoul(words[1], NULL, 10);
+ int i, j;
+ struct ao_pyro *pyro = &ao_eeprom->config.pyro[p];
+
+ for (i = 2; i < nword; i++) {
+ for (j = 0; j < NUM_PYRO_VALUES; j++)
+ if (!strcmp (words[i], 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);
+ printf("pyro %d condition %s value %d\n", p, words[i-1], val);
+ *((int16_t *) ((char *) pyro + ao_pyro_values[j].offset)) = val;
+ }
+ }
+ }
+#endif
+ else if (nword == 2 && strcmp(words[0], "log-format") == 0) {
+ ao_eeprom->log_format = strtoul(words[1], NULL, 10);
+ } else if (nword == 2 && strcmp(words[0], "serial-number") == 0) {
+ ao_eeprom->serial_number = strtoul(words[1], NULL, 10);
+ } else if (nword >= 6 && strcmp(words[0], "Accel") == 0) {
+ ao_eeprom->config.accel_plus_g = atoi(words[3]);
+ ao_eeprom->config.accel_minus_g = atoi(words[5]);
+#if HAS_GYRO
+ } else if (nword >= 8 && strcmp(words[0], "IMU") == 0) {
+ ao_eeprom->config.accel_zero_along = atoi(words[3]);
+ ao_eeprom->config.accel_zero_across = atoi(words[5]);
+ ao_eeprom->config.accel_zero_through = atoi(words[7]);
+#endif
+ } else if (nword >= 4 && strcmp(words[0], "Main") == 0) {
+ ao_eeprom->config.main_deploy = atoi(words[2]);
+ } else if (nword >= 3 && strcmp(words[0], "Apogee") == 0 &&
+ strcmp(words[1], "lockout:") == 0) {
+ ao_eeprom->config.apogee_lockout = atoi(words[2]);
+ } else if (nword >= 3 && strcmp(words[0], "Pad") == 0 &&
+ strcmp(words[1], "orientation:") == 0) {
+ ao_eeprom->config.pad_orientation = atoi(words[2]);
+ }
+ }
+ return ao_eeprom;
+}
+
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,
int ao_flight_debug;
+struct ao_eeprom *eeprom;
+uint32_t eeprom_offset;
+
FILE *emulator_in;
char *emulator_app;
char *emulator_name;
#include <ao_config.h>
#include <ao_fake_flight.h>
+#include <ao_eeprom_read.h>
+#include <ao_log.h>
#define ao_config_get()
#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 struct ao_mpu6000_sample ao_ground_mpu6000;
#endif
+#if HAS_ACCEL
+int ao_error_h_sq_avg;
+#endif
+
void
ao_test_exit(void)
{
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_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();
return;
}
- if (!fgets(line, sizeof (line), emulator_in)) {
- 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 (eeprom) {
#if TELEMEGA
- if ((log_format == AO_LOG_FORMAT_TELEMEGA_OLD || 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_number = uint16(bytes, 0);
- 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);
- ao_ground_accel_along = int16(bytes, 8);
- ao_ground_accel_across = int16(bytes, 10);
- ao_ground_accel_through = int16(bytes, 12);
- ao_ground_roll = int16(bytes, 14);
- ao_ground_pitch = int16(bytes, 16);
- ao_ground_yaw = int16(bytes, 18);
- 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 '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);
- ao_data_static.hmc5883.x = int16(bytes, 20);
- ao_data_static.hmc5883.y = int16(bytes, 22);
- ao_data_static.hmc5883.z = int16(bytes, 24);
-#if HAS_MMA655X
- ao_data_static.mma655x = int16(bytes, 26);
- if (ao_config.pad_orientation != AO_PAD_ORIENTATION_ANTENNA_UP)
- ao_data_static.mma655x = ao_data_accel_invert(ao_data_static.mma655x);
+ struct ao_log_mega *log_mega;
#endif
- ao_records_read++;
- ao_insert();
- return;
- case 'G':
- ao_gps_prev = ao_gps_static;
- ao_gps_static.tick = tick;
- ao_gps_static.latitude = int32(bytes, 0);
- ao_gps_static.longitude = int32(bytes, 4);
- {
- int32_t altitude = int32(bytes, 8);
- AO_TELEMETRY_LOCATION_SET_ALTITUDE(&ao_gps_static, altitude);
- }
- ao_gps_static.flags = bytes[13];
- if (!ao_gps_count)
- ao_gps_first = ao_gps_static;
- ao_gps_count++;
- break;
- }
- continue;
- } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) {
- if (strcmp(words[1], "reserved:") == 0)
- ao_ms5607_prom.reserved = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "sens:") == 0)
- ao_ms5607_prom.sens = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "off:") == 0)
- ao_ms5607_prom.off = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tcs:") == 0)
- ao_ms5607_prom.tcs = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tco:") == 0)
- ao_ms5607_prom.tco = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tref:") == 0)
- ao_ms5607_prom.tref = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tempsens:") == 0)
- ao_ms5607_prom.tempsens = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "crc:") == 0)
- ao_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[i], 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);
- printf("pyro %d condition %s value %d\n", p, words[i-1], val);
- *((int16_t *) ((char *) pyro + ao_pyro_values[j].offset)) = val;
- }
- }
- }
+#if TELEMETRUM_V2
+ struct ao_log_metrum *log_metrum;
#endif
#if EASYMINI
- if ((log_format == AO_LOG_FORMAT_EASYMINI1 || log_format == AO_LOG_FORMAT_EASYMINI2) && nword == 14 && 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_started = 1;
- ao_flight_number = uint16(bytes, 0);
- ao_ground_pres = uint32(bytes, 4);
- ao_ground_height = ao_pa_to_altitude(ao_ground_pres);
-#if 0
- printf("ground pres %d height %d\n", ao_ground_pres, ao_ground_height);
- printf("sens %d off %d tcs %d tco %d tref %d tempsens %d crc %d\n",
- ao_ms5607_prom.sens,
- ao_ms5607_prom.off,
- ao_ms5607_prom.tcs,
- ao_ms5607_prom.tco,
- ao_ms5607_prom.tref,
- ao_ms5607_prom.tempsens,
- ao_ms5607_prom.crc);
+ struct ao_log_mini *log_mini;
#endif
- break;
- case 'A':
- ao_data_static.tick = tick;
- ao_data_static.ms5607_raw.pres = int24(bytes, 0);
- ao_data_static.ms5607_raw.temp = int24(bytes, 3);
-#if 0
- printf("raw pres %d temp %d\n",
- ao_data_static.ms5607_raw.pres,
- ao_data_static.ms5607_raw.temp);
+#if TELEMETRUM_V1
+ struct ao_log_record *log_record;
#endif
- ao_ms5607_convert(&ao_data_static.ms5607_raw, &value);
-// printf("pres %d height %d\n", value.pres, ao_pa_to_altitude(value.pres));
- ao_records_read++;
+
+ 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;
}
- continue;
- } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) {
- if (strcmp(words[1], "reserved:") == 0)
- ao_ms5607_prom.reserved = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "sens:") == 0)
- ao_ms5607_prom.sens = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "off:") == 0)
- ao_ms5607_prom.off = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tcs:") == 0)
- ao_ms5607_prom.tcs = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tco:") == 0)
- ao_ms5607_prom.tco = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tref:") == 0)
- ao_ms5607_prom.tref = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tempsens:") == 0)
- ao_ms5607_prom.tempsens = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "crc:") == 0)
- ao_ms5607_prom.crc = strtoul(words[2], NULL, 10);
- continue;
- }
+ 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:
+ 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;
#endif
#if TELEMETRUM_V2
- if (log_format == AO_LOG_FORMAT_TELEMETRUM && nword == 14 && 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_number = uint16(bytes, 0);
- 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_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;
- 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.mma655x = int16(bytes, 8);
- ao_records_read++;
- ao_insert();
- return;
- }
- continue;
- } else if (nword == 3 && strcmp(words[0], "ms5607") == 0) {
- if (strcmp(words[1], "reserved:") == 0)
- ao_ms5607_prom.reserved = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "sens:") == 0)
- ao_ms5607_prom.sens = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "off:") == 0)
- ao_ms5607_prom.off = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tcs:") == 0)
- ao_ms5607_prom.tcs = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tco:") == 0)
- ao_ms5607_prom.tco = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tref:") == 0)
- ao_ms5607_prom.tref = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "tempsens:") == 0)
- ao_ms5607_prom.tempsens = strtoul(words[2], NULL, 10);
- else if (strcmp(words[1], "crc:") == 0)
- ao_ms5607_prom.crc = strtoul(words[2], NULL, 10);
- continue;
- }
-#endif
-#if TELEMETRUM_V1
- 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';
- }
#endif
- else if (nword == 2 && strcmp(words[0], "log-format") == 0) {
- log_format = strtoul(words[1], NULL, 10);
- } else if (nword == 2 && strcmp(words[0], "serial-number") == 0) {
- ao_serial_number = 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]);
-#ifdef TELEMEGA
- } else if (nword >= 8 && strcmp(words[0], "IMU") == 0) {
- ao_config.accel_zero_along = atoi(words[3]);
- ao_config.accel_zero_across = atoi(words[5]);
- ao_config.accel_zero_through = atoi(words[7]);
+#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
- } 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 >= 3 && strcmp(words[0], "Pad") == 0 &&
- strcmp(words[1], "orientation:") == 0) {
- ao_config.pad_orientation = 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 || TELEMETRUM_V2 || EASYMINI
- (void) a;
- (void) b;
-#else
- switch (type) {
- case 'F':
- ao_flight_ground_accel = a;
- ao_flight_number = b;
- 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;
- 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_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();
}