From 51c0b95cbff5201480ffa1b0f1f9815ba99424d6 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 12 Dec 2020 10:40:29 +0100 Subject: [PATCH 1/9] Make TURN ASSIST axis aware, use target bank angle, and disable in ACRO mode --- src/main/flight/pid.c | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 00cd8ac9c5..e9a3c3859d 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -843,13 +843,13 @@ 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, flight_dynamics_index_t axis) { fpVector3_t targetRates; targetRates.x = 0.0f; targetRates.y = 0.0f; - if (STATE(AIRPLANE)) { + if (STATE(AIRPLANE) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { if (calculateCosTiltAngle() >= 0.173648f) { // Ideal banked turn follow the equations: // forward_vel^2 / radius = Gravity * tan(roll_angle) @@ -871,8 +871,8 @@ 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; + float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[axis]); + float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); } @@ -986,7 +986,9 @@ void FAST_CODE pidController(float dT) } if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - pidTurnAssistant(pidState); + for (int axis = 0; axis < 3; axis++) { + pidTurnAssistant(pidState, axis); + } canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } From 7ec6afd1f3f9d4a385af8126252093f4ef7c5f1e Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 12 Dec 2020 10:54:01 +0100 Subject: [PATCH 2/9] Change target bank angle code --- src/main/flight/pid.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e9a3c3859d..40eebf7a8e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -843,7 +843,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, flight_dynamics_index_t axis) +static void NOINLINE pidTurnAssistant(pidState_t *pidState) { fpVector3_t targetRates; targetRates.x = 0.0f; @@ -871,7 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, flight_dynamics_inde airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge - float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[axis]); + float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[ROLL]); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); @@ -986,9 +986,9 @@ void FAST_CODE pidController(float dT) } if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - for (int axis = 0; axis < 3; axis++) { - pidTurnAssistant(pidState, axis); - } + //for (int axis = 0; axis < 3; axis++) { + pidTurnAssistant(pidState); + //} canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } From 232d526ab12549e8d1553505aba8af3c5cd4c952 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 12 Dec 2020 11:31:46 +0100 Subject: [PATCH 3/9] no message --- src/main/flight/pid.c | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 40eebf7a8e..f390966023 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -871,7 +871,7 @@ 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 bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[ROLL]); + float bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); @@ -986,9 +986,7 @@ void FAST_CODE pidController(float dT) } if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - //for (int axis = 0; axis < 3; axis++) { - pidTurnAssistant(pidState); - //} + pidTurnAssistant(pidState); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } From 214f939a1f06e51cb402fde9eacfaccf81da1a9a Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 12 Dec 2020 14:21:45 +0100 Subject: [PATCH 4/9] Attempted fix (does not work) --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index f390966023..a21ae688d3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -871,7 +871,7 @@ 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 bankAngleTarget = pidRcCommandToAngle(rcCommand[ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); + float bankAngleTarget = pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); From cf2e35908adbfbe4a6bbc726c896c86f68c1f83b Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 12 Dec 2020 21:42:19 +0100 Subject: [PATCH 5/9] Convert bank angle to radians --- src/main/flight/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a21ae688d3..0623c826b6 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -871,7 +871,7 @@ 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 bankAngleTarget = pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL]); + float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); From 95ab3fe22b1ad336e6898e2d9a5bc2339d467acf Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 13 Dec 2020 11:02:42 +0100 Subject: [PATCH 6/9] Take target bank angle calculation out of pidTurnAssistant --- src/main/flight/pid.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0623c826b6..000e33c9fa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -843,7 +843,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) { fpVector3_t targetRates; targetRates.x = 0.0f; @@ -871,7 +871,6 @@ 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 bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); @@ -986,7 +985,8 @@ void FAST_CODE pidController(float dT) } if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { - pidTurnAssistant(pidState); + float bankAngleTarget = DECIDEGREES_TO_RADIANS(pidRcCommandToAngle(rcCommand[FD_ROLL], pidProfile()->max_angle_inclination[FD_ROLL])); + pidTurnAssistant(pidState, bankAngleTarget); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT } From 2270a8c3df9a926e74378de05b962cdbc7283537 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 13 Dec 2020 11:15:09 +0100 Subject: [PATCH 7/9] Modes check outside of pidTurnAssistant --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 000e33c9fa..8d94c3b4fc 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -849,7 +849,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge targetRates.x = 0.0f; targetRates.y = 0.0f; - if (STATE(AIRPLANE) && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE) || navigationRequiresTurnAssistance())) { + if (STATE(AIRPLANE)) { if (calculateCosTiltAngle() >= 0.173648f) { // Ideal banked turn follow the equations: // forward_vel^2 / radius = Gravity * tan(roll_angle) @@ -984,7 +984,7 @@ void FAST_CODE pidController(float dT) levelingEnabled = false; } - if (FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance()) { + 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])); pidTurnAssistant(pidState, bankAngleTarget); canUseFpvCameraMix = false; // FPVANGLEMIX is incompatible with TURN_ASSISTANT From a1f42fcfe2cc287a8ad37a04fc89d5542fadb0e5 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 14 Dec 2020 15:24:50 +0100 Subject: [PATCH 8/9] Take pitch angle into account for turn rate calculation (technically correct but doesn't make huge difference) --- src/main/flight/pid.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8d94c3b4fc..e733bc43bf 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -843,7 +843,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, float bankAngleTarget) +static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarget, float pitchAngleTarget) { fpVector3_t targetRates; targetRates.x = 0.0f; @@ -871,7 +871,8 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge - float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn; + float turnRatePitchAdjustmentFactor = cos_approx(fabsf(pitchAngleTarget)); + float coordinatedTurnRateEarthFrame = GRAVITY_CMSS * tan_approx(-bankAngleTarget) / airspeedForCoordinatedTurn * turnRatePitchAdjustmentFactor; targetRates.z = RADIANS_TO_DEGREES(coordinatedTurnRateEarthFrame); } @@ -986,7 +987,8 @@ void FAST_CODE pidController(float dT) 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])); - pidTurnAssistant(pidState, bankAngleTarget); + 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 } From 37f2954ad1c33902e6c4a9362d5538e4646d038a Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 15 Dec 2020 14:41:01 +0100 Subject: [PATCH 9/9] Constain bank angle for calculation to 60 degrees. --- src/main/flight/pid.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index e733bc43bf..1f27af2b6f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -871,6 +871,7 @@ static void NOINLINE pidTurnAssistant(pidState_t *pidState, float bankAngleTarge airspeedForCoordinatedTurn = constrainf(airspeedForCoordinatedTurn, 300, 6000); // Calculate rate of turn in Earth frame according to FAA's Pilot's Handbook of Aeronautical Knowledge + 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;