Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

Add yaw gain for TURN_ASSIST mode

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-05-07 21:50:35 +10:00
parent 4ca5835fa9
commit c4b9b53354
4 changed files with 11 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

1
docs/Cli.md
View file

@ -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. |

3
src/main/fc/cli.c
View file

@ -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) },

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

@ -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

3
src/main/flight/pid.h
View file

@ -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 {