[flight plan] add a max_speed attribute to fp instruction (#3387)

This is only for the rotorcraft firmware at the moment, and it is
replacing the explicit call to guidance_h_SetMaxSpeed to simplify the
flight plans.

conf/flight_plans/flight_plan.dtd

@@ -173,7 +173,8 @@ pre_call CDATA #IMPLIED
 post_call CDATA #IMPLIED
 nav_type CDATA #IMPLIED
 nav_params CDATA #IMPLIED
-until CDATA #REQUIRED>
+until CDATA #REQUIRED
+max_speed CDATA #IMPLIED>
 
 <!ATTLIST attitude
 roll CDATA #REQUIRED
@@ -209,7 +210,8 @@ pre_call CDATA #IMPLIED
 post_call CDATA #IMPLIED
 nav_type CDATA #IMPLIED
 nav_params CDATA #IMPLIED
-until CDATA #IMPLIED>
+until CDATA #IMPLIED
+max_speed CDATA #IMPLIED>
 
 <!ATTLIST path
 wpts CDATA #REQUIRED
@@ -220,7 +222,8 @@ height CDATA #IMPLIED
 approaching_time CDATA #IMPLIED
 exceeding_time CDATA #IMPLIED
 throttle CDATA #IMPLIED
-climb CDATA #IMPLIED>
+climb CDATA #IMPLIED
+max_speed CDATA #IMPLIED>
 
 <!ATTLIST set
 var CDATA #REQUIRED
@@ -265,7 +268,8 @@ pre_call CDATA #IMPLIED
 post_call CDATA #IMPLIED
 nav_type CDATA #IMPLIED
 nav_params CDATA #IMPLIED
-until CDATA #IMPLIED>
+until CDATA #IMPLIED
+max_speed CDATA #IMPLIED>
 
 <!ATTLIST eight
 center CDATA #REQUIRED
@@ -278,8 +282,8 @@ throttle CDATA #IMPLIED
 pre_call CDATA #IMPLIED
 post_call CDATA #IMPLIED
 until CDATA #IMPLIED
-radius CDATA #REQUIRED>
+radius CDATA #REQUIRED
-
+max_speed CDATA #IMPLIED>
 
 <!ATTLIST oval
 p1 CDATA #REQUIRED
@@ -292,7 +296,8 @@ pitch CDATA #IMPLIED
 throttle CDATA #IMPLIED
 pre_call CDATA #IMPLIED
 post_call CDATA #IMPLIED
-until CDATA #IMPLIED>
+until CDATA #IMPLIED
+max_speed CDATA #IMPLIED>
 
 <!ATTLIST survey_rectangle
 grid CDATA #REQUIRED
@@ -303,7 +308,8 @@ pre_call CDATA #IMPLIED
 post_call CDATA #IMPLIED
 nav_type CDATA #IMPLIED
 nav_params CDATA #IMPLIED
-until CDATA #IMPLIED>
+until CDATA #IMPLIED
+max_speed CDATA #IMPLIED>
 
 <!ATTLIST stay
 wp CDATA #REQUIRED
@@ -316,7 +322,8 @@ pre_call CDATA #IMPLIED
 post_call CDATA #IMPLIED
 nav_type CDATA #IMPLIED
 nav_params CDATA #IMPLIED
-height CDATA #IMPLIED>
+height CDATA #IMPLIED
+max_speed CDATA #IMPLIED>
 
 <!ATTLIST guided
 commands CDATA #REQUIRED
@@ -325,7 +332,8 @@ until CDATA #IMPLIED
 pre_call CDATA #IMPLIED
 post_call CDATA #IMPLIED
 nav_type CDATA #IMPLIED
-nav_params CDATA #IMPLIED>
+nav_params CDATA #IMPLIED
+max_speed CDATA #IMPLIED>
 
 <!ATTLIST deroute
 block CDATA #REQUIRED>

doc/sphinx/source/user_guide/flight_plans.rst

@@ -436,6 +436,8 @@ The vertical control is achieved using the vmode attribute of these stages. The
 
 The default control is done with the throttle. However, setting the ``pitch`` attribute to ``auto`` and the ``throttle`` attribute to a constant allows a vertical control only by controlling the attitude of the A/C. The pitch attribute also can be set to any value (in degrees) while the throttle control is in use: it usually affects the airspeed of the aircraft.
 
+The speed control is usually controlled separately. However, for the rotorcraft firmware, it is possible to set the maximum speed from the attribute ``max_speed``. This is only possible for the instructions where the guidance control is involved (heading, go, path, circle, oval, eight, stay).
+
 The different navigation modes are detailed in the next sections. 
 
 

sw/airborne/firmwares/fixedwing/nav.h

@@ -217,6 +217,7 @@ bool nav_approaching_xy(float x, float y, float from_x, float from_y, float appr
     {h_ctl_roll_setpoint = _roll;} \
   }
 
+#define NavSetMaxSpeed(_speed) {} // not used
 
 #define nav_IncreaseShift(x) { if (x==0) nav_shift = 0; else nav_shift += x; }
 

sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c

@@ -378,6 +378,9 @@ struct StabilizationSetpoint guidance_h_from_nav(bool in_flight)
     guidance_h_nav_enter();
   }
 
+  if (nav.fp_max_speed > 0.f) {
+    guidance_h_SetMaxSpeed(nav.fp_max_speed);
+  }
   if (nav.horizontal_mode == NAV_HORIZONTAL_MODE_NONE) {
     struct StabilizationSetpoint sp;
     STAB_SP_SET_EULERS_ZERO(sp);

sw/airborne/firmwares/rotorcraft/navigation.c

@@ -91,6 +91,7 @@ void nav_init(void)
   nav.climb = 0.f;
   nav.fp_altitude = SECURITY_HEIGHT;
   nav.nav_altitude = SECURITY_HEIGHT;
+  nav.fp_max_speed = -1.f;
   flight_altitude = SECURITY_ALT;
 
   nav.too_far_from_home = false;

sw/airborne/firmwares/rotorcraft/navigation.h

@@ -137,6 +137,7 @@ struct RotorcraftNavigation {
   float climb;              ///< climb setpoint (in m/s)
   float fp_altitude;        ///< altitude setpoint from flight plan (in meters)
   float nav_altitude;       ///< current altitude setpoint (in meters): might differ from fp_altitude depending on altitude shift from operator
+  float fp_max_speed;       ///< maximum speed setpoint from flight plan (in m/s), negative value means unset or invalid, do not use
 
   // misc
   float dist2_to_home;        ///< squared distance to home waypoint
@@ -288,6 +289,10 @@ bool nav_check_wp_time(struct EnuCoor_f *wp, uint16_t stay_time);
 /** Set the heading of the rotorcraft, nothing else */
 #define NavHeading nav_set_heading_rad
 
+/** Set maximum speed */
+#define NavSetMaxSpeed(_speed) {  \
+    nav.fp_max_speed = _speed;    \
+  }
 
 /***********************************************************
  * built in navigation routines

sw/airborne/firmwares/rover/navigation.h

@@ -232,6 +232,7 @@ bool nav_check_wp_time(struct EnuCoor_f *wp, float stay_time);
 /** Set the heading of the rover, nothing else */
 #define NavHeading nav_set_heading_rad
 
+#define NavSetMaxSpeed(_speed) {} // not used
 
 
 /***********************************************************

sw/tools/generators/gen_flight_plan.ml

@@ -354,6 +354,10 @@ let fp_pre_call = fun out x ->
 let fp_post_call = fun out x ->
   try lprintf out "%s;\n" (ExtXml.attrib x "post_call") with _ -> ()
 
+(* print speed instructions *)
+let output_speed = fun out x t ->
+  let max_speed = ExtXml.attrib_or_default x "max_speed" "-1." in
+  lprintf out "%sSetMaxSpeed(%s);\n" t max_speed
 
 (* test until condition test if any, post_call before leaving *)
 let stage_until = fun out x ->
@@ -426,6 +430,7 @@ let rec print_stage = fun out index_of_waypoints x ->
         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
+        output_speed out x t;
         lprintf out "%sHeading(RadOfDeg(%s)%s);\n" t (parsed_attrib x "course") p;
         ignore (output_vmode out x "" "");
         stage_until out x;
@@ -487,6 +492,7 @@ let rec print_stage = fun out index_of_waypoints x ->
         left ();
         lprintf out "} else {\n";
         right ();
+        output_speed out x t;
         let hmode = output_hmode out x wp last_wp in
         let vmode = output_vmode out x wp last_wp in
         if vmode = "glide" && hmode <> "route" then
@@ -500,6 +506,7 @@ let rec print_stage = fun out index_of_waypoints x ->
         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
+        output_speed out x t;
         begin
           try
             let wp = get_index_waypoint (ExtXml.attrib x "wp") index_of_waypoints in
@@ -534,6 +541,7 @@ let rec print_stage = fun out index_of_waypoints x ->
         let _vmode = output_vmode out x wp "" in
         let t = ExtXml.attrib_or_default x "nav_type" "Nav" in
         let p = try ", " ^ (Xml.attrib x "nav_params") with _ -> "" in
+        output_speed out x t;
         lprintf out "%sCircleWaypoint(%s, %s%s);\n" t wp r p;
         stage_until out x;
         fp_post_call out x;
@@ -545,6 +553,7 @@ let rec print_stage = fun out index_of_waypoints x ->
         let flags = ExtXml.attrib_or_default x "flags" "0" in
         let t = ExtXml.attrib_or_default x "nav_type" "Nav" in
         let p = try ", " ^ (Xml.attrib x "nav_params") with _ -> "" in
+        output_speed out x t;
         lprintf out "%sGuided(%s, %s%s);\n" t flags cmds p;
         stage_until out x;
         fp_post_call out x;
@@ -559,6 +568,7 @@ let rec print_stage = fun out index_of_waypoints x ->
         let center = get_index_waypoint (ExtXml.attrib x "center") index_of_waypoints
         and turn_about = get_index_waypoint (ExtXml.attrib x "turn_around") index_of_waypoints in
         let r = parsed_attrib x "radius" in
+        output_speed out x "Nav";
         let _vmode = output_vmode out x center "" in
         lprintf out "Eight(%s, %s, %s);\n" center turn_about r;
         stage_until out x;
@@ -574,6 +584,7 @@ let rec print_stage = fun out index_of_waypoints x ->
         let p1 = get_index_waypoint (ExtXml.attrib x "p1") index_of_waypoints
         and p2 = get_index_waypoint (ExtXml.attrib x "p2") index_of_waypoints in
         let r = parsed_attrib  x "radius" in
+        output_speed out x "Nav";
         let _vmode = output_vmode out x p1 "" in
         lprintf out "Oval(%s, %s, %s);\n" p1 p2 r;
         stage_until out x;
@@ -640,6 +651,7 @@ let rec print_stage = fun out index_of_waypoints x ->
         let t = ExtXml.attrib_or_default x "nav_type" "Nav" in
         let p = try ", " ^ (Xml.attrib x "nav_params") with _ -> "" in
         stage out;
+        output_speed out x t;
         if orientation <> "NS" && orientation <> "WE" then
           failwith (sprintf "Unknown survey orientation (NS or WE): %s" orientation);
         lprintf out "%sSurveyRectangleInit(%s, %s, %s, %s%s);\n" t wp1 wp2 grid orientation p;