From ecc89f0b7ed405d67795b0f0f80b7bf144f3eccd Mon Sep 17 00:00:00 2001 From: Grant Morphett Date: Thu, 9 Mar 2017 20:14:55 +1100 Subject: [PATCH] Rover: Adding LOITER_TIME and LOITER_TIME_UNLIM functionality --- APMrover2/Rover.h | 13 +++- APMrover2/Steering.cpp | 30 ++++++- APMrover2/commands_logic.cpp | 147 ++++++++++++++++++++++++++++------- APMrover2/system.cpp | 2 +- 4 files changed, 159 insertions(+), 33 deletions(-) diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index ff94c2c011..a9ff2a514c 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -360,9 +360,10 @@ private: static const LogStructure log_structure[]; // Loiter control - uint16_t loiter_time_max; // How long we should loiter at the nav_waypoint (time in seconds) - uint32_t loiter_time; // How long have we been loitering - The start time in millis - + uint16_t loiter_duration; // How long we should loiter at the nav_waypoint (time in seconds) + uint32_t loiter_start_time; // How long have we been loitering - The start time in millis + bool active_loiter; // TRUE if we actively return to the loitering waypoint if we drift off + bool previously_reached_wp; // set to true if we have EVER reached the waypoint float distance_past_wp; // record the distance we have gone past the wp private: @@ -495,6 +496,10 @@ private: bool verify_command_callback(const AP_Mission::Mission_Command& cmd); void do_nav_wp(const AP_Mission::Mission_Command& cmd); bool verify_nav_wp(const AP_Mission::Mission_Command& cmd); + void do_loiter_unlimited(const AP_Mission::Mission_Command& cmd); + void do_loiter_time(const AP_Mission::Mission_Command& cmd); + bool verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd); + bool verify_loiter_time(const AP_Mission::Mission_Command& cmd); void do_wait_delay(const AP_Mission::Mission_Command& cmd); void do_within_distance(const AP_Mission::Mission_Command& cmd); void do_change_speed(const AP_Mission::Mission_Command& cmd); @@ -502,6 +507,8 @@ private: void do_digicam_configure(const AP_Mission::Mission_Command& cmd); void do_digicam_control(const AP_Mission::Mission_Command& cmd); void init_capabilities(void); + bool in_stationary_loiter(void); + void set_loiter_active(const AP_Mission::Mission_Command& cmd); public: bool print_log_menu(void); diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp index 4c7de23018..408a9b3cc2 100644 --- a/APMrover2/Steering.cpp +++ b/APMrover2/Steering.cpp @@ -81,6 +81,24 @@ bool Rover::use_pivot_steering(void) } +/* + test if we are loitering AND should be stopped at a waypoint +*/ +bool Rover::in_stationary_loiter() +{ + // Confirm we are in AUTO mode and need to loiter for a time period + if ((loiter_start_time > 0) && (control_mode == AUTO)) { + // Check if active loiter is enabled AND we are outside the waypoint loiter radius + // then the vehicle still needs to move so return false + if (active_loiter && (wp_distance > g.waypoint_radius)) { + return false; + } + } + + return true; +} + + /* calculate the throtte for auto-throttle modes */ @@ -88,7 +106,7 @@ void Rover::calc_throttle(float target_speed) { // If not autostarting OR we are loitering at a waypoint // then set the throttle to minimum - if (!auto_check_trigger() || ((loiter_time > 0) && (control_mode == AUTO))) { + if (!auto_check_trigger() || in_stationary_loiter()) { channel_throttle->servo_out = g.throttle_min.get(); return; } @@ -166,7 +184,13 @@ void Rover::calc_lateral_acceleration() { switch (control_mode) { case AUTO: - nav_controller->update_waypoint(prev_WP, next_WP); + // If we have reached the waypoint previously navigate + // back to it from our current position + if (previously_reached_wp && (loiter_duration > 0)) { + nav_controller->update_waypoint(current_loc, next_WP); + } else { + nav_controller->update_waypoint(prev_WP, next_WP); + } break; case RTL: @@ -199,7 +223,7 @@ void Rover::calc_lateral_acceleration() void Rover::calc_nav_steer() { // check to see if the rover is loitering - if ((loiter_time > 0) && (control_mode == AUTO)) { + if (in_stationary_loiter()) { channel_steer->servo_out = 0; return; } diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index dc2a162e7b..253bb99ab4 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -31,6 +31,14 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) do_RTL(); break; + case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely + do_loiter_unlimited(cmd); + break; + + case MAV_CMD_NAV_LOITER_TIME: + do_loiter_time(cmd); + break; + // Conditional commands case MAV_CMD_CONDITION_DELAY: do_wait_delay(cmd); @@ -153,21 +161,38 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd) case MAV_CMD_NAV_RETURN_TO_LAUNCH: return verify_RTL(); + case MAV_CMD_NAV_LOITER_UNLIM: + return verify_loiter_unlimited(cmd); + + case MAV_CMD_NAV_LOITER_TIME: + return verify_loiter_time(cmd); + case MAV_CMD_CONDITION_DELAY: return verify_wait_delay(); case MAV_CMD_CONDITION_DISTANCE: return verify_within_distance(); + // do commands (always return true) + case MAV_CMD_DO_CHANGE_SPEED: + case MAV_CMD_DO_SET_HOME: + case MAV_CMD_DO_SET_SERVO: + case MAV_CMD_DO_SET_RELAY: + case MAV_CMD_DO_REPEAT_SERVO: + case MAV_CMD_DO_REPEAT_RELAY: + case MAV_CMD_DO_CONTROL_VIDEO: + case MAV_CMD_DO_DIGICAM_CONFIGURE: + case MAV_CMD_DO_DIGICAM_CONTROL: + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + case MAV_CMD_DO_SET_ROI: + return true; + default: - if (cmd.id > MAV_CMD_CONDITION_LAST) { - // this is a command that doesn't require verify - return true; - } - gcs_send_text_P(SEVERITY_HIGH,PSTR("verify_conditon: Unsupported command")); + // error message + gcs_send_text_fmt(PSTR("Skipping invalid cmd #%i"), cmd.id); + // return true if we do not recognize the command so that we move on to the next command return true; } - return false; } /********************************************************************************/ @@ -183,62 +208,111 @@ void Rover::do_RTL(void) void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd) { + // just starting so we haven't previously reached the waypoint + previously_reached_wp = false; + // this will be used to remember the time in millis after we reach or pass the WP. - loiter_time = 0; + loiter_start_time = 0; + // this is the delay, stored in seconds - loiter_time_max = abs(cmd.p1); + loiter_duration = cmd.p1; // this is the distance we travel past the waypoint - not there yet so 0 initially distance_past_wp = 0; - set_next_WP(cmd.content.location); + Location cmdloc = cmd.content.location; + set_next_WP(cmdloc); } +void Rover::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) +{ + active_loiter = true; + do_nav_wp(cmd); + loiter_duration = 100; // an arbitrary large loiter time +} + +// do_loiter_time - initiate loitering at a point for a given time period +// if the vehicle is moved off the loiter point (i.e. a boat in a current) +// then the vehicle will actively return to the loiter coords. +void Rover::do_loiter_time(const AP_Mission::Mission_Command& cmd) +{ + active_loiter = true; + do_nav_wp(cmd); +} + + /********************************************************************************/ // Verify Nav (Must) commands /********************************************************************************/ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) { + // Have we reached the waypoint i.e. are we within waypoint_radius of the waypoint if ((wp_distance > 0) && (wp_distance <= g.waypoint_radius)) { - // Check if we need to loiter at this waypoint - if (loiter_time_max > 0) { - if (loiter_time == 0) { // check if we are just starting loiter - gcs_send_text_fmt(PSTR("Reached Waypoint #%i - Loiter for %i seconds"), + // check if we are loitering at this waypoint - the message sent to the GCS is different + if (loiter_duration > 0) { + // Check if this is the first time we have reached the waypoint + if (!previously_reached_wp) { + gcs_send_text_fmt(PSTR("Reached waypoint #%i. Loiter for %i seconds"), (unsigned)cmd.index, - (unsigned)loiter_time_max); + (unsigned)loiter_duration); // record the current time i.e. start timer - loiter_time = millis(); + loiter_start_time = millis(); + previously_reached_wp = true; } + + distance_past_wp = wp_distance; + // Check if we have loiter long enough - if (((millis() - loiter_time) / 1000) < loiter_time_max) { + if (((millis() - loiter_start_time) / 1000) < loiter_duration) { return false; } } else { - gcs_send_text_fmt(PSTR("Reached Waypoint #%i dist %um"), + gcs_send_text_fmt(PSTR("Reached waypoint #%i. Distance %um"), (unsigned)cmd.index, (unsigned)get_distance(current_loc, next_WP)); } + // set loiter_duration to 0 so we know we aren't or have finished loitering + loiter_duration = 0; return true; } // have we gone past the waypoint? // We should always go through the waypoint i.e. the above code - // first before we go past it. - if (location_passed_point(current_loc, prev_WP, next_WP)) { - // check if we have gone futher past the wp then last time and output new message if we have - if ((uint32_t)distance_past_wp != (uint32_t)get_distance(current_loc, next_WP)) { - distance_past_wp = get_distance(current_loc, next_WP); - gcs_send_text_fmt(PSTR("Passed Waypoint #%i dist %um"), + // first before we go past it but sometimes we don't. + // OR have we reached the waypoint previously be we aren't actively loitering + // This second check is required for when we roll past the waypoint radius + if (location_passed_point(current_loc, prev_WP, next_WP) || + (!active_loiter && previously_reached_wp)) { + // As we have passed the waypoint navigation needs to be done from current location + prev_WP = current_loc; + // Check if this is the first time we have reached the waypoint even though we have gone past it + if (!previously_reached_wp) { + gcs_send_text_fmt(PSTR("Reached waypoint #%i. Loiter for %i seconds"), + (unsigned)cmd.index, + (unsigned)loiter_duration); + // record the current time i.e. start timer + loiter_start_time = millis(); + previously_reached_wp = true; + distance_past_wp = wp_distance; + } + + // check if distance to the WP has changed and output new message if it has + float dist_to_wp = get_distance(current_loc, next_WP); + if ((uint32_t)distance_past_wp != (uint32_t)dist_to_wp) { + distance_past_wp = dist_to_wp; + gcs_send_text_fmt(PSTR("Passed waypoint #%i. Distance %um"), (unsigned)cmd.index, (unsigned)distance_past_wp); } + // Check if we need to loiter at this waypoint - if (loiter_time_max > 0) { - if (((millis() - loiter_time) / 1000) < loiter_time_max) { + if (loiter_duration > 0) { + if (((millis() - loiter_start_time) / 1000) < loiter_duration) { return false; } } - distance_past_wp = 0; + // set loiter_duration to 0 so we know we aren't or have finished loitering + loiter_duration = 0; return true; } @@ -264,6 +338,27 @@ bool Rover::verify_RTL() return false; } +bool Rover::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd) +{ + // Continually set loiter start time to now so it never finishes + loiter_start_time += millis(); + verify_nav_wp(cmd); + return false; +} + +// verify_loiter_time - check if we have loitered long enough +bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd) +{ + bool result = verify_nav_wp(cmd); + if (result) { + gcs_send_text_P(SEVERITY_MEDIUM, PSTR("Finished active loiter\n")); + // if we have finished active loitering - turn it off + active_loiter = false; + } + return result; +} + + /********************************************************************************/ // Condition (May) commands /********************************************************************************/ diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index bb5ed7fed8..7fe0b79596 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -282,7 +282,7 @@ void Rover::set_mode(enum mode mode) // If we are changing out of AUTO mode reset the loiter timer if (control_mode == AUTO) - loiter_time = 0; + loiter_start_time = 0; control_mode = mode; throttle_last = 0;