[nav] remove nav_manual mode (#3234)

This mode is not really used and not even recommended. It is also not implemented in all firmwares. If a direct call to stabilization command is needed, it should be done with a specific function but not from a builtin instruction from the flight plan. It was discussed in #3208, but not applyied because of nav_heli_spinup. A new h_mode 'NONE' allows to disable guidance and stabilization while in NAV mode, which allows to set directly the commands.
paparazzi · Feb 5, 2024 · bbcb166 · bbcb166
1 parent fe35af7
commit bbcb166
Show file tree

Hide file tree

Showing 9 changed files with 36 additions and 93 deletions.
diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd
@@ -28,18 +28,17 @@
 <!ELEMENT exceptions (exception*)>
 
 <!ELEMENT blocks (block+)>
-<!ELEMENT block (exception|while|heading|attitude|manual|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|home|path|guided)*>
+<!ELEMENT block (exception|while|heading|attitude|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|home|path|guided)*>
 
 <!ELEMENT include (arg|with)*>
 <!ELEMENT arg EMPTY>
 <!ELEMENT with EMPTY>
 
-<!ELEMENT while (exception|while|heading|attitude|manual|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path|guided)*>
-<!ELEMENT for (exception|while|heading|attitude|manual|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path|guided)*>
+<!ELEMENT while (exception|while|heading|attitude|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path|guided)*>
+<!ELEMENT for (exception|while|heading|attitude|go|xyz|set|call|call_once|circle|deroute|stay|follow|survey_rectangle|for|return|eight|oval|path|guided)*>
 <!ELEMENT exception EMPTY>
 <!ELEMENT heading EMPTY>
 <!ELEMENT attitude EMPTY>
-<!ELEMENT manual EMPTY>
 <!ELEMENT go EMPTY>
 <!ELEMENT xyz EMPTY>
 <!ELEMENT set EMPTY>
@@ -188,21 +187,6 @@ nav_type CDATA #IMPLIED
 nav_params CDATA #IMPLIED
 until CDATA #IMPLIED>
 
-<!ATTLIST manual
-pitch CDATA #REQUIRED
-roll CDATA #REQUIRED
-yaw CDATA #REQUIRED
-vmode CDATA #IMPLIED
-alt CDATA #IMPLIED
-height CDATA #IMPLIED
-throttle CDATA #IMPLIED
-climb CDATA #IMPLIED
-pre_call CDATA #IMPLIED
-post_call CDATA #IMPLIED
-nav_type CDATA #IMPLIED
-nav_params CDATA #IMPLIED
-until CDATA #IMPLIED>
-
 <!ATTLIST go
 wp CDATA #REQUIRED
 wp_qdr CDATA #IMPLIED

diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h
@@ -217,8 +217,6 @@ bool nav_approaching_xy(float x, float y, float from_x, float from_y, float appr
     {h_ctl_roll_setpoint = _roll;} \
   }
 
-#define NavSetManual(_roll, _pitch, _yaw) _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"")
-
 
 #define nav_IncreaseShift(x) { if (x==0) nav_shift = 0; else nav_shift += x; }
 

diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -451,16 +451,8 @@ void guidance_h_from_nav(bool in_flight)
     guidance_h_nav_enter();
   }
 
-  if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_MANUAL) {
-    #ifdef COMMAND_ROLL
-    stabilization_cmd[COMMAND_ROLL]  = nav.cmd_roll;
-    #endif
-    #ifdef COMMAND_PITCH
-    stabilization_cmd[COMMAND_PITCH] = nav.cmd_pitch;
-    #endif
-    #ifdef COMMAND_YAW
-    stabilization_cmd[COMMAND_YAW]   = nav.cmd_yaw;
-    #endif
+  if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_NONE) {
+    return; // don't call guidance nor stabilization
   } else if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_ATTITUDE) {
     if (nav.setpoint_mode == NAV_SETPOINT_MODE_QUAT) {
       // directly apply quat setpoint

diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -86,9 +86,6 @@ void nav_init(void)
   FLOAT_RATES_ZERO(nav.rates);
 
   nav.throttle = 0;
-  nav.cmd_roll = 0;
-  nav.cmd_pitch = 0;
-  nav.cmd_yaw = 0;
   nav.roll = 0.f;
   nav.pitch = 0.f;
   nav.heading = 0.f;
@@ -305,25 +302,6 @@ void nav_home(void)
   nav_run();
 }
 
-/** Set manual roll, pitch and yaw without stabilization
- *
- * @param[in] roll command in pprz scale (int32_t)
- * @param[in] pitch command in pprz scale (int32_t)
- * @param[in] yaw command in pprz scale (int32_t)
- *
- * This function allows to directly set commands from the flight plan,
- * if in nav_manual mode.
- * This is for instance useful for helicopters during the spinup
- */
-void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw)
-{
-  nav.horizontal_mode = NAV_HORIZONTAL_MODE_MANUAL;
-  nav.setpoint_mode = NAV_SETPOINT_MODE_MANUAL;
-  nav.cmd_roll = roll;
-  nav.cmd_pitch = pitch;
-  nav.cmd_yaw = yaw;
-}
-
 /** Returns squared horizontal distance to given point */
 float get_dist2_to_point(struct EnuCoor_f *p)
 {

diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h
@@ -86,7 +86,7 @@
 #define NAV_HORIZONTAL_MODE_ROUTE     1
 #define NAV_HORIZONTAL_MODE_CIRCLE    2
 #define NAV_HORIZONTAL_MODE_ATTITUDE  3
-#define NAV_HORIZONTAL_MODE_MANUAL    4
+#define NAV_HORIZONTAL_MODE_NONE      4
 #define NAV_HORIZONTAL_MODE_GUIDED    5
 
 #define NAV_VERTICAL_MODE_MANUAL      0
@@ -128,9 +128,6 @@ struct RotorcraftNavigation {
   struct EnuCoor_f speed;   ///< speed setpoint (in m/s)
   struct EnuCoor_f accel;   ///< accel setpoint (in m/s)
   uint32_t throttle;        ///< throttle command (in pprz_t)
-  int32_t cmd_roll;         ///< roll command (in pprz_t)
-  int32_t cmd_pitch;        ///< pitch command (in pprz_t)
-  int32_t cmd_yaw;          ///< yaw command (in pprz_t)
   float roll;               ///< roll angle (in radians)
   float pitch;              ///< pitch angle (in radians)
   float heading;            ///< heading setpoint (in radians)
@@ -203,7 +200,6 @@ extern float get_dist2_to_waypoint(uint8_t wp_id);
 extern float get_dist2_to_point(struct EnuCoor_f *p);
 extern void compute_dist2_to_home(void);
 extern void nav_home(void);
-extern void nav_set_manual(int32_t roll, int32_t pitch, int32_t yaw);
 
 extern void nav_reset_reference(void) __attribute__((unused));
 extern void nav_reset_alt(void) __attribute__((unused));
@@ -239,7 +235,6 @@ static inline void NavResurrect(void)
 }
 
 
-#define NavSetManual nav_set_manual
 #define NavSetFailsafe nav_set_failsafe
 
 #define NavSetGroundReferenceHere nav_reset_reference

diff --git a/sw/airborne/firmwares/rover/guidance/rover_guidance.c b/sw/airborne/firmwares/rover/guidance/rover_guidance.c
@@ -142,11 +142,7 @@ void rover_guidance_enter(void)
 //    rover_guidance_nav_enter();
 //  }
 //
-//  if (horizontal_mode == HORIZONTAL_MODE_MANUAL) {
-//    stabilization_cmd[COMMAND_ROLL]  = nav_cmd_roll;
-//    stabilization_cmd[COMMAND_PITCH] = nav_cmd_pitch;
-//    stabilization_cmd[COMMAND_YAW]   = nav_cmd_yaw;
-//  } else if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
+//  if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
 //    struct Int32Eulers sp_cmd_i;
 //    sp_cmd_i.phi = nav_roll;
 //    sp_cmd_i.theta = nav_pitch;

diff --git a/sw/airborne/firmwares/rover/navigation.h b/sw/airborne/firmwares/rover/navigation.h
@@ -197,8 +197,6 @@ static inline void NavResurrect(void)
   if (autopilot_get_mode() == AP_MODE_NAV) { autopilot_set_motors_on(true); }
 }
 
-
-#define NavSetManual(_roll, _pitch, _yaw) _Pragma("GCC error \"Manual mode in flight plan for fixedwing is not available\"")
 #define NavSetFailsafe nav_set_failsafe
 
 #define NavSetGroundReferenceHere nav_reset_reference

diff --git a/sw/airborne/modules/nav/nav_heli_spinup.c b/sw/airborne/modules/nav/nav_heli_spinup.c
@@ -24,6 +24,7 @@
  */
 
 #include "modules/nav/nav_heli_spinup.h"
+#include "firmwares/rotorcraft/stabilization.h"
 #include "navigation.h"
 #include "paparazzi.h"
 
@@ -41,11 +42,18 @@ void nav_heli_spinup_setup(uint16_t duration, float throttle)
   nav_heli_spinup.duration = (duration > 0) ? duration : 1;
   nav_heli_spinup.throttle = throttle * MAX_PPRZ;
 
+#ifdef COMMAND_ROLL
+  stabilization_cmd[COMMAND_ROLL]  = 0;
+#endif
+#ifdef COMMAND_PITCH
+  stabilization_cmd[COMMAND_PITCH] = 0;
+#endif
+#ifdef COMMAND_YAW
+  stabilization_cmd[COMMAND_YAW]   = 0;
+#endif
   nav.throttle = 0;
-  nav.cmd_roll = 0;
-  nav.cmd_pitch = 0;
-  nav.cmd_yaw = 0;
-  nav.horizontal_mode = NAV_HORIZONTAL_MODE_ATTITUDE;
+
+  nav.horizontal_mode = NAV_HORIZONTAL_MODE_NONE;
   nav.vertical_mode = NAV_VERTICAL_MODE_MANUAL;
 }
 
@@ -59,11 +67,18 @@ bool nav_heli_spinup_run(void)
     return false;
   }
 
-  nav.cmd_roll = 0;
-  nav.cmd_pitch = 0;
-  nav.cmd_yaw = 0;
-  nav.horizontal_mode = NAV_HORIZONTAL_MODE_MANUAL;
-  nav.vertical_mode = NAV_VERTICAL_MODE_MANUAL;
+#ifdef COMMAND_ROLL
+  stabilization_cmd[COMMAND_ROLL]  = 0;
+#endif
+#ifdef COMMAND_PITCH
+  stabilization_cmd[COMMAND_PITCH] = 0;
+#endif
+#ifdef COMMAND_YAW
+  stabilization_cmd[COMMAND_YAW]   = 0;
+#endif
   nav.throttle = stage_time * nav_heli_spinup.throttle / nav_heli_spinup.duration;
+
+  nav.horizontal_mode = NAV_HORIZONTAL_MODE_NONE;
+  nav.vertical_mode = NAV_VERTICAL_MODE_MANUAL;
   return true;
 }
diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml
@@ -242,14 +242,11 @@ let pprz_throttle = fun s ->
 let output_vmode = fun out stage_xml wp last_wp ->
   let pitch = try Xml.attrib stage_xml "pitch" with _ -> "0.0" in
   let t = ExtXml.attrib_or_default stage_xml "nav_type" "Nav" in
-  if String.lowercase_ascii (Xml.tag stage_xml) <> "manual"
+  if pitch = "auto"
   then begin
-    if pitch = "auto"
-    then begin
-      lprintf out "%sVerticalAutoPitchMode(%s);\n" t (pprz_throttle (parsed_attrib stage_xml "throttle"))
-    end else begin
-      lprintf out "%sVerticalAutoThrottleMode(RadOfDeg(%s));\n" t (parse pitch);
-    end
+    lprintf out "%sVerticalAutoPitchMode(%s);\n" t (pprz_throttle (parsed_attrib stage_xml "throttle"))
+  end else begin
+    lprintf out "%sVerticalAutoThrottleMode(RadOfDeg(%s));\n" t (parse pitch);
   end;
 
   let vmode = try ExtXml.attrib stage_xml "vmode" with _ -> "alt" in
@@ -335,7 +332,7 @@ let rec index_stage = fun x ->
         incr stage; (* To count the loop stage *)
         Xml.Element (Xml.tag x, Xml.attribs x@["no", soi n], l)
       | "return" | "goto"  | "deroute" | "exit_block" | "follow" | "call" | "call_once" | "home"
-      | "heading" | "attitude" | "manual" | "go" | "stay" | "xyz" | "set" | "circle" | "guided" ->
+      | "heading" | "attitude" | "go" | "stay" | "xyz" | "set" | "circle" | "guided" ->
         incr stage;
         Xml.Element (Xml.tag x, Xml.attribs x@["no", soi !stage], Xml.children x)
       | "survey_rectangle" | "eight" | "oval"->
@@ -455,16 +452,6 @@ let rec print_stage = fun out index_of_waypoints x ->
         stage_until out x;
         fp_post_call out x;
         lprintf out "break;\n"
-      | "manual" ->
-        stage out;
-        fp_pre_call out x;
-        let t = ExtXml.attrib_or_default x "nav_type" "Nav" in
-        let p = try ", " ^ (Xml.attrib x "nav_params") with _ -> "" in
-        lprintf out "%sSetManual(%s, %s, %s%s);\n" t (parsed_attrib x "roll") (parsed_attrib x "pitch") (parsed_attrib x "yaw") p;
-        ignore (output_vmode out x "" "");
-        stage_until out x;
-        fp_post_call out x;
-        lprintf out "break;\n"
       | "go" ->
         stage out;
         fp_pre_call out x;