From 682f3c0e7ebb23db61a564c5884ba35c2d5df6d6 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 22 Mar 2016 17:42:17 +1100 Subject: [PATCH] Copter: FlightMode - convert RTL flight mode --- ArduCopter/Copter.cpp | 3 - ArduCopter/Copter.h | 33 +------ ArduCopter/FlightMode.h | 67 +++++++++++++ ArduCopter/commands_logic.cpp | 2 +- ArduCopter/control_auto.cpp | 6 +- ArduCopter/control_rtl.cpp | 171 ++++++++++++++++++---------------- ArduCopter/events.cpp | 2 +- ArduCopter/flight_mode.cpp | 14 +-- ArduCopter/heli.cpp | 6 +- ArduCopter/landing_gear.cpp | 2 +- 10 files changed, 175 insertions(+), 131 deletions(-) diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 40a2bd0967..b955ac7fdc 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -33,8 +33,6 @@ Copter::Copter(void) home_bearing(0), home_distance(0), wp_distance(0), - rtl_state(RTL_InitialClimb), - rtl_state_complete(false), smart_rtl_state(SmartRTL_PathFollow), simple_cos_yaw(1.0f), simple_sin_yaw(0.0f), @@ -62,7 +60,6 @@ Copter::Copter(void) pmTest1(0), fast_loopTimer(0), mainLoop_count(0), - rtl_loiter_start_time(0), auto_trim_counter(0), in_mavlink_delay(false), param_loader(var_info) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 86bc769b55..f05854a215 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -392,20 +392,6 @@ private: float descend_max; // centimetres } nav_payload_place; - // RTL - RTLState rtl_state; // records state of rtl (initial climb, returning home, etc) - bool rtl_state_complete; // set to true if the current state is completed - - struct { - // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain - Location_Class origin_point; - Location_Class climb_target; - Location_Class return_target; - Location_Class descent_target; - bool land; - bool terrain_used; - } rtl_path; - // SmartRTL SmartRTLState smart_rtl_state; // records state of SmartRTL @@ -532,8 +518,6 @@ private: uint32_t fast_loopTimer; // Counter of main loop executions. Used for performance monitoring and failsafe processing uint16_t mainLoop_count; - // Loiter timer - Records how long we have been in loiter - uint32_t rtl_loiter_start_time; // arm_time_ms - Records when vehicle was armed. Will be Zero if we are disarmed. uint32_t arm_time_ms; @@ -862,20 +846,6 @@ private: bool throw_height_good(); bool throw_position_good(); - bool rtl_init(bool ignore_checks); - void rtl_restart_without_terrain(); - void rtl_run(bool disarm_on_land=true); - void rtl_climb_start(); - void rtl_return_start(); - void rtl_climb_return_run(); - void rtl_loiterathome_start(); - void rtl_loiterathome_run(); - void rtl_descent_start(); - void rtl_descent_run(); - void rtl_land_start(); - void rtl_land_run(bool disarm_on_land); - void rtl_build_path(bool terrain_following_allowed); - void rtl_compute_return_target(bool terrain_following_allowed); bool smart_rtl_init(bool ignore_checks); void smart_rtl_exit(); void smart_rtl_run(); @@ -884,6 +854,7 @@ private: void smart_rtl_pre_land_position_run(); void smart_rtl_land(); void smart_rtl_save_position(); + bool sport_init(bool ignore_checks); void sport_run(); void crash_check(); @@ -1102,6 +1073,8 @@ private: Copter::FlightMode_LOITER flightmode_loiter{*this}; + Copter::FlightMode_RTL flightmode_rtl{*this}; + #if FRAME_CONFIG == HELI_FRAME Copter::FlightMode_STABILIZE_Heli flightmode_stabilize{*this}; #else diff --git a/ArduCopter/FlightMode.h b/ArduCopter/FlightMode.h index 17f8efe603..2e295754ff 100644 --- a/ArduCopter/FlightMode.h +++ b/ArduCopter/FlightMode.h @@ -510,3 +510,70 @@ private: void gps_run(); void nogps_run(); }; + + + +class FlightMode_RTL : public FlightMode { + +public: + + FlightMode_RTL(Copter &copter) : + Copter::FlightMode(copter) + { } + + bool init(bool ignore_checks) override; + void run() override { // should be called at 100hz or more + return run(true); + } + void run(bool disarm_on_land); // should be called at 100hz or more + + bool requires_GPS() const override { return true; } + bool has_manual_throttle() const override { return false; } + bool allows_arming(bool from_gcs) const override { return true; }; + bool is_autopilot() const override { return true; } + + RTLState state() { return _state; } + + // this should probably not be exposed + bool state_complete() { return _state_complete; } + + bool landing_gear_should_be_deployed(); + + void restart_without_terrain(); + +protected: + + const char *name() const override { return "RTL"; } + const char *name4() const override { return "RTL "; } + +private: + + void climb_start(); + void return_start(); + void climb_return_run(); + void loiterathome_start(); + void loiterathome_run(); + void descent_start(); + void descent_run(); + void land_start(); + void land_run(bool disarm_on_land); + void build_path(bool terrain_following_allowed); + void compute_return_target(bool terrain_following_allowed); + + // RTL + RTLState _state = RTL_InitialClimb; // records state of rtl (initial climb, returning home, etc) + bool _state_complete = false; // set to true if the current state is completed + + struct { + // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain + Location_Class origin_point; + Location_Class climb_target; + Location_Class return_target; + Location_Class descent_target; + bool land; + bool terrain_used; + } rtl_path; + + // Loiter timer - Records how long we have been in loiter + uint32_t _loiter_start_time = 0; +}; diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 1e2c6d520b..bfeff4110b 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -970,7 +970,7 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd) // returns true with RTL has completed successfully bool Copter::verify_RTL() { - return (rtl_state_complete && (rtl_state == RTL_FinalDescent || rtl_state == RTL_Land)); + return (flightmode_rtl.state_complete() && (flightmode_rtl.state() == RTL_FinalDescent || flightmode_rtl.state() == RTL_Land)); } // verify_spline_wp - check if we have reached the next way point using spline diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index 3783b410d6..64003d8455 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -417,7 +417,7 @@ bool Copter::FlightMode_AUTO::landing_gear_should_be_deployed() case Auto_Land: return true; case Auto_RTL: - switch(_copter.rtl_state) { + switch(_copter.flightmode_rtl.state()) { case RTL_LoiterAtHome: case RTL_Land: case RTL_FinalDescent: @@ -438,7 +438,7 @@ void Copter::FlightMode_AUTO::rtl_start() _mode = Auto_RTL; // call regular rtl flight mode initialisation and ask it to ignore checks - _copter.rtl_init(true); + _copter.flightmode_rtl.init(true); } // auto_rtl_run - rtl in AUTO flight mode @@ -446,7 +446,7 @@ void Copter::FlightMode_AUTO::rtl_start() void Copter::FlightMode_AUTO::rtl_run() { // call regular rtl flight mode run function - _copter.rtl_run(false); + _copter.flightmode_rtl.run(false); } // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index bd245336bc..e4fe72cca3 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -8,13 +8,13 @@ */ // rtl_init - initialise rtl controller -bool Copter::rtl_init(bool ignore_checks) +bool Copter::FlightMode_RTL::init(bool ignore_checks) { - if (position_ok() || ignore_checks) { + if (_copter.position_ok() || ignore_checks) { // initialise waypoint and spline controller wp_nav->wp_and_spline_init(); - rtl_build_path(!failsafe.terrain); - rtl_climb_start(); + build_path(!_copter.failsafe.terrain); + climb_start(); return true; }else{ return false; @@ -22,35 +22,35 @@ bool Copter::rtl_init(bool ignore_checks) } // re-start RTL with terrain following disabled -void Copter::rtl_restart_without_terrain() +void Copter::FlightMode_RTL::restart_without_terrain() { // log an error - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL); + _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL); if (rtl_path.terrain_used) { - rtl_build_path(false); - rtl_climb_start(); + build_path(false); + climb_start(); gcs().send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing"); } } // rtl_run - runs the return-to-launch controller // should be called at 100hz or more -void Copter::rtl_run(bool disarm_on_land) +void Copter::FlightMode_RTL::run(bool disarm_on_land) { // check if we need to move to next state - if (rtl_state_complete) { - switch (rtl_state) { + if (_state_complete) { + switch (_state) { case RTL_InitialClimb: - rtl_return_start(); + return_start(); break; case RTL_ReturnHome: - rtl_loiterathome_start(); + loiterathome_start(); break; case RTL_LoiterAtHome: - if (rtl_path.land || failsafe.radio) { - rtl_land_start(); + if (rtl_path.land || _copter.failsafe.radio) { + land_start(); }else{ - rtl_descent_start(); + descent_start(); } break; case RTL_FinalDescent: @@ -63,35 +63,35 @@ void Copter::rtl_run(bool disarm_on_land) } // call the correct run function - switch (rtl_state) { + switch (_state) { case RTL_InitialClimb: - rtl_climb_return_run(); + climb_return_run(); break; case RTL_ReturnHome: - rtl_climb_return_run(); + climb_return_run(); break; case RTL_LoiterAtHome: - rtl_loiterathome_run(); + loiterathome_run(); break; case RTL_FinalDescent: - rtl_descent_run(); + descent_run(); break; case RTL_Land: - rtl_land_run(disarm_on_land); + land_run(disarm_on_land); break; } } // rtl_climb_start - initialise climb to RTL altitude -void Copter::rtl_climb_start() +void Copter::FlightMode_RTL::climb_start() { - rtl_state = RTL_InitialClimb; - rtl_state_complete = false; + _state = RTL_InitialClimb; + _state_complete = false; // RTL_SPEED == 0 means use WPNAV_SPEED if (g.rtl_speed_cms != 0) { @@ -101,8 +101,8 @@ void Copter::rtl_climb_start() // set the destination if (!wp_nav->set_wp_destination(rtl_path.climb_target)) { // this should not happen because rtl_build_path will have checked terrain data was available - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); - set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE); + _copter.Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); + _copter.set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE); return; } wp_nav->set_fast_waypoint(true); @@ -112,23 +112,23 @@ void Copter::rtl_climb_start() } // rtl_return_start - initialise return to home -void Copter::rtl_return_start() +void Copter::FlightMode_RTL::return_start() { - rtl_state = RTL_ReturnHome; - rtl_state_complete = false; + _state = RTL_ReturnHome; + _state_complete = false; if (!wp_nav->set_wp_destination(rtl_path.return_target)) { // failure must be caused by missing terrain data, restart RTL - rtl_restart_without_terrain(); + restart_without_terrain(); } // initialise yaw to point home (maybe) - set_auto_yaw_mode(get_default_auto_yaw_mode(true)); + set_auto_yaw_mode(_copter.get_default_auto_yaw_mode(true)); } // rtl_climb_return_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more -void Copter::rtl_climb_return_run() +void Copter::FlightMode_RTL::climb_return_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { @@ -148,7 +148,7 @@ void Copter::rtl_climb_return_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!_copter.failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { @@ -160,13 +160,13 @@ void Copter::rtl_climb_return_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - failsafe_terrain_set_status(wp_nav->update_wpnav()); + _copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control->update_z_controller(); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ @@ -175,18 +175,18 @@ void Copter::rtl_climb_return_run() } // check if we've completed this stage of RTL - rtl_state_complete = wp_nav->reached_wp_destination(); + _state_complete = wp_nav->reached_wp_destination(); } // rtl_loiterathome_start - initialise return to home -void Copter::rtl_loiterathome_start() +void Copter::FlightMode_RTL::loiterathome_start() { - rtl_state = RTL_LoiterAtHome; - rtl_state_complete = false; - rtl_loiter_start_time = millis(); + _state = RTL_LoiterAtHome; + _state_complete = false; + _loiter_start_time = millis(); // yaw back to initial take-off heading yaw unless pilot has already overridden yaw - if(get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) { + if(_copter.get_default_auto_yaw_mode(true) != AUTO_YAW_HOLD) { set_auto_yaw_mode(AUTO_YAW_RESETTOARMEDYAW); } else { set_auto_yaw_mode(AUTO_YAW_HOLD); @@ -195,7 +195,7 @@ void Copter::rtl_loiterathome_start() // rtl_climb_return_descent_run - implements the initial climb, return home and descent portions of RTL which all rely on the wp controller // called by rtl_run at 100hz or more -void Copter::rtl_loiterathome_run() +void Copter::FlightMode_RTL::loiterathome_run() { // if not auto armed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) { @@ -215,7 +215,7 @@ void Copter::rtl_loiterathome_run() // process pilot's yaw input float target_yaw_rate = 0; - if (!failsafe.radio) { + if (!_copter.failsafe.radio) { // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); if (!is_zero(target_yaw_rate)) { @@ -227,13 +227,13 @@ void Copter::rtl_loiterathome_run() motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - failsafe_terrain_set_status(wp_nav->update_wpnav()); + _copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control->update_z_controller(); // call attitude controller - if (auto_yaw_mode == AUTO_YAW_HOLD) { + if (_copter.auto_yaw_mode == AUTO_YAW_HOLD) { // roll & pitch from waypoint controller, yaw rate from pilot attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); }else{ @@ -242,24 +242,24 @@ void Copter::rtl_loiterathome_run() } // check if we've completed this stage of RTL - if ((millis() - rtl_loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) { - if (auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) { + if ((millis() - _loiter_start_time) >= (uint32_t)g.rtl_loiter_time.get()) { + if (_copter.auto_yaw_mode == AUTO_YAW_RESETTOARMEDYAW) { // check if heading is within 2 degrees of heading when vehicle was armed - if (labs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) { - rtl_state_complete = true; + if (labs(wrap_180_cd(ahrs.yaw_sensor-_copter.initial_armed_bearing)) <= 200) { + _state_complete = true; } } else { // we have loitered long enough - rtl_state_complete = true; + _state_complete = true; } } } // rtl_descent_start - initialise descent to final alt -void Copter::rtl_descent_start() +void Copter::FlightMode_RTL::descent_start() { - rtl_state = RTL_FinalDescent; - rtl_state_complete = false; + _state = RTL_FinalDescent; + _state_complete = false; // Set wp navigation target to above home wp_nav->init_loiter_target(wp_nav->get_wp_destination()); @@ -273,7 +273,7 @@ void Copter::rtl_descent_start() // rtl_descent_run - implements the final descent to the RTL_ALT // called by rtl_run at 100hz or more -void Copter::rtl_descent_run() +void Copter::FlightMode_RTL::descent_run() { int16_t roll_control = 0, pitch_control = 0; float target_yaw_rate = 0; @@ -295,12 +295,12 @@ void Copter::rtl_descent_run() } // process pilot's input - if (!failsafe.radio) { - if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ + if (!_copter.failsafe.radio) { + if ((g.throttle_behavior & THR_BEHAVE_HIGH_THROTTLE_CANCELS_LAND) != 0 && _copter.rc_throttle_control_in_filter.get() > LAND_CANCEL_TRIGGER_THR){ Log_Write_Event(DATA_LAND_CANCELLED_BY_PILOT); // exit land if throttle is high - if (!set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { - set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); + if (!_copter.set_mode(LOITER, MODE_REASON_THROTTLE_LAND_ESCAPE)) { + _copter.set_mode(ALT_HOLD, MODE_REASON_THROTTLE_LAND_ESCAPE); } } @@ -334,14 +334,14 @@ void Copter::rtl_descent_run() attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain()); // check if we've reached within 20cm of final altitude - rtl_state_complete = labs(rtl_path.descent_target.alt - current_loc.alt) < 20; + _state_complete = labs(rtl_path.descent_target.alt - _copter.current_loc.alt) < 20; } // rtl_loiterathome_start - initialise controllers to loiter over home -void Copter::rtl_land_start() +void Copter::FlightMode_RTL::land_start() { - rtl_state = RTL_Land; - rtl_state_complete = false; + _state = RTL_Land; + _state_complete = false; // Set wp navigation target to above home wp_nav->init_loiter_target(wp_nav->get_wp_destination()); @@ -356,9 +356,22 @@ void Copter::rtl_land_start() set_auto_yaw_mode(AUTO_YAW_HOLD); } +bool Copter::FlightMode_RTL::landing_gear_should_be_deployed() +{ + switch(_state) { + case RTL_LoiterAtHome: + case RTL_Land: + case RTL_FinalDescent: + return true; + default: + return false; + } + return false; +} + // rtl_returnhome_run - return home // called by rtl_run at 100hz or more -void Copter::rtl_land_run(bool disarm_on_land) +void Copter::FlightMode_RTL::land_run(bool disarm_on_land) { // if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) { @@ -376,25 +389,25 @@ void Copter::rtl_land_run(bool disarm_on_land) // disarm when the landing detector says we've landed if (ap.land_complete && disarm_on_land) { - init_disarm_motors(); + _copter.init_disarm_motors(); } // check if we've completed this stage of RTL - rtl_state_complete = ap.land_complete; + _state_complete = ap.land_complete; return; } // set motors to full range motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); - land_run_horizontal_control(); - land_run_vertical_control(); + _copter.land_run_horizontal_control(); + _copter.land_run_vertical_control(); // check if we've completed this stage of RTL - rtl_state_complete = ap.land_complete; + _state_complete = ap.land_complete; } -void Copter::rtl_build_path(bool terrain_following_allowed) +void Copter::FlightMode_RTL::build_path(bool terrain_following_allowed) { // origin point is our stopping point Vector3f stopping_point; @@ -404,7 +417,7 @@ void Copter::rtl_build_path(bool terrain_following_allowed) rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME); // compute return target - rtl_compute_return_target(terrain_following_allowed); + compute_return_target(terrain_following_allowed); // climb target is above our origin point at the return altitude rtl_path.climb_target = Location_Class(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame()); @@ -419,28 +432,28 @@ void Copter::rtl_build_path(bool terrain_following_allowed) // compute the return target - home or rally point // return altitude in cm above home at which vehicle should return home // return target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set) -void Copter::rtl_compute_return_target(bool terrain_following_allowed) +void Copter::FlightMode_RTL::compute_return_target(bool terrain_following_allowed) { // set return target to nearest rally point or home position (Note: alt is absolute) #if AC_RALLY == ENABLED - rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt); + rtl_path.return_target = _copter.rally.calc_best_rally_or_home_location(_copter.current_loc, ahrs.get_home().alt); #else rtl_path.return_target = ahrs.get_home(); #endif // curr_alt is current altitude above home or above terrain depending upon use_terrain - int32_t curr_alt = current_loc.alt; + int32_t curr_alt = _copter.current_loc.alt; // decide if we should use terrain altitudes - rtl_path.terrain_used = terrain_use() && terrain_following_allowed; + rtl_path.terrain_used = _copter.terrain_use() && terrain_following_allowed; if (rtl_path.terrain_used) { // attempt to retrieve terrain alt for current location, stopping point and origin int32_t origin_terr_alt, return_target_terr_alt; if (!rtl_path.origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) || !rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) || - !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) { + !_copter.current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) { rtl_path.terrain_used = false; - Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); + _copter.Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); } } @@ -478,10 +491,10 @@ void Copter::rtl_compute_return_target(bool terrain_following_allowed) // if terrain altitudes are being used, the code below which reduces the return_target's altitude can lead to // the vehicle not climbing at all as RTL begins. This can be overly conservative and it might be better // to apply the fence alt limit independently on the origin_point and return_target - if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { + if ((_copter.fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { // get return target as alt-above-home so it can be compared to fence's alt if (rtl_path.return_target.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_HOME, target_alt)) { - float fence_alt = fence.get_safe_alt_max()*100.0f; + float fence_alt = _copter.fence.get_safe_alt_max()*100.0f; if (target_alt > fence_alt) { // reduce target alt to the fence alt rtl_path.return_target.alt -= (target_alt - fence_alt); diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 4d692d852c..6367447d28 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -178,7 +178,7 @@ void Copter::failsafe_terrain_on_event() if (should_disarm_on_failsafe()) { init_disarm_motors(); } else if (control_mode == RTL) { - rtl_restart_without_terrain(); + flightmode_rtl.restart_without_terrain(); } else { set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE); } diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 4d17407002..a6e0221d1f 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -96,7 +96,10 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) break; case RTL: - success = rtl_init(ignore_checks); + success = flightmode_rtl.init(ignore_checks); + if (success) { + flightmode = &flightmode_rtl; + } break; case DRIFT: @@ -209,10 +212,6 @@ void Copter::update_flight_mode() switch (control_mode) { - case RTL: - rtl_run(); - break; - case DRIFT: drift_run(); break; @@ -323,7 +322,6 @@ bool Copter::mode_requires_GPS() return flightmode->requires_GPS(); } switch (control_mode) { - case RTL: case SMART_RTL: case DRIFT: case POSHOLD: @@ -373,7 +371,6 @@ void Copter::notify_flight_mode() return; } switch (control_mode) { - case RTL: case AVOID_ADSB: case GUIDED_NOGPS: case SMART_RTL: @@ -388,9 +385,6 @@ void Copter::notify_flight_mode() // set flight mode string switch (control_mode) { - case RTL: - notify.set_flight_mode_str("RTL "); - break; case DRIFT: notify.set_flight_mode_str("DRIF"); break; diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index bbaeff9e5d..cb7887a637 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -23,8 +23,8 @@ void Copter::heli_init() // should be called at 50hz void Copter::check_dynamic_flight(void) { - if (!motor->armed() || !motors->rotor_runup_complete() || - control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && flightmode_auto.mode() == Auto_Land)) { + if (!motors->armed() || !motors->rotor_runup_complete() || + control_mode == LAND || (control_mode==RTL && flightmode_rtl.state() == RTL_Land) || (control_mode == AUTO && flightmode_auto.mode() == Auto_Land)) { heli_dynamic_flight_counter = 0; heli_flags.dynamic_flight = false; return; @@ -109,7 +109,7 @@ void Copter::heli_update_landing_swash() case RTL: case SMART_RTL: - if (rtl_state == RTL_Land) { + if (flightmode_rtl.state() == RTL_Land) { motors->set_collective_for_landing(true); }else{ motors->set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed); diff --git a/ArduCopter/landing_gear.cpp b/ArduCopter/landing_gear.cpp index 86b0c0a525..81c82b54b0 100644 --- a/ArduCopter/landing_gear.cpp +++ b/ArduCopter/landing_gear.cpp @@ -15,7 +15,7 @@ void Copter::landinggear_update() // if we are doing an automatic landing procedure, force the landing gear to deploy. // To-Do: should we pause the auto-land procedure to give time for gear to come down? if (control_mode == LAND || - (control_mode == RTL && (rtl_state == RTL_LoiterAtHome || rtl_state == RTL_Land || rtl_state == RTL_FinalDescent)) || + (control_mode == RTL && flightmode_rtl.landing_gear_should_be_deployed()) || (control_mode == AUTO && flightmode_auto.landing_gear_should_be_deployed())) { landinggear.set_position(AP_LandingGear::LandingGear_Deploy_And_Keep_Deployed); }