From 721c200bccef749db4264db7bc52553521dfd213 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 8 Mar 2019 14:41:50 +0900 Subject: [PATCH] Rover: support guided-within-auto also adds support for sending position targets to offboard controller which is assumed to return velocity commands --- APMrover2/GCS_Mavlink.cpp | 10 ++--- APMrover2/commands_logic.cpp | 23 +++++++++++ APMrover2/mode.h | 24 +++++++++++- APMrover2/mode_auto.cpp | 75 ++++++++++++++++++++++++++++++++++-- APMrover2/mode_guided.cpp | 18 +++++++++ 5 files changed, 140 insertions(+), 10 deletions(-) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index 62861c4566..d8cfb44b93 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -525,7 +525,7 @@ bool GCS_MAVLINK_Rover::in_hil_mode() const bool GCS_MAVLINK_Rover::handle_guided_request(AP_Mission::Mission_Command &cmd) { - if (rover.control_mode != &rover.mode_guided) { + if (!rover.control_mode->in_guided_mode()) { // only accept position updates when in GUIDED mode return false; } @@ -666,7 +666,7 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l // param2 : Speed - normalized to 0 .. 1 // exit if vehicle is not in Guided mode - if (rover.control_mode != &rover.mode_guided) { + if (!rover.control_mode->in_guided_mode()) { return MAV_RESULT_FAILED; } @@ -750,7 +750,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) mavlink_msg_set_attitude_target_decode(msg, &packet); // exit if vehicle is not in Guided mode - if (rover.control_mode != &rover.mode_guided) { + if (!rover.control_mode->in_guided_mode()) { break; } @@ -782,7 +782,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_local_ned_decode(msg, &packet); // exit if vehicle is not in Guided mode - if (rover.control_mode != &rover.mode_guided) { + if (!rover.control_mode->in_guided_mode()) { break; } @@ -899,7 +899,7 @@ void GCS_MAVLINK_Rover::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_global_int_decode(msg, &packet); // exit if vehicle is not in Guided mode - if (rover.control_mode != &rover.mode_guided) { + if (!rover.control_mode->in_guided_mode()) { break; } // check for supported coordinate frames diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index e00d31ca82..cc440a853e 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -37,6 +37,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) do_nav_wp(cmd, true); break; + case MAV_CMD_NAV_GUIDED_ENABLE: // accept navigation commands from external nav computer + do_nav_guided_enable(cmd); + break; + case MAV_CMD_NAV_SET_YAW_SPEED: do_nav_set_yaw_speed(cmd); break; @@ -158,6 +162,9 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_LOITER_TIME: return verify_loiter_time(cmd); + case MAV_CMD_NAV_GUIDED_ENABLE: + return verify_nav_guided_enable(cmd); + case MAV_CMD_CONDITION_DELAY: return verify_wait_delay(); @@ -219,6 +226,16 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_sto set_desired_location(cmdloc, next_leg_bearing_cd); } +void ModeAuto::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) +{ + if (cmd.p1 > 0) { + // initialise guided limits + //rover.mode_guided.limit_init_time_and_pos(); + + start_guided(cmd.content.location); + } +} + // do_set_yaw_speed - turn to a specified heading and achieve and given speed void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd) { @@ -295,6 +312,12 @@ bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) return result; } +// check if guided has completed +bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) +{ + return false; +} + // verify_yaw - return true if we have reached the desired heading bool ModeAuto::verify_nav_set_yaw_speed() { diff --git a/APMrover2/mode.h b/APMrover2/mode.h index 9226896dba..ea4752cf5d 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -67,6 +67,9 @@ public: // return if in non-manual mode : Auto, Guided, RTL, SmartRTL virtual bool is_autopilot_mode() const { return false; } + // return if external control is allowed in this mode (Guided or Guided-within-Auto) + virtual bool in_guided_mode() const { return false; } + // returns true if vehicle can be armed or disarmed from the transmitter in this mode virtual bool allows_arming_from_transmitter() { return !is_autopilot_mode(); } @@ -254,6 +257,9 @@ public: // attributes of the mode bool is_autopilot_mode() const override { return true; } + // return if external control is allowed in this mode (Guided or Guided-within-Auto) + bool in_guided_mode() const override { return _submode == Auto_Guided; } + // return distance (in meters) to destination float get_distance_to_destination() const override; @@ -282,7 +288,8 @@ protected: Auto_WP, // drive to a given location Auto_HeadingAndSpeed, // turn to a given heading Auto_RTL, // perform RTL within auto mode - Auto_Loiter // perform Loiter within auto mode + Auto_Loiter, // perform Loiter within auto mode + Auto_Guided // handover control to external navigation system from within auto mode } _submode; private: @@ -301,11 +308,13 @@ private: bool verify_command(const AP_Mission::Mission_Command& cmd); void do_RTL(void); void do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination); + void do_nav_guided_enable(const AP_Mission::Mission_Command& cmd); void do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd); bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); bool verify_RTL(); bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd); bool verify_loiter_time(const AP_Mission::Mission_Command& cmd); + bool verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd); bool verify_nav_set_yaw_speed(); void do_wait_delay(const AP_Mission::Mission_Command& cmd); void do_within_distance(const AP_Mission::Mission_Command& cmd); @@ -316,6 +325,8 @@ private: void do_set_reverse(const AP_Mission::Mission_Command& cmd); bool start_loiter(); + void start_guided(const Location& target_loc); + void send_guided_position_target(); enum Mis_Done_Behave { MIS_DONE_BEHAVE_HOLD = 0, @@ -328,6 +339,11 @@ private: uint32_t loiter_start_time; // How long have we been loitering - The start time in millis bool previously_reached_wp; // set to true if we have EVER reached the waypoint + // Guided variables + Location guided_target; // location target sent to external navigation + bool guided_target_valid; // true if guided_target is valid + uint32_t guided_target_sent_ms; // system time that target was last sent to offboard navigation + // Conditional command // A value used in condition commands (eg delay, change alt, etc.) // For example in a change altitude command, it is the altitude to change to. @@ -352,9 +368,15 @@ public: // attributes of the mode bool is_autopilot_mode() const override { return true; } + // return if external control is allowed in this mode (Guided or Guided-within-Auto) + bool in_guided_mode() const override { return true; } + // return distance (in meters) to destination float get_distance_to_destination() const override; + // return true if vehicle has reached destination + bool reached_destination() override; + // set desired location, heading and speed void set_desired_location(const struct Location& destination, float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN) override; void set_desired_heading_and_speed(float yaw_angle_cd, float target_speed) override; diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index 8264dc2381..78c7d47087 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -1,6 +1,8 @@ #include "mode.h" #include "Rover.h" +#define AUTO_GUIDED_SEND_TARGET_MS 1000 + bool ModeAuto::_enter() { // fail to enter auto if no mission commands @@ -85,10 +87,18 @@ void ModeAuto::update() case Auto_RTL: rover.mode_rtl.update(); break; - + case Auto_Loiter: rover.mode_loiter.update(); break; + + case Auto_Guided: + { + // send location target to offboard navigation system + send_guided_position_target(); + rover.mode_guided.update(); + break; + } } } @@ -113,12 +123,25 @@ void ModeAuto::set_desired_location(const struct Location& destination, float ne // return true if vehicle has reached or even passed destination bool ModeAuto::reached_destination() { - if (_submode == Auto_WP) { + switch (_submode) { + case Auto_WP: return _reached_destination; - } - if (_submode == Auto_RTL) { + break; + case Auto_HeadingAndSpeed: + // always return true because this is the safer option to allow missions to continue + return true; + break; + case Auto_RTL: return rover.mode_rtl.reached_destination(); + break; + case Auto_Loiter: + return rover.mode_loiter.reached_destination(); + break; + case Auto_Guided: + return rover.mode_guided.reached_destination(); + break; } + // we should never reach here but just in case, return true to allow missions to continue return true; } @@ -151,6 +174,50 @@ void ModeAuto::start_RTL() } } +// hand over control to external navigation controller in AUTO mode +void ModeAuto::start_guided(const Location& loc) +{ + if (rover.mode_guided.enter()) { + _submode = Auto_Guided; + + // initialise guided start time and position as reference for limit checking + //rover.mode_guided.limit_init_time_and_pos(); + + // sanity check target location + Location loc_sanitised = loc; + if ((loc_sanitised.lat != 0) || (loc_sanitised.lng != 0)) { + location_sanitize(rover.current_loc, loc_sanitised); + guided_target = loc_sanitised; + guided_target_valid = true; + } else { + guided_target_valid = false; + } + } +} + +// send latest position target to offboard navigation system +void ModeAuto::send_guided_position_target() +{ + if (!guided_target_valid) { + return; + } + + // send at maximum of 1hz + uint32_t now_ms = AP_HAL::millis(); + if ((guided_target_sent_ms == 0) || (now_ms - guided_target_sent_ms > AUTO_GUIDED_SEND_TARGET_MS)) { + guided_target_sent_ms = now_ms; + + // get system id and component id of offboard navigation system + uint8_t sysid = 0; + uint8_t compid = 0; + mavlink_channel_t chan; + if (GCS_MAVLINK::find_by_mavtype(MAV_TYPE_ONBOARD_CONTROLLER, sysid, compid, chan)) { + gcs().chan(chan-MAVLINK_COMM_0).send_set_position_target_global_int(sysid, compid, guided_target); + } + } + +} + /* check for triggering of start of auto mode */ diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index 985530a62f..0d02618659 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -116,6 +116,24 @@ float ModeGuided::get_distance_to_destination() const return _distance_to_destination; } +// return true if vehicle has reached or even passed destination +bool ModeGuided::reached_destination() +{ + switch (_guided_mode) { + case Guided_WP: + return _reached_destination; + break; + case Guided_HeadingAndSpeed: + case Guided_TurnRateAndSpeed: + case Guided_Loiter: + return true; + break; + } + + // we should never reach here but just in case, return true is the safer option + return true; +} + // set desired location void ModeGuided::set_desired_location(const struct Location& destination, float next_leg_bearing_cd)