From 9a579ea4b4fcbc55678e955783d908c685fb6732 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 4 Feb 2021 10:00:03 +0100 Subject: [PATCH 1/2] Ability to trim pitch angle for level flight --- docs/Settings.md | 1 + src/main/fc/settings.yaml | 6 ++++++ src/main/flight/pid.c | 24 +++++++++++++++++++++++- src/main/flight/pid.h | 2 ++ 4 files changed, 32 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 547e03cacde..3c64f833d30 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -120,6 +120,7 @@ | fw_i_yaw | 10 | Fixed-wing rate stabilisation I-gain for YAW | | fw_iterm_limit_stick_position | 0.5 | Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side | | fw_iterm_throw_limit | 165 | Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. | +| fw_level_pitch_trim | 0 | Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level | | fw_loiter_direction | RIGHT | Direction of loitering: center point on right wing (clockwise - default), or center point on left wing (counterclockwise). If equal YAW then can be changed in flight using a yaw stick. | | fw_min_throttle_down_pitch | 0 | Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees) | | fw_p_level | 20 | Fixed-wing attitude stabilisation P-gain | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c3e286d37ec..0e2b51d4fd9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1874,6 +1874,12 @@ groups: condition: USE_GYRO_KALMAN min: 1 max: 16000 + - name: fw_level_pitch_trim + description: "Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level" + default_value: "0" + field: fixedWingLevelTrim + min: -10 + max: 10 - name: PG_PID_AUTOTUNE_CONFIG type: pidAutotuneConfig_t diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fc1db033703..be792baa6a2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -154,8 +154,9 @@ static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn; static EXTENDED_FASTRAM filterApplyFnPtr dTermLpf2FilterApplyFn; static EXTENDED_FASTRAM bool levelingEnabled = false; +static EXTENDED_FASTRAM float fixedWingLevelTrim; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 1); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -280,6 +281,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .kalman_w = 4, .kalman_sharpness = 100, .kalmanEnabled = 0, + .fixedWingLevelTrim = 0, ); bool pidInitFilters(void) @@ -534,6 +536,24 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h if ((axis == FD_PITCH) && STATE(AIRPLANE) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle); + + //PITCH trim applied by a AutoLevel flight mode and manual pitch trimming + if (axis == FD_PITCH && STATE(AIRPLANE)) { + /* + * fixedWingLevelTrim has opposite sign to rcCommand. + * Positive rcCommand means nose should point downwards + * Negative rcCommand mean nose should point upwards + * This is counter intuitive and a natural way suggests that + should mean UP + * This is why fixedWingLevelTrim has opposite sign to rcCommand + * Positive fixedWingLevelTrim means nose should point upwards + * Negative fixedWingLevelTrim means nose should point downwards + */ + DEBUG_SET(DEBUG_ALWAYS, 0, fixedWingLevelTrim); + DEBUG_SET(DEBUG_ALWAYS, 1, angleTarget); + angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); + DEBUG_SET(DEBUG_ALWAYS, 2, angleTarget); + } + const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); @@ -1108,6 +1128,8 @@ void pidInit(void) gyroKalmanInitialize(pidProfile()->kalman_q, pidProfile()->kalman_w, pidProfile()->kalman_sharpness); } #endif + + fixedWingLevelTrim = pidProfile()->fixedWingLevelTrim; } const pidBank_t * pidBank(void) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7f83accab6c..83b7b6380ca 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -150,6 +150,8 @@ typedef struct pidProfile_s { uint16_t kalman_w; uint16_t kalman_sharpness; uint8_t kalmanEnabled; + + float fixedWingLevelTrim; } pidProfile_t; typedef struct pidAutotuneConfig_s { From def19bc8ce4a32ad37d0687970f100569e5a9bb8 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 5 Feb 2021 09:21:33 +0100 Subject: [PATCH 2/2] Remove unused debug --- src/main/flight/pid.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index be792baa6a2..23fde837b74 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -548,10 +548,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h * Positive fixedWingLevelTrim means nose should point upwards * Negative fixedWingLevelTrim means nose should point downwards */ - DEBUG_SET(DEBUG_ALWAYS, 0, fixedWingLevelTrim); - DEBUG_SET(DEBUG_ALWAYS, 1, angleTarget); angleTarget -= DEGREES_TO_DECIDEGREES(fixedWingLevelTrim); - DEBUG_SET(DEBUG_ALWAYS, 2, angleTarget); } const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]);