Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

[nav] remove nav_manual mode (#3234)

Browse Source 
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.
This commit is contained in:
Gautier Hattenberger
2024-02-05 09:40:09 +01:00
committed by GitHub
parent fe35af7382
commit bbcb166d37
9 changed files with 36 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/flight_plans/flight_plan.dtd
+3 -19
View File
@@ -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
sw/airborne/firmwares/fixedwing/nav.h
-2
View File
@@ -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; }
sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+2 -10
View File
@@ -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
sw/airborne/firmwares/rotorcraft/navigation.c
-22
View File
@@ -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)
{
sw/airborne/firmwares/rotorcraft/navigation.h
+1 -6
View File
@@ -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
sw/airborne/firmwares/rover/guidance/rover_guidance.c
+1 -5
View File
@@ -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;
sw/airborne/firmwares/rover/navigation.h
-2
View File
@@ -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
sw/airborne/modules/nav/nav_heli_spinup.c
+24 -9
View File
@@ -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;
}
sw/tools/generators/gen_flight_plan.ml
+5 -18
View File
@@ -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;