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>

4 files changed, 24 insertions, 6 deletions

diff --git a/src/core/ao_log.h b/src/core/ao_log.h
index 93b01778..036d6f2d 100644
--- 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 ac1590db..e03687ad 100644
--- a/src/core/ao_log_mega.c
+++ b/src/core/ao_log_mega.c
@@ -95,6 +95,14 @@ ao_log(void)
 #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;
 	ao_log_mega(&log);
diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index 7a1eff8e..dec44f9f 100644
--- 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 9336bdf9..a2dac979 100644
--- 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);