From 2ac72823142ea294c6b1fc54e1115da4cf468884 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Fri, 30 Jan 2015 21:46:23 +0100 Subject: [PATCH] Use different scale for inflight adjustments to PIDs of FP based PID controllers. --- src/main/io/rc_controls.c | 38 ++++++++++++---------- src/test/Makefile | 1 + src/test/unit/rc_controls_unittest.cc | 47 +++++++++++---------------- 3 files changed, 40 insertions(+), 46 deletions(-) diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 203ed2b928..b73a867f15 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -32,10 +32,12 @@ #include "drivers/system.h" #include "flight/flight.h" +#include "flight/navigation.h" #include "drivers/sensor.h" #include "drivers/accgyro.h" +#include "sensors/barometer.h" #include "sensors/battery.h" #include "sensors/sensors.h" #include "sensors/gyro.h" @@ -395,10 +397,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->P_f[PIDPITCH] + delta; - pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = (int)pidProfile->P_f[PIDROLL] + delta; - pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f); + pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f); + pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDPITCH] + delta; pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -408,10 +410,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->I_f[PIDPITCH] + delta; - pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = (int)pidProfile->I_f[PIDROLL] + delta; - pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 10.0f); + pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 10.0f); + pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDPITCH] + delta; pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -421,10 +423,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_PITCH_ROLL_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->D_f[PIDPITCH] + delta; - pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c - newFloatValue = (int)pidProfile->D_f[PIDROLL] + delta; - pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 10.0f); + pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 10.0f); + pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDPITCH] + delta; pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -434,8 +436,8 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_P: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->P_f[PIDYAW] + delta; - pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f); + pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->P8[PIDYAW] + delta; pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -443,8 +445,8 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_I: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->I_f[PIDYAW] + delta; - pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 10.0f); + pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->I8[PIDYAW] + delta; pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c @@ -452,8 +454,8 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm break; case ADJUSTMENT_YAW_D: if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { - newFloatValue = (int)pidProfile->D_f[PIDYAW] + delta; - pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c + newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 10.0f); + pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c } else { newValue = (int)pidProfile->D8[PIDYAW] + delta; pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c diff --git a/src/test/Makefile b/src/test/Makefile index 6642d98b8f..9f3c5db21e 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -263,6 +263,7 @@ $(OBJECT_DIR)/rc_controls_unittest.o : \ $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@ rc_controls_unittest : \ + $(OBJECT_DIR)/common/maths.o \ $(OBJECT_DIR)/io/rc_controls.o \ $(OBJECT_DIR)/rc_controls_unittest.o \ $(OBJECT_DIR)/gtest_main.a diff --git a/src/test/unit/rc_controls_unittest.cc b/src/test/unit/rc_controls_unittest.cc index 91ec4f515f..3e611ce1ed 100644 --- a/src/test/unit/rc_controls_unittest.cc +++ b/src/test/unit/rc_controls_unittest.cc @@ -20,6 +20,7 @@ extern "C" { #include "platform.h" + #include "common/maths.h" #include "common/axis.h" #include "flight/flight.h" @@ -32,16 +33,6 @@ extern "C" { extern "C" { extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile); - -int constrain(int amt, int low, int high) -{ - if (amt < low) - return low; - else if (amt > high) - return high; - else - return amt; -} } TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde) @@ -594,15 +585,15 @@ TEST(RcControlsTest, processPIDIncreasePidController0) EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); // and - EXPECT_EQ(pidProfile.P8[PIDPITCH], 1); - EXPECT_EQ(pidProfile.P8[PIDROLL], 6); - EXPECT_EQ(pidProfile.P8[YAW], 8); - EXPECT_EQ(pidProfile.I8[PIDPITCH], 11); - EXPECT_EQ(pidProfile.I8[PIDROLL], 16); - EXPECT_EQ(pidProfile.I8[YAW], 18); - EXPECT_EQ(pidProfile.D8[PIDPITCH], 21); - EXPECT_EQ(pidProfile.D8[PIDROLL], 26); - EXPECT_EQ(pidProfile.D8[YAW], 28); + EXPECT_EQ(1, pidProfile.P8[PIDPITCH]); + EXPECT_EQ(6, pidProfile.P8[PIDROLL]); + EXPECT_EQ(8, pidProfile.P8[YAW]); + EXPECT_EQ(11, pidProfile.I8[PIDPITCH]); + EXPECT_EQ(16, pidProfile.I8[PIDROLL]); + EXPECT_EQ(18, pidProfile.I8[YAW]); + EXPECT_EQ(21, pidProfile.D8[PIDPITCH]); + EXPECT_EQ(26, pidProfile.D8[PIDROLL]); + EXPECT_EQ(28, pidProfile.D8[YAW]); } TEST(RcControlsTest, processPIDIncreasePidController2) @@ -680,15 +671,15 @@ TEST(RcControlsTest, processPIDIncreasePidController2) EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); // and - EXPECT_EQ(pidProfile.P_f[PIDPITCH], 1.0f); - EXPECT_EQ(pidProfile.P_f[PIDROLL], 6.0f); - EXPECT_EQ(pidProfile.P_f[PIDYAW], 8.0f); - EXPECT_EQ(pidProfile.I_f[PIDPITCH], 11.0f); - EXPECT_EQ(pidProfile.I_f[PIDROLL], 16.0f); - EXPECT_EQ(pidProfile.I_f[PIDYAW], 18.0f); - EXPECT_EQ(pidProfile.D_f[PIDPITCH], 21.0f); - EXPECT_EQ(pidProfile.D_f[PIDROLL], 26.0f); - EXPECT_EQ(pidProfile.D_f[PIDYAW], 28.0f); + EXPECT_EQ(0.1f, pidProfile.P_f[PIDPITCH]); + EXPECT_EQ(5.1f, pidProfile.P_f[PIDROLL]); + EXPECT_EQ(7.1f, pidProfile.P_f[PIDYAW]); + EXPECT_EQ(10.1f, pidProfile.I_f[PIDPITCH]); + EXPECT_EQ(15.1f, pidProfile.I_f[PIDROLL]); + EXPECT_EQ(17.1f, pidProfile.I_f[PIDYAW]); + EXPECT_EQ(20.1f, pidProfile.D_f[PIDPITCH]); + EXPECT_EQ(25.1f, pidProfile.D_f[PIDROLL]); + EXPECT_EQ(27.1f, pidProfile.D_f[PIDYAW]); }