altos: Actually record ground averages for 6dof sensor

This gets the long-term averages for the 6dof sensors recorded into
the first flight log record.

Signed-off-by: Keith Packard <keithp@keithp.com>

diff --git a/src/core/ao_log.h b/src/core/ao_log.h
index 93b0177809f928dec32fea2c09b58435213057b5..036d6f2de56c134ab445c85b5b709c7e019a5076 100644 (file)
--- a/src/core/ao_log.h
+++ b/src/core/ao_log.h
@@ -205,9 +205,9 @@ struct ao_log_mega {
                        int16_t         ground_accel_along;     /* 16 */
                        int16_t         ground_accel_across;    /* 12 */
                        int16_t         ground_accel_through;   /* 14 */
-                       int16_t         ground_gyro_roll;       /* 18 */
-                       int16_t         ground_gyro_pitch;      /* 20 */
-                       int16_t         ground_gyro_yaw;        /* 22 */
+                       int16_t         ground_roll;            /* 18 */
+                       int16_t         ground_pitch;           /* 20 */
+                       int16_t         ground_yaw;             /* 22 */
                } flight;                                       /* 24 */
                /* AO_LOG_STATE */
                struct {

diff --git a/src/core/ao_log_mega.c b/src/core/ao_log_mega.c
index ac1590db760f67ee82506a07ced3b87ce711480d..e03687ada912d8352527179caa9117e8645bd630 100644 (file)
--- a/src/core/ao_log_mega.c
+++ b/src/core/ao_log_mega.c
@@ -94,6 +94,14 @@ ao_log(void)
        log.tick = ao_sample_tick;
 #if HAS_ACCEL
        log.u.flight.ground_accel = ao_ground_accel;
+#endif
+#if HAS_GYRO
+       log.u.flight.ground_accel_along = ao_ground_accel_along;
+       log.u.flight.ground_accel_across = ao_ground_accel_across;
+       log.u.flight.ground_accel_through = ao_ground_accel_through;
+       log.u.flight.ground_pitch = ao_ground_pitch;
+       log.u.flight.ground_yaw = ao_ground_yaw;
+       log.u.flight.ground_roll = ao_ground_roll;
 #endif
        log.u.flight.ground_pres = ao_ground_pres;
        log.u.flight.flight = ao_flight_number;

diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index 7a1eff8e7e5f361680b7af8897d2db71e9187bde..dec44f9fa0910aa422ce8e7771c75001aba3fb94 100644 (file)
--- a/src/core/ao_sample.c
+++ b/src/core/ao_sample.c
@@ -172,8 +172,8 @@ ao_sample_preflight_update(void)
                ao_sample_preflight_set();
 }
 
+#if 0
 #if HAS_GYRO
-
 static int32_t p_filt;
 static int32_t y_filt;
 
@@ -189,6 +189,7 @@ static gyro_t inline ao_gyro(void) {
        return ao_sqrt(p*p + y*y);
 }
 #endif
+#endif
 
 uint8_t
 ao_sample(void)
@@ -217,6 +218,8 @@ ao_sample(void)
 #endif
 #if HAS_GYRO
                ao_sample_accel_along = ao_data_along(ao_data);
+               ao_sample_accel_across = ao_data_across(ao_data);
+               ao_sample_accel_through = ao_data_through(ao_data);
                ao_sample_pitch = ao_data_pitch(ao_data);
                ao_sample_yaw = ao_data_yaw(ao_data);
                ao_sample_roll = ao_data_roll(ao_data);
@@ -229,8 +232,7 @@ ao_sample(void)
                                ao_sample_preflight_update();
                        ao_kalman();
 #if HAS_GYRO
-                       ao_sample_angle += ao_gyro();
-                       ao_sample_roll_angle += (ao_sample_roll - ao_ground_roll);
+                       /* do quaternion stuff here... */
 #endif
                }
                ao_sample_data = ao_data_ring_next(ao_sample_data);

diff --git a/src/core/ao_sample.h b/src/core/ao_sample.h
index 9336bdf9c8bfda847a2e0cfbe9166ed96baccedf..a2dac9792a8bc2c13a5de441daa3fd50adb731b6 100644 (file)
--- a/src/core/ao_sample.h
+++ b/src/core/ao_sample.h
@@ -111,6 +111,14 @@ extern __pdata accel_t     ao_ground_accel;        /* startup acceleration */
 extern __pdata accel_t         ao_accel_2g;            /* factory accel calibration */
 extern __pdata int32_t ao_accel_scale;         /* sensor to m/s² conversion */
 #endif
+#if HAS_GYRO
+extern __pdata accel_t ao_ground_accel_along;
+extern __pdata accel_t ao_ground_accel_across;
+extern __pdata accel_t ao_ground_accel_through;
+extern __pdata gyro_t  ao_ground_pitch;
+extern __pdata gyro_t  ao_ground_yaw;
+extern __pdata gyro_t  ao_ground_roll;
+#endif
 
 void ao_sample_init(void);