From 44a436642b3c6f84ce4e8bd430d46d2a444c11f0 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 14 Sep 2017 22:21:33 +0900 Subject: [PATCH] Copter: complete rename to SmartRTL Also fix parameter description for CH7_OPT --- ArduCopter/ArduCopter.cpp | 2 +- ArduCopter/Copter.cpp | 2 +- ArduCopter/Copter.h | 4 ++-- ArduCopter/Parameters.cpp | 12 ++++++------ ArduCopter/control_safe_rtl.cpp | 24 ++++++++++++------------ ArduCopter/defines.h | 14 +++++++------- ArduCopter/motors.cpp | 2 +- ArduCopter/switches.cpp | 2 +- ArduCopter/system.cpp | 2 +- 9 files changed, 32 insertions(+), 32 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 1bcdbda7f8..8e409bab12 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -64,7 +64,7 @@ * Roberto Navoni :Library testing, Porting to VRBrain * Sandro Benigno :Camera support, MinimOSD * Sandro Tognana :PosHold flight mode - * Sebastian Quilter :SafeRTL + * Sebastian Quilter :SmartRTL * ..and many more. * * Code commit statistics can be found here: https://github.com/ArduPilot/ardupilot/graphs/contributors diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index fa35db220e..4d32eb7a18 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -37,7 +37,7 @@ Copter::Copter(void) : guided_mode(Guided_TakeOff), rtl_state(RTL_InitialClimb), rtl_state_complete(false), - smart_rtl_state(SafeRTL_PathFollow), + smart_rtl_state(SmartRTL_PathFollow), circle_pilot_yaw_override(false), simple_cos_yaw(1.0f), simple_sin_yaw(0.0f), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 9baff113d8..6aca80cb17 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -414,8 +414,8 @@ private: bool terrain_used; } rtl_path; - // SafeRTL - SafeRTLState smart_rtl_state; // records state of SafeRTL + // SmartRTL + SmartRTLState smart_rtl_state; // records state of SmartRTL // Circle bool circle_pilot_yaw_override; // true if pilot is overriding yaw diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index a848859dc7..fb3e5095c7 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -389,42 +389,42 @@ const AP_Param::Info Copter::var_info[] = { // @Param: CH7_OPT // @DisplayName: Channel 7 option // @Description: Select which function is performed when CH7 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL // @User: Standard GSCALAR(ch7_option, "CH7_OPT", AUXSW_DO_NOTHING), // @Param: CH8_OPT // @DisplayName: Channel 8 option // @Description: Select which function is performed when CH8 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL // @User: Standard GSCALAR(ch8_option, "CH8_OPT", AUXSW_DO_NOTHING), // @Param: CH9_OPT // @DisplayName: Channel 9 option // @Description: Select which function is performed when CH9 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL // @User: Standard GSCALAR(ch9_option, "CH9_OPT", AUXSW_DO_NOTHING), // @Param: CH10_OPT // @DisplayName: Channel 10 option // @Description: Select which function is performed when CH10 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL // @User: Standard GSCALAR(ch10_option, "CH10_OPT", AUXSW_DO_NOTHING), // @Param: CH11_OPT // @DisplayName: Channel 11 option // @Description: Select which function is performed when CH11 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL // @User: Standard GSCALAR(ch11_option, "CH11_OPT", AUXSW_DO_NOTHING), // @Param: CH12_OPT // @DisplayName: Channel 12 option // @Description: Select which function is performed when CH12 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 52:SafeRTL + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 9:Camera Trigger, 10:RangeFinder, 11:Fence, 13:Super Simple Mode, 14:Acro Trainer, 15:Sprayer, 16:Auto, 17:AutoTune, 18:Land, 19:Gripper, 21:Parachute Enable, 22:Parachute Release, 23:Parachute 3pos, 24:Auto Mission Reset, 25:AttCon Feed Forward, 26:AttCon Accel Limits, 27:Retract Mount, 28:Relay On/Off, 34:Relay2 On/Off, 35:Relay3 On/Off, 36:Relay4 On/Off, 29:Landing Gear, 30:Lost Copter Sound, 31:Motor Emergency Stop, 32:Motor Interlock, 33:Brake, 37:Throw, 38:ADSB-Avoidance, 39:PrecLoiter, 40:Object Avoidance, 41:ArmDisarm, 42:SmartRTL // @User: Standard GSCALAR(ch12_option, "CH12_OPT", AUXSW_DO_NOTHING), diff --git a/ArduCopter/control_safe_rtl.cpp b/ArduCopter/control_safe_rtl.cpp index a0a35e2157..48ac4481ad 100644 --- a/ArduCopter/control_safe_rtl.cpp +++ b/ArduCopter/control_safe_rtl.cpp @@ -3,7 +3,7 @@ /* * Init and run calls for Smart_RTL flight mode * - * This code uses the SafeRTL path that is already in memory, and feeds it into WPNav, one point at a time. + * This code uses the SmartRTL path that is already in memory, and feeds it into WPNav, one point at a time. * Once the copter is close to home, it will run a standard land controller. */ @@ -23,7 +23,7 @@ bool Copter::smart_rtl_init(bool ignore_checks) set_auto_yaw_mode(get_default_auto_yaw_mode(true)); // wait for cleanup of return path - smart_rtl_state = SafeRTL_WaitForPathCleanup; + smart_rtl_state = SmartRTL_WaitForPathCleanup; return true; } @@ -39,19 +39,19 @@ void Copter::smart_rtl_exit() void Copter::smart_rtl_run() { switch (smart_rtl_state) { - case SafeRTL_WaitForPathCleanup: + case SmartRTL_WaitForPathCleanup: smart_rtl_wait_cleanup_run(); break; - case SafeRTL_PathFollow: + case SmartRTL_PathFollow: smart_rtl_path_follow_run(); break; - case SafeRTL_PreLandPosition: + case SmartRTL_PreLandPosition: smart_rtl_pre_land_position_run(); break; - case SafeRTL_Descend: + case SmartRTL_Descend: rtl_descent_run(); // Re-using the descend method from normal rtl mode. break; - case SafeRTL_Land: + case SmartRTL_Land: rtl_land_run(); // Re-using the land method from normal rtl mode. break; } @@ -67,7 +67,7 @@ void Copter::smart_rtl_wait_cleanup_run() // check if return path is computed and if yes, begin journey home if (g2.smart_rtl.request_thorough_cleanup()) { - smart_rtl_state = SafeRTL_PathFollow; + smart_rtl_state = SmartRTL_PathFollow; } } @@ -81,7 +81,7 @@ void Copter::smart_rtl_path_follow_run() if (g2.smart_rtl.get_num_points() == 0) { // this is the very last point, add 2m to the target alt and move to pre-land state next_point.z -= 2.0f; - smart_rtl_state = SafeRTL_PreLandPosition; + smart_rtl_state = SmartRTL_PreLandPosition; fast_waypoint = false; } // send target to waypoint controller @@ -89,7 +89,7 @@ void Copter::smart_rtl_path_follow_run() wp_nav->set_fast_waypoint(fast_waypoint); } else { // this can only happen if we fail to get the semaphore which should never happen but just in case, land - smart_rtl_state = SafeRTL_PreLandPosition; + smart_rtl_state = SmartRTL_PreLandPosition; } } @@ -115,11 +115,11 @@ void Copter::smart_rtl_pre_land_position_run() // choose descend and hold, or land based on user parameter rtl_alt_final if (g.rtl_alt_final <= 0 || failsafe.radio) { rtl_land_start(); - smart_rtl_state = SafeRTL_Land; + smart_rtl_state = SmartRTL_Land; } else { rtl_path.descent_target.alt = g.rtl_alt_final; rtl_descent_start(); - smart_rtl_state = SafeRTL_Descend; + smart_rtl_state = SmartRTL_Descend; } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 04aa1510dd..ae932bc41b 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -70,7 +70,7 @@ enum aux_sw_func { AUXSW_PRECISION_LOITER = 39, // enable precision loiter AUXSW_AVOID_PROXIMITY = 40, // enable object avoidance using proximity sensors (ie. horizontal lidar) AUXSW_ARMDISARM = 41, // arm or disarm vehicle - AUXSW_SMART_RTL = 42, // change to SafeRTL flight mode + AUXSW_SMART_RTL = 42, // change to SmartRTL flight mode AUXSW_SWITCH_MAX, }; @@ -222,12 +222,12 @@ enum RTLState { }; // Safe RTL states -enum SafeRTLState { - SafeRTL_WaitForPathCleanup, - SafeRTL_PathFollow, - SafeRTL_PreLandPosition, - SafeRTL_Descend, - SafeRTL_Land +enum SmartRTLState { + SmartRTL_WaitForPathCleanup, + SmartRTL_PathFollow, + SmartRTL_PreLandPosition, + SmartRTL_Descend, + SmartRTL_Land }; // Alt_Hold states diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index afbd09eb24..382f05645d 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -179,7 +179,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs) } calc_distance_and_bearing(); - // Reset SafeRTL return location. If activated, SafeRTL will ultimately try to land at this point + // Reset SmartRTL return location. If activated, SmartRTL will ultimately try to land at this point g2.smart_rtl.reset_path(position_ok()); // enable gps velocity based centrefugal force compensation diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index fad66d9b9e..626da8b57d 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -597,7 +597,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) case AUXSW_SMART_RTL: if (ch_flag == AUX_SWITCH_HIGH) { - // engage SafeRTL (if not possible we remain in current flight mode) + // engage SmartRTL (if not possible we remain in current flight mode) set_mode(SMART_RTL, MODE_REASON_TX_COMMAND); } else { // return to flight mode switch's flight mode if we are currently in RTL diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 584a35f89b..dc741bbf60 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -228,7 +228,7 @@ void Copter::init_ardupilot() // initialise mission library mission.init(); - // initialize SafeRTL + // initialize SmartRTL g2.smart_rtl.init(); // initialise DataFlash library