fix omega discontinuous (#3250)

* fix omega discontinuous

* readable code, add comment

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