Copter: remove position-vector methods

ArduCopter/Copter.h

@@ -788,10 +788,6 @@ class Copter : public AP_HAL::HAL::Callbacks {
     void convert_pid_parameters(void);
     void convert_lgr_parameters(void);
 
-    // position_vector.cpp
-    float pv_alt_above_origin(float alt_above_home_cm);
-    float pv_alt_above_home(float alt_above_origin_cm);
-
     // precision_landing.cpp
     void init_precland();
     void update_precland();

ArduCopter/GCS_Mavlink.cpp

@@ -1033,7 +1033,12 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
                 pos_vector += copter.inertial_nav.get_position();
             } else {
                 // convert from alt-above-home to alt-above-ekf-origin
-                pos_vector.z = copter.pv_alt_above_origin(pos_vector.z);
+                if (!AP::ahrs().home_is_set()) {
+                    break;
+                }
+                const Location &origin = copter.inertial_nav.get_origin();
+                pos_vector.z += AP::ahrs().get_home().alt;
+                pos_vector.z -= origin.alt;
             }
         }
 

ArduCopter/inertia.cpp

@@ -15,16 +15,16 @@ void Copter::read_inertia()
         return;
     }
 
-    // without home return alt above the EKF origin
-    if (!ahrs.home_is_set()) {
-        // with inertial nav we can update the altitude and climb rate at 50hz
-        current_loc.alt = inertial_nav.get_altitude();
+    Location::ALT_FRAME frame;
+    if (ahrs.home_is_set()) {
+        frame = Location::ALT_FRAME_ABOVE_HOME;
     } else {
-        // with inertial nav we can update the altitude and climb rate at 50hz
-        current_loc.alt = pv_alt_above_home(inertial_nav.get_altitude());
+        // without home use alt above the EKF origin
+        frame = Location::ALT_FRAME_ABOVE_ORIGIN;
     }
+    current_loc.set_alt_cm(inertial_nav.get_altitude(), frame);
+    current_loc.change_alt_frame(Location::ALT_FRAME_ABOVE_HOME);
 
     // set flags and get velocity
-    current_loc.relative_alt = true;
     climb_rate = inertial_nav.get_velocity_z();
 }

Tools/autotest/arducopter.py

@@ -2063,6 +2063,41 @@ def test_setting_modes_via_auxswitch(self):
         if ex is not None:
             raise ex
 
+    def fly_guided_stop(self,
+                        timeout=20,
+                        groundspeed_tolerance=0.05,
+                        climb_tolerance=0.001):
+        """stop the vehicle moving in guided mode"""
+        self.progress("Stopping vehicle")
+        tstart = self.get_sim_time()
+        while True:
+            if self.get_sim_time_cached() - tstart > timeout:
+                raise NotAchievedException("Vehicle did not stop")
+            # send a position-control command
+            self.mav.mav.set_position_target_local_ned_send(
+                0, # timestamp
+                1, # target system_id
+                1, # target component id
+                mavutil.mavlink.MAV_FRAME_BODY_NED,
+                0b1111111111111000, # mask specifying use-only-x-y-z
+                0, # x
+                0, # y
+                0, # z
+                0, # vx
+                0, # vy
+                0, # vz
+                0, # afx
+                0, # afy
+                0, # afz
+                0, # yaw
+                0, # yawrate
+            )
+            m = self.mav.recv_match(type='VFR_HUD', blocking=True)
+            print("%s" % str(m))
+            if (m.groundspeed < groundspeed_tolerance and
+                m.climb < climb_tolerance):
+                break
+
     def fly_guided_move_relative(self, lat, lon, alt):
         startpos = self.mav.recv_match(type='GLOBAL_POSITION_INT',
                                        blocking=True)
@@ -2097,6 +2132,57 @@ def fly_guided_move_relative(self, lat, lon, alt):
             if delta > 10:
                 break
 
+    def fly_guided_move_local(self, x, y, z_up, timeout=100):
+        """move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED"""
+        startpos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)
+        self.progress("startpos=%s" % str(startpos))
+
+        tstart = self.get_sim_time()
+        while True:
+            if self.get_sim_time() - tstart > timeout:
+                raise NotAchievedException("Did not start to move")
+            # send a position-control command
+            self.mav.mav.set_position_target_local_ned_send(
+                0, # timestamp
+                1, # target system_id
+                1, # target component id
+                mavutil.mavlink.MAV_FRAME_LOCAL_NED,
+                0b1111111111111000, # mask specifying use-only-x-y-z
+                x, # x
+                y, # y
+                -z_up,# z
+                0, # vx
+                0, # vy
+                0, # vz
+                0, # afx
+                0, # afy
+                0, # afz
+                0, # yaw
+                0, # yawrate
+            )
+            m = self.mav.recv_match(type='VFR_HUD', blocking=True)
+            print("%s" % m)
+            if m.groundspeed > 0.5:
+                break
+
+        self.progress("Waiting for vehicle to stop...")
+        self.wait_groundspeed(1, 100, timeout=timeout)
+
+        stoppos = self.mav.recv_match(type='LOCAL_POSITION_NED', blocking=True)
+        self.progress("stop_pos=%s" % str(stoppos))
+
+        x_achieved = stoppos.x - startpos.x
+        if x_achieved - x > 1:
+            raise NotAchievedException("Did not achieve x position: want=%f got=%f" % (x, x_achieved))
+
+        y_achieved = stoppos.y - startpos.y
+        if y_achieved - y > 1:
+            raise NotAchievedException("Did not achieve y position: want=%f got=%f" % (y, y_achieved))
+
+        z_achieved = stoppos.z - startpos.z
+        if z_achieved - z_up > 1:
+            raise NotAchievedException("Did not achieve z position: want=%f got=%f" % (z_up, z_achieved))
+
     def earth_to_body(self, vector):
         m = self.mav.messages["ATTITUDE"]
         x = rotmat.Vector3(m.roll, m.pitch, m.yaw)
@@ -2218,6 +2304,10 @@ def fly_guided_change_submode(self):
             """move the vehicle using set_position_target_global_int"""
             self.fly_guided_move_relative(5, 5, 10)
 
+            """move the vehicle using MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED"""
+            self.fly_guided_stop()
+            self.fly_guided_move_local(5, 5, 10)
+
             self.progress("Landing")
             self.mavproxy.send('switch 2\n')  # land mode
             self.wait_mode('LAND')