Fix the usage of current rotation rate.

This makes the flight actually almost stable.
balrog-kun · Jun 29, 2011 · 2f7c718 · 2f7c718
1 parent 2d91aa7
commit 2f7c718
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 6 deletions.
diff --git a/ahrs.c b/ahrs.c
@@ -75,10 +75,10 @@ static void gyro_ahrs_update(void) {
 #define DIFF_RES 4
 #define REF_RES (DIFF_RES + 6)
 
-	ahrs_roll_rate = ((int16_t) x << 6) -
-		((x_ref + (1 << (REF_RES - 7))) >> (REF_RES - 6));
-	ahrs_pitch_rate = ((int16_t) y << 6) -
-		((y_ref + (1 << (REF_RES - 7))) >> (REF_RES - 6));
+	ahrs_roll_rate = ((x_ref + (1 << (REF_RES - 7))) >> (REF_RES - 6)) -
+		((int16_t) x << 6);
+	ahrs_pitch_rate = ((y_ref + (1 << (REF_RES - 7))) >> (REF_RES - 6)) -
+		((int16_t) y << 6);
 	sei();
 
 	diff = (diff/* + (1 << (DIFF_RES - 1))*/) >> DIFF_RES;

diff --git a/pilot.c b/pilot.c
@@ -243,8 +243,8 @@ void control_update(void) {
 	int32_t a, b, c, d;
 
 	cli();
-	cur_pitch = (ahrs_pitch >> 16) + (ahrs_pitch_rate >> 3);
-	cur_roll = (ahrs_roll >> 16) + (ahrs_roll_rate >> 3);
+	cur_pitch = (ahrs_pitch >> 16) + (ahrs_pitch_rate >> 2);
+	cur_roll = (ahrs_roll >> 16) + (ahrs_roll_rate >> 2);
 	cur_yaw = ahrs_yaw + (ahrs_yaw_rate << 1);
 	sei();