From fe69c10bf36b54413f7c7de4abd8e2ac9c261814 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Thu, 28 Apr 2011 22:31:54 +0200 Subject: Test pitch/roll stabilization. --- main.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) 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 { 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 { 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()) { -- cgit v1.2.3