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

Merge pull request #6407 from avsaase/avs-turn-assist-target-bank-angle

Browse source 
TURN ASSIST improvements for Fixed Wing
This commit is contained in:
Paweł Spychalski 2021-02-26 19:41:08 +01:00 • committed by GitHub
commit 4951a27bc3
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 8 additions and 5 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -861,7 +861,7 @@ float pidHeadingHold(float dT)
* TURN ASSISTANT mode is an assisted mode to do a Yaw rotation on a ground plane, allowing one-stick turn in RATE more
* and keeping ROLL and PITCH attitude though the turn.
*/
static void NOINLINE pidTurnAssistant(pidState_t *pidState)
static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget, float pitchAngleTarget)
{
fpVector3_t targetRates;
targetRates.x = 0.0f;
@ -889,8 +889,9 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState)
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 coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngle) / airspeedForCoordinatedTurn;
bankAngleTarget = constrainf(bankAngleTarget, -DEGREES_TO_RADIANS(60), DEGREES_TO_RADIANS(60));
float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget));
float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor;
targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame);
}
@ -1003,8 +1004,10 @@ void FAST_CODE pidController(float dT)
levelingEnabled = false;
}
if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) {
pidTurnAssistant(pidState);
if ((FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) {
float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]));
float pitchAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_PITCH], pidProfile()->max_angle_inclination[FD_PITCH]));
pidTurnAssistant(pidState, bankAngleTarget, pitchAngleTarget);
canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT
}