From 29782c3d2eb390dc8e04ca7ce8a76b6d9f70b1e4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Wed, 12 Dec 2018 11:10:20 +1100 Subject: [PATCH] Rover: move AP_Mission object into ModeAuto class This mirrors what we did for Copter --- APMrover2/Log.cpp | 2 +- APMrover2/Parameters.cpp | 2 +- APMrover2/RC_Channel.cpp | 6 +-- APMrover2/Rover.h | 43 ------------------ APMrover2/commands.cpp | 6 +-- APMrover2/commands_logic.cpp | 84 ++++++++++++++++++------------------ APMrover2/mode.cpp | 1 - APMrover2/mode.h | 47 +++++++++++++++++++- APMrover2/system.cpp | 6 +-- 9 files changed, 99 insertions(+), 98 deletions(-) diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp index b02af45edd..86ea1d7379 100644 --- a/APMrover2/Log.cpp +++ b/APMrover2/Log.cpp @@ -215,7 +215,7 @@ void Rover::Log_Write_Startup(uint8_t type) LOG_PACKET_HEADER_INIT(LOG_STARTUP_MSG), time_us : AP_HAL::micros64(), startup_type : type, - command_total : mission.num_commands() + command_total : mode_auto.mission.num_commands() }; DataFlash.WriteBlock(&pkt, sizeof(pkt)); } diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 852f4d9dec..930820e169 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -397,7 +397,7 @@ const AP_Param::Info Rover::var_info[] = { // @Group: MIS_ // @Path: ../libraries/AP_Mission/AP_Mission.cpp - GOBJECT(mission, "MIS_", AP_Mission), + GOBJECTN(mode_auto.mission, mission, "MIS_", AP_Mission), // @Group: RSSI_ // @Path: ../libraries/AP_RSSI/AP_RSSI.cpp diff --git a/APMrover2/RC_Channel.cpp b/APMrover2/RC_Channel.cpp index 0d1af1dabc..52d7c4f352 100644 --- a/APMrover2/RC_Channel.cpp +++ b/APMrover2/RC_Channel.cpp @@ -138,7 +138,7 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi // if disarmed clear mission and set home to current location if (!rover.arming.is_armed()) { - rover.mission.clear(); + rover.mode_auto.mission.clear(); rover.set_home_to_current_location(false); return; } @@ -155,8 +155,8 @@ void RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const aux_swi cmd.id = MAV_CMD_NAV_WAYPOINT; // save command - if (rover.mission.add_cmd(cmd)) { - hal.console->printf("Added waypoint %u", (unsigned)rover.mission.num_commands()); + if (rover.mode_auto.mission.add_cmd(cmd)) { + hal.console->printf("Added waypoint %u", (unsigned)rover.mode_auto.mission.num_commands()); } } } diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 5d6d2fb2e3..c761a47241 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -195,12 +195,6 @@ private: // selected navigation controller AP_Navigation *nav_controller; - // Mission library - AP_Mission mission{ - FUNCTOR_BIND_MEMBER(&Rover::start_command, bool, const AP_Mission::Mission_Command&), - FUNCTOR_BIND_MEMBER(&Rover::verify_command_callback, bool, const AP_Mission::Mission_Command&), - FUNCTOR_BIND_MEMBER(&Rover::exit_mission, void)}; - #if AP_AHRS_NAVEKF_AVAILABLE OpticalFlow optflow; #endif @@ -307,14 +301,6 @@ private: uint32_t control_sensors_enabled; uint32_t control_sensors_health; - // 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. - int32_t condition_value; - // A starting value used to check the status of a conditional command. - // For example in a delay command the condition_start records that start time for the delay - int32_t condition_start; - // 3D Location vectors // Location structure defined in AP_Common // The home location used for RTL. The location is set when we first get stable GPS lock @@ -339,11 +325,6 @@ private: static const AP_Param::Info var_info[]; static const LogStructure log_structure[]; - // Loiter control - 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 previously_reached_wp; // set to true if we have EVER reached the waypoint - // time that rudder/steering arming has been running uint32_t rudder_arm_timer; @@ -417,30 +398,6 @@ private: // commands_logic.cpp void update_mission(void); - bool start_command(const AP_Mission::Mission_Command& cmd); - void exit_mission(); - bool verify_command_callback(const AP_Mission::Mission_Command& cmd); - 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_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_set_yaw_speed(); - void do_wait_delay(const AP_Mission::Mission_Command& cmd); - void do_within_distance(const AP_Mission::Mission_Command& cmd); - bool verify_wait_delay(); - bool verify_within_distance(); - 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); - - enum Mis_Done_Behave { - MIS_DONE_BEHAVE_HOLD = 0, - MIS_DONE_BEHAVE_LOITER = 1 - }; // commands.cpp void update_home_from_EKF(); diff --git a/APMrover2/commands.cpp b/APMrover2/commands.cpp index cba5b0d516..86bc59b4e3 100644 --- a/APMrover2/commands.cpp +++ b/APMrover2/commands.cpp @@ -55,8 +55,8 @@ bool Rover::set_home(const Location& loc, bool lock) // log new home position which mission library will pull from ahrs if (should_log(MASK_LOG_CMD)) { AP_Mission::Mission_Command temp_cmd; - if (mission.read_cmd_from_storage(0, temp_cmd)) { - DataFlash.Log_Write_Mission_Cmd(mission, temp_cmd); + if (mode_auto.mission.read_cmd_from_storage(0, temp_cmd)) { + DataFlash.Log_Write_Mission_Cmd(mode_auto.mission, temp_cmd); } } } @@ -67,7 +67,7 @@ bool Rover::set_home(const Location& loc, bool lock) } // Save Home to EEPROM - mission.write_home_to_storage(); + mode_auto.mission.write_home_to_storage(); // send text of home position to ground stations gcs().send_text(MAV_SEVERITY_INFO, "Set HOME to %.6f %.6f at %.2fm", diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp index 96652b0c66..7643a40b88 100644 --- a/APMrover2/commands_logic.cpp +++ b/APMrover2/commands_logic.cpp @@ -4,8 +4,8 @@ void Rover::update_mission(void) { if (control_mode == &mode_auto) { - if (ahrs.home_is_set() && mission.num_commands() > 1) { - mission.update(); + if (ahrs.home_is_set() && mode_auto.mission.num_commands() > 1) { + mode_auto.mission.update(); } } } @@ -13,11 +13,11 @@ void Rover::update_mission(void) /********************************************************************************/ // Command Event Handlers /********************************************************************************/ -bool Rover::start_command(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd) { // log when new commands start - if (should_log(MASK_LOG_CMD)) { - DataFlash.Log_Write_Mission_Cmd(mission, cmd); + if (rover.should_log(MASK_LOG_CMD)) { + rover.DataFlash.Log_Write_Mission_Cmd(mission, cmd); } gcs().send_text(MAV_SEVERITY_INFO, "Executing %s(ID=%i)", @@ -68,12 +68,12 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) 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 (camera_mount.get_mode() == MAV_MOUNT_MODE_GPS_POINT) { - camera_mount.set_mode_to_default(); + 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 - camera_mount.set_roi_target(cmd.content.location); + rover.camera_mount.set_roi_target(cmd.content.location); } break; #endif @@ -102,22 +102,22 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd) } // exit_mission - callback function called from ap-mission when the mission has completed -void Rover::exit_mission() +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 && set_mode(mode_loiter, MODE_REASON_MISSION_END)) { + if (g2.mis_done_behave == MIS_DONE_BEHAVE_LOITER && rover.set_mode(rover.mode_loiter, MODE_REASON_MISSION_END)) { return; } - set_mode(mode_hold, MODE_REASON_MISSION_END); + 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 Rover::verify_command_callback(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_command_callback(const AP_Mission::Mission_Command& cmd) { const bool cmd_complete = verify_command(cmd); @@ -138,7 +138,7 @@ 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 Rover::verify_command(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) { switch (cmd.id) { case MAV_CMD_NAV_WAYPOINT: @@ -184,13 +184,13 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd) // Nav (Must) commands /********************************************************************************/ -void Rover::do_RTL(void) +void ModeAuto::do_RTL(void) { // start rtl in auto mode - mode_auto.start_RTL(); + start_RTL(); } -void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_at_destination) +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; @@ -210,12 +210,12 @@ void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd, bool always_stop_a // retrieve and sanitize target location Location cmdloc = cmd.content.location; - location_sanitize(current_loc, cmdloc); - mode_auto.set_desired_location(cmdloc, next_leg_bearing_cd); + location_sanitize(rover.current_loc, cmdloc); + set_desired_location(cmdloc, next_leg_bearing_cd); } // do_set_yaw_speed - turn to a specified heading and achieve and given speed -void Rover::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd) { float desired_heading_cd; @@ -229,17 +229,17 @@ void Rover::do_nav_set_yaw_speed(const AP_Mission::Mission_Command& cmd) } // set auto target - const float speed_max = control_mode->get_speed_default(); - mode_auto.set_desired_heading_and_speed(desired_heading_cd, constrain_float(cmd.content.set_yaw_speed.speed, -speed_max, speed_max)); + 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 Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd) { // exit immediately if we haven't reached the destination - if (!mode_auto.reached_destination()) { + if (!reached_destination()) { return false; } @@ -269,19 +269,19 @@ bool Rover::verify_nav_wp(const AP_Mission::Mission_Command& cmd) } } -bool Rover::verify_RTL() +bool ModeAuto::verify_RTL() { - return mode_auto.reached_destination(); + return reached_destination(); } -bool Rover::verify_loiter_unlimited(const AP_Mission::Mission_Command& cmd) +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 Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd) +bool ModeAuto::verify_loiter_time(const AP_Mission::Mission_Command& cmd) { const bool result = verify_nav_wp(cmd); if (result) { @@ -291,22 +291,22 @@ bool Rover::verify_loiter_time(const AP_Mission::Mission_Command& cmd) } // verify_yaw - return true if we have reached the desired heading -bool Rover::verify_nav_set_yaw_speed() +bool ModeAuto::verify_nav_set_yaw_speed() { - return mode_auto.reached_heading(); + return reached_heading(); } /********************************************************************************/ // Condition (May) commands /********************************************************************************/ -void Rover::do_wait_delay(const AP_Mission::Mission_Command& cmd) +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 Rover::do_within_distance(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_within_distance(const AP_Mission::Mission_Command& cmd) { condition_value = cmd.content.distance.meters; } @@ -315,7 +315,7 @@ void Rover::do_within_distance(const AP_Mission::Mission_Command& cmd) // Verify Condition (May) commands /********************************************************************************/ -bool Rover::verify_wait_delay() +bool ModeAuto::verify_wait_delay() { if (static_cast(millis() - condition_start) > static_cast(condition_value)) { condition_value = 0; @@ -324,9 +324,9 @@ bool Rover::verify_wait_delay() return false; } -bool Rover::verify_within_distance() +bool ModeAuto::verify_within_distance() { - if (mode_auto.get_distance_to_destination() < condition_value) { + if (get_distance_to_destination() < condition_value) { condition_value = 0; return true; } @@ -338,24 +338,24 @@ bool Rover::verify_within_distance() // Do (Now) commands /********************************************************************************/ -void Rover::do_change_speed(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd) { // set speed for active mode - if (control_mode->set_desired_speed(cmd.content.speed.target_ms)) { + 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 Rover::do_set_home(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_set_home(const AP_Mission::Mission_Command& cmd) { - if (cmd.p1 == 1 && have_position) { - set_home_to_current_location(false); + if (cmd.p1 == 1 && rover.have_position) { + rover.set_home_to_current_location(false); } else { - set_home(cmd.content.location, false); + rover.set_home(cmd.content.location, false); } } -void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd) +void ModeAuto::do_set_reverse(const AP_Mission::Mission_Command& cmd) { - control_mode->set_reversed(cmd.p1 == 1); + set_reversed(cmd.p1 == 1); } diff --git a/APMrover2/mode.cpp b/APMrover2/mode.cpp index e3d0d788d6..67119d7f4c 100644 --- a/APMrover2/mode.cpp +++ b/APMrover2/mode.cpp @@ -8,7 +8,6 @@ Mode::Mode() : channel_steer(rover.channel_steer), channel_throttle(rover.channel_throttle), channel_lateral(rover.channel_lateral), - mission(rover.mission), attitude_control(rover.g2.attitude_control) { } diff --git a/APMrover2/mode.h b/APMrover2/mode.h index c036fafa0f..53daff00e2 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -4,6 +4,7 @@ #include #include +#include #include "defines.h" @@ -195,7 +196,6 @@ protected: class RC_Channel *&channel_steer; class RC_Channel *&channel_throttle; class RC_Channel *&channel_lateral; - class AP_Mission &mission; class AR_AttitudeControl &attitude_control; @@ -267,6 +267,11 @@ public: // start RTL (within auto) void start_RTL(); + AP_Mission mission{ + FUNCTOR_BIND_MEMBER(&ModeAuto::start_command, bool, const AP_Mission::Mission_Command&), + FUNCTOR_BIND_MEMBER(&ModeAuto::verify_command_callback, bool, const AP_Mission::Mission_Command&), + FUNCTOR_BIND_MEMBER(&ModeAuto::exit_mission, void)}; + protected: bool _enter() override; @@ -289,6 +294,46 @@ private: bool auto_triggered; bool _reached_heading; // true when vehicle has reached desired heading in TurnToHeading sub mode + + bool start_command(const AP_Mission::Mission_Command& cmd); + void exit_mission(); + bool verify_command_callback(const AP_Mission::Mission_Command& cmd); + + 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_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_set_yaw_speed(); + void do_wait_delay(const AP_Mission::Mission_Command& cmd); + void do_within_distance(const AP_Mission::Mission_Command& cmd); + bool verify_wait_delay(); + bool verify_within_distance(); + 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); + + enum Mis_Done_Behave { + MIS_DONE_BEHAVE_HOLD = 0, + MIS_DONE_BEHAVE_LOITER = 1 + }; + + // Loiter control + 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 previously_reached_wp; // set to true if we have EVER reached the waypoint + + // 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. + int32_t condition_value; + // A starting value used to check the status of a conditional command. + // For example in a delay command the condition_start records that start time for the delay + int32_t condition_start; + }; diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index f5943e6604..fe2b5cf972 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -181,11 +181,11 @@ void Rover::startup_ground(void) startup_INS_ground(); // initialise mission library - mission.init(); + mode_auto.mission.init(); // initialise DataFlash library #if LOGGING_ENABLED == ENABLED - DataFlash.set_mission(&mission); + DataFlash.set_mission(&mode_auto.mission); DataFlash.setVehicle_Startup_Log_Writer( FUNCTOR_BIND(&rover, &Rover::Log_Write_Vehicle_Startup_Messages, void) ); @@ -354,7 +354,7 @@ bool Rover::disarm_motors(void) } if (control_mode != &mode_auto) { // reset the mission on disarm if we are not in auto - mission.reset(); + mode_auto.mission.reset(); } // only log if disarming was successful