From d9805a936620c235e0708520615d4e0930ebd49d Mon Sep 17 00:00:00 2001 From: cindyywang Date: Sat, 16 Mar 2019 10:14:34 +0900 Subject: [PATCH] Rover: move methods in commands_logic.cpp to mode_auto.cpp --- APMrover2/commands_logic.cpp | 408 ----------------------------------- APMrover2/mode_auto.cpp | 407 ++++++++++++++++++++++++++++++++++ 2 files changed, 407 insertions(+), 408 deletions(-) diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 01a3daca69..88649ef57d 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -9,411 +9,3 @@ void Rover::update_mission(void) } } } - -/********************************************************************************/ -// Command Event Handlers -/********************************************************************************/ -bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) -{ - // log when new commands start - if (rover.should_log(MASK_LOG_CMD)) { - rover.logger.Write_Mission_Cmd(mission, cmd); - } - - gcs().send_text(MAV_SEVERITY_INFO, "Executing %s(ID=%i)", - cmd.type(), cmd.id); - - switch (cmd.id) { - case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint - do_nav_wp(cmd, false); - break; - - case MAV_CMD_NAV_RETURN_TO_LAUNCH: - do_RTL(); - break; - - case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely - case MAV_CMD_NAV_LOITER_TIME: // Loiter for specified time - 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; - - // Conditional commands - case MAV_CMD_CONDITION_DELAY: - do_wait_delay(cmd); - break; - - case MAV_CMD_CONDITION_DISTANCE: - do_within_distance(cmd); - break; - - // Do commands - case MAV_CMD_DO_CHANGE_SPEED: - do_change_speed(cmd); - break; - - case MAV_CMD_DO_SET_HOME: - do_set_home(cmd); - break; - -#if MOUNT == ENABLED - // Sets the region of interest (ROI) for a sensor set or the - // vehicle itself. This can then be used by the vehicles control - // system to control the vehicle attitude and the attitude of various - // devices such as cameras. - // |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| - case MAV_CMD_DO_SET_ROI: - if (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { - // switch off the camera tracking if enabled - if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - rover.camera_mount.set_mode_to_default(); - } - } else { - // send the command to the camera mount - rover.camera_mount.set_roi_target(cmd.content.location); - } - break; -#endif - - case MAV_CMD_DO_SET_REVERSE: - do_set_reverse(cmd); - break; - - case MAV_CMD_DO_FENCE_ENABLE: - if (cmd.p1 == 0) { //disable - g2.fence.enable(false); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); - } else { //enable fence - g2.fence.enable(true); - gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); - } - break; - - case MAV_CMD_DO_GUIDED_LIMITS: - do_guided_limits(cmd); - break; - - default: - // return false for unhandled commands - return false; - } - - // if we got this far we must have been successful - return true; -} - -// exit_mission - callback function called from ap-mission when the mission has completed -void ModeAuto::exit_mission() -{ - // play a tone - AP_Notify::events.mission_complete = 1; - // send message - gcs().send_text(MAV_SEVERITY_NOTICE, "Mission Complete"); - - if (g2.mis_done_behave == MIS_DONE_BEHAVE_LOITER && rover.set_mode(rover.mode_loiter, MODE_REASON_MISSION_END)) { - return; - } - - if (g2.mis_done_behave == MIS_DONE_BEHAVE_ACRO && rover.set_mode(rover.mode_acro, MODE_REASON_MISSION_END)) { - return; - } - - rover.set_mode(rover.mode_hold, MODE_REASON_MISSION_END); -} - -// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run -// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode -bool ModeAuto::verify_command_callback(const AP_Mission::Mission_Command& cmd) -{ - const bool cmd_complete = verify_command(cmd); - - // send message to GCS - if (cmd_complete) { - gcs().send_mission_item_reached_message(cmd.index); - } - - return cmd_complete; -} - -/******************************************************************************* -Verify command Handlers - -Each type of mission element has a "verify" operation. The verify -operation returns true when the mission element has completed and we -should move onto the next mission element. -Return true if we do not recognize the command so that we move on to the next command -*******************************************************************************/ - -bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) -{ - switch (cmd.id) { - case MAV_CMD_NAV_WAYPOINT: - return verify_nav_wp(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_NAV_GUIDED_ENABLE: - return verify_nav_guided_enable(cmd); - - case MAV_CMD_CONDITION_DELAY: - return verify_wait_delay(); - - case MAV_CMD_CONDITION_DISTANCE: - return verify_within_distance(); - - case MAV_CMD_NAV_SET_YAW_SPEED: - return verify_nav_set_yaw_speed(); - - // do commands (always return true) - case MAV_CMD_DO_CHANGE_SPEED: - case MAV_CMD_DO_SET_HOME: - case MAV_CMD_DO_CONTROL_VIDEO: - case MAV_CMD_DO_SET_CAM_TRIGG_DIST: - 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: - // error message - gcs().send_text(MAV_SEVERITY_WARNING, "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; - } -} - -/********************************************************************************/ -// Nav (Must) commands -/********************************************************************************/ - -void ModeAuto::do_RTL(void) -{ - // start rtl in auto mode - start_RTL(); -} - -void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination) -{ - // 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_start_time = 0; - - // this is the delay, stored in seconds - loiter_duration = cmd.p1; - - // get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot) - // in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point - float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN; - if (!always_stop_at_destination && loiter_duration == 0) { - next_leg_bearing_cd = mission.get_next_ground_course_cd(MODE_NEXT_HEADING_UNKNOWN); - } - - // retrieve and sanitize target location - Location cmdloc = cmd.content.location; - location_sanitize(rover.current_loc, cmdloc); - 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) { - 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) -{ - float desired_heading_cd; - - // get final angle, 1 = Relative, 0 = Absolute - if (cmd.content.set_yaw_speed.relative_angle > 0) { - // relative angle - desired_heading_cd = wrap_180_cd(ahrs.yaw_sensor + cmd.content.set_yaw_speed.angle_deg * 100.0f); - } else { - // absolute angle - desired_heading_cd = cmd.content.set_yaw_speed.angle_deg * 100.0f; - } - - // set auto target - const float speed_max = get_speed_default(); - set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max)); -} - -/********************************************************************************/ -// Verify Nav (Must) commands -/********************************************************************************/ -bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) -{ - // exit immediately if we haven't reached the destination - if (!reached_destination()) { - return false; - } - - // Check if this is the first time we have noticed reaching the waypoint - if (!previously_reached_wp) { - previously_reached_wp = true; - - // check if we are loitering at this waypoint - the message sent to the GCS is different - if (loiter_duration > 0) { - // send message including loiter time - gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds", - static_cast(cmd.index), - static_cast(loiter_duration)); - // record the current time i.e. start timer - loiter_start_time = millis(); - } else { - // send simpler message to GCS - gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u", static_cast(cmd.index)); - } - } - - // Check if we have loitered long enough - if (loiter_duration == 0) { - return true; - } else { - return (((millis() - loiter_start_time) / 1000) >= loiter_duration); - } -} - -bool ModeAuto::verify_RTL() -{ - return reached_destination(); -} - -bool ModeAuto::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd) -{ - verify_nav_wp(cmd); - return false; -} - -// verify_loiter_time - check if we have loitered long enough -bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) -{ - const bool result = verify_nav_wp(cmd); - if (result) { - gcs().send_text(MAV_SEVERITY_WARNING, "Finished active loiter"); - } - return result; -} - -// check if guided has completed -bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) -{ - // 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.loc) <= 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 -bool ModeAuto::verify_nav_set_yaw_speed() -{ - return reached_heading(); -} - -/********************************************************************************/ -// Condition (May) commands -/********************************************************************************/ - -void ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd) -{ - condition_start = millis(); - condition_value = static_cast(cmd.content.delay.seconds * 1000); // convert seconds to milliseconds -} - -void ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd) -{ - condition_value = cmd.content.distance.meters; -} - -/********************************************************************************/ -// Verify Condition (May) commands -/********************************************************************************/ - -bool ModeAuto::verify_wait_delay() -{ - if (static_cast(millis() - condition_start) > static_cast(condition_value)) { - condition_value = 0; - return true; - } - return false; -} - -bool ModeAuto::verify_within_distance() -{ - if (get_distance_to_destination() < condition_value) { - condition_value = 0; - return true; - } - return false; -} - - -/********************************************************************************/ -// Do (Now) commands -/********************************************************************************/ - -void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) -{ - // set speed for active mode - if (set_desired_speed(cmd.content.speed.target_ms)) { - gcs().send_text(MAV_SEVERITY_INFO, "speed: %.1f m/s", static_cast(cmd.content.speed.target_ms)); - } -} - -void ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd) -{ - if (cmd.p1 == 1 && rover.have_position) { - if (!rover.set_home_to_current_location(false)) { - // ignored... - } - } else { - if (!rover.set_home(cmd.content.location, false)) { - // ignored... - } - } -} - -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_auto.cpp b/APMrover2/mode_auto.cpp index 434325516f..97351fb0ed 100644 --- a/APMrover2/mode_auto.cpp +++ b/APMrover2/mode_auto.cpp @@ -281,3 +281,410 @@ void ModeAuto::send_guided_position_target() } } + +/********************************************************************************/ +// Command Event Handlers +/********************************************************************************/ +bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) +{ + // log when new commands start + if (rover.should_log(MASK_LOG_CMD)) { + rover.logger.Write_Mission_Cmd(mission, cmd); + } + + gcs().send_text(MAV_SEVERITY_INFO, "Executing %s(ID=%i)", + cmd.type(), cmd.id); + + switch (cmd.id) { + case MAV_CMD_NAV_WAYPOINT: // Navigate to Waypoint + do_nav_wp(cmd, false); + break; + + case MAV_CMD_NAV_RETURN_TO_LAUNCH: + do_RTL(); + break; + + case MAV_CMD_NAV_LOITER_UNLIM: // Loiter indefinitely + case MAV_CMD_NAV_LOITER_TIME: // Loiter for specified time + 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; + + // Conditional commands + case MAV_CMD_CONDITION_DELAY: + do_wait_delay(cmd); + break; + + case MAV_CMD_CONDITION_DISTANCE: + do_within_distance(cmd); + break; + + // Do commands + case MAV_CMD_DO_CHANGE_SPEED: + do_change_speed(cmd); + break; + + case MAV_CMD_DO_SET_HOME: + do_set_home(cmd); + break; + +#if MOUNT == ENABLED + // Sets the region of interest (ROI) for a sensor set or the + // vehicle itself. This can then be used by the vehicles control + // system to control the vehicle attitude and the attitude of various + // devices such as cameras. + // |Region of interest mode. (see MAV_ROI enum)| Waypoint index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple cameras etc.)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| + case MAV_CMD_DO_SET_ROI: + if (cmd.content.location.alt == 0 && cmd.content.location.lat == 0 && cmd.content.location.lng == 0) { + // switch off the camera tracking if enabled + if (rover.camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { + rover.camera_mount.set_mode_to_default(); + } + } else { + // send the command to the camera mount + rover.camera_mount.set_roi_target(cmd.content.location); + } + break; +#endif + + case MAV_CMD_DO_SET_REVERSE: + do_set_reverse(cmd); + break; + + case MAV_CMD_DO_FENCE_ENABLE: + if (cmd.p1 == 0) { //disable + g2.fence.enable(false); + gcs().send_text(MAV_SEVERITY_INFO, "Fence Disabled"); + } else { //enable fence + g2.fence.enable(true); + gcs().send_text(MAV_SEVERITY_INFO, "Fence Enabled"); + } + break; + + case MAV_CMD_DO_GUIDED_LIMITS: + do_guided_limits(cmd); + break; + + default: + // return false for unhandled commands + return false; + } + + // if we got this far we must have been successful + return true; +} + +// exit_mission - callback function called from ap-mission when the mission has completed +void ModeAuto::exit_mission() +{ + // play a tone + AP_Notify::events.mission_complete = 1; + // send message + gcs().send_text(MAV_SEVERITY_NOTICE, "Mission Complete"); + + if (g2.mis_done_behave == MIS_DONE_BEHAVE_LOITER && rover.set_mode(rover.mode_loiter, MODE_REASON_MISSION_END)) { + return; + } + + if (g2.mis_done_behave == MIS_DONE_BEHAVE_ACRO && rover.set_mode(rover.mode_acro, MODE_REASON_MISSION_END)) { + return; + } + + rover.set_mode(rover.mode_hold, MODE_REASON_MISSION_END); +} + +// verify_command_callback - callback function called from ap-mission at 10hz or higher when a command is being run +// we double check that the flight mode is AUTO to avoid the possibility of ap-mission triggering actions while we're not in AUTO mode +bool ModeAuto::verify_command_callback(const AP_Mission::Mission_Command& cmd) +{ + const bool cmd_complete = verify_command(cmd); + + // send message to GCS + if (cmd_complete) { + gcs().send_mission_item_reached_message(cmd.index); + } + + return cmd_complete; +} + +/******************************************************************************* +Verify command Handlers + +Each type of mission element has a "verify" operation. The verify +operation returns true when the mission element has completed and we +should move onto the next mission element. +Return true if we do not recognize the command so that we move on to the next command +*******************************************************************************/ + +bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) +{ + switch (cmd.id) { + case MAV_CMD_NAV_WAYPOINT: + return verify_nav_wp(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_NAV_GUIDED_ENABLE: + return verify_nav_guided_enable(cmd); + + case MAV_CMD_CONDITION_DELAY: + return verify_wait_delay(); + + case MAV_CMD_CONDITION_DISTANCE: + return verify_within_distance(); + + case MAV_CMD_NAV_SET_YAW_SPEED: + return verify_nav_set_yaw_speed(); + + // do commands (always return true) + case MAV_CMD_DO_CHANGE_SPEED: + case MAV_CMD_DO_SET_HOME: + case MAV_CMD_DO_CONTROL_VIDEO: + case MAV_CMD_DO_SET_CAM_TRIGG_DIST: + 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: + // error message + gcs().send_text(MAV_SEVERITY_WARNING, "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; + } +} + +/********************************************************************************/ +// Nav (Must) commands +/********************************************************************************/ + +void ModeAuto::do_RTL(void) +{ + // start rtl in auto mode + start_RTL(); +} + +void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination) +{ + // 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_start_time = 0; + + // this is the delay, stored in seconds + loiter_duration = cmd.p1; + + // get heading to following waypoint (auto mode reduces speed to allow corning without large overshoot) + // in case of non-zero loiter duration, we provide heading-unknown to signal we should stop at the point + float next_leg_bearing_cd = MODE_NEXT_HEADING_UNKNOWN; + if (!always_stop_at_destination && loiter_duration == 0) { + next_leg_bearing_cd = mission.get_next_ground_course_cd(MODE_NEXT_HEADING_UNKNOWN); + } + + // retrieve and sanitize target location + Location cmdloc = cmd.content.location; + location_sanitize(rover.current_loc, cmdloc); + 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) { + 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) +{ + float desired_heading_cd; + + // get final angle, 1 = Relative, 0 = Absolute + if (cmd.content.set_yaw_speed.relative_angle > 0) { + // relative angle + desired_heading_cd = wrap_180_cd(ahrs.yaw_sensor + cmd.content.set_yaw_speed.angle_deg * 100.0f); + } else { + // absolute angle + desired_heading_cd = cmd.content.set_yaw_speed.angle_deg * 100.0f; + } + + // set auto target + const float speed_max = get_speed_default(); + set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max)); +} + +/********************************************************************************/ +// Verify Nav (Must) commands +/********************************************************************************/ +bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) +{ + // exit immediately if we haven't reached the destination + if (!reached_destination()) { + return false; + } + + // Check if this is the first time we have noticed reaching the waypoint + if (!previously_reached_wp) { + previously_reached_wp = true; + + // check if we are loitering at this waypoint - the message sent to the GCS is different + if (loiter_duration > 0) { + // send message including loiter time + gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u. Loiter for %u seconds", + static_cast(cmd.index), + static_cast(loiter_duration)); + // record the current time i.e. start timer + loiter_start_time = millis(); + } else { + // send simpler message to GCS + gcs().send_text(MAV_SEVERITY_INFO, "Reached waypoint #%u", static_cast(cmd.index)); + } + } + + // Check if we have loitered long enough + if (loiter_duration == 0) { + return true; + } else { + return (((millis() - loiter_start_time) / 1000) >= loiter_duration); + } +} + +bool ModeAuto::verify_RTL() +{ + return reached_destination(); +} + +bool ModeAuto::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd) +{ + verify_nav_wp(cmd); + return false; +} + +// verify_loiter_time - check if we have loitered long enough +bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) +{ + const bool result = verify_nav_wp(cmd); + if (result) { + gcs().send_text(MAV_SEVERITY_WARNING, "Finished active loiter"); + } + return result; +} + +// check if guided has completed +bool ModeAuto::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) +{ + // 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.loc) <= 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 +bool ModeAuto::verify_nav_set_yaw_speed() +{ + return reached_heading(); +} + +/********************************************************************************/ +// Condition (May) commands +/********************************************************************************/ + +void ModeAuto::do_wait_delay(const AP_Mission::Mission_Command& cmd) +{ + condition_start = millis(); + condition_value = static_cast(cmd.content.delay.seconds * 1000); // convert seconds to milliseconds +} + +void ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd) +{ + condition_value = cmd.content.distance.meters; +} + +/********************************************************************************/ +// Verify Condition (May) commands +/********************************************************************************/ + +bool ModeAuto::verify_wait_delay() +{ + if (static_cast(millis() - condition_start) > static_cast(condition_value)) { + condition_value = 0; + return true; + } + return false; +} + +bool ModeAuto::verify_within_distance() +{ + if (get_distance_to_destination() < condition_value) { + condition_value = 0; + return true; + } + return false; +} + + +/********************************************************************************/ +// Do (Now) commands +/********************************************************************************/ + +void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) +{ + // set speed for active mode + if (set_desired_speed(cmd.content.speed.target_ms)) { + gcs().send_text(MAV_SEVERITY_INFO, "speed: %.1f m/s", static_cast(cmd.content.speed.target_ms)); + } +} + +void ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd) +{ + if (cmd.p1 == 1 && rover.have_position) { + if (!rover.set_home_to_current_location(false)) { + // ignored... + } + } else { + if (!rover.set_home(cmd.content.location, false)) { + // ignored... + } + } +} + +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); +}