From c4b9b533544e542a9d35f7df50818d222cb0c9ba Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Sun, 7 May 2017 21:50:35 +1000 Subject: [PATCH] Add yaw gain for TURN_ASSIST mode --- docs/Cli.md | 1 + src/main/fc/cli.c | 3 ++- src/main/flight/pid.c | 9 ++++++--- src/main/flight/pid.h | 3 ++- 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/docs/Cli.md b/docs/Cli.md index 650c0aa019..60363c3795 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -312,6 +312,7 @@ Re-apply any new defaults as desired. | gimbal_mode | NORMAL | When feature SERVO_TILT is enabled, this can be either NORMAL or MIXTILT | | 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_reference_airspeed | 1000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | +| fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | mode_range_logic_operator | OR | Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. | | default_rate_profile | 0 | Default = profile number | | mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you aquirre valid GPS fix. | diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 236d1b7eab..909e52ea96 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -743,7 +743,8 @@ static const clivalue_t valueTable[] = { { "dterm_setpoint_weight", VAR_FLOAT | PROFILE_VALUE, .config.minmax = {0, 2 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_setpoint_weight) }, #ifdef USE_SERVOS { "fw_iterm_throw_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { FW_ITERM_THROW_LIMIT_MIN, FW_ITERM_THROW_LIMIT_MAX}, PG_PID_PROFILE, offsetof(pidProfile_t, fixedWingItermThrowLimit) }, - { "fw_reference_airspeed", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 5000}, PG_PID_PROFILE, offsetof(pidProfile_t, fixedWingReferenceAirspeed) }, + { "fw_reference_airspeed", VAR_FLOAT | PROFILE_VALUE, .config.minmax = { 1, 5000}, PG_PID_PROFILE, offsetof(pidProfile_t, fixedWingReferenceAirspeed) }, + { "fw_turn_assist_yaw_gain", VAR_FLOAT | PROFILE_VALUE, .config.minmax = { 0, 2}, PG_PID_PROFILE, offsetof(pidProfile_t, fixedWingCoordinatedYawGain) }, #endif #ifdef USE_DTERM_NOTCH { "dterm_notch_hz", VAR_UINT16 | PROFILE_VALUE, .config.minmax = {0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_soft_notch_hz) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f29e70e378..287b58834d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -107,7 +107,7 @@ int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_ static pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT]; -PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 2); +PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 3); PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { @@ -191,6 +191,7 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = 1000, + .fixedWingCoordinatedYawGain = 1.0f, ); void pidInit(void) @@ -656,10 +657,12 @@ static void pidTurnAssistant(pidState_t *pidState) // Constrain to somewhat sane limits - 10km/h - 216km/h airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); + // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge float bankAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - float coordinatedTurnRateOffset = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn; + float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn; - targetRates.V.Z = pidState[YAW].rateTarget + RADIANS_TO_DEGREES(coordinatedTurnRateOffset); + // Apply additionan gain + targetRates.V.Z = pidState[YAW].rateTarget + RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame * pidProfile()->fixedWingCoordinatedYawGain); } else { // Don't allow coordinated turn calculation if airplane is in hard bank or steep climb/dive diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 032696fbe7..e9cf360bf4 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -103,7 +103,8 @@ typedef struct pidProfile_s { // Airplane-specific parameters uint16_t fixedWingItermThrowLimit; - float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned + float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned + float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. } pidProfile_t; typedef struct pidAutotuneConfig_s {