Sub: Fix SET_POSITION_TARGET_LOCAL_NED to be above origin

ArduSub/GCS_Mavlink.cpp

@@ -624,9 +624,6 @@ void GCS_MAVLINK_Sub::handleMessage(const mavlink_message_t &msg)
                     packet.coordinate_frame == MAV_FRAME_BODY_NED ||
                     packet.coordinate_frame == MAV_FRAME_BODY_OFFSET_NED) {
                 pos_vector += sub.inertial_nav.get_position_neu_cm();
-            } else {
-                // convert from alt-above-home to alt-above-ekf-origin
-                pos_vector.z = sub.pv_alt_above_origin(pos_vector.z);
             }
         }
 

ArduSub/Sub.h

@@ -516,8 +516,6 @@ class Sub : public AP_Vehicle {
     void set_surfaced(bool at_surface);
     void set_bottomed(bool at_bottom);
     void motors_output();
-    Vector3f pv_location_to_vector(const Location& loc);
-    float pv_alt_above_origin(float alt_above_home_cm);
     void init_rc_in();
     void init_rc_out();
     void enable_motor_output();

ArduSub/commands_logic.cpp

@@ -509,7 +509,8 @@ bool Sub::verify_circle(const AP_Mission::Mission_Command& cmd)
     // check if we've reached the edge
     if (auto_mode == Auto_CircleMoveToEdge) {
         if (wp_nav.reached_wp_destination()) {
-            Vector3f circle_center = pv_location_to_vector(cmd.content.location);
+            Vector3f circle_center;
+            UNUSED_RESULT(cmd.content.location.get_vector_from_origin_NEU(circle_center));
 
             // set target altitude if not provided
             if (is_zero(circle_center.z)) {

ArduSub/control_auto.cpp

@@ -463,10 +463,11 @@ void Sub::set_auto_yaw_roi(const Location &roi_location)
 #endif  // HAL_MOUNT_ENABLED
     } else {
 #if HAL_MOUNT_ENABLED
-        // check if mount type requires us to rotate the quad
+        // check if mount type requires us to rotate the sub
         if (!camera_mount.has_pan_control()) {
-            roi_WP = pv_location_to_vector(roi_location);
-            set_auto_yaw_mode(AUTO_YAW_ROI);
+            if (roi_location.get_vector_from_origin_NEU(roi_WP)) {
+                set_auto_yaw_mode(AUTO_YAW_ROI);
+            }
         }
         // send the command to the camera mount
         camera_mount.set_roi_target(roi_location);
@@ -478,9 +479,10 @@ void Sub::set_auto_yaw_roi(const Location &roi_location)
         //      3: point at a location given by alt, lon, lat parameters
         //      4: point at a target given a target id (can't be implemented)
 #else
-        // if we have no camera mount aim the quad at the location
-        roi_WP = pv_location_to_vector(roi_location);
-        set_auto_yaw_mode(AUTO_YAW_ROI);
+        // if we have no camera mount aim the sub at the location
+        if (roi_location.get_vector_from_origin_NEU(roi_WP)) {
+            set_auto_yaw_mode(AUTO_YAW_ROI);
+        }
 #endif  // HAL_MOUNT_ENABLED
     }
 }