From 2ac79b3430c7b371ee7d4427102f6955d0924e7d Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 3 Oct 2022 18:01:55 +0200 Subject: [PATCH 1/4] [nav] add a guided instruction for the flight plan It allows (for rotorcraft) to use all the possibilities of the guided mode from a single instruction in the flight plan, but without leaving the NAV mode as it was done before. Thus all capabilities of the FP are still accessible. --- conf/flight_plans/flight_plan.dtd | 16 ++++-- conf/flight_plans/rotorcraft_guided_demo.xml | 52 +++++++++++++++++++ .../firmwares/rotorcraft/autopilot_guided.c | 6 ++- .../firmwares/rotorcraft/autopilot_guided.h | 8 +++ .../rotorcraft/guidance/guidance_h.c | 10 ++-- .../rotorcraft/guidance/guidance_v.c | 9 ++-- sw/airborne/firmwares/rotorcraft/navigation.h | 2 + sw/include/std.h | 4 ++ sw/tools/generators/gen_flight_plan.ml | 13 ++++- 9 files changed, 106 insertions(+), 14 deletions(-) create mode 100644 conf/flight_plans/rotorcraft_guided_demo.xml diff --git a/conf/flight_plans/flight_plan.dtd b/conf/flight_plans/flight_plan.dtd index 6b878ba19a..7aa9874b98 100644 --- a/conf/flight_plans/flight_plan.dtd +++ b/conf/flight_plans/flight_plan.dtd @@ -28,14 +28,14 @@ - + - - + + @@ -56,6 +56,7 @@ + + + diff --git a/conf/flight_plans/rotorcraft_guided_demo.xml b/conf/flight_plans/rotorcraft_guided_demo.xml new file mode 100644 index 0000000000..1d85c8cd6f --- /dev/null +++ b/conf/flight_plans/rotorcraft_guided_demo.xml @@ -0,0 +1,52 @@ + + + +
+ // Useful Combination of the flags fir the autopilot_guided_update + #define GUIDED_FLAG_XY_VEL_BODY GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL + #define GUIDED_FLAG_XY_VEL_BODY_YAW_RATE GUIDED_FLAG_XY_BODY|GUIDED_FLAG_XY_VEL|GUIDED_FLAG_YAW_RATE + #define GUIDED_FLAG_XY_OFFSET_Z_VEL GUIDED_FLAG_XY_OFFSET|GUIDED_FLAG_Z_VEL +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_guided.c b/sw/airborne/firmwares/rotorcraft/autopilot_guided.c index 0388b3f07e..7adfce5a38 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_guided.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot_guided.c @@ -95,8 +95,10 @@ bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading) */ void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw) { - /* only update setpoints when in guided mode */ - if (autopilot_get_mode() != AP_MODE_GUIDED) { + /* only update setpoints when in guided mode + * or in nav mode when using the 'guided' instruction of the fligh plan + */ + if (autopilot_get_mode() != AP_MODE_GUIDED && autopilot_get_mode() != AP_MODE_NAV) { return; } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_guided.h b/sw/airborne/firmwares/rotorcraft/autopilot_guided.h index 8cfc77cefb..5dc4520ecb 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_guided.h +++ b/sw/airborne/firmwares/rotorcraft/autopilot_guided.h @@ -103,5 +103,13 @@ extern void autopilot_guided_parse_GUIDED(uint8_t *buf); #define GUIDED_FLAG_Z_VEL (1<<6) #define GUIDED_FLAG_YAW_RATE (1<<7) +/** Macro for the flight plan insctructions + */ +#define NavGuided(_flags, _x, _y, _z, _yaw) { \ + horizontal_mode = HORIZONTAL_MODE_GUIDED; \ + vertical_mode = VERTICAL_MODE_GUIDED; \ + autopilot_guided_update(_flags, _x, _y, _z, _yaw); \ +} + #endif /* AUTOPILOT_GUIDED_H */ diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index d427e70098..0f5774d864 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -609,6 +609,8 @@ void guidance_h_from_nav(bool in_flight) //make sure the heading is right before leaving horizontal_mode attitude guidance_hybrid_reset_heading(&sp_cmd_i); #endif + } else if (horizontal_mode == HORIZONTAL_MODE_GUIDED) { + guidance_h_guided_run(in_flight); } else { #if HYBRID_NAVIGATION @@ -719,7 +721,7 @@ void guidance_h_guided_run(bool in_flight) bool guidance_h_set_guided_pos(float x, float y) { - if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) { + if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED || guidance_h.mode == GUIDANCE_H_MODE_NAV) { ClearBit(guidance_h.sp.mask, 5); guidance_h.sp.pos.x = POS_BFP_OF_REAL(x); guidance_h.sp.pos.y = POS_BFP_OF_REAL(y); @@ -730,7 +732,7 @@ bool guidance_h_set_guided_pos(float x, float y) bool guidance_h_set_guided_heading(float heading) { - if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) { + if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED || guidance_h.mode == GUIDANCE_H_MODE_NAV) { ClearBit(guidance_h.sp.mask, 7); guidance_h.sp.heading = heading; FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading); @@ -749,7 +751,7 @@ bool guidance_h_set_guided_body_vel(float vx, float vy) bool guidance_h_set_guided_vel(float vx, float vy) { - if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) { + if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED || guidance_h.mode == GUIDANCE_H_MODE_NAV) { SetBit(guidance_h.sp.mask, 5); guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx); guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy); @@ -760,7 +762,7 @@ bool guidance_h_set_guided_vel(float vx, float vy) bool guidance_h_set_guided_heading_rate(float rate) { - if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED) { + if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED || guidance_h.mode == GUIDANCE_H_MODE_NAV) { SetBit(guidance_h.sp.mask, 7); guidance_h.sp.heading_rate = rate; return true; diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 4b4c9980ea..72dd33ffd8 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -488,6 +488,8 @@ void guidance_v_from_nav(bool in_flight) GuidanceVSetRef(guidance_v_z_sp, guidance_v_zd_sp, 0); guidance_v_z_sum_err = 0; guidance_v_delta_t = nav_throttle; + } else if (vertical_mode == VERTICAL_MODE_GUIDED) { + guidance_v_guided_run(in_flight); } #if HYBRID_NAVIGATION guidance_hybrid_vertical(); @@ -550,7 +552,7 @@ void guidance_v_guided_run(bool in_flight) bool guidance_v_set_guided_z(float z) { - if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) { + if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED || guidance_v_mode == GUIDANCE_V_MODE_NAV) { /* disable vertical velocity setpoints */ guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD; @@ -567,10 +569,9 @@ bool guidance_v_set_guided_z(float z) bool guidance_v_set_guided_vz(float vz) { - if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) { + if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED || guidance_v_mode == GUIDANCE_V_MODE_NAV) { /* enable vertical velocity setpoints */ guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_CLIMB; - /* set speed setting */ guidance_v_zd_sp = SPEED_BFP_OF_REAL(vz); @@ -581,7 +582,7 @@ bool guidance_v_set_guided_vz(float vz) bool guidance_v_set_guided_th(float th) { - if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED) { + if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED || guidance_v_mode == GUIDANCE_V_MODE_NAV) { /* enable vertical velocity setpoints */ guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_THROTTLE; diff --git a/sw/airborne/firmwares/rotorcraft/navigation.h b/sw/airborne/firmwares/rotorcraft/navigation.h index 0ca5348424..46e9e59e67 100644 --- a/sw/airborne/firmwares/rotorcraft/navigation.h +++ b/sw/airborne/firmwares/rotorcraft/navigation.h @@ -62,6 +62,7 @@ extern int32_t nav_circle_radius, nav_circle_qdr, nav_circle_radians; #define HORIZONTAL_MODE_CIRCLE 2 #define HORIZONTAL_MODE_ATTITUDE 3 #define HORIZONTAL_MODE_MANUAL 4 +#define HORIZONTAL_MODE_GUIDED 5 extern int32_t nav_roll, nav_pitch; ///< with #INT32_ANGLE_FRAC extern int32_t nav_heading; ///< with #INT32_ANGLE_FRAC extern int32_t nav_cmd_roll, nav_cmd_pitch, nav_cmd_yaw; @@ -80,6 +81,7 @@ extern float flight_altitude; #define VERTICAL_MODE_MANUAL 0 #define VERTICAL_MODE_CLIMB 1 #define VERTICAL_MODE_ALT 2 +#define VERTICAL_MODE_GUIDED 3 extern float dist2_to_home; ///< squared distance to home waypoint extern bool too_far_from_home; diff --git a/sw/include/std.h b/sw/include/std.h index 6243cfd627..4f974568ac 100644 --- a/sw/include/std.h +++ b/sw/include/std.h @@ -30,6 +30,10 @@ #include #include +#ifdef SITL +#include // for debuging in simulation +#endif + /* some convenience macros to print debug/config messages at compile time */ #include "message_pragmas.h" diff --git a/sw/tools/generators/gen_flight_plan.ml b/sw/tools/generators/gen_flight_plan.ml index e3c2944bb3..0bea504e2a 100644 --- a/sw/tools/generators/gen_flight_plan.ml +++ b/sw/tools/generators/gen_flight_plan.ml @@ -336,7 +336,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" -> + | "heading" | "attitude" | "manual" | "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"-> @@ -552,6 +552,17 @@ let rec print_stage = fun out index_of_waypoints x -> stage_until out x; fp_post_call out x; lprintf out "break;\n" + | "guided" -> + stage out; + fp_pre_call out x; + let cmds = ExtXml.attrib x "commands" in + 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 + lprintf out "%sGuided(%s, %s%s);\n" t flags cmds p; + stage_until out x; + fp_post_call out x; + lprintf out "break;\n" | "eight" -> stage out; lprintf out "nav_eight_init();\n"; From 4b6b0acf87991f4e1b3a6c869362cb76e5a37a7f Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 3 Oct 2022 22:57:01 +0200 Subject: [PATCH 2/4] [module] add tag speed control --- conf/modules/tag_tracking.xml | 6 +++ .../modules/computer_vision/tag_tracking.c | 45 ++++++++++++++++++- .../modules/computer_vision/tag_tracking.h | 5 +++ 3 files changed, 54 insertions(+), 2 deletions(-) diff --git a/conf/modules/tag_tracking.xml b/conf/modules/tag_tracking.xml index 1c9f7172a0..c45b88d415 100644 --- a/conf/modules/tag_tracking.xml +++ b/conf/modules/tag_tracking.xml @@ -22,6 +22,10 @@ + + + + @@ -29,6 +33,8 @@ + + diff --git a/sw/airborne/modules/computer_vision/tag_tracking.c b/sw/airborne/modules/computer_vision/tag_tracking.c index 526982e95f..96cfeccfa1 100644 --- a/sw/airborne/modules/computer_vision/tag_tracking.c +++ b/sw/airborne/modules/computer_vision/tag_tracking.c @@ -127,13 +127,29 @@ float speed_circle = 0.03; #endif #ifndef TAG_TRACKING_PREDICT_TIME -#define TAG_TRACKING_PREDICT_TIME 1.5f +#define TAG_TRACKING_PREDICT_TIME 1.f #endif #ifndef TAG_TRACKING_MAX_OFFSET #define TAG_TRACKING_MAX_OFFSET 2.0f #endif +#ifndef TAG_TRACKING_KP +#define TAG_TRACKING_KP 0.5f +#endif + +#ifndef TAG_TRACKING_KPZ +#define TAG_TRACKING_KPZ 0.2f +#endif + +#ifndef TAG_TRACKING_MAX_SPEED +#define TAG_TRACKING_MAX_SPEED 4.f +#endif + +#ifndef TAG_TRACKING_MAX_VZ +#define TAG_TRACKING_MAX_VZ 2.f +#endif + // generated in modules.h static const float tag_track_dt = TAG_TRACKING_PROPAGATE_PERIOD; @@ -233,7 +249,7 @@ void tag_tracking_parse_target_pos(uint8_t *buf) } // Update and display tracking WP -static void update_wp(bool report) +static void update_wp(bool report UNUSED) { #ifdef TAG_TRACKING_WP struct FloatVect3 target_pos_enu, target_pos_pred; @@ -263,6 +279,7 @@ void tag_tracking_init() FLOAT_VECT3_ZERO(tag_track_private.meas); FLOAT_VECT3_ZERO(tag_tracking.pos); FLOAT_VECT3_ZERO(tag_tracking.speed); + FLOAT_VECT3_ZERO(tag_tracking.speed_cmd); struct FloatEulers euler = { TAG_TRACKING_BODY_TO_CAM_PHI, TAG_TRACKING_BODY_TO_CAM_THETA, @@ -273,6 +290,8 @@ void tag_tracking_init() TAG_TRACKING_CAM_POS_X, TAG_TRACKING_CAM_POS_Y, TAG_TRACKING_CAM_POS_Z); + tag_tracking.kp = TAG_TRACKING_KP; + tag_tracking.kpz = TAG_TRACKING_KPZ; // Bind to ABI message AbiBindMsgJEVOIS_MSG(TAG_TRACKING_ID, &tag_track_ev, tag_track_cb); @@ -365,6 +384,28 @@ void tag_tracking_report() } } +/** Control function + * + * calling this function only updates the command vector + * it can be applied to the guidance control using the guided mode + * or from the flight plan with 'guided' instruction + */ +void tag_tracking_compute_speed(void) +{ + if (tag_tracking.status == TAG_TRACKING_RUNNING) { + // compute speed command as estimated tag speed + gain * position error + struct NedCoor_f pos = *stateGetPositionNed_f(); + tag_tracking.speed_cmd.x = tag_tracking.speed.x + tag_tracking.kp * (tag_tracking.pos.x - pos.x); + tag_tracking.speed_cmd.y = tag_tracking.speed.y + tag_tracking.kp * (tag_tracking.pos.y - pos.y); + tag_tracking.speed_cmd.z = tag_tracking.speed.z + tag_tracking.kpz * (tag_tracking.pos.z - pos.z); + VECT2_STRIM(tag_tracking.speed_cmd, -TAG_TRACKING_MAX_SPEED, TAG_TRACKING_MAX_SPEED); // trim max horizontal speed + BoundAbs(tag_tracking.speed_cmd.z, TAG_TRACKING_MAX_VZ); // tim max vertical speed + } + else { + // filter is not runing, set speed command to zero + FLOAT_VECT3_ZERO(tag_tracking.speed_cmd); + } +} // Simulate detection using a WP coordinate #if defined SITL && defined TAG_TRACKING_SIM_WP diff --git a/sw/airborne/modules/computer_vision/tag_tracking.h b/sw/airborne/modules/computer_vision/tag_tracking.h index 2c895606cb..be0a80faca 100644 --- a/sw/airborne/modules/computer_vision/tag_tracking.h +++ b/sw/airborne/modules/computer_vision/tag_tracking.h @@ -31,6 +31,7 @@ #include "std.h" #include "math/pprz_algebra_float.h" +#include "math/pprz_geodetic_float.h" // Searching status #define TAG_TRACKING_SEARCHING 0 @@ -49,6 +50,9 @@ struct tag_tracking_public { uint8_t status; ///< tracking status flag uint8_t motion_type; ///< type of tag motion float predict_time; ///< prediction time for WP tag + struct NedCoor_f speed_cmd; ///< speed command to track the tag position + float kp; ///< horizontal tracking command gain + float kpz; ///< vertical tracking command gain }; extern struct tag_tracking_public tag_tracking; @@ -58,6 +62,7 @@ extern void tag_tracking_propagate(void); extern void tag_tracking_propagate_start(void); extern void tag_tracking_report(void); extern void tag_tracking_parse_target_pos(uint8_t *buf); +extern void tag_tracking_compute_speed(void); #endif // TAG_TRACKING_H From edac780492ebb23d61ae10b9c7925be7687b6d4e Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Tue, 4 Oct 2022 11:14:41 +0200 Subject: [PATCH 3/4] {conf] add demo flight plan [imu] connect aligner with 1efilter for imu --- conf/airframes/ENAC/rover_ostrich.xml | 13 +- .../ENAC/demo_aruco_drop_indoor.xml | 182 ++++++++++++++++++ 2 files changed, 190 insertions(+), 5 deletions(-) create mode 100644 conf/flight_plans/ENAC/demo_aruco_drop_indoor.xml diff --git a/conf/airframes/ENAC/rover_ostrich.xml b/conf/airframes/ENAC/rover_ostrich.xml index 15478f9895..78955ee99c 100644 --- a/conf/airframes/ENAC/rover_ostrich.xml +++ b/conf/airframes/ENAC/rover_ostrich.xml @@ -72,17 +72,20 @@
- + + - + + + +
@@ -135,7 +138,7 @@ - +
diff --git a/conf/flight_plans/ENAC/demo_aruco_drop_indoor.xml b/conf/flight_plans/ENAC/demo_aruco_drop_indoor.xml new file mode 100644 index 0000000000..2624b984e4 --- /dev/null +++ b/conf/flight_plans/ENAC/demo_aruco_drop_indoor.xml @@ -0,0 +1,182 @@ + + + +
+ #ifndef SwitchServoOn + #define SwitchServoOn(_x) {} + #endif + #ifndef SwitchServoOff + #define SwitchServoOff(_x) {} + #endif + #define DropOpen SwitchServoOff + #define DropClose SwitchServoOn + + #ifdef NAV_C + #ifndef TAG_TRACKING_COORD_TO_M + #define TAG_TRACKING_COORD_TO_M (1.f / 1000.f) + #endif + + static void fp_tag_cb(uint8_t sender_id UNUSED, + uint8_t type, char * id UNUSED, + uint8_t nb UNUSED, int16_t * coord, uint16_t * dim UNUSED, + struct FloatQuat quat UNUSED, char * extra UNUSED) + { + if (type == JEVOIS_MSG_D3) { + tag_distance = coord[2] * TAG_TRACKING_COORD_TO_M; + tag_valid = true; + } + } + #endif + + #define Neq(a,b) (a!=b) +
+ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 1fbbdf80bb9fe70d57ce3dc09cda14584955a47f Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Thu, 6 Oct 2022 14:23:41 +0200 Subject: [PATCH 4/4] [guidance] change guidance api names Guidance set functions are not limited to guidance mode. Use them when possible to set masks correctly. --- .../firmwares/rotorcraft/autopilot_guided.c | 41 ++++----- .../rotorcraft/guidance/guidance_h.c | 84 +++++++------------ .../rotorcraft/guidance/guidance_h.h | 25 +++--- .../rotorcraft/guidance/guidance_v.c | 62 +++++--------- .../rotorcraft/guidance/guidance_v.h | 15 ++-- .../orange_avoider/orange_avoider_guided.c | 14 ++-- sw/airborne/modules/wedgebug/wedgebug.c | 22 ++--- 7 files changed, 106 insertions(+), 157 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_guided.c b/sw/airborne/firmwares/rotorcraft/autopilot_guided.c index 7adfce5a38..b33fcd8458 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_guided.c +++ b/sw/airborne/firmwares/rotorcraft/autopilot_guided.c @@ -36,9 +36,9 @@ bool autopilot_guided_goto_ned(float x, float y, float z, float heading) { if (autopilot_get_mode() == AP_MODE_GUIDED) { - guidance_h_set_guided_pos(x, y); - guidance_h_set_guided_heading(heading); - guidance_v_set_guided_z(z); + guidance_h_set_pos(x, y); + guidance_h_set_heading(heading); + guidance_v_set_z(z); return true; } return false; @@ -72,9 +72,9 @@ bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dya bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading) { if (autopilot_get_mode() == AP_MODE_GUIDED) { - guidance_h_set_guided_vel(vx, vy); - guidance_h_set_guided_heading(heading); - guidance_v_set_guided_vz(vz); + guidance_h_set_vel(vx, vy); + guidance_h_set_heading(heading); + guidance_v_set_vz(vz); return true; } return false; @@ -95,24 +95,17 @@ bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading) */ void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw) { - /* only update setpoints when in guided mode - * or in nav mode when using the 'guided' instruction of the fligh plan - */ - if (autopilot_get_mode() != AP_MODE_GUIDED && autopilot_get_mode() != AP_MODE_NAV) { - return; - } - // handle x,y struct FloatVect2 setpoint = {.x = x, .y = y}; if (bit_is_set(flags, 5)) { // velocity setpoint if (bit_is_set(flags, 1)) { // set velocity in body frame - guidance_h_set_guided_body_vel(setpoint.x, setpoint.y); + guidance_h_set_body_vel(setpoint.x, setpoint.y); } else { - guidance_h_set_guided_vel(setpoint.x, setpoint.y); + guidance_h_set_vel(setpoint.x, setpoint.y); } } else { // position setpoint if (!bit_is_set(flags, 0) && !bit_is_set(flags, 1)) { // set absolute position setpoint - guidance_h_set_guided_pos(setpoint.x, setpoint.y); + guidance_h_set_pos(setpoint.x, setpoint.y); } else { if (stateIsLocalCoordinateValid()) { if (bit_is_set(flags, 1)) { // set position as offset in body frame @@ -124,40 +117,42 @@ void autopilot_guided_update(uint8_t flags, float x, float y, float z, float yaw setpoint.x += stateGetPositionNed_f()->x; setpoint.y += stateGetPositionNed_f()->y; } - guidance_h_set_guided_pos(setpoint.x, setpoint.y); + guidance_h_set_pos(setpoint.x, setpoint.y); } } } //handle z if (bit_is_set(flags, 6)) { // speed set-point - guidance_v_set_guided_vz(z); + guidance_v_set_vz(z); } else { // position set-point if (bit_is_set(flags, 2)) { // set position as offset in NED frame if (stateIsLocalCoordinateValid()) { z += stateGetPositionNed_f()->z; - guidance_v_set_guided_z(z); + guidance_v_set_z(z); } } else { - guidance_v_set_guided_z(z); + guidance_v_set_z(z); } } //handle yaw if (bit_is_set(flags, 7)) { // speed set-point - guidance_h_set_guided_heading_rate(yaw); + guidance_h_set_heading_rate(yaw); } else { // position set-point if (bit_is_set(flags, 3)) { // set yaw as offset yaw += stateGetNedToBodyEulers_f()->psi; // will be wrapped to [-pi,pi] later } - guidance_h_set_guided_heading(yaw); + guidance_h_set_heading(yaw); } } /** Parse GUIDED_SETPOINT_NED messages from datalink */ void autopilot_guided_parse_GUIDED(uint8_t *buf) { - if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID) { return; } + if (DL_GUIDED_SETPOINT_NED_ac_id(buf) != AC_ID || autopilot_get_mode() != AP_MODE_GUIDED) { + return; + } autopilot_guided_update( DL_GUIDED_SETPOINT_NED_flags(buf), diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c index 0f5774d864..d08749d149 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c @@ -556,35 +556,28 @@ void guidance_h_hover_enter(void) guidance_h.sp.speed.x = 0; guidance_h.sp.speed.y = 0; - /* disable horizontal velocity setpoints, - * might still be activated in guidance_h_read_rc if GUIDANCE_H_USE_SPEED_REF - */ - ClearBit(guidance_h.sp.mask, 5); - ClearBit(guidance_h.sp.mask, 7); - /* set horizontal setpoint to current position */ - VECT2_COPY(guidance_h.sp.pos, *stateGetPositionNed_i()); - + guidance_h_set_pos( + stateGetPositionNed_f()->x, + stateGetPositionNed_f()->y); /* reset guidance reference */ reset_guidance_reference_from_current_position(); /* set guidance to current heading and position */ guidance_h.rc_sp.psi = stateGetNedToBodyEulers_f()->psi; - guidance_h.sp.heading = guidance_h.rc_sp.psi; + guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi); } void guidance_h_nav_enter(void) { - ClearBit(guidance_h.sp.mask, 5); - ClearBit(guidance_h.sp.mask, 7); - /* horizontal position setpoint from navigation/flightplan */ - INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot); - + guidance_h_set_pos( + POS_FLOAT_OF_BFP(navigation_carrot.y), + POS_FLOAT_OF_BFP(navigation_carrot.x)); reset_guidance_reference_from_current_position(); - + /* set nav_heading to current heading */ nav_heading = stateGetNedToBodyEulers_i()->psi; - guidance_h.sp.heading = stateGetNedToBodyEulers_f()->psi; + guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi); } void guidance_h_from_nav(bool in_flight) @@ -617,14 +610,15 @@ void guidance_h_from_nav(bool in_flight) INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_target); guidance_hybrid_run(); #else - INT32_VECT2_NED_OF_ENU(guidance_h.sp.pos, navigation_carrot); - + // set guidance in NED + guidance_h_set_pos( + POS_FLOAT_OF_BFP(navigation_carrot.y), + POS_FLOAT_OF_BFP(navigation_carrot.x)); guidance_h_update_reference(); #if GUIDANCE_HEADING_IS_FREE /* set psi command */ - guidance_h.sp.heading = ANGLE_FLOAT_OF_BFP(nav_heading); - FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading); + guidance_h_set_heading(ANGLE_FLOAT_OF_BFP(nav_heading)); #endif #if GUIDANCE_INDI @@ -719,55 +713,39 @@ void guidance_h_guided_run(bool in_flight) stabilization_attitude_run(in_flight); } -bool guidance_h_set_guided_pos(float x, float y) +void guidance_h_set_pos(float x, float y) { - if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED || guidance_h.mode == GUIDANCE_H_MODE_NAV) { - ClearBit(guidance_h.sp.mask, 5); - guidance_h.sp.pos.x = POS_BFP_OF_REAL(x); - guidance_h.sp.pos.y = POS_BFP_OF_REAL(y); - return true; - } - return false; + ClearBit(guidance_h.sp.mask, 5); + guidance_h.sp.pos.x = POS_BFP_OF_REAL(x); + guidance_h.sp.pos.y = POS_BFP_OF_REAL(y); } -bool guidance_h_set_guided_heading(float heading) +void guidance_h_set_heading(float heading) { - if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED || guidance_h.mode == GUIDANCE_H_MODE_NAV) { - ClearBit(guidance_h.sp.mask, 7); - guidance_h.sp.heading = heading; - FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading); - return true; - } - return false; + ClearBit(guidance_h.sp.mask, 7); + guidance_h.sp.heading = heading; + FLOAT_ANGLE_NORMALIZE(guidance_h.sp.heading); } -bool guidance_h_set_guided_body_vel(float vx, float vy) +void guidance_h_set_body_vel(float vx, float vy) { float psi = stateGetNedToBodyEulers_f()->psi; float newvx = cosf(-psi) * vx + sinf(-psi) * vy; float newvy = -sinf(-psi) * vx + cosf(-psi) * vy; - return guidance_h_set_guided_vel(newvx, newvy); + guidance_h_set_vel(newvx, newvy); } -bool guidance_h_set_guided_vel(float vx, float vy) +void guidance_h_set_vel(float vx, float vy) { - if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED || guidance_h.mode == GUIDANCE_H_MODE_NAV) { - SetBit(guidance_h.sp.mask, 5); - guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx); - guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy); - return true; - } - return false; + SetBit(guidance_h.sp.mask, 5); + guidance_h.sp.speed.x = SPEED_BFP_OF_REAL(vx); + guidance_h.sp.speed.y = SPEED_BFP_OF_REAL(vy); } -bool guidance_h_set_guided_heading_rate(float rate) +void guidance_h_set_heading_rate(float rate) { - if (guidance_h.mode == GUIDANCE_H_MODE_GUIDED || guidance_h.mode == GUIDANCE_H_MODE_NAV) { - SetBit(guidance_h.sp.mask, 7); - guidance_h.sp.heading_rate = rate; - return true; - } - return false; + SetBit(guidance_h.sp.mask, 7); + guidance_h.sp.heading_rate = rate; } const struct Int32Vect2 *guidance_h_get_pos_err(void) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h index e2fad414b2..2567fa6e09 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h @@ -128,38 +128,33 @@ extern void guidance_h_set_igain(uint32_t igain); */ extern void guidance_h_guided_run(bool in_flight); -/** Set horizontal position setpoint in GUIDED mode. +/** Set horizontal position setpoint. * @param x North position (local NED frame) in meters. * @param y East position (local NED frame) in meters. - * @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED) */ -extern bool guidance_h_set_guided_pos(float x, float y); +extern void guidance_h_set_pos(float x, float y); -/** Set heading setpoint in GUIDED mode. +/** Set heading setpoint. * @param heading Setpoint in radians. - * @return TRUE if setpoint was set (currently in GUIDANCE_H_MODE_GUIDED) */ -extern bool guidance_h_set_guided_heading(float heading); +extern void guidance_h_set_heading(float heading); -/** Set body relative horizontal velocity setpoint in GUIDED mode. +/** Set body relative horizontal velocity setpoint. * @param vx forward velocity (body frame) in meters/sec. * @param vy right velocity (body frame) in meters/sec. - * @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED) */ -extern bool guidance_h_set_guided_body_vel(float vx, float vy); +extern void guidance_h_set_body_vel(float vx, float vy); -/** Set horizontal velocity setpoint in GUIDED mode. +/** Set horizontal velocity setpoint. * @param vx North velocity (local NED frame) in meters/sec. * @param vy East velocity (local NED frame) in meters/sec. - * @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED) */ -extern bool guidance_h_set_guided_vel(float vx, float vy); +extern void guidance_h_set_vel(float vx, float vy); -/** Set heading rate setpoint in GUIDED mode. +/** Set heading rate setpoint. * @param rate Heading rate in radians. - * @return TRUE if setpoints were set (currently in GUIDANCE_H_MODE_GUIDED) */ -extern bool guidance_h_set_guided_heading_rate(float rate); +extern void guidance_h_set_heading_rate(float rate); /** Gets the position error * @param none. diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index 72dd33ffd8..eb5cf88cff 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -506,12 +506,8 @@ void guidance_v_from_nav(bool in_flight) void guidance_v_guided_enter(void) { - /* disable vertical velocity setpoints */ - guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD; - - /* set current altitude as setpoint and reset speed setpoint */ - guidance_v_z_sp = stateGetPositionNed_i()->z; - guidance_v_zd_sp = 0; + /* set current altitude as setpoint */ + guidance_v_set_z(stateGetPositionNed_f()->z); /* reset guidance reference */ guidance_v_z_sum_err = 0; @@ -550,49 +546,33 @@ void guidance_v_guided_run(bool in_flight) stabilization_cmd[COMMAND_THRUST] = guidance_v_delta_t; } -bool guidance_v_set_guided_z(float z) +void guidance_v_set_z(float z) { - if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED || guidance_v_mode == GUIDANCE_V_MODE_NAV) { - /* disable vertical velocity setpoints */ - guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD; - - /* set altitude setpoint */ - guidance_v_z_sp = POS_BFP_OF_REAL(z); - - /* reset speed setting */ - guidance_v_zd_sp = 0; - - return true; - } - return false; + /* disable vertical velocity setpoints */ + guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_ZHOLD; + /* set altitude setpoint */ + guidance_v_z_sp = POS_BFP_OF_REAL(z); + /* reset speed setting */ + guidance_v_zd_sp = 0; } -bool guidance_v_set_guided_vz(float vz) +void guidance_v_set_vz(float vz) { - if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED || guidance_v_mode == GUIDANCE_V_MODE_NAV) { - /* enable vertical velocity setpoints */ - guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_CLIMB; - /* set speed setting */ - guidance_v_zd_sp = SPEED_BFP_OF_REAL(vz); - - return true; - } - return false; + /* enable vertical velocity setpoints */ + guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_CLIMB; + /* set speed setting */ + guidance_v_zd_sp = SPEED_BFP_OF_REAL(vz); } -bool guidance_v_set_guided_th(float th) +void guidance_v_set_th(float th) { - if (guidance_v_mode == GUIDANCE_V_MODE_GUIDED || guidance_v_mode == GUIDANCE_V_MODE_NAV) { - /* enable vertical velocity setpoints */ - guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_THROTTLE; + /* enable vertical velocity setpoints */ + guidance_v_guided_mode = GUIDANCE_V_GUIDED_MODE_THROTTLE; - /* reset guidance reference */ - GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0); - guidance_v_th_sp = (int32_t)(MAX_PPRZ * th); - Bound(guidance_v_th_sp, 0, MAX_PPRZ); - return true; - } - return false; + /* reset guidance reference */ + GuidanceVSetRef(stateGetPositionNed_i()->z, stateGetSpeedNed_i()->z, 0); + guidance_v_th_sp = (int32_t)(MAX_PPRZ * th); + Bound(guidance_v_th_sp, 0, MAX_PPRZ); } diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h index 965e9c2449..18f4bb15cc 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.h @@ -133,19 +133,20 @@ extern void guidance_v_guided_enter(void); */ extern void guidance_v_guided_run(bool in_flight); -/** Set z setpoint in GUIDED mode. +/** Set z position setpoint. * @param z Setpoint (down is positive) in meters. - * @return TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED) */ -extern bool guidance_v_set_guided_z(float z); +extern void guidance_v_set_z(float z); -/** Set z velocity setpoint in GUIDED mode. +/** Set z velocity setpoint. * @param vz Setpoint (down is positive) in meters/second. - * @return TRUE if setpoint was set (currently in GUIDANCE_V_MODE_GUIDED) */ -extern bool guidance_v_set_guided_vz(float vz); +extern void guidance_v_set_vz(float vz); -extern bool guidance_v_set_guided_th(float th); +/** Set throttle setpoint. + * @param th Throttle setpoint between 0. and 1. + */ +extern void guidance_v_set_th(float th); #define guidance_v_SetKi(_val) { \ guidance_v_ki = _val; \ diff --git a/sw/airborne/modules/orange_avoider/orange_avoider_guided.c b/sw/airborne/modules/orange_avoider/orange_avoider_guided.c index b4db1657a5..7f867fa2cb 100644 --- a/sw/airborne/modules/orange_avoider/orange_avoider_guided.c +++ b/sw/airborne/modules/orange_avoider/orange_avoider_guided.c @@ -150,13 +150,13 @@ void orange_avoider_guided_periodic(void) } else if (obstacle_free_confidence == 0){ navigation_state = OBSTACLE_FOUND; } else { - guidance_h_set_guided_body_vel(speed_sp, 0); + guidance_h_set_body_vel(speed_sp, 0); } break; case OBSTACLE_FOUND: // stop - guidance_h_set_guided_body_vel(0, 0); + guidance_h_set_body_vel(0, 0); // randomly select new search direction chooseRandomIncrementAvoidance(); @@ -165,20 +165,20 @@ void orange_avoider_guided_periodic(void) break; case SEARCH_FOR_SAFE_HEADING: - guidance_h_set_guided_heading_rate(avoidance_heading_direction * oag_heading_rate); + guidance_h_set_heading_rate(avoidance_heading_direction * oag_heading_rate); // make sure we have a couple of good readings before declaring the way safe if (obstacle_free_confidence >= 2){ - guidance_h_set_guided_heading(stateGetNedToBodyEulers_f()->psi); + guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi); navigation_state = SAFE; } break; case OUT_OF_BOUNDS: // stop - guidance_h_set_guided_body_vel(0, 0); + guidance_h_set_body_vel(0, 0); // start turn back into arena - guidance_h_set_guided_heading_rate(avoidance_heading_direction * RadOfDeg(15)); + guidance_h_set_heading_rate(avoidance_heading_direction * RadOfDeg(15)); navigation_state = REENTER_ARENA; @@ -187,7 +187,7 @@ void orange_avoider_guided_periodic(void) // force floor center to opposite side of turn to head back into arena if (floor_count >= floor_count_threshold && avoidance_heading_direction * floor_centroid_frac >= 0.f){ // return to heading mode - guidance_h_set_guided_heading(stateGetNedToBodyEulers_f()->psi); + guidance_h_set_heading(stateGetNedToBodyEulers_f()->psi); // reset safe counter obstacle_free_confidence = 0; diff --git a/sw/airborne/modules/wedgebug/wedgebug.c b/sw/airborne/modules/wedgebug/wedgebug.c index 1e50f7fc3e..9c531ac99c 100644 --- a/sw/airborne/modules/wedgebug/wedgebug.c +++ b/sw/airborne/modules/wedgebug/wedgebug.c @@ -50,7 +50,7 @@ #include "math/pprz_algebra.h"// Needed for vector operations (simple subtraction) #include "math/pprz_geodetic_float.h"// Needed for NedCoor_f #include "generated/flight_plan.h" // Needed for WP (waypoint) functions and WP IDs (as defined in "ralphthesis2020_stereo_cyberzoo.xml") -#include "firmwares/rotorcraft/autopilot_guided.h" // Needed for guidance functions such as "autopilot_guided_goto_ned" and "guidance_h_set_guided_heading" +#include "firmwares/rotorcraft/autopilot_guided.h" // Needed for guidance functions such as "autopilot_guided_goto_ned" and "guidance_h_set_heading" #include // needed for basic math functions #include "autopilot.h" // Needed to set states (GUIDED vs NAV) #include // Needed to measure time @@ -2357,7 +2357,7 @@ void wedgebug_periodic() // Else, adjust heading and check confidence that heading is reached else { // This happens continuously, as long as the state is active. It stops when the flag has been set below // Set desired heading - guidance_h_set_guided_heading(heading_towards_setpoint_WNED( + guidance_h_set_heading(heading_towards_setpoint_WNED( &VPBESTEDGECOORDINATESwned));// heading_towards_waypoint(WP_GOAL)); // If heading appears to be reached increase confidence @@ -2388,8 +2388,8 @@ void wedgebug_periodic() else { // This happens continuously, as long as the state is active. It stops when the flag has been set below. // Sets setpoint to edge position and orientates robot towards it as well - guidance_h_set_guided_pos(VPBESTEDGECOORDINATESwned.x, VPBESTEDGECOORDINATESwned.y); - guidance_v_set_guided_z(VPBESTEDGECOORDINATESwned.z); + guidance_h_set_pos(VPBESTEDGECOORDINATESwned.x, VPBESTEDGECOORDINATESwned.y); + guidance_v_set_z(VPBESTEDGECOORDINATESwned.z); // If edge appears to be reached increase confidence if (is_setpoint_reached(&VPBESTEDGECOORDINATESwned, &VRwned, @@ -2425,19 +2425,19 @@ void wedgebug_periodic() printf("POSITION_EDGE = %d\n", POSITION_EDGE); // This ensures that the robot stay on the edge at all times - guidance_h_set_guided_pos(VPBESTEDGECOORDINATESwned.x, VPBESTEDGECOORDINATESwned.y); - guidance_v_set_guided_z(VPBESTEDGECOORDINATESwned.z); + guidance_h_set_pos(VPBESTEDGECOORDINATESwned.x, VPBESTEDGECOORDINATESwned.y); + guidance_v_set_z(VPBESTEDGECOORDINATESwned.z); // 1. Orientates robot towards the final goal // If robot faces goal, robot stays on current edge if (is_heading_reached_flag) { printf("Heading is reached\n"); - guidance_h_set_guided_heading(heading_towards_waypoint(WP_GOAL)); + guidance_h_set_heading(heading_towards_waypoint(WP_GOAL)); } // Else, adjust heading and check confidence that heading is reached else { // This happens continuously, as long as the state is active. It stops when the flag has been set. // Set desired heading - guidance_h_set_guided_heading(heading_towards_waypoint(WP_GOAL)); + guidance_h_set_heading(heading_towards_waypoint(WP_GOAL)); // If heading appears to be reached increase confidence if (is_heading_reached(heading_towards_waypoint(WP_GOAL), heading, threshold_distance_to_angle)) { @@ -2523,7 +2523,7 @@ void wedgebug_periodic() // 1. The robot is tasked to stay at the holding point for the entire duration of this state // Making drone hover, so that it does not drift from its current position - guidance_h_set_guided_pos(VHOLDINGPOINTwned.x, VHOLDINGPOINTwned.y); guidance_v_set_guided_z(VHOLDINGPOINTwned.z); + guidance_h_set_pos(VHOLDINGPOINTwned.x, VHOLDINGPOINTwned.y); guidance_v_set_z(VHOLDINGPOINTwned.z); // 2. Checking if edges are located // Running function to detect and save edge @@ -2594,7 +2594,7 @@ void wedgebug_periodic() // Else, adjust angle to to face more left and check confidence that left heading has been reached else { // This happens continuously, as long as the state is active. It stops when the flag has been set below // Set heading to maximum left heading - guidance_h_set_guided_heading(initial_heading.heading_max_left); + guidance_h_set_heading(initial_heading.heading_max_left); // If heading appears to be reached increase confidence if (is_heading_reached(initial_heading.heading_max_left, heading, threshold_distance_to_angle)) { @@ -2620,7 +2620,7 @@ void wedgebug_periodic() // Else, adjust angle to to face more right and check confidence that right heading has been reached else { // Set heading to maximum left heading - guidance_h_set_guided_heading(initial_heading.heading_max_right); + guidance_h_set_heading(initial_heading.heading_max_right); // If heading appears to be reached increase confidence if (is_heading_reached(initial_heading.heading_max_right, heading, threshold_distance_to_angle)) {