From 757c6372f00b0012bea1348ede8983fac6770a09 Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Thu, 14 Mar 2024 13:23:25 +0100 Subject: [PATCH] fix omega discontinuous (#3250) * fix omega discontinuous * readable code, add comment --- .../rotorcraft/guidance/guidance_indi_hybrid.c | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c index ee4fa76bd5f..b400dc14d61 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c @@ -473,8 +473,15 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa // Use the current roll angle to determine the corresponding heading rate of change. float coordinated_turn_roll = eulers_zxy.phi; - if( (guidance_euler_cmd.theta > 0.0f) && ( fabs(guidance_euler_cmd.phi) < guidance_euler_cmd.theta)) { - coordinated_turn_roll = ((guidance_euler_cmd.phi > 0.0f) - (guidance_euler_cmd.phi < 0.0f)) * guidance_euler_cmd.theta; + // When tilting backwards (e.g. waypoint behind the drone), we have to yaw around to face the direction + // of flight even when the drone is not rolling much (yet). Determine the shortest direction in which to yaw by + // looking at the roll angle. + if( (eulers_zxy.theta > 0.0f) && ( fabs(eulers_zxy.phi) < eulers_zxy.theta)) { + if (eulers_zxy.phi > 0.0f) { + coordinated_turn_roll = eulers_zxy.theta; + } else { + coordinated_turn_roll = -eulers_zxy.theta; + } } if (fabsf(coordinated_turn_roll) < max_phi) {