diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index 302c3503e9..fcacaa5fa6 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/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> +until CDATA #IMPLIED +max_speed CDATA #IMPLIED> +climb CDATA #IMPLIED +max_speed CDATA #IMPLIED> +until CDATA #IMPLIED +max_speed CDATA #IMPLIED> - +radius CDATA #REQUIRED +max_speed CDATA #IMPLIED> +until CDATA #IMPLIED +max_speed CDATA #IMPLIED> +until CDATA #IMPLIED +max_speed CDATA #IMPLIED> +height CDATA #IMPLIED +max_speed CDATA #IMPLIED> +nav_params CDATA #IMPLIED +max_speed CDATA #IMPLIED> diff --git a/doc/sphinx/source/user_guide/flight_plans.rst b/doc/sphinx/source/user_guide/flight_plans.rst index 253bede0fa..f0a7b3a1c7 100644 --- a/doc/sphinx/source/user_guide/flight_plans.rst +++ b/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. diff --git a/sw/airborne/firmwares/fixedwing/nav.h b/sw/airborne/firmwares/fixedwing/nav.h index c8c40d5b22..ba06b62870 100644 --- a/sw/airborne/firmwares/fixedwing/nav.h +++ b/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; } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 803556bc59..53d367cb0d 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/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); diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c index ca43506da3..cc2f529ccd 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.c +++ b/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; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 24913235de..bf386efc7a 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/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 diff --git a/sw/airborne/firmwares/rover/navigation.h b/sw/airborne/firmwares/rover/navigation.h index eac0b5bb7e..0ccb9f6b68 100644 --- a/sw/airborne/firmwares/rover/navigation.h +++ b/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 /*********************************************************** diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml index f903e72ef3..8ff0d2dc70 100644 --- a/sw/tools/generators/gen_flight_plan.ml +++ b/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;