Copter: reload takeoff cmd on arm when using AllowTakeOffWithoutRaisi…

…ngThrottle

USING AUTO_OPTIONS 135 we set auto mode and arm from it if we got a takeoff cmd as first nav cmd.
Doing this, the takeoff cmd is loaded when we enter the auto mode and set the target alt relatively to origin https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/mode_auto.cpp#L343
This is never called again. So when we reset the home from the first arming, we don't update the target alt relativly to origin. This leads to a wrong takeoff alt settle due to the alt drift on ground.

ArduCopter/mode.h

@@ -725,6 +725,8 @@ class ModeAuto : public Mode {
         float up;     // desired speed upwards in m/s. 0 if unset
         float down;   // desired speed downwards in m/s. 0 if unset
     } desired_speed_override;
+
+    Location takeoff_loc;  // location of takeoff command
 };
 
 #if AUTOTUNE_ENABLED == ENABLED

ArduCopter/mode_auto.cpp

@@ -335,19 +335,18 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
         alt_target_terrain = true;
     } else {
         // set horizontal target
-        Location dest(dest_loc);
-        dest.lat = copter.current_loc.lat;
-        dest.lng = copter.current_loc.lng;
+        takeoff_loc = Location(copter.current_loc.lat, copter.current_loc.lng, dest_loc.alt, dest_loc.get_alt_frame());
 
         // get altitude target above EKF origin
-        if (!dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) {
+        if (!takeoff_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) {
             // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
             LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
             // fall back to altitude above current altitude
-            alt_target_cm = current_alt_cm + dest.alt;
+            alt_target_cm = current_alt_cm + takeoff_loc.alt;
         }
     }
 
+
     // sanity check target
     int32_t alt_target_min_cm = current_alt_cm + (copter.ap.land_complete ? 100 : 0);
     alt_target_cm = MAX(alt_target_cm, alt_target_min_cm);
@@ -992,7 +991,10 @@ void ModeAuto::takeoff_run()
     // if the user doesn't want to raise the throttle we can set it automatically
     // note that this can defeat the disarm check on takeoff
     if ((copter.g2.auto_options & (int32_t)Options::AllowTakeOffWithoutRaisingThrottle) != 0) {
-        copter.set_auto_armed(true);
+        if (motors->armed() && copter.ap.auto_armed == false) {
+            takeoff_start(takeoff_loc);  // reload the takeoff command after arming home update and restart takeoff start sequence
+            copter.set_auto_armed(true);
+        }
     }
     auto_takeoff.run();
 }