diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 0cdda6ad7d..1cf2ffe874 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -72,6 +72,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(update_trigger, 50, 600), SCHED_TASK(gcs_failsafe_check, 10, 600), SCHED_TASK(compass_accumulate, 50, 900), + SCHED_TASK(smart_rtl_update, 3, 100), SCHED_TASK(update_notify, 50, 300), SCHED_TASK(one_second_loop, 1, 3000), SCHED_TASK(compass_cal_update, 50, 100), diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 4cfca7f89b..8f366d3c04 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -241,7 +241,7 @@ const AP_Param::Info Rover::var_info[] = { // @Param: MODE1 // @DisplayName: Mode1 - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard // @Description: Driving mode for switch position 1 (910 to 1230 and above 2049) GSCALAR(mode1, "MODE1", MANUAL), @@ -249,35 +249,35 @@ const AP_Param::Info Rover::var_info[] = { // @Param: MODE2 // @DisplayName: Mode2 // @Description: Driving mode for switch position 2 (1231 to 1360) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode2, "MODE2", MANUAL), // @Param: MODE3 // @DisplayName: Mode3 // @Description: Driving mode for switch position 3 (1361 to 1490) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode3, "MODE3", MANUAL), // @Param: MODE4 // @DisplayName: Mode4 // @Description: Driving mode for switch position 4 (1491 to 1620) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode4, "MODE4", MANUAL), // @Param: MODE5 // @DisplayName: Mode5 // @Description: Driving mode for switch position 5 (1621 to 1749) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode5, "MODE5", MANUAL), // @Param: MODE6 // @DisplayName: Mode6 // @Description: Driving mode for switch position 6 (1750 to 2049) - // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,15:Guided + // @Values: 0:Manual,1:Acro,3:Steering,4:Hold,10:Auto,11:RTL,12:SmartRTL,15:Guided // @User: Standard GSCALAR(mode6, "MODE6", MANUAL), @@ -513,6 +513,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @User: Standard AP_GROUPINFO("ACRO_TURN_RATE", 12, ParametersG2, acro_turn_rate, 180.0f), + // @Group: SRTL_ + // @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp + AP_SUBGROUPINFO(smart_rtl, "SRTL_", 13, ParametersG2, AP_SmartRTL), + AP_GROUPEND }; @@ -524,7 +528,8 @@ ParametersG2::ParametersG2(void) #endif beacon(rover.serial_manager), motors(rover.ServoRelayEvents), - attitude_control(rover.ahrs) + attitude_control(rover.ahrs), + smart_rtl(rover.ahrs) { AP_Param::setup_object_defaults(this, var_info); } diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 9af6e0653e..39e77c9682 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -319,6 +319,9 @@ public: // acro mode turn rate maximum AP_Float acro_turn_rate; + + // Safe RTL library + AP_SmartRTL smart_rtl; }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 34594034be..433bcb53d0 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -67,6 +67,7 @@ #include #include #include +#include #include #include // Mode Filter from Filter library #include // Filter library - butterworth filter @@ -111,6 +112,7 @@ public: friend class ModeSteering; friend class ModeManual; friend class ModeRTL; + friend class ModeSmartRTL; Rover(void); @@ -384,6 +386,7 @@ private: ModeAuto mode_auto; ModeSteering mode_steering; ModeRTL mode_rtl; + ModeSmartRTL mode_smartrtl; // cruise throttle and speed learning struct { @@ -568,6 +571,7 @@ private: void change_arm_state(void); bool arm_motors(AP_Arming::ArmingMethod method); bool disarm_motors(void); + void smart_rtl_update(); // test.cpp void print_hit_enter(); diff --git a/APMrover2/control_modes.cpp b/APMrover2/control_modes.cpp index d5aab1aa36..ba87692909 100644 --- a/APMrover2/control_modes.cpp +++ b/APMrover2/control_modes.cpp @@ -24,6 +24,9 @@ Mode *Rover::control_mode_from_num(const enum mode num) case RTL: ret = &mode_rtl; break; + case SMART_RTL: + ret = &mode_smartrtl; + break; case GUIDED: ret = &mode_guided; break; diff --git a/APMrover2/defines.h b/APMrover2/defines.h index 6f67349078..3a5c109519 100644 --- a/APMrover2/defines.h +++ b/APMrover2/defines.h @@ -33,6 +33,7 @@ enum mode { HOLD = 4, AUTO = 10, RTL = 11, + SMART_RTL = 12, GUIDED = 15, INITIALISING = 16 }; diff --git a/APMrover2/mode.h b/APMrover2/mode.h index ef2836cd87..0cab1f293f 100644 --- a/APMrover2/mode.h +++ b/APMrover2/mode.h @@ -338,6 +338,40 @@ protected: bool _enter() override; }; +class ModeSmartRTL : public Mode +{ +public: + + uint32_t mode_number() const override { return SMART_RTL; } + const char *name4() const override { return "SRTL"; } + + // methods that affect movement of the vehicle in this mode + void update() override; + + // attributes of the mode + bool is_autopilot_mode() const override { return true; } + bool failsafe_throttle_suppress() const override { return false; } + + float get_distance_to_destination() const override { return _distance_to_destination; } + bool reached_destination() override { return smart_rtl_state == SmartRTL_StopAtHome; } + + // save current position for use by the smart_rtl flight mode + void save_position(bool save_pos); + +protected: + + // Safe RTL states + enum SmartRTLState { + SmartRTL_WaitForPathCleanup, + SmartRTL_PathFollow, + SmartRTL_StopAtHome, + SmartRTL_Failure + } smart_rtl_state; + + bool _enter() override; + bool _load_point; +}; + class ModeSteering : public Mode { diff --git a/APMrover2/mode_smart_rtl.cpp b/APMrover2/mode_smart_rtl.cpp new file mode 100644 index 0000000000..5b1cd78926 --- /dev/null +++ b/APMrover2/mode_smart_rtl.cpp @@ -0,0 +1,79 @@ +#include "mode.h" +#include "Rover.h" + +bool ModeSmartRTL::_enter() +{ + // SmartRTL requires EKF (not DCM) + Location ekf_origin; + if (!ahrs.get_origin(ekf_origin)) { + return false; + } + + // refuse to enter SmartRTL if smart RTL's home has not been set + if (!g2.smart_rtl.is_active()) { + return false; + } + + // RTL never reverses + rover.set_reverse(false); + + // init state + smart_rtl_state = SmartRTL_WaitForPathCleanup; + _reached_destination = false; + + return true; +} + +void ModeSmartRTL::update() +{ + switch (smart_rtl_state) { + case SmartRTL_WaitForPathCleanup: + // check if return path is computed and if yes, begin journey home + if (g2.smart_rtl.request_thorough_cleanup()) { + smart_rtl_state = SmartRTL_PathFollow; + _load_point = true; + } + // Note: this may lead to an unnecessary 20ms slow down of the vehicle (but it is unlikely) + stop_vehicle(); + break; + + case SmartRTL_PathFollow: + // load point if required + if (_load_point) { + Vector3f next_point; + if (!g2.smart_rtl.pop_point(next_point)) { + // if not more points, we have reached home + gcs().send_text(MAV_SEVERITY_INFO, "Reached destination"); + smart_rtl_state = SmartRTL_StopAtHome; + break; + } + _load_point = false; + // set target destination to new point + if (!set_desired_location_NED(next_point)) { + gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination"); + smart_rtl_state = SmartRTL_Failure; + } + } + // check if we've reached the next point + _distance_to_destination = get_distance(rover.current_loc, _destination); + if (_distance_to_destination <= rover.g.waypoint_radius || location_passed_point(rover.current_loc, _origin, _destination)) { + _load_point = true; + } + // continue driving towards destination + calc_steering_to_waypoint(_origin, _destination); + calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true); + break; + + case SmartRTL_StopAtHome: + case SmartRTL_Failure: + _reached_destination = true; + stop_vehicle(); + break; + } +} + +// save current position for use by the smart_rtl flight mode +void ModeSmartRTL::save_position(bool save_pos) +{ + g2.smart_rtl.update(true, save_pos); +} diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 23b2aa9278..83c184231f 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -133,6 +133,8 @@ void Rover::init_ardupilot() // give AHRS the range beacon sensor ahrs.set_beacon(&g2.beacon); + // initialize SmartRTL + g2.smart_rtl.init(); init_capabilities(); @@ -332,6 +334,9 @@ bool Rover::arm_motors(AP_Arming::ArmingMethod method) return false; } + // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point + g2.smart_rtl.reset_path(true); + change_arm_state(); return true; } @@ -354,3 +359,10 @@ bool Rover::disarm_motors(void) return true; } + +// save current position for use by the smart_rtl mode +void Rover::smart_rtl_update() +{ + const bool save_position = hal.util->get_soft_armed() && (control_mode != &mode_smartrtl); + mode_smartrtl.save_position(save_position); +}