diff options
| -rw-r--r-- | main.cpp | 18 |
1 files changed, 17 insertions, 1 deletions
@@ -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()) {
|