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

Self-level modes expo

Browse source 
Adds angle setpoint roll/pitch expo for self-level modes.

Previously the angle setpoint was a simple linear calculation based on the stick deflection percentage and the angle limit. This makes control very jumpy around center stick and people would often resort to adding expo in their radios to compensate. This then adds the complication of wanting expo in the radio when in self-level but not when in acro - leading to complicated mixes, virtual switches, etc.

This PR adds separate self-level expo settings for roll/pitch so the user can customize the axis behavior. Yaw is excluded because this axis uses normal rates and expo controlling the rotational rate around the yaw axis and not an angle setpoint. The roll/pitch expo can range from 0 (off) to 100 (max) like other RC expo settings. For example:
```
set roll_level_expo = 30
set pitch_level_expo = 40
```
The default values are 0 which disables any expo and behaves as before.

The settings are available in the CMS rate profile menu.
This commit is contained in:
Bruce Luckcuck 2021-01-25 11:35:02 -05:00
parent 34614231d3
commit 87f322927a
7 changed files with 51 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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