diff --git a/ArduCopter/APM_Config.h b/ArduCopter/APM_Config.h index dbd050a06a..332704f4d1 100644 --- a/ArduCopter/APM_Config.h +++ b/ArduCopter/APM_Config.h @@ -25,6 +25,8 @@ //#define WINCH_ENABLED DISABLED // disable winch support //#define MODE_AUTO_ENABLED DISABLED // disable auto mode support //#define MODE_POSHOLD_ENABLED DISABLED // disable poshold mode support +//#define MODE_SMARTRTL_ENABLED DISABLED // disable smartrtl mode support + // features below are disabled by default on all boards //#define CAL_ALWAYS_REBOOT // flight controller will reboot after compass or accelerometer calibration completes diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index f3701bcae0..ae55fd7cfb 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -106,7 +106,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(update_altitude, 10, 100), SCHED_TASK(run_nav_updates, 50, 100), SCHED_TASK(update_throttle_hover,100, 90), +#if MODE_SMARTRTL_ENABLED == ENABLED SCHED_TASK_CLASS(Copter::ModeSmartRTL, &copter.mode_smartrtl, save_position, 3, 100), +#endif SCHED_TASK(three_hz_loop, 3, 75), SCHED_TASK(compass_accumulate, 100, 100), SCHED_TASK_CLASS(AP_Baro, &copter.barometer, accumulate, 50, 90), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 6cf2d06a14..2b23883932 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -984,7 +984,9 @@ private: #endif ModeThrow mode_throw; ModeGuidedNoGPS mode_guided_nogps; +#if MODE_SMARTRTL_ENABLED == ENABLED ModeSmartRTL mode_smartrtl; +#endif #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED ModeFlowHold mode_flowhold; #endif diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 4fb8d5699b..4a787caa22 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -944,9 +944,11 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(toy_mode, "TMODE", 20, ParametersG2, ToyMode), #endif +#if MODE_SMARTRTL_ENABLED == ENABLED // @Group: SRTL_ // @Path: ../libraries/AP_SmartRTL/AP_SmartRTL.cpp AP_SUBGROUPINFO(smart_rtl, "SRTL_", 21, ParametersG2, AP_SmartRTL), +#endif #if WINCH_ENABLED == ENABLED // @Group: WENC @@ -997,8 +999,9 @@ ParametersG2::ParametersG2(void) #if ADVANCED_FAILSAFE == ENABLED ,afs(copter.mission, copter.barometer, copter.gps, copter.rcmap) #endif +#if MODE_SMARTRTL_ENABLED == ENABLED ,smart_rtl(copter.ahrs) - ,temp_calibration(copter.barometer, copter.ins) +#endif #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED ,mode_flowhold_ptr(&copter.mode_flowhold) #endif diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 54f5cb344b..fe97f03de7 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -549,8 +549,10 @@ public: // control over servo output ranges SRV_Channels servo_channels; +#if MODE_SMARTRTL_ENABLED == ENABLED // Safe RTL library AP_SmartRTL smart_rtl; +#endif // wheel encoder and winch #if WINCH_ENABLED == ENABLED diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index db3e19df94..bb95a58aed 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -35,7 +35,9 @@ void Copter::set_home_to_current_location_inflight() { return; } // we have successfully set AHRS home, set it for SmartRTL +#if MODE_SMARTRTL_ENABLED == ENABLED g2.smart_rtl.set_home(true); +#endif } } @@ -48,7 +50,9 @@ bool Copter::set_home_to_current_location(bool lock) { return false; } // we have successfully set AHRS home, set it for SmartRTL +#if MODE_SMARTRTL_ENABLED == ENABLED g2.smart_rtl.set_home(true); +#endif return true; } return false; diff --git a/ArduCopter/config.h b/ArduCopter/config.h index b72a2973d6..b89091a535 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -279,6 +279,12 @@ # define MODE_POSHOLD_ENABLED ENABLED #endif +////////////////////////////////////////////////////////////////////////////// +// SmartRTL - allows vehicle to retrace a (loop-eliminated) breadcrumb home +#ifndef MODE_SMARTRTL_ENABLED +# define MODE_SMARTRTL_ENABLED ENABLED +#endif + ////////////////////////////////////////////////////////////////////////////// // RADIO CONFIGURATION ////////////////////////////////////////////////////////////////////////////// diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 53993e8af9..cee4783e29 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -118,9 +118,11 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) ret = &mode_guided_nogps; break; +#if MODE_SMARTRTL_ENABLED == ENABLED case SMART_RTL: ret = &mode_smartrtl; break; +#endif #if OPTFLOW == ENABLED case FLOWHOLD: @@ -249,10 +251,12 @@ void Copter::exit_mode(Copter::Mode *&old_flightmode, // cancel any takeoffs in progress takeoff_stop(); +#if MODE_SMARTRTL_ENABLED == ENABLED // call smart_rtl cleanup if (old_flightmode == &mode_smartrtl) { mode_smartrtl.exit(); } +#endif #if FRAME_CONFIG == HELI_FRAME // firmly reset the flybar passthrough to false when exiting acro mode. diff --git a/ArduCopter/mode_smart_rtl.cpp b/ArduCopter/mode_smart_rtl.cpp index 582d7ec3c0..ebeed8ad53 100644 --- a/ArduCopter/mode_smart_rtl.cpp +++ b/ArduCopter/mode_smart_rtl.cpp @@ -1,5 +1,7 @@ #include "Copter.h" +#if MODE_SMARTRTL_ENABLED == ENABLED + /* * Init and run calls for Smart_RTL flight mode * @@ -147,3 +149,5 @@ int32_t Copter::ModeSmartRTL::wp_bearing() const { return wp_nav->get_wp_bearing_to_destination(); } + +#endif diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index c4b2ca2ab6..b4097d6596 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -193,7 +193,9 @@ bool Copter::init_arm_motors(bool arming_from_gcs) update_super_simple_bearing(false); // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point +#if MODE_SMARTRTL_ENABLED == ENABLED g2.smart_rtl.set_home(position_ok()); +#endif // enable gps velocity based centrefugal force compensation ahrs.set_correct_centrifugal(true); diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 4d882e66e7..0113e230b7 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -236,8 +236,10 @@ void Copter::init_ardupilot() mission.init(); #endif +#if MODE_SMARTRTL_ENABLED == ENABLED // initialize SmartRTL g2.smart_rtl.init(); +#endif // initialise DataFlash library #if MODE_AUTO_ENABLED == ENABLED