Copter: avoid working with uninitialised home location

ArduCopter/defines.h

@@ -160,6 +160,7 @@ enum GuidedMode {
 
 // RTL states
 enum RTLState {
+    RTL_Starting,
     RTL_InitialClimb,
     RTL_ReturnHome,
     RTL_LoiterAtHome,

ArduCopter/mode.h

@@ -1009,10 +1009,9 @@ private:
     void climb_return_run();
     void loiterathome_start();
     void loiterathome_run();
-    void build_path(bool terrain_following_allowed);
+    void build_path();
-    void compute_return_target(bool terrain_following_allowed);
+    void compute_return_target();
 
-    // 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
 
@@ -1029,7 +1028,7 @@ private:
     // Loiter timer - Records how long we have been in loiter
     uint32_t _loiter_start_time;
 
-    bool path_built;
+    bool terrain_following_allowed;
 };
 
 

ArduCopter/mode_rtl.cpp

@@ -19,8 +19,9 @@ bool Copter::ModeRTL::init(bool ignore_checks)
     }
     // initialise waypoint and spline controller
     wp_nav->wp_and_spline_init();
-    path_built = false;
+    _state = RTL_Starting;
-    climb_start();
+    _state_complete = true; // see run() method below
+    terrain_following_allowed = !copter.failsafe.terrain;
     return true;
 }
 
@@ -29,8 +30,9 @@ void Copter::ModeRTL::restart_without_terrain()
 {
     AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL);
     if (rtl_path.terrain_used) {
-        build_path(false);
+        terrain_following_allowed = false;
-        climb_start();
+        _state = RTL_Starting;
+        _state_complete = true;
         gcs().send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing");
     }
 }
@@ -39,14 +41,17 @@ void Copter::ModeRTL::restart_without_terrain()
 // should be called at 100hz or more
 void Copter::ModeRTL::run(bool disarm_on_land)
 {
-    if (!path_built) {
+    if (!motors->armed()) {
-        path_built = true;
+        return;
-        build_path(!copter.failsafe.terrain);
     }
 
     // check if we need to move to next state
     if (_state_complete) {
         switch (_state) {
+        case RTL_Starting:
+            build_path();
+            climb_start();
+            break;
         case RTL_InitialClimb:
             return_start();
             break;
@@ -72,6 +77,11 @@ void Copter::ModeRTL::run(bool disarm_on_land)
     // call the correct run function
     switch (_state) {
 
+    case RTL_Starting:
+        // should not be reached:
+        _state = RTL_InitialClimb;
+        FALLTHROUGH;
+
     case RTL_InitialClimb:
         climb_return_run();
         break;
@@ -385,7 +395,7 @@ void Copter::ModeRTL::land_run(bool disarm_on_land)
     land_run_vertical_control();
 }
 
-void Copter::ModeRTL::build_path(bool terrain_following_allowed)
+void Copter::ModeRTL::build_path()
 {
     // origin point is our stopping point
     Vector3f stopping_point;
@@ -395,7 +405,7 @@ void Copter::ModeRTL::build_path(bool terrain_following_allowed)
     rtl_path.origin_point.change_alt_frame(Location::AltFrame::ABOVE_HOME);
 
     // compute return target
-    compute_return_target(terrain_following_allowed);
+    compute_return_target();
 
     // climb target is above our origin point at the return altitude
     rtl_path.climb_target = Location(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame());
@@ -410,7 +420,7 @@ void Copter::ModeRTL::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::ModeRTL::compute_return_target(bool terrain_following_allowed)
+void Copter::ModeRTL::compute_return_target()
 {
     // set return target to nearest rally point or home position (Note: alt is absolute)
 #if AC_RALLY == ENABLED