rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #1952 from betaflight/level_fix 

Fix Level mode angle calculation // refactor cli angle parameters
This commit is contained in:
borisbstyle 2016-12-31 01:45:33 +01:00 committed by GitHub
parent 882735e91b 6114e4a42d
commit 843cd2de9b
6 changed files with 17 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/main/fc/config.c
View File

@ -176,15 +176,13 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->dterm_notch_cutoff = 160;
pidProfile->vbatPidCompensation = 0;
pidProfile->pidAtMinThrottle = PID_STABILISATION_ON;
pidProfile->max_angle_inclination = 70.0f; // 70 degrees
// Betaflight PID controller parameters
pidProfile->levelAngleLimit = 70.0f; // 70 degrees
pidProfile->setpointRelaxRatio = 30;
pidProfile->dtermSetpointWeight = 200;
pidProfile->yawRateAccelLimit = 20.0f;
pidProfile->rateAccelLimit = 0.0f;
pidProfile->itermThrottleThreshold = 350;
pidProfile->levelSensitivity = 2.0f;
pidProfile->levelSensitivity = 100.0f;
}
void resetProfile(profile_t *profile)

10
src/main/fc/fc_main.c
View File

@ -97,8 +97,7 @@ static float throttlePIDAttenuation;
uint16_t filteredCycleTime;
bool isRXDataNew;
static bool armingCalibrationWasInitialised;
static float setpointRate[3];
static float rcDeflection[3];
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
float getThrottlePIDAttenuation(void) {
return throttlePIDAttenuation;
@ -112,6 +111,10 @@ float getRcDeflection(int axis) {
return rcDeflection[axis];
}
float getRcDeflectionAbs(int axis) {
return rcDeflectionAbs[axis];
}
void applyAndSaveAccelerometerTrimsDelta(rollAndPitchTrims_t *rollAndPitchTrimsDelta)
{
accelerometerConfig()->accelerometerTrims.values.roll += rollAndPitchTrimsDelta->values.roll;
@ -149,7 +152,8 @@ void calculateSetpointRate(int axis, int16_t rc) {
if (rcRate > 2.0f) rcRate = rcRate + (RC_RATE_INCREMENTAL * (rcRate - 2.0f));
rcCommandf = rc / 500.0f;
rcDeflection[axis] = ABS(rcCommandf);
rcDeflection[axis] = rcCommandf;
rcDeflectionAbs[axis] = ABS(rcCommandf);
if (rcExpo) {
float expof = rcExpo / 100.0f;

1
src/main/fc/fc_main.h
View File

@ -37,3 +37,4 @@ void taskMainPidLoop(timeUs_t currentTimeUs);
float getThrottlePIDAttenuation(void);
float getSetpointRate(int axis);
float getRcDeflection(int axis);
float getRcDeflectionAbs(int axis);

8
src/main/flight/pid.c
View File

@ -162,7 +162,7 @@ float calcHorizonLevelStrength(const pidProfile_t *pidProfile) {
if(pidProfile->D8[PIDLEVEL] == 0){
horizonLevelStrength = 0;
} else {
const float mostDeflectedPos = MAX(getRcDeflection(FD_ROLL), getRcDeflection(FD_PITCH));
const float mostDeflectedPos = MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
// Progressively turn off the horizon self level strength as the stick is banged over
horizonLevelStrength = (1.0f - mostDeflectedPos); // 1 at centre stick, 0 = max stick deflection
horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * horizonTransition) + 1, 0, 1);
@ -172,11 +172,11 @@ float calcHorizonLevelStrength(const pidProfile_t *pidProfile) {
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
float errorAngle = pidProfile->levelSensitivity * rcCommand[axis];
float errorAngle = pidProfile->levelSensitivity * getRcDeflection(axis);
#ifdef GPS
errorAngle += GPS_angle[axis];
#endif
errorAngle = constrainf(errorAngle, -pidProfile->max_angle_inclination, pidProfile->max_angle_inclination);
errorAngle = constrainf(errorAngle, -pidProfile->levelAngleLimit, pidProfile->levelAngleLimit);
errorAngle = (errorAngle - ((attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f));
if(FLIGHT_MODE(ANGLE_MODE)) {
// ANGLE mode - control is angle based, so control loop is needed
@ -252,7 +252,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
if (axis != FD_YAW) {
float dynC = c[axis];
if (pidProfile->setpointRelaxRatio < 100) {
const float rcDeflection = getRcDeflection(axis);
const float rcDeflection = getRcDeflectionAbs(axis);
dynC = c[axis];
if (currentPidSetpoint > 0) {
if ((currentPidSetpoint - previousSetpoint[axis]) < previousSetpoint[axis])

2
src/main/flight/pid.h
View File

@ -72,7 +72,7 @@ typedef struct pidProfile_s {
uint8_t dterm_average_count; // Configurable delta count for dterm
uint8_t vbatPidCompensation; // Scale PIDsum to battery voltage
uint8_t pidAtMinThrottle; // Disable/Enable pids on zero throttle. Normally even without airmode P and D would be active.
float max_angle_inclination;
float levelAngleLimit;
// Betaflight PID controller parameters
uint16_t itermThrottleThreshold; // max allowed throttle delta before errorGyroReset in ms

4
src/main/io/serial_cli.c
View File

@ -833,7 +833,6 @@ const clivalue_t valueTable[] = {
{ "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &mixerConfig()->yaw_motor_direction, .config.minmax = { -1, 1 } },
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
{ "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } },
{ "max_angle_inclination", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.max_angle_inclination, .config.minmax = { 10.0f, 120.0f } },
#ifdef USE_SERVOS
{ "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } },
{ "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } },
@ -925,7 +924,8 @@ const clivalue_t valueTable[] = {
{ "i_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDVEL], .config.minmax = { 0, 200 } },
{ "d_vel", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDVEL], .config.minmax = { 0, 200 } },
{ "level_sensitivity", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 0.1, 3.0 } },
{ "level_stick_sensitivity", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelSensitivity, .config.minmax = { 10.0f, 200.0f } },
{ "level_angle_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.levelAngleLimit, .config.minmax = { 10.0f, 120.0f } },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &blackboxConfig()->rate_num, .config.minmax = { 1, 32 } },