altos: Average MPU6000 values on ground for later use

Having long-term ground averages recorded to the eeprom file will make
post-flight analysis of the data better.

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

diff --git a/src/core/ao_data.h b/src/core/ao_data.h
index 6fdd19cb9b7ca88cc72d1b6fd52073cd3ac032a2..7e2f85d8fb569894c09a75672f57f046df048007 100644 (file)
--- a/src/core/ao_data.h
+++ b/src/core/ao_data.h
@@ -288,4 +288,25 @@ typedef int16_t accel_t;
 
 #endif
 
+#if !HAS_GYRO && HAS_MPU6000
+
+#define HAS_GYRO       1
+
+typedef int16_t        gyro_t;
+typedef int32_t angle_t;
+
+/* Y axis is aligned with the direction of motion (along) */
+/* X axis is aligned in the other board axis (across) */
+/* Z axis is aligned perpendicular to the board (through) */
+
+#define ao_data_along(packet)  ((packet)->mpu6000.accel_y)
+#define ao_data_across(packet) ((packet)->mpu6000.accel_x)
+#define ao_data_through(packet)        ((packet)->mpu6000.accel_z)
+
+#define ao_data_roll(packet)   ((packet)->mpu6000.gyro_y)
+#define ao_data_pitch(packet)  ((packet)->mpu6000.gyro_x)
+#define ao_data_yaw(packet)    ((packet)->mpu6000.gyro_z)
+
+#endif
+
 #endif /* _AO_DATA_H_ */

diff --git a/src/core/ao_log.h b/src/core/ao_log.h
index 4eaed4207a16a56a94260bf6519162db8b3841c1..93b0177809f928dec32fea2c09b58435213057b5 100644 (file)
--- a/src/core/ao_log.h
+++ b/src/core/ao_log.h
@@ -199,10 +199,16 @@ struct ao_log_mega {
        union {                                         /* 4 */
                /* AO_LOG_FLIGHT */
                struct {
-                       uint16_t        flight;         /* 4 */
-                       int16_t         ground_accel;   /* 6 */
-                       uint32_t        ground_pres;    /* 8 */
-               } flight;                               /* 12 */
+                       uint16_t        flight;                 /* 4 */
+                       int16_t         ground_accel;           /* 6 */
+                       uint32_t        ground_pres;            /* 8 */
+                       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 */
+               } flight;                                       /* 24 */
                /* AO_LOG_STATE */
                struct {
                        uint16_t        state;

diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index 985c094046de9ad9923f0aa7803c6a9f899e3c46..7a1eff8e7e5f361680b7af8897d2db71e9187bde 100644 (file)
--- a/src/core/ao_sample.c
+++ b/src/core/ao_sample.c
@@ -37,6 +37,16 @@ __pdata alt_t                ao_sample_height;
 #if HAS_ACCEL
 __pdata accel_t                ao_sample_accel;
 #endif
+#if HAS_GYRO
+__pdata accel_t                ao_sample_accel_along;
+__pdata accel_t                ao_sample_accel_across;
+__pdata accel_t                ao_sample_accel_through;
+__pdata gyro_t         ao_sample_roll;
+__pdata gyro_t         ao_sample_pitch;
+__pdata gyro_t         ao_sample_yaw;
+__pdata angle_t                ao_sample_angle;
+__pdata angle_t                ao_sample_roll_angle;
+#endif
 
 __data uint8_t         ao_sample_data;
 
@@ -53,6 +63,15 @@ __pdata accel_t              ao_accel_2g;            /* factory accel calibration */
 __pdata int32_t                ao_accel_scale;         /* sensor to m/s² conversion */
 #endif
 
+#if HAS_GYRO
+__pdata accel_t                ao_ground_accel_along;
+__pdata accel_t                ao_ground_accel_across;
+__pdata accel_t                ao_ground_accel_through;
+__pdata gyro_t         ao_ground_pitch;
+__pdata gyro_t         ao_ground_yaw;
+__pdata gyro_t         ao_ground_roll;
+#endif
+
 static __pdata uint8_t ao_preflight;           /* in preflight mode */
 
 static __pdata uint16_t        nsamples;
@@ -60,6 +79,14 @@ __pdata int32_t ao_sample_pres_sum;
 #if HAS_ACCEL
 __pdata int32_t ao_sample_accel_sum;
 #endif
+#if HAS_GYRO
+__pdata int32_t ao_sample_accel_along_sum;
+__pdata int32_t ao_sample_accel_across_sum;
+__pdata int32_t        ao_sample_accel_through_sum;
+__pdata int32_t ao_sample_pitch_sum;
+__pdata int32_t ao_sample_yaw_sum;
+__pdata int32_t        ao_sample_roll_sum;
+#endif
 
 static void
 ao_sample_preflight_add(void)
@@ -68,6 +95,14 @@ ao_sample_preflight_add(void)
        ao_sample_accel_sum += ao_sample_accel;
 #endif
        ao_sample_pres_sum += ao_sample_pres;
+#if HAS_GYRO
+       ao_sample_accel_along_sum += ao_sample_accel_along;
+       ao_sample_accel_across_sum += ao_sample_accel_across;
+       ao_sample_accel_through_sum += ao_sample_accel_through;
+       ao_sample_pitch_sum += ao_sample_pitch;
+       ao_sample_yaw_sum += ao_sample_yaw;
+       ao_sample_roll_sum += ao_sample_roll;
+#endif
        ++nsamples;
 }
 
@@ -80,8 +115,23 @@ ao_sample_preflight_set(void)
 #endif
        ao_ground_pres = ao_sample_pres_sum >> 9;
        ao_ground_height = pres_to_altitude(ao_ground_pres);
-       nsamples = 0;
        ao_sample_pres_sum = 0;
+#if HAS_GYRO
+       ao_ground_accel_along = ao_sample_accel_along_sum >> 9;
+       ao_ground_accel_across = ao_sample_accel_across_sum >> 9;
+       ao_ground_accel_through = ao_sample_accel_through_sum >> 9;
+       ao_ground_pitch = ao_sample_pitch_sum >> 9;
+       ao_ground_yaw = ao_sample_yaw_sum >> 9;
+       ao_ground_roll = ao_sample_roll_sum >> 9;
+       ao_sample_accel_along_sum = 0;
+       ao_sample_accel_across_sum = 0;
+       ao_sample_accel_through_sum = 0;
+       ao_sample_pitch_sum = 0;
+       ao_sample_yaw_sum = 0;
+       ao_sample_roll_sum = 0;
+       ao_sample_angle = 0;
+#endif 
+       nsamples = 0;
 }
 
 static void
@@ -122,6 +172,24 @@ ao_sample_preflight_update(void)
                ao_sample_preflight_set();
 }
 
+#if HAS_GYRO
+
+static int32_t p_filt;
+static int32_t y_filt;
+
+static gyro_t inline ao_gyro(void) {
+       gyro_t  p = ao_sample_pitch - ao_ground_pitch;
+       gyro_t  y = ao_sample_yaw - ao_ground_yaw;
+
+       p_filt = p_filt - (p_filt >> 6) + p;
+       y_filt = y_filt - (y_filt >> 6) + y;
+
+       p = p_filt >> 6;
+       y = y_filt >> 6;
+       return ao_sqrt(p*p + y*y);
+}
+#endif
+
 uint8_t
 ao_sample(void)
 {
@@ -147,6 +215,12 @@ ao_sample(void)
                        ao_sample_accel = ao_data_accel_invert(ao_sample_accel);
                ao_data_set_accel(ao_data, ao_sample_accel);
 #endif
+#if HAS_GYRO
+               ao_sample_accel_along = ao_data_along(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);
+#endif
 
                if (ao_preflight)
                        ao_sample_preflight();
@@ -154,6 +228,10 @@ ao_sample(void)
                        if (ao_flight_state < ao_flight_boost)
                                ao_sample_preflight_update();
                        ao_kalman();
+#if HAS_GYRO
+                       ao_sample_angle += ao_gyro();
+                       ao_sample_roll_angle += (ao_sample_roll - ao_ground_roll);
+#endif
                }
                ao_sample_data = ao_data_ring_next(ao_sample_data);
        }
@@ -170,6 +248,21 @@ ao_sample_init(void)
 #if HAS_ACCEL
        ao_sample_accel_sum = 0;
        ao_sample_accel = 0;
+#endif
+#if HAS_GYRO
+       ao_sample_accel_along_sum = 0;
+       ao_sample_accel_across_sum = 0;
+       ao_sample_accel_through_sum = 0;
+       ao_sample_accel_along = 0;
+       ao_sample_accel_across = 0;
+       ao_sample_accel_through = 0;
+       ao_sample_pitch_sum = 0;
+       ao_sample_yaw_sum = 0;
+       ao_sample_roll_sum = 0;
+       ao_sample_pitch = 0;
+       ao_sample_yaw = 0;
+       ao_sample_roll = 0;
+       ao_sample_angle = 0;
 #endif
        ao_sample_data = ao_data_head;
        ao_preflight = TRUE;

diff --git a/src/core/ao_sqrt.c b/src/core/ao_sqrt.c
index 09c2e319be51aea52261c336cffde83789ced07a..3a550eaa74e48a4fab3abbe40f1a02cbc18d33f2 100644 (file)
--- a/src/core/ao_sqrt.c
+++ b/src/core/ao_sqrt.c
@@ -15,7 +15,9 @@
  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
  */
 
+#ifndef AO_FLIGHT_TEST
 #include "ao.h"
+#endif
 
 /* Adapted from int_sqrt.c in the linux kernel, which is licensed GPLv2 */
 /**

diff --git a/src/test/ao_flight_test.c b/src/test/ao_flight_test.c
index acdf4d926a3ead68460fd6b284d2e656a30ebf8f..cdd1f2369898726ce1739ded988043d6c4924386 100644 (file)
--- a/src/test/ao_flight_test.c
+++ b/src/test/ao_flight_test.c
@@ -236,10 +236,14 @@ extern int32_t    ao_accel_scale;
 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"
 
@@ -309,7 +313,7 @@ ao_mpu6000_accel(int16_t sensor)
 }
 
 static double
-ao_mpu6000_gyro(int16_t sensor)
+ao_mpu6000_gyro(int32_t sensor)
 {
        return sensor / 32767.0 * MPU6000_GYRO_FULLSCALE;
 }
@@ -370,6 +374,7 @@ ao_insert(void)
                if (!ao_summary) {
                        printf("%7.2f height %8.2f accel %8.3f "
 #if MEGAMETRUM
+                              "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 "
 #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",
@@ -377,6 +382,9 @@ ao_insert(void)
                               height,
                               accel,
 #if MEGAMETRUM
+                              ao_mpu6000_gyro(ao_sample_roll_angle) / 100.0,
+                              ao_mpu6000_gyro(ao_sample_angle) / 100.0,
+                              ao_sample_qangle,
                               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),
@@ -715,12 +723,14 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick)
        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;
@@ -731,6 +741,8 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick)
 
        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,
@@ -740,6 +752,7 @@ update_orientation (double rate_x, double rate_y, double rate_z, int tick)
                ao_current.q1,
                ao_current.q2,
                ao_current.q3);
+#endif
 }
 #endif
 
@@ -845,7 +858,7 @@ ao_sleep(void *wchan)
                                                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);
+                                               update_orientation(rate_x * M_PI / 180, rate_z * M_PI / 180, rate_y * M_PI / 180, tick);
                                        }
                                        ao_records_read++;
                                        ao_insert();