mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 04:45:24 +03:00
Use different scale for inflight adjustments to PIDs of FP based PID
controllers.
This commit is contained in:
Dominic Clifton
parent
ea386e6da2
commit
2ac7282314
3 changed files with 40 additions and 46 deletions
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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]);
|
||||
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue