From 2e4d4f56d95690c208365d607351b242fa023247 Mon Sep 17 00:00:00 2001 From: EyesWider Date: Tue, 4 Jun 2024 14:24:42 -0400 Subject: [PATCH 1/3] Added re-initialization for guided_change_x commands --- ArduPlane/GCS_Mavlink.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 8ffa3f82972e8..3d847e0e553fc 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -859,6 +859,13 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com plane.mode_guided.set_radius_and_direction(packet.param3, requested_position.loiter_ccw); } + // Need to re-initialize these, otherwise DO_REPOSITION will be overruled by GUIDED_CHANGE_HEADING and GUIDED_CHANGE_ALTITUDE if previously sent + plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; + plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. + plane.guided_state.target_alt_time_ms = 0; + plane.guided_state.last_target_alt = 0; + return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; @@ -1101,6 +1108,10 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_comma } if (plane.do_change_speed(packet.param1, packet.param2, packet.param3)) { + + // Need to re-initialize this, otherwise DO_CHANGE_SPEED will be overrided by any prior GUIDED_CHANGE_SPEED command while in GUIDED + plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. + return MAV_RESULT_ACCEPTED; } return MAV_RESULT_FAILED; From ec0fba77b74981dcd73a641331bff846ace22aae Mon Sep 17 00:00:00 2001 From: EyesWider Date: Wed, 5 Jun 2024 15:05:37 -0400 Subject: [PATCH 2/3] Moving guided resets into common methods --- ArduPlane/GCS_Mavlink.cpp | 9 +++------ ArduPlane/Plane.h | 5 +++++ ArduPlane/commands_logic.cpp | 23 +++++++++++++++++++++++ ArduPlane/mode.cpp | 11 +++++------ ArduPlane/mode.h | 4 ++++ 5 files changed, 40 insertions(+), 12 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 3d847e0e553fc..911dbad960255 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -860,11 +860,8 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com } // Need to re-initialize these, otherwise DO_REPOSITION will be overruled by GUIDED_CHANGE_HEADING and GUIDED_CHANGE_ALTITUDE if previously sent - plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range - plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; - plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. - plane.guided_state.target_alt_time_ms = 0; - plane.guided_state.last_target_alt = 0; + plane.reset_guided_hdg(); + plane.reset_guided_alt(); return MAV_RESULT_ACCEPTED; } @@ -1110,7 +1107,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_comma if (plane.do_change_speed(packet.param1, packet.param2, packet.param3)) { // Need to re-initialize this, otherwise DO_CHANGE_SPEED will be overrided by any prior GUIDED_CHANGE_SPEED command while in GUIDED - plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. + plane.reset_guided_spd(); return MAV_RESULT_ACCEPTED; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 613cc25a9b248..6241a1c1e9824 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -954,6 +954,11 @@ class Plane : public AP_Vehicle { bool start_command_callback(const AP_Mission::Mission_Command &cmd); bool verify_command_callback(const AP_Mission::Mission_Command& cmd); float get_wp_radius() const; +#if OFFBOARD_GUIDED == ENABLED + void reset_guided_hdg(); + void reset_guided_alt(); + void reset_guided_spd(); +#endif bool is_land_command(uint16_t cmd) const; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 728ff97ad9af4..7e15855c21612 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1306,3 +1306,26 @@ bool Plane::in_auto_mission_id(uint16_t command) const { return control_mode == &mode_auto && mission.get_current_nav_id() == command; } + + +// Clear guided_state Heading values +void Plane::reset_guided_hdg() +{ + plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; +} + +// Clear guided_state altitude values +void Plane::reset_guided_alt() +{ + plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. + plane.guided_state.target_alt_time_ms = 0; + plane.guided_state.last_target_alt = 0; +} + +// Clear guided_state speed value +void Plane::reset_guided_spd() +{ + plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. +} + diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 0a4415ff8b65a..55578c1929240 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -55,12 +55,11 @@ bool Mode::enter() plane.guided_state.last_forced_throttle_ms = 0; #if OFFBOARD_GUIDED == ENABLED - plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range - plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; - plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. - plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. - plane.guided_state.target_alt_time_ms = 0; - plane.guided_state.last_target_alt = 0; + + plane.reset_guided_hdg(); + plane.reset_guided_alt(); + plane.reset_guided_spd(); + #endif #if AP_CAMERA_ENABLED diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index cfe306813b31c..8498f37788d83 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -141,6 +141,10 @@ class Mode // true if voltage correction should be applied to throttle virtual bool use_battery_compensation() const; + void reset_guided_hdg(); + void reset_guided_alt(); + void reset_guided_spd(); + protected: // subclasses override this to perform checks before entering the mode From 93289b09a7192360a28c5fb180d33f8c2b613087 Mon Sep 17 00:00:00 2001 From: EyesWider Date: Wed, 5 Jun 2024 17:28:59 -0400 Subject: [PATCH 3/3] Moved methods to mode_guided --- ArduPlane/GCS_Mavlink.cpp | 6 +++--- ArduPlane/Plane.h | 5 ----- ArduPlane/commands_logic.cpp | 22 ---------------------- ArduPlane/mode.cpp | 6 +++--- ArduPlane/mode.h | 7 +++++++ ArduPlane/mode_guided.cpp | 21 +++++++++++++++++++++ 6 files changed, 34 insertions(+), 33 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 911dbad960255..9815daec7c09f 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -860,8 +860,8 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_int_do_reposition(const mavlink_com } // Need to re-initialize these, otherwise DO_REPOSITION will be overruled by GUIDED_CHANGE_HEADING and GUIDED_CHANGE_ALTITUDE if previously sent - plane.reset_guided_hdg(); - plane.reset_guided_alt(); + plane.mode_guided.reset_guided_hdg(); + plane.mode_guided.reset_guided_alt(); return MAV_RESULT_ACCEPTED; } @@ -1107,7 +1107,7 @@ MAV_RESULT GCS_MAVLINK_Plane::handle_command_DO_CHANGE_SPEED(const mavlink_comma if (plane.do_change_speed(packet.param1, packet.param2, packet.param3)) { // Need to re-initialize this, otherwise DO_CHANGE_SPEED will be overrided by any prior GUIDED_CHANGE_SPEED command while in GUIDED - plane.reset_guided_spd(); + plane.mode_guided.reset_guided_spd(); return MAV_RESULT_ACCEPTED; } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 6241a1c1e9824..613cc25a9b248 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -954,11 +954,6 @@ class Plane : public AP_Vehicle { bool start_command_callback(const AP_Mission::Mission_Command &cmd); bool verify_command_callback(const AP_Mission::Mission_Command& cmd); float get_wp_radius() const; -#if OFFBOARD_GUIDED == ENABLED - void reset_guided_hdg(); - void reset_guided_alt(); - void reset_guided_spd(); -#endif bool is_land_command(uint16_t cmd) const; diff --git a/ArduPlane/commands_logic.cpp b/ArduPlane/commands_logic.cpp index 7e15855c21612..54993f9f32943 100644 --- a/ArduPlane/commands_logic.cpp +++ b/ArduPlane/commands_logic.cpp @@ -1307,25 +1307,3 @@ bool Plane::in_auto_mission_id(uint16_t command) const return control_mode == &mode_auto && mission.get_current_nav_id() == command; } - -// Clear guided_state Heading values -void Plane::reset_guided_hdg() -{ - plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range - plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; -} - -// Clear guided_state altitude values -void Plane::reset_guided_alt() -{ - plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. - plane.guided_state.target_alt_time_ms = 0; - plane.guided_state.last_target_alt = 0; -} - -// Clear guided_state speed value -void Plane::reset_guided_spd() -{ - plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. -} - diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index 55578c1929240..63073c5b9236e 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -56,9 +56,9 @@ bool Mode::enter() #if OFFBOARD_GUIDED == ENABLED - plane.reset_guided_hdg(); - plane.reset_guided_alt(); - plane.reset_guided_spd(); + plane.mode_guided.reset_guided_hdg(); + plane.mode_guided.reset_guided_alt(); + plane.mode_guided.reset_guided_spd(); #endif diff --git a/ArduPlane/mode.h b/ArduPlane/mode.h index 8498f37788d83..81d39123280cd 100644 --- a/ArduPlane/mode.h +++ b/ArduPlane/mode.h @@ -9,6 +9,7 @@ #include "quadplane.h" #include #include +#include "config.h" class AC_PosControl; class AC_AttitudeControl_Multi; @@ -312,6 +313,12 @@ class ModeGuided : public Mode void update_target_altitude() override; +#if OFFBOARD_GUIDED == ENABLED + void reset_guided_hdg(); + void reset_guided_alt(); + void reset_guided_spd(); +#endif + protected: bool _enter() override; diff --git a/ArduPlane/mode_guided.cpp b/ArduPlane/mode_guided.cpp index 86a3bf60c1aee..b846c6725a0d7 100644 --- a/ArduPlane/mode_guided.cpp +++ b/ArduPlane/mode_guided.cpp @@ -153,3 +153,24 @@ void ModeGuided::update_target_altitude() Mode::update_target_altitude(); } } + +// Clear guided_state Heading values +void ModeGuided::reset_guided_hdg() +{ + plane.guided_state.target_heading = -4; // radians here are in range -3.14 to 3.14, so a default value needs to be outside that range + plane.guided_state.target_heading_type = GUIDED_HEADING_NONE; +} + +// Clear guided_state altitude values +void ModeGuided::reset_guided_alt() +{ + plane.guided_state.target_alt = -1; // same as above, although a target alt of -1 is rare on plane. + plane.guided_state.target_alt_time_ms = 0; + plane.guided_state.last_target_alt = 0; +} + +// Clear guided_state speed value +void ModeGuided::reset_guided_spd() +{ + plane.guided_state.target_airspeed_cm = -1; // same as above, although an airspeed of -1 is rare on plane. +} \ No newline at end of file