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

Merge pull request #10516 from etracer65/level_mode_expo

Browse source 
Self-level modes expo
This commit is contained in:
Michael Keller 2021-02-14 17:25:16 +13:00 • committed by GitHub
commit aeb2a4f381
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
7 changed files with 51 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cli/settings.c
View file

@ -976,6 +976,8 @@ const clivalue_t valueTable[] = {
{ "roll_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_ROLL]) },
{ "pitch_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_PITCH]) },
{ "yaw_rate_limit", VAR_UINT16 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { CONTROL_RATE_CONFIG_RATE_LIMIT_MIN, CONTROL_RATE_CONFIG_RATE_LIMIT_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, rate_limit[FD_YAW]) },
{ "roll_level_expo", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, levelExpo[FD_ROLL]) },
{ "pitch_level_expo", VAR_UINT8 | PROFILE_RATE_VALUE, .config.minmaxUnsigned = { 0, CONTROL_RATE_CONFIG_RC_EXPO_MAX }, PG_CONTROL_RATE_PROFILES, offsetof(controlRateConfig_t, levelExpo[FD_PITCH]) },
// PG_SERIAL_CONFIG
{ "reboot_character", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 48, 126 }, PG_SERIAL_CONFIG, offsetof(serialConfig_t, reboot_character) },

3
src/main/cms/cms_menu_imu.c
View file

@ -392,6 +392,9 @@ static const OSD_Entry cmsx_menuRateProfileEntries[] =
{ "THR LIM TYPE",OME_TAB, NULL, &(OSD_TAB_t) { &rateProfile.throttle_limit_type, THROTTLE_LIMIT_TYPE_COUNT - 1, osdTableThrottleLimitType}, 0 },
{ "THR LIM %", OME_UINT8, NULL, &(OSD_UINT8_t) { &rateProfile.throttle_limit_percent, 25, 100, 1}, 0 },
{ "ROLL LVL EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.levelExpo[FD_ROLL], 0, 100, 1, 10 }, 0 },
{ "PITCH LVL EXPO", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &rateProfile.levelExpo[FD_PITCH], 0, 100, 1, 10 }, 0 },
{ "BACK", OME_Back, NULL, NULL, 0 },
{ NULL, OME_END, NULL, NULL, 0 }
};

4
src/main/fc/controlrate_profile.c
View file

@ -37,7 +37,7 @@
controlRateConfig_t *currentControlRateProfile;
PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 3);
PG_REGISTER_ARRAY_WITH_RESET_FN(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles, PG_CONTROL_RATE_PROFILES, 4);
void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
{
@ -65,6 +65,8 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *controlRateConfig)
.tpaMode = TPA_MODE_D,
.profileName = { 0 },
.quickRatesRcExpo = 0,
.levelExpo[FD_ROLL] = 0,
.levelExpo[FD_PITCH] = 0,
);
}
}

1
src/main/fc/controlrate_profile.h
View file

@ -68,6 +68,7 @@ typedef struct controlRateConfig_s {
uint8_t tpaMode; // Controls which PID terms TPA effects
char profileName[MAX_RATE_PROFILE_NAME_LENGTH + 1]; // Descriptive name for rate profile
uint8_t quickRatesRcExpo; // Sets expo on rc command for quick rates
uint8_t levelExpo[2]; // roll/pitch level mode expo
} controlRateConfig_t;
PG_DECLARE_ARRAY(controlRateConfig_t, CONTROL_RATE_PROFILE_COUNT, controlRateProfiles);

16
src/main/flight/pid.c
View file

@ -333,11 +333,23 @@ float pidApplyThrustLinearization(float motorOutput)
#endif
#if defined(USE_ACC)
// calculate the stick deflection while applying level mode expo
static float getLevelModeRcDeflection(uint8_t axis)
{
const float stickDeflection = getRcDeflection(axis);
if (axis < FD_YAW) {
const float expof = currentControlRateProfile->levelExpo[axis] / 100.0f;
return power3(stickDeflection) * expof + stickDeflection * (1 - expof);
} else {
return stickDeflection;
}
}
// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
{
// start with 1.0 at center stick, 0.0 at max stick deflection:
float horizonLevelStrength = 1.0f - MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
float horizonLevelStrength = 1.0f - MAX(fabsf(getLevelModeRcDeflection(FD_ROLL)), fabsf(getLevelModeRcDeflection(FD_PITCH)));
// 0 at level, 90 at vertical, 180 at inverted (degrees):
const float currentInclination = MAX(ABS(attitude.values.roll), ABS(attitude.values.pitch)) / 10.0f;
@ -395,7 +407,7 @@ STATIC_UNIT_TESTED float calcHorizonLevelStrength(void)
STATIC_UNIT_TESTED FAST_CODE_NOINLINE float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
// calculate error angle and limit the angle to the max inclination
// rcDeflection is in range [-1.0, 1.0]
float angle = pidProfile->levelAngleLimit * getRcDeflection(axis);
float angle = pidProfile->levelAngleLimit * getLevelModeRcDeflection(axis);
#ifdef USE_GPS_RESCUE
angle += gpsRescueAngle[axis] / 100; // ANGLE IS IN CENTIDEGREES
#endif

1
src/test/Makefile
View file

@ -376,6 +376,7 @@ pid_unittest_SRC := \
$(USER_DIR)/common/filter.c \
$(USER_DIR)/common/maths.c \
$(USER_DIR)/drivers/accgyro/gyro_sync.c \
$(USER_DIR)/fc/controlrate_profile.c \
$(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/flight/pid.c \
$(USER_DIR)/flight/pid_init.c \

29
src/test/unit/pid_unittest.cc
View file

@ -34,18 +34,21 @@ int16_t debug[DEBUG16_VALUE_COUNT];
uint8_t debugMode;
extern "C" {
#include "platform.h"
#include "build/debug.h"
#include "common/axis.h"
#include "common/maths.h"
#include "common/filter.h"
#include "config/config.h"
#include "config/config_reset.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "drivers/sound_beeper.h"
#include "drivers/time.h"
#include "fc/controlrate_profile.h"
#include "fc/core.h"
#include "fc/rc.h"
@ -59,6 +62,9 @@ extern "C" {
#include "io/gps.h"
#include "pg/pg.h"
#include "pg/pg_ids.h"
#include "sensors/gyro.h"
#include "sensors/acceleration.h"
@ -66,6 +72,7 @@ extern "C" {
attitudeEulerAngles_t attitude;
PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 2);
bool unitLaunchControlActive = false;
launchControlMode_e unitLaunchControlMode = LAUNCH_CONTROL_MODE_NORMAL;
@ -87,6 +94,7 @@ extern "C" {
UNUSED(currentPidSetpoint);
return value;
}
void initRcProcessing(void) { }
}
pidProfile_t *pidProfile;
@ -179,6 +187,10 @@ void resetTest(void) {
unitLaunchControlActive = false;
pidProfile->launchControlMode = unitLaunchControlMode;
pidInit(pidProfile);
loadControlRateProfile();
currentControlRateProfile->levelExpo[FD_ROLL] = 0;
currentControlRateProfile->levelExpo[FD_PITCH] = 0;
// Run pidloop for a while after reset
for (int loop = 0; loop < 20; loop++) {
@ -379,6 +391,19 @@ TEST(pidControllerTest, testPidLevel) {
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(0, currentPidSetpoint);
// Test level mode expo
enableFlightMode(ANGLE_MODE);
attitude.values.roll = 0;
attitude.values.pitch = 0;
setStickPosition(FD_ROLL, 0.5f);
setStickPosition(FD_PITCH, -0.5f);
currentControlRateProfile->levelExpo[FD_ROLL] = 50;
currentControlRateProfile->levelExpo[FD_PITCH] = 26;
currentPidSetpoint = pidLevel(FD_ROLL, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(85.9375, currentPidSetpoint);
currentPidSetpoint = pidLevel(FD_PITCH, pidProfile, &angleTrim, currentPidSetpoint);
EXPECT_FLOAT_EQ(-110.6875, currentPidSetpoint);
}