summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--ao-tools/Makefile.am2
-rw-r--r--ao-tools/lib/Makefile.am1
-rw-r--r--ao-tools/lib/cc.h116
-rw-r--r--configure.ac1
-rw-r--r--src/drivers/ao_gps_skytraq.c6
-rw-r--r--src/test/Makefile2
-rw-r--r--src/test/ao_flight_test.c213
7 files changed, 339 insertions, 2 deletions
diff --git a/ao-tools/Makefile.am b/ao-tools/Makefile.am
index 871b8205..4cf1f382 100644
--- a/ao-tools/Makefile.am
+++ b/ao-tools/Makefile.am
@@ -1 +1 @@
-SUBDIRS=lib ao-rawload ao-dbg ao-bitbang ao-eeprom ao-list ao-load ao-telem ao-stmload ao-send-telem ao-sky-flash
+SUBDIRS=lib ao-rawload ao-dbg ao-bitbang ao-eeprom ao-list ao-load ao-telem ao-stmload ao-send-telem ao-sky-flash ao-mega
diff --git a/ao-tools/lib/Makefile.am b/ao-tools/lib/Makefile.am
index 1f8f2e42..a664d083 100644
--- a/ao-tools/lib/Makefile.am
+++ b/ao-tools/lib/Makefile.am
@@ -34,6 +34,7 @@ libao_tools_a_SOURCES = \
cc-telem.c \
cc-telemetry.c \
cc-telemetry.h \
+ cc-mega.c \
cp-usb-async.c \
cp-usb-async.h \
i0.c \
diff --git a/ao-tools/lib/cc.h b/ao-tools/lib/cc.h
index 6257ee44..625540bb 100644
--- a/ao-tools/lib/cc.h
+++ b/ao-tools/lib/cc.h
@@ -269,6 +269,122 @@ struct cc_telem {
int
cc_telem_parse(const char *input_line, struct cc_telem *telem);
+struct ao_log_mega {
+ char type; /* 0 */
+ uint8_t is_config; /* 1 */
+ uint16_t tick; /* 2 */
+ union { /* 4 */
+ /* AO_LOG_FLIGHT */
+ struct {
+ uint16_t flight; /* 4 */
+ int16_t ground_accel; /* 6 */
+ uint32_t ground_pres; /* 8 */
+ } flight; /* 12 */
+ /* AO_LOG_STATE */
+ struct {
+ uint16_t state;
+ uint16_t reason;
+ } state;
+ /* AO_LOG_SENSOR */
+ struct {
+ uint32_t pres; /* 4 */
+ uint32_t temp; /* 8 */
+ int16_t accel_x; /* 12 */
+ int16_t accel_y; /* 14 */
+ int16_t accel_z; /* 16 */
+ int16_t gyro_x; /* 18 */
+ int16_t gyro_y; /* 20 */
+ int16_t gyro_z; /* 22 */
+ int16_t mag_x; /* 24 */
+ int16_t mag_y; /* 26 */
+ int16_t mag_z; /* 28 */
+ int16_t accel; /* 30 */
+ } sensor; /* 32 */
+ /* AO_LOG_TEMP_VOLT */
+ struct {
+ int16_t v_batt; /* 4 */
+ int16_t v_pbatt; /* 6 */
+ int16_t n_sense; /* 8 */
+ int16_t sense[10]; /* 10 */
+ } volt; /* 30 */
+ /* AO_LOG_GPS_TIME */
+ struct {
+ int32_t latitude; /* 4 */
+ int32_t longitude; /* 8 */
+ int16_t altitude; /* 12 */
+ uint8_t hour; /* 14 */
+ uint8_t minute; /* 15 */
+ uint8_t second; /* 16 */
+ uint8_t flags; /* 17 */
+ uint8_t year; /* 18 */
+ uint8_t month; /* 19 */
+ uint8_t day; /* 20 */
+ uint8_t pad; /* 21 */
+ } gps; /* 22 */
+ /* AO_LOG_GPS_SAT */
+ struct {
+ uint16_t channels; /* 4 */
+ struct {
+ uint8_t svid;
+ uint8_t c_n;
+ } sats[12]; /* 6 */
+ } gps_sat; /* 30 */
+
+ struct {
+ uint32_t kind;
+ int32_t data[6];
+ } config_int;
+
+ struct {
+ uint32_t kind;
+ char string[24];
+ } config_str;
+
+ /* Raw bytes */
+ uint8_t bytes[28];
+ } u;
+};
+
+#define AO_CONFIG_CONFIG 1
+#define AO_CONFIG_MAIN 2
+#define AO_CONFIG_APOGEE 3
+#define AO_CONFIG_LOCKOUT 4
+#define AO_CONFIG_FREQUENCY 5
+#define AO_CONFIG_RADIO_ENABLE 6
+#define AO_CONFIG_ACCEL_CAL 7
+#define AO_CONFIG_RADIO_CAL 8
+#define AO_CONFIG_MAX_LOG 9
+#define AO_CONFIG_IGNITE_MODE 10
+#define AO_CONFIG_PAD_ORIENTATION 11
+#define AO_CONFIG_SERIAL_NUMBER 12
+#define AO_CONFIG_LOG_FORMAT 13
+#define AO_CONFIG_MS5607_RESERVED 14
+#define AO_CONFIG_MS5607_SENS 15
+#define AO_CONFIG_MS5607_OFF 16
+#define AO_CONFIG_MS5607_TCS 17
+#define AO_CONFIG_MS5607_TCO 18
+#define AO_CONFIG_MS5607_TREF 19
+#define AO_CONFIG_MS5607_TEMPSENS 20
+#define AO_CONFIG_MS5607_CRC 21
+
+
+#define AO_LOG_FLIGHT 'F'
+#define AO_LOG_SENSOR 'A'
+#define AO_LOG_TEMP_VOLT 'T'
+#define AO_LOG_DEPLOY 'D'
+#define AO_LOG_STATE 'S'
+#define AO_LOG_GPS_TIME 'G'
+#define AO_LOG_GPS_LAT 'N'
+#define AO_LOG_GPS_LON 'W'
+#define AO_LOG_GPS_ALT 'H'
+#define AO_LOG_GPS_SAT 'V'
+#define AO_LOG_GPS_DATE 'Y'
+
+#define AO_LOG_CONFIG 'c'
+
+int
+cc_mega_parse(const char *input_line, struct ao_log_mega *l);
+
#ifndef TRUE
#define TRUE 1
#define FALSE 0
diff --git a/configure.ac b/configure.ac
index 0fcd97e2..ad10ac3c 100644
--- a/configure.ac
+++ b/configure.ac
@@ -164,6 +164,7 @@ ao-tools/ao-telem/Makefile
ao-tools/ao-stmload/Makefile
ao-tools/ao-send-telem/Makefile
ao-tools/ao-sky-flash/Makefile
+ao-tools/ao-mega/Makefile
ao-utils/Makefile
src/Version
])
diff --git a/src/drivers/ao_gps_skytraq.c b/src/drivers/ao_gps_skytraq.c
index b7dc0a86..d637a602 100644
--- a/src/drivers/ao_gps_skytraq.c
+++ b/src/drivers/ao_gps_skytraq.c
@@ -515,7 +515,13 @@ gps_update(void) __reentrant
ao_timer_set_adc_interval(0);
#endif
ao_skytraq_sendstruct(ao_gps_115200);
+ ao_delay(AO_MS_TO_TICKS(500));
+ ao_gps_set_speed(AO_SERIAL_SPEED_4800);
+ ao_delay(AO_MS_TO_TICKS(500));
+ ao_skytraq_sendstruct(ao_gps_115200);
+ ao_delay(AO_MS_TO_TICKS(500));
ao_gps_set_speed(AO_SERIAL_SPEED_115200);
+ ao_delay(AO_MS_TO_TICKS(500));
/* It's a binary protocol; abandon attempts to escape */
for (;;)
diff --git a/src/test/Makefile b/src/test/Makefile
index 44cee904..0dcdc949 100644
--- a/src/test/Makefile
+++ b/src/test/Makefile
@@ -29,7 +29,7 @@ ao_flight_test_accel: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kal
cc $(CFLAGS) -o $@ -DFORCE_ACCEL=1 ao_flight_test.c
ao_flight_test_mm: ao_flight_test.c ao_host.h ao_flight.c ao_sample.c ao_kalman.c $(INCS)
- cc -DMEGAMETRUM=1 $(CFLAGS) -o $@ $<
+ cc -DMEGAMETRUM=1 $(CFLAGS) -o $@ $< -lm
ao_gps_test: ao_gps_test.c ao_gps_sirf.c ao_gps_print.c ao_host.h
cc $(CFLAGS) -o $@ $<
diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index 7180f02d..acdf4d92 100644
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -547,6 +547,202 @@ int32(uint8_t *bytes, int off)
static int log_format;
+#if MEGAMETRUM
+
+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)
+{
+ 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;
+ }
+}
+
+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)
+{
+ 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);
+
+}
+
+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;
+
+ 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);
+
+ 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);
+
+ 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
+
void
ao_sleep(void *wchan)
{
@@ -635,6 +831,21 @@ ao_sleep(void *wchan)
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, rate_z, rate_y, tick);
}
ao_records_read++;
ao_insert();
@@ -779,6 +990,8 @@ ao_sleep(void *wchan)
continue;
#if MEGAMETRUM
+ (void) a;
+ (void) b;
#else
switch (type) {
case 'F':
generated by cgit v1.2.3 (git 2.39.1) at 2025-10-15 21:00:13 +0000