Test pitch/roll stabilization.

1 files changed, 17 insertions, 1 deletions

diff --git a/main.cpp b/main.cpp
index 7589f35..e3d9441 100644
--- a/main.cpp
+++ b/main.cpp
@@ -93,6 +93,10 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 			magn.init();

 			

 			systime_t time = chTimeNow();

+			

+			int32_t pitch_angle_accum = 0;

+			int32_t roll_angle_accum = 0;

+			

 			while (1) {

 				gyro.update();

 				acc.update();

@@ -131,7 +135,19 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 					int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),

 					int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/

 				

-				motors.update(ppmsum.data[2], 0, 0, 0);

+				int16_t pitch_angle_target = (ppmsum.data[1] - 500) * 8;

+				int16_t roll_angle_target = (ppmsum.data[0] - 500) * 8;

+				

+				//pitch_angle_accum += pitch_angle_target - pitch;

+				//roll_angle_accum += roll_angle_target - roll;

+				

+				int16_t pitch_rate_target = ((pitch_angle_target - pitch) * 4096 + pitch_angle_accum * 256) >> 16;

+				int16_t roll_rate_target = ((roll_angle_target - roll) * 4096 + roll_angle_accum * 256) >> 16;

+				

+				int16_t pitch_rate_comp = ((pitch_rate_target - gyro.x) * 2048) >> 16;

+				int16_t roll_rate_comp = ((roll_rate_target - gyro.y) * 2048) >> 16;

+				

+				motors.update(ppmsum.data[2], pitch_rate_comp, roll_rate_comp, 0);

 				

 				time += MS2ST(10);

 				if(time > chTimeNow()) {