Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

Use different scale for inflight adjustments to PIDs of FP based PID

Browse source 
controllers.
This commit is contained in:
Dominic Clifton 2015-01-30 21:46:23 +01:00
parent ea386e6da2
commit 2ac7282314
3 changed files with 40 additions and 46 deletions
Download patch file Download diff file Expand all files Collapse all files

38
src/main/io/rc_controls.c
View file

@ -32,10 +32,12 @@
#include "drivers/system.h" #include "drivers/system.h"
#include "flight/flight.h" #include "flight/flight.h"
#include "flight/navigation.h"
#include "drivers/sensor.h" #include "drivers/sensor.h"
#include "drivers/accgyro.h" #include "drivers/accgyro.h"
#include "sensors/barometer.h"
#include "sensors/battery.h" #include "sensors/battery.h"
#include "sensors/sensors.h" #include "sensors/sensors.h"
#include "sensors/gyro.h" #include "sensors/gyro.h"
@ -395,10 +397,10 @@ void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustm
break; break;
case ADJUSTMENT_PITCH_ROLL_P: case ADJUSTMENT_PITCH_ROLL_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = (int)pidProfile->P_f[PIDPITCH] + delta; newFloatValue = pidProfile->P_f[PIDPITCH] + (float)(delta / 10.0f);
pidProfile->P_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->P_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = (int)pidProfile->P_f[PIDROLL] + delta; newFloatValue = pidProfile->P_f[PIDROLL] + (float)(delta / 10.0f);
pidProfile->P_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->P_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else { } else {
newValue = (int)pidProfile->P8[PIDPITCH] + delta; newValue = (int)pidProfile->P8[PIDPITCH] + delta;
pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c 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; break;
case ADJUSTMENT_PITCH_ROLL_I: case ADJUSTMENT_PITCH_ROLL_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = (int)pidProfile->I_f[PIDPITCH] + delta; newFloatValue = pidProfile->I_f[PIDPITCH] + (float)(delta / 10.0f);
pidProfile->I_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->I_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = (int)pidProfile->I_f[PIDROLL] + delta; newFloatValue = pidProfile->I_f[PIDROLL] + (float)(delta / 10.0f);
pidProfile->I_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->I_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else { } else {
newValue = (int)pidProfile->I8[PIDPITCH] + delta; newValue = (int)pidProfile->I8[PIDPITCH] + delta;
pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c 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; break;
case ADJUSTMENT_PITCH_ROLL_D: case ADJUSTMENT_PITCH_ROLL_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = (int)pidProfile->D_f[PIDPITCH] + delta; newFloatValue = pidProfile->D_f[PIDPITCH] + (float)(delta / 10.0f);
pidProfile->D_f[PIDPITCH] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->D_f[PIDPITCH] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
newFloatValue = (int)pidProfile->D_f[PIDROLL] + delta; newFloatValue = pidProfile->D_f[PIDROLL] + (float)(delta / 10.0f);
pidProfile->D_f[PIDROLL] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->D_f[PIDROLL] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else { } else {
newValue = (int)pidProfile->D8[PIDPITCH] + delta; newValue = (int)pidProfile->D8[PIDPITCH] + delta;
pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c 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; break;
case ADJUSTMENT_YAW_P: case ADJUSTMENT_YAW_P:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = (int)pidProfile->P_f[PIDYAW] + delta; newFloatValue = pidProfile->P_f[PIDYAW] + (float)(delta / 10.0f);
pidProfile->P_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->P_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else { } else {
newValue = (int)pidProfile->P8[PIDYAW] + delta; newValue = (int)pidProfile->P8[PIDYAW] + delta;
pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c 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; break;
case ADJUSTMENT_YAW_I: case ADJUSTMENT_YAW_I:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = (int)pidProfile->I_f[PIDYAW] + delta; newFloatValue = pidProfile->I_f[PIDYAW] + (float)(delta / 10.0f);
pidProfile->I_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->I_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else { } else {
newValue = (int)pidProfile->I8[PIDYAW] + delta; newValue = (int)pidProfile->I8[PIDYAW] + delta;
pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c 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; break;
case ADJUSTMENT_YAW_D: case ADJUSTMENT_YAW_D:
if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) { if (IS_PID_CONTROLLER_FP_BASED(pidProfile->pidController)) {
newFloatValue = (int)pidProfile->D_f[PIDYAW] + delta; newFloatValue = pidProfile->D_f[PIDYAW] + (float)(delta / 10.0f);
pidProfile->D_f[PIDYAW] = constrain(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c pidProfile->D_f[PIDYAW] = constrainf(newFloatValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c
} else { } else {
newValue = (int)pidProfile->D8[PIDYAW] + delta; newValue = (int)pidProfile->D8[PIDYAW] + delta;
pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c

1
src/test/Makefile
View file

@ -263,6 +263,7 @@ $(OBJECT_DIR)/rc_controls_unittest.o : \
$(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@ $(CXX) $(CXX_FLAGS) $(TEST_CFLAGS) -c $(TEST_DIR)/rc_controls_unittest.cc -o $@
rc_controls_unittest : \ rc_controls_unittest : \
$(OBJECT_DIR)/common/maths.o \
$(OBJECT_DIR)/io/rc_controls.o \ $(OBJECT_DIR)/io/rc_controls.o \
$(OBJECT_DIR)/rc_controls_unittest.o \ $(OBJECT_DIR)/rc_controls_unittest.o \
$(OBJECT_DIR)/gtest_main.a $(OBJECT_DIR)/gtest_main.a

47
src/test/unit/rc_controls_unittest.cc
View file

@ -20,6 +20,7 @@
extern "C" { extern "C" {
#include "platform.h" #include "platform.h"
#include "common/maths.h"
#include "common/axis.h" #include "common/axis.h"
#include "flight/flight.h" #include "flight/flight.h"
@ -32,16 +33,6 @@ extern "C" {
extern "C" { extern "C" {
extern void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfig, pidProfile_t *pidProfile); 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) TEST(RcControlsTest, updateActivatedModesWithAllInputsAtMidde)
@ -594,15 +585,15 @@ TEST(RcControlsTest, processPIDIncreasePidController0)
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
// and // and
EXPECT_EQ(pidProfile.P8[PIDPITCH], 1); EXPECT_EQ(1, pidProfile.P8[PIDPITCH]);
EXPECT_EQ(pidProfile.P8[PIDROLL], 6); EXPECT_EQ(6, pidProfile.P8[PIDROLL]);
EXPECT_EQ(pidProfile.P8[YAW], 8); EXPECT_EQ(8, pidProfile.P8[YAW]);
EXPECT_EQ(pidProfile.I8[PIDPITCH], 11); EXPECT_EQ(11, pidProfile.I8[PIDPITCH]);
EXPECT_EQ(pidProfile.I8[PIDROLL], 16); EXPECT_EQ(16, pidProfile.I8[PIDROLL]);
EXPECT_EQ(pidProfile.I8[YAW], 18); EXPECT_EQ(18, pidProfile.I8[YAW]);
EXPECT_EQ(pidProfile.D8[PIDPITCH], 21); EXPECT_EQ(21, pidProfile.D8[PIDPITCH]);
EXPECT_EQ(pidProfile.D8[PIDROLL], 26); EXPECT_EQ(26, pidProfile.D8[PIDROLL]);
EXPECT_EQ(pidProfile.D8[YAW], 28); EXPECT_EQ(28, pidProfile.D8[YAW]);
} }
TEST(RcControlsTest, processPIDIncreasePidController2) TEST(RcControlsTest, processPIDIncreasePidController2)
@ -680,15 +671,15 @@ TEST(RcControlsTest, processPIDIncreasePidController2)
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask); EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
// and // and
EXPECT_EQ(pidProfile.P_f[PIDPITCH], 1.0f); EXPECT_EQ(0.1f, pidProfile.P_f[PIDPITCH]);
EXPECT_EQ(pidProfile.P_f[PIDROLL], 6.0f); EXPECT_EQ(5.1f, pidProfile.P_f[PIDROLL]);
EXPECT_EQ(pidProfile.P_f[PIDYAW], 8.0f); EXPECT_EQ(7.1f, pidProfile.P_f[PIDYAW]);
EXPECT_EQ(pidProfile.I_f[PIDPITCH], 11.0f); EXPECT_EQ(10.1f, pidProfile.I_f[PIDPITCH]);
EXPECT_EQ(pidProfile.I_f[PIDROLL], 16.0f); EXPECT_EQ(15.1f, pidProfile.I_f[PIDROLL]);
EXPECT_EQ(pidProfile.I_f[PIDYAW], 18.0f); EXPECT_EQ(17.1f, pidProfile.I_f[PIDYAW]);
EXPECT_EQ(pidProfile.D_f[PIDPITCH], 21.0f); EXPECT_EQ(20.1f, pidProfile.D_f[PIDPITCH]);
EXPECT_EQ(pidProfile.D_f[PIDROLL], 26.0f); EXPECT_EQ(25.1f, pidProfile.D_f[PIDROLL]);
EXPECT_EQ(pidProfile.D_f[PIDYAW], 28.0f); EXPECT_EQ(27.1f, pidProfile.D_f[PIDYAW]);
} }