X-Git-Url: https://git.gag.com/?p=fw%2Faltos;a=blobdiff_plain;f=src%2Fcore%2Fao_sample.c;h=47c5ea2e3660d15d0dcb4d526c1136653bb729e5;hp=6461def06abbb764a6ce5dc4e7ac6ed91dbd50d2;hb=58f08c4b3cb9049d0c9cb02cde0d8dbdc3d33920;hpb=aa305da7e5dc182c99c09e422c053d85ed48d5d5

diff --git a/src/core/ao_sample.c b/src/core/ao_sample.c
index 6461def0..47c5ea2e 100644
--- a/src/core/ao_sample.c
+++ b/src/core/ao_sample.c
@@ -20,6 +20,10 @@
 #include <ao_data.h>
 #endif
 
+#if HAS_GYRO
+#include <ao_quaternion.h>
+#endif
+
 /*
  * Current sensor values
  */
@@ -37,6 +41,15 @@ __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_orient;
+#endif
 
 __data uint8_t		ao_sample_data;
 
@@ -53,6 +66,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 +82,16 @@ __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;
+static struct ao_quaternion ao_rotation;
+static struct ao_quaternion ao_pad_orientation;
+#endif
 
 static void
 ao_sample_preflight_add(void)
@@ -68,6 +100,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,10 +120,102 @@ 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_orient = 0;
+
+	/* No rotation yet */
+	ao_quaternion_init_zero_rotation(&ao_rotation);
+
+	/* Take the pad IMU acceleration values and compute our current direction
+	 */
+	ao_quaternion_init_vector(&ao_pad_orientation,
+				  ao_ground_accel_across - ao_config.accel_zero_across,
+				  ao_ground_accel_through - ao_config.accel_zero_through,
+				  -ao_ground_accel_along - ao_config.accel_zero_along);
+
+	ao_quaternion_normalize(&ao_pad_orientation,
+				&ao_pad_orientation);
+				  
+#endif	
+	nsamples = 0;
 }
 
+#if HAS_GYRO
+static void
+ao_sample_rotate(void)
+{
+#ifdef AO_FLIGHT_TEST
+	float	dt = (ao_sample_tick - ao_sample_prev_tick) / 100.0;
+#else
+	static const float dt = 1/100.0;
+#endif
+	float	x = ao_mpu6000_gyro(ao_sample_pitch - ao_ground_pitch) * dt;
+	float	y = ao_mpu6000_gyro(ao_sample_yaw - ao_ground_yaw) * dt;
+	float	z = ao_mpu6000_gyro(ao_sample_roll - ao_ground_roll) * dt;
+
+	float			n_2, n;
+	float			s, c;
+	
+	struct ao_quaternion	rot;
+	struct ao_quaternion	point;
+
+	/* The amount of rotation is just the length of the vector. Now,
+	 * here's the trick -- assume that the rotation amount is small. In this case,
+	 * sin(x) ≃ x, so we can just make this the sin.
+	 */
+
+	n_2 = x*x + y*y + z*z;
+	n = sqrtf(n_2);
+	s = n / 2;
+	if (s > 1)
+		s = 1;
+	c = sqrtf(1 - s*s);
+
+	/* Make unit vector */
+	if (n > 0) {
+		x /= n;
+		y /= n;
+		z /= n;
+	}
+
+	/* Now compute the unified rotation quaternion */
+
+	ao_quaternion_init_rotation(&rot,
+				    x, y, z,
+				    s, c);
+
+	/* Integrate with the previous rotation amount */
+	ao_quaternion_multiply(&ao_rotation, &ao_rotation, &rot);
+
+	/* And normalize to make sure it remains a unit vector */
+	ao_quaternion_normalize(&ao_rotation, &ao_rotation);
+
+	/* Compute pitch angle from vertical by taking the pad
+	 * orientation vector and rotating it by the current total
+	 * rotation value. That will be a unit vector pointing along
+	 * the airframe axis. The Z value will be the cosine of the
+	 * change in the angle from vertical since boost
+	 */
+
+	ao_quaternion_rotate(&point, &ao_pad_orientation, &ao_rotation);
+
+	ao_sample_orient = acosf(rotz) * (float) (180.0/M_PI);
+}
+#endif
+
 static void
 ao_sample_preflight(void)
 {
@@ -122,6 +254,25 @@ ao_sample_preflight_update(void)
 		ao_sample_preflight_set();
 }
 
+#if 0
+#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
+#endif
+
 uint8_t
 ao_sample(void)
 {
@@ -134,10 +285,12 @@ ao_sample(void)
 		ao_data = (struct ao_data *) &ao_data_ring[ao_sample_data];
 		ao_sample_tick = ao_data->tick;
 
+#if HAS_BARO
 		ao_data_pres_cook(ao_data);
 		ao_sample_pres = ao_data_pres(ao_data);
 		ao_sample_alt = pres_to_altitude(ao_sample_pres);
 		ao_sample_height = ao_sample_alt - ao_ground_height;
+#endif
 
 #if HAS_ACCEL
 		ao_sample_accel = ao_data_accel_cook(ao_data);
@@ -145,6 +298,14 @@ 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_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);
+#endif
 
 		if (ao_preflight)
 			ao_sample_preflight();
@@ -152,7 +313,13 @@ ao_sample(void)
 			if (ao_flight_state < ao_flight_boost)
 				ao_sample_preflight_update();
 			ao_kalman();
+#if HAS_GYRO
+			ao_sample_rotate();
+#endif
 		}
+#ifdef AO_FLIGHT_TEST
+		ao_sample_prev_tick = ao_sample_tick;
+#endif
 		ao_sample_data = ao_data_ring_next(ao_sample_data);
 	}
 	return !ao_preflight;
@@ -168,6 +335,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_orient = 0;
 #endif
 	ao_sample_data = ao_data_head;
 	ao_preflight = TRUE;