readable code, add comment

paparazzi · Mar 12, 2024 · 9cd0d15 · 9cd0d15
1 parent f609987
commit 9cd0d15
Showing 1 changed file with 8 additions and 1 deletion.
diff --git 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;
 
+  // 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)) {
-    coordinated_turn_roll = ((eulers_zxy.phi > 0.0f) - (eulers_zxy.phi < 0.0f)) * 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) {