From 89f0418c0f21f2af0f199f01952b40444f00ffe7 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 8 Mar 2019 17:53:22 +0900 Subject: [PATCH] Rover: support do-guided-limits mission command --- APMrover2/commands_logic.cpp | 34 +++++++++++++++++++++++++++---- APMrover2/mode.h | 15 ++++++++++++++ APMrover2/mode_auto.cpp | 5 ++++- APMrover2/mode_guided.cpp | 39 ++++++++++++++++++++++++++++++++++++ 4 files changed, 88 insertions(+), 5 deletions(-) diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index cc440a853e..b6fd5900a2 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -96,6 +96,10 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) } break; + case MAV_CMD_DO_GUIDED_LIMITS: + do_guided_limits(cmd); + break; + default: // return false for unhandled commands return false; @@ -182,6 +186,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_DO_SET_ROI: case MAV_CMD_DO_SET_REVERSE: case MAV_CMD_DO_FENCE_ENABLE: + case MAV_CMD_DO_GUIDED_LIMITS: return true; default: @@ -226,12 +231,10 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_sto set_desired_location(cmdloc, next_leg_bearing_cd); } +// start guided within auto to allow external navigation system to control vehicle 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); } } @@ -315,7 +318,21 @@ bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) // check if guided has completed bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { - return false; + // if we failed to enter guided or this command disables guided + // return true so we move to next command + if (_submode != Auto_Guided || cmd.p1 == 0) { + return true; + } + + // if a location target was set, return true once vehicle is close + if (guided_target_valid) { + if (rover.current_loc.get_distance(guided_target) <= rover.g.waypoint_radius) { + return true; + } + } + + // guided command complete once a limit is breached + return rover.mode_guided.limit_breached(); } // verify_yaw - return true if we have reached the desired heading @@ -391,3 +408,12 @@ void ModeAuto::do_set_reverse(const AP_Mission::Mission_Command& cmd) { set_reversed(cmd.p1 == 1); } + +// set timeout and position limits for guided within auto +void ModeAuto::do_guided_limits(const AP_Mission::Mission_Command& cmd) +{ + rover.mode_guided.limit_set( + cmd.p1 * 1000, // convert seconds to ms + cmd.content.guided_limits.horiz_max); +} + diff --git a/APMrover2/mode.h b/APMrover2/mode.h index ea4752cf5d..2b2e030059 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -323,6 +323,7 @@ private: void do_change_speed(const AP_Mission::Mission_Command& cmd); void do_set_home(const AP_Mission::Mission_Command& cmd); void do_set_reverse(const AP_Mission::Mission_Command& cmd); + void do_guided_limits(const AP_Mission::Mission_Command& cmd); bool start_loiter(); void start_guided(const Location& target_loc); @@ -388,6 +389,12 @@ public: // vehicle start loiter bool start_loiter(); + // guided limits + void limit_set(uint32_t timeout_ms, float horiz_max); + void limit_clear(); + void limit_init_time_and_location(); + bool limit_breached(); + protected: enum GuidedMode { @@ -405,6 +412,14 @@ protected: bool have_attitude_target; // true if we have a valid attitude target uint32_t _des_att_time_ms; // system time last call to set_desired_attitude was made (used for timeout) float _desired_yaw_rate_cds;// target turn rate centi-degrees per second + + // limits + struct Guided_Limit { + uint32_t timeout_ms;// timeout (in seconds) from the time that guided is invoked + float horiz_max; // horizontal position limit in meters from where guided mode was initiated (0 = no limit) + uint32_t start_time;// system time in milliseconds that control was handed to the external computer + Location start_loc; // starting location for checking horiz_max limit + } limit; }; diff --git a/APMrover2/mode_auto.cpp b/APMrover2/mode_auto.cpp index 78c7d47087..b53df70b95 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -20,6 +20,9 @@ bool ModeAuto::_enter() // other initialisation auto_triggered = false; + // clear guided limits + rover.mode_guided.limit_clear(); + // restart mission processing mission.start_or_resume(); return true; @@ -181,7 +184,7 @@ void ModeAuto::start_guided(const Location& loc) _submode = Auto_Guided; // initialise guided start time and position as reference for limit checking - //rover.mode_guided.limit_init_time_and_pos(); + rover.mode_guided.limit_init_time_and_location(); // sanity check target location Location loc_sanitised = loc; diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp index 0d02618659..5261a89b51 100644 --- a/APMrover2/mode_guided.cpp +++ b/APMrover2/mode_guided.cpp @@ -201,3 +201,42 @@ bool ModeGuided::start_loiter() } return false; } + +// set guided timeout and movement limits +void ModeGuided::limit_set(uint32_t timeout_ms, float horiz_max) +{ + limit.timeout_ms = timeout_ms; + limit.horiz_max = horiz_max; +} + +// clear/turn off guided limits +void ModeGuided::limit_clear() +{ + limit.timeout_ms = 0; + limit.horiz_max = 0.0f; +} + +// initialise guided start time and location as reference for limit checking +// only called from AUTO mode's start_guided method +void ModeGuided::limit_init_time_and_location() +{ + limit.start_time = AP_HAL::millis(); + limit.start_loc = rover.current_loc; +} + +// returns true if guided mode has breached a limit +bool ModeGuided::limit_breached() +{ + // check if we have passed the timeout + if ((limit.timeout_ms > 0) && (millis() - limit.start_time >= limit.timeout_ms)) { + return true; + } + + // check if we have gone beyond horizontal limit + if (limit.horiz_max > 0.0f) { + return (rover.current_loc.get_distance(limit.start_loc) > limit.horiz_max); + } + + // if we got this far we must be within limits + return false; +}