mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-16 12:55:19 +03:00
Allow inflight adjustments for floating-point based PID controllers.
This commit is contained in:
Dominic Clifton
parent
e33fd411c5
commit
f77a762b48
12 changed files with 295 additions and 34 deletions
|
|
@ -17,7 +17,9 @@
|
|||
#include <stdint.h>
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
extern "C" {
|
||||
#include "platform.h"
|
||||
#include "common/axis.h"
|
||||
#include "flight/flight.h"
|
||||
|
||||
|
|
@ -29,6 +31,8 @@ extern "C" {
|
|||
#include "gtest/gtest.h"
|
||||
|
||||
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)
|
||||
|
|
@ -479,6 +483,215 @@ TEST(RcControlsTest, processRcRateProfileAdjustments)
|
|||
EXPECT_EQ(adjustmentStateMask, expectedAdjustmentStateMask);
|
||||
}
|
||||
|
||||
static const adjustmentConfig_t pidPitchAndRollPAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
static const adjustmentConfig_t pidPitchAndRollIAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
static const adjustmentConfig_t pidPitchAndRollDAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
static const adjustmentConfig_t pidYawPAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_YAW_P,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
static const adjustmentConfig_t pidYawIAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_YAW_I,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
static const adjustmentConfig_t pidYawDAdjustmentConfig = {
|
||||
.adjustmentFunction = ADJUSTMENT_YAW_D,
|
||||
.mode = ADJUSTMENT_MODE_STEP,
|
||||
.data = { { 1 } }
|
||||
};
|
||||
|
||||
TEST(RcControlsTest, processPIDIncreasePidController0)
|
||||
{
|
||||
// given
|
||||
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
|
||||
memset(&modeActivationConditions, 0, sizeof (modeActivationConditions));
|
||||
|
||||
escAndServoConfig_t escAndServoConfig;
|
||||
memset(&escAndServoConfig, 0, sizeof (escAndServoConfig));
|
||||
|
||||
pidProfile_t pidProfile;
|
||||
memset(&pidProfile, 0, sizeof (pidProfile));
|
||||
pidProfile.pidController = 0;
|
||||
pidProfile.P8[PIDPITCH] = 0;
|
||||
pidProfile.P8[PIDROLL] = 5;
|
||||
pidProfile.P8[YAW] = 7;
|
||||
pidProfile.I8[PIDPITCH] = 10;
|
||||
pidProfile.I8[PIDROLL] = 15;
|
||||
pidProfile.I8[YAW] = 17;
|
||||
pidProfile.D8[PIDPITCH] = 20;
|
||||
pidProfile.D8[PIDROLL] = 25;
|
||||
pidProfile.D8[YAW] = 27;
|
||||
|
||||
// and
|
||||
controlRateConfig_t controlRateConfig;
|
||||
memset(&controlRateConfig, 0, sizeof (controlRateConfig));
|
||||
|
||||
// and
|
||||
memset(&rxConfig, 0, sizeof (rxConfig));
|
||||
rxConfig.mincheck = DEFAULT_MIN_CHECK;
|
||||
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
|
||||
rxConfig.midrc = 1500;
|
||||
|
||||
adjustmentStateMask = 0;
|
||||
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
|
||||
|
||||
configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig);
|
||||
configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig);
|
||||
configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig);
|
||||
configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig);
|
||||
configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig);
|
||||
configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig);
|
||||
|
||||
// and
|
||||
uint8_t index;
|
||||
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
|
||||
rcData[index] = PWM_RANGE_MIDDLE;
|
||||
}
|
||||
|
||||
// and
|
||||
resetCallCounters();
|
||||
resetMillis();
|
||||
|
||||
// and
|
||||
rcData[AUX1] = PWM_RANGE_MAX;
|
||||
rcData[AUX2] = PWM_RANGE_MAX;
|
||||
rcData[AUX3] = PWM_RANGE_MAX;
|
||||
|
||||
// and
|
||||
uint8_t expectedAdjustmentStateMask =
|
||||
(1 << 0) |
|
||||
(1 << 1) |
|
||||
(1 << 2) |
|
||||
(1 << 3) |
|
||||
(1 << 4) |
|
||||
(1 << 5);
|
||||
|
||||
// when
|
||||
useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile);
|
||||
processRcAdjustments(&controlRateConfig, &rxConfig);
|
||||
|
||||
// then
|
||||
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6);
|
||||
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);
|
||||
}
|
||||
|
||||
TEST(RcControlsTest, processPIDIncreasePidController2)
|
||||
{
|
||||
// given
|
||||
modeActivationCondition_t modeActivationConditions[MAX_MODE_ACTIVATION_CONDITION_COUNT];
|
||||
memset(&modeActivationConditions, 0, sizeof (modeActivationConditions));
|
||||
|
||||
escAndServoConfig_t escAndServoConfig;
|
||||
memset(&escAndServoConfig, 0, sizeof (escAndServoConfig));
|
||||
|
||||
pidProfile_t pidProfile;
|
||||
memset(&pidProfile, 0, sizeof (pidProfile));
|
||||
pidProfile.pidController = 2;
|
||||
pidProfile.P_f[PIDPITCH] = 0.0f;
|
||||
pidProfile.P_f[PIDROLL] = 5.0f;
|
||||
pidProfile.P_f[PIDYAW] = 7.0f;
|
||||
pidProfile.I_f[PIDPITCH] = 10.0f;
|
||||
pidProfile.I_f[PIDROLL] = 15.0f;
|
||||
pidProfile.I_f[PIDYAW] = 17.0f;
|
||||
pidProfile.D_f[PIDPITCH] = 20.0f;
|
||||
pidProfile.D_f[PIDROLL] = 25.0f;
|
||||
pidProfile.D_f[PIDYAW] = 27.0f;
|
||||
|
||||
// and
|
||||
controlRateConfig_t controlRateConfig;
|
||||
memset(&controlRateConfig, 0, sizeof (controlRateConfig));
|
||||
|
||||
// and
|
||||
memset(&rxConfig, 0, sizeof (rxConfig));
|
||||
rxConfig.mincheck = DEFAULT_MIN_CHECK;
|
||||
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
|
||||
rxConfig.midrc = 1500;
|
||||
|
||||
adjustmentStateMask = 0;
|
||||
memset(&adjustmentStates, 0, sizeof(adjustmentStates));
|
||||
|
||||
configureAdjustment(0, AUX1 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollPAdjustmentConfig);
|
||||
configureAdjustment(1, AUX2 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollIAdjustmentConfig);
|
||||
configureAdjustment(2, AUX3 - NON_AUX_CHANNEL_COUNT, &pidPitchAndRollDAdjustmentConfig);
|
||||
configureAdjustment(3, AUX1 - NON_AUX_CHANNEL_COUNT, &pidYawPAdjustmentConfig);
|
||||
configureAdjustment(4, AUX2 - NON_AUX_CHANNEL_COUNT, &pidYawIAdjustmentConfig);
|
||||
configureAdjustment(5, AUX3 - NON_AUX_CHANNEL_COUNT, &pidYawDAdjustmentConfig);
|
||||
|
||||
// and
|
||||
uint8_t index;
|
||||
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
|
||||
rcData[index] = PWM_RANGE_MIDDLE;
|
||||
}
|
||||
|
||||
// and
|
||||
resetCallCounters();
|
||||
resetMillis();
|
||||
|
||||
// and
|
||||
rcData[AUX1] = PWM_RANGE_MAX;
|
||||
rcData[AUX2] = PWM_RANGE_MAX;
|
||||
rcData[AUX3] = PWM_RANGE_MAX;
|
||||
|
||||
// and
|
||||
uint8_t expectedAdjustmentStateMask =
|
||||
(1 << 0) |
|
||||
(1 << 1) |
|
||||
(1 << 2) |
|
||||
(1 << 3) |
|
||||
(1 << 4) |
|
||||
(1 << 5);
|
||||
|
||||
// when
|
||||
useRcControlsConfig(modeActivationConditions, &escAndServoConfig, &pidProfile);
|
||||
processRcAdjustments(&controlRateConfig, &rxConfig);
|
||||
|
||||
// then
|
||||
EXPECT_EQ(CALL_COUNTER(COUNTER_QUEUE_CONFIRMATION_BEEP), 6);
|
||||
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);
|
||||
|
||||
}
|
||||
|
||||
extern "C" {
|
||||
void saveConfigAndNotify(void) {}
|
||||
void generateThrottleCurve(controlRateConfig_t *, escAndServoConfig_t *) {}
|
||||
|
|
@ -494,6 +707,8 @@ void mwDisarm(void) {}
|
|||
uint8_t getCurrentControlRateProfile(void) {
|
||||
return 0;
|
||||
}
|
||||
void GPS_reset_home_position(void) {}
|
||||
void baroSetCalibrationCycles(uint16_t) {}
|
||||
|
||||
uint8_t armingFlags = 0;
|
||||
int16_t heading;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue