Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 14:18:00 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

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

Browse Source 
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.
This commit is contained in:
Gautier Hattenberger
2024-10-04 08:53:30 +02:00
committed by GitHub
parent cea5c644e5
commit ade23ab09c
8 changed files with 43 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/flight_plan.dtd
+18 -10
View File
@@ -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
+2
View File
@@ -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
+1
View File
@@ -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
+3
View File
@@ -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
+1
View File
@@ -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
+5
View File
@@ -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
+1
View File
@@ -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
+12
View File
@@ -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;