Send_position_target_global_int: Set mavlink altitude frame to be target's frame

ArduCopter/GCS_Mavlink.cpp

@@ -84,9 +84,10 @@ void GCS_MAVLINK_Copter::send_position_target_global_int()
         return;
     }
 
-    // convert altitude frame to AMSL (this may use the terrain database)
-    if (!target.change_alt_frame(Location::AltFrame::ABSOLUTE)) {
-        return;
+    // Note, currently get_wp() only returns either ABOVE_TERRAIN or ABOVE_ORIGIN
+    MAV_FRAME frame;
+    if (!location_alt_frame_to_mavlink_coordinate_frame(target, frame)) {
+        return; // failed altitude frame conversion
     }
     static constexpr uint16_t POSITION_TARGET_TYPEMASK_LAST_BYTE = 0xF000;
     static constexpr uint16_t TYPE_MASK = POSITION_TARGET_TYPEMASK_VX_IGNORE | POSITION_TARGET_TYPEMASK_VY_IGNORE | POSITION_TARGET_TYPEMASK_VZ_IGNORE |
@@ -95,7 +96,7 @@ void GCS_MAVLINK_Copter::send_position_target_global_int()
     mavlink_msg_position_target_global_int_send(
         chan,
         AP_HAL::millis(), // time_boot_ms
-        MAV_FRAME_GLOBAL, // targets are always global altitude
+        frame,      // target MAV_FRAME
         TYPE_MASK, // ignore everything except the x/y/z components
         target.lat, // latitude as 1e7
         target.lng, // longitude as 1e7
@@ -1356,6 +1357,7 @@ void GCS_MAVLINK_Copter::handleMessage(const mavlink_message_t &msg)
                 copter.mode_guided.init(true);
                 break;
             }
+            copter.mode_guided.set_command_altframe(loc.get_alt_frame()); // Set command altitude frame for target reporting
             copter.mode_guided.set_destination_posvel(pos_neu_cm, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds);
         } else if (pos_ignore && !vel_ignore) {
             copter.mode_guided.set_velaccel(vel_vector, accel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds);

ArduCopter/mode.h

@@ -205,6 +205,8 @@ class Mode {
     static bool auto_takeoff_no_nav_active;
     static float auto_takeoff_no_nav_alt_cm;
 
+    Location::AltFrame command_altframe;
+
 public:
     // Navigation Yaw control
     class AutoYaw {
@@ -895,6 +897,8 @@ class ModeGuided : public Mode {
     bool set_destination_posvel(const Vector3f& destination, const Vector3f& velocity, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
     bool set_destination_posvelaccel(const Vector3f& destination, const Vector3f& velocity, const Vector3f& acceleration, bool use_yaw = false, float yaw_cd = 0.0, bool use_yaw_rate = false, float yaw_rate_cds = 0.0, bool yaw_relative = false);
 
+    void set_command_altframe(Location::AltFrame altframe) { command_altframe = altframe; }
+
     // get position, velocity and acceleration targets
     const Vector3p& get_target_pos() const;
     const Vector3f& get_target_vel() const;

ArduCopter/mode_auto.cpp

@@ -270,6 +270,9 @@ void ModeAuto::takeoff_start(const Location& dest_loc)
         return;
     }
 
+    // Set the current command altitude frame for target reporting consistency
+    command_altframe = dest.get_alt_frame();
+
     // initialise yaw
     auto_yaw.set_mode(AUTO_YAW_HOLD);
 
@@ -290,6 +293,9 @@ void ModeAuto::wp_start(const Location& dest_loc)
         return;
     }
 
+    // Set the current command altitude frame for target reporting consistency
+    command_altframe = dest_loc.get_alt_frame();
+
     _mode = SubMode::WP;
 
     // initialise yaw
@@ -669,7 +675,12 @@ bool ModeAuto::get_wp(Location& destination) const
     case SubMode::NAVGUIDED:
         return copter.mode_guided.get_wp(destination);
     case SubMode::WP:
-        return wp_nav->get_oa_wp_destination(destination);
+        // get the current destination & change altitude frame to the original command altframe
+        // only returns false if terrain data is unavailable
+        if (wp_nav->get_oa_wp_destination(destination) && destination.change_alt_frame(command_altframe)) {
+            return true;
+        }
+        return false;
     case SubMode::RTL:
         return copter.mode_rtl.get_wp(destination);
     default:
@@ -1164,6 +1175,9 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
         return;
     }
 
+    // Set the current command altitude frame for target reporting consistency
+    command_altframe = dest_loc.get_alt_frame();
+
     _mode = SubMode::WP;
 
     // this will be used to remember the time in millis after we reach or pass the WP.
@@ -1245,6 +1259,9 @@ void ModeAuto::do_land(const AP_Mission::Mission_Command& cmd)
 
         const Location target_loc = terrain_adjusted_location(cmd);
 
+        // Set the current command altitude frame for target reporting consistency
+        command_altframe = target_loc.get_alt_frame();
+
         wp_start(target_loc);
     } else {
         // set landing state
@@ -1286,6 +1303,9 @@ void ModeAuto::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
         }
     }
 
+    // Set the current command altitude frame for target reporting consistency
+    command_altframe = target_loc.get_alt_frame();
+
     // start way point navigator and provide it the desired location
     wp_start(target_loc);
 }
@@ -1295,6 +1315,9 @@ void ModeAuto::do_circle(const AP_Mission::Mission_Command& cmd)
 {
     const Location circle_center = loc_from_cmd(cmd, copter.current_loc);
 
+    // Set the current command altitude frame for target reporting consistency
+    command_altframe = circle_center.get_alt_frame();
+
     // calculate radius
     uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1
 
@@ -1330,6 +1353,9 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
         target_loc.lng = copter.current_loc.lng;
     }
 
+    // Set the current command altitude frame for target reporting consistency
+    command_altframe = target_loc.get_alt_frame();
+
     if (!target_loc.get_alt_cm(Location::AltFrame::ABOVE_HOME, loiter_to_alt.alt)) {
         loiter_to_alt.reached_destination_xy = true;
         loiter_to_alt.reached_alt = true;
@@ -1367,6 +1393,9 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
         return;
     }
 
+    // Set the current command altitude frame for target reporting consistency
+    command_altframe = dest_loc.get_alt_frame();
+
     _mode = SubMode::WP;
 
     // this will be used to remember the time in millis after we reach or pass the WP.
@@ -1553,6 +1582,9 @@ void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd)
 
         const Location target_loc = terrain_adjusted_location(cmd);
 
+        // Set the current command altitude frame for target reporting consistency
+        command_altframe = target_loc.get_alt_frame();
+
         wp_start(target_loc);
     } else {
         nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;

ArduCopter/mode_guided.cpp

@@ -117,6 +117,9 @@ bool ModeGuided::do_user_takeoff_start(float takeoff_alt_cm)
     }
     target_loc.set_alt_cm(takeoff_alt_cm, frame);
 
+    // Set the current command altitude frame for target reporting consistency
+    command_altframe = frame;
+
     if (!wp_nav->set_wp_destination_loc(target_loc)) {
         // failure to set destination can only be because of missing terrain data
         AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::FAILED_TO_SET_DESTINATION);
@@ -375,9 +378,24 @@ bool ModeGuided::get_wp(Location& destination) const
 {
     switch (guided_mode) {
     case SubMode::WP:
-        return wp_nav->get_oa_wp_destination(destination);
+        if (!wp_nav->get_oa_wp_destination(destination)) {
+            return false;
+        }
+
+        // change altitude frame to the original command frame: only returns false if terrain data is unavailabe
+        if (!destination.change_alt_frame(command_altframe)) {
+            return false;
+        }
+
+        return true;
     case SubMode::Pos:
         destination = Location(guided_pos_target_cm.tofloat(), guided_pos_terrain_alt ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
+
+        // change altitude frame to the original command frame: only returns false if terrain data is unavailabe
+        if (!destination.change_alt_frame(command_altframe)) {
+            return false;
+        }
+
         return true;
     default:
         return false;
@@ -433,6 +451,9 @@ bool ModeGuided::set_destination(const Location& dest_loc, bool use_yaw, float y
     // set yaw state
     set_yaw_state(use_yaw, yaw_cd, use_yaw_rate, yaw_rate_cds, relative_yaw);
 
+    // Set the current command altitude frame for target reporting consistency
+    command_altframe = dest_loc.get_alt_frame();
+
     // set position target and zero velocity and acceleration
     Vector3f pos_target_f;
     bool terrain_alt;