Copter: support LOCAL_NED in auto mode

ArduCopter/mode_auto.cpp

@@ -154,41 +154,52 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
 {
     _mode = Auto_TakeOff;
 
-    Location dest(dest_loc);
-
-    if (!copter.current_loc.initialised()) {
-        // vehicle doesn't know where it is ATM.  We should not
-        // initialise our takeoff destination without knowing this!
-        return;
-    }
+    if (dest_loc.local_frame) {
+        float dest_alt = dest_loc.alt;
+        const Vector3f& curr_pos = inertial_nav.get_position();
+        //you can't go lower than you currently are and you can't takeoff to less than 100cm above
+        if (dest_loc.alt < curr_pos.z || ((dest_loc.alt - curr_pos.z) < 100)) {
+            dest_alt = curr_pos.z + 100;
+        }
+        // no need to check return status because terrain data is not used
+        wp_nav->set_wp_destination(Vector3f(curr_pos.x, curr_pos.y, dest_alt), false);
+    } else {
+        Location dest(dest_loc);
 
-    // set horizontal target
-    dest.lat = copter.current_loc.lat;
-    dest.lng = copter.current_loc.lng;
+        if (!copter.current_loc.initialised()) {
+            // vehicle doesn't know where it is ATM.  We should not
+            // initialise our takeoff destination without knowing this!
+            return;
+        }
 
-    // get altitude target
-    int32_t alt_target;
-    if (!dest.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_target)) {
-        // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
-        AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
-        // fall back to altitude above current altitude
-        alt_target = copter.current_loc.alt + dest.alt;
-    }
+        // set horizontal target
+        dest.lat = copter.current_loc.lat;
+        dest.lng = copter.current_loc.lng;
+
+        // get altitude target
+        int32_t alt_target;
+        if (!dest.get_alt_cm(Location::AltFrame::ABOVE_HOME, alt_target)) {
+            // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data
+            AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA);
+            // fall back to altitude above current altitude
+            alt_target = copter.current_loc.alt + dest.alt;
+        }
 
-    // sanity check target
-    if (alt_target < copter.current_loc.alt) {
-        dest.set_alt_cm(copter.current_loc.alt, Location::AltFrame::ABOVE_HOME);
-    }
-    // Note: if taking off from below home this could cause a climb to an unexpectedly high altitude
-    if (alt_target < 100) {
-        dest.set_alt_cm(100, Location::AltFrame::ABOVE_HOME);
-    }
+        // sanity check target
+        if (alt_target < copter.current_loc.alt) {
+            dest.set_alt_cm(copter.current_loc.alt, Location::AltFrame::ABOVE_HOME);
+        }
+        // Note: if taking off from below home this could cause a climb to an unexpectedly high altitude
+        if (alt_target < 100) {
+            dest.set_alt_cm(100, Location::AltFrame::ABOVE_HOME);
+        }
 
-    // set waypoint controller target
-    if (!wp_nav->set_wp_destination(dest)) {
-        // failure to set destination can only be because of missing terrain data
-        copter.failsafe_terrain_on_event();
-        return;
+        // set waypoint controller target
+        if (!wp_nav->set_wp_destination(dest)) {
+            // failure to set destination can only be because of missing terrain data
+            copter.failsafe_terrain_on_event();
+            return;
+        }
     }
 
     // initialise yaw
@@ -322,7 +333,7 @@ void ModeAuto::circle_start()
 // auto_spline_start - initialises waypoint controller to implement flying to a particular destination using the spline controller
 //  seg_end_type can be SEGMENT_END_STOP, SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE.  If Straight or Spline the next_destination should be provided
 void ModeAuto::spline_start(const Location& destination, bool stopped_at_start,
-                               AC_WPNav::spline_segment_end_type seg_end_type, 
+                               AC_WPNav::spline_segment_end_type seg_end_type,
                                const Location& next_destination)
 {
     _mode = Auto_Spline;
@@ -488,7 +499,7 @@ bool ModeAuto::start_command(const AP_Mission::Mission_Command& cmd)
         // point the camera to a specified angle
         do_mount_control(cmd);
         break;
-    
+
     case MAV_CMD_DO_FENCE_ENABLE:
 #if AC_FENCE == ENABLED
         if (cmd.p1 == 0) { //disable
@@ -844,7 +855,7 @@ void ModeAuto::land_run()
 
     // set motors to full range
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
-    
+
     land_run_horizontal_control();
     land_run_vertical_control();
 }
@@ -1126,15 +1137,25 @@ Location ModeAuto::loc_from_cmd(const AP_Mission::Mission_Command& cmd) const
 // do_nav_wp - initiate move to next waypoint
 void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
 {
-    Location target_loc = loc_from_cmd(cmd);
 
     // this will be used to remember the time in millis after we reach or pass the WP.
     loiter_time = 0;
     // this is the delay, stored in seconds
     loiter_time_max = cmd.p1;
 
-    // Set wp navigation target
-    wp_start(target_loc);
+    if (cmd.content.location.local_frame) {
+        //going to (0,0,0) would be a disaster waiting to happen as the origin is presumably on the ground
+        if (cmd.content.location.lat == 0 && cmd.content.location.lng == 0 && cmd.content.location.alt == 0) {
+            const Vector3f& curr_pos = inertial_nav.get_position();
+            wp_start(curr_pos);
+        } else {
+            wp_start(Vector3f(cmd.content.location.lat, cmd.content.location.lng, cmd.content.location.alt));
+        }
+    } else {
+        Location target_loc = loc_from_cmd(cmd);
+        // Set wp navigation target
+        wp_start(target_loc);
+    }
 
     // if no delay as well as not final waypoint set the waypoint as "fast"
     AP_Mission::Mission_Command temp_cmd;
@@ -1771,7 +1792,7 @@ bool ModeAuto::verify_loiter_to_alt()
 // returns true with RTL has completed successfully
 bool ModeAuto::verify_RTL()
 {
-    return (copter.mode_rtl.state_complete() && 
+    return (copter.mode_rtl.state_complete() &&
             (copter.mode_rtl.state() == ModeRTL::RTL_FinalDescent || copter.mode_rtl.state() == ModeRTL::RTL_Land) &&
             (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE));
 }