GCS_MAVLink: added ALTITUDE message #141

libraries/GCS_MAVLink/GCS.h

@@ -330,6 +330,8 @@ class GCS_MAVLINK
     void send_uavionix_adsb_out_status() const;
     void send_autopilot_state_for_gimbal_device() const;
 
+    void send_altitude() const;
+
     // lock a channel, preventing use by MAVLink
     void lock(bool _lock) {
         _locked = _lock;

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -60,6 +60,8 @@
 #include "MissionItemProtocol_Rally.h"
 #include "MissionItemProtocol_Fence.h"
 
+#include <AP_Terrain/AP_Terrain.h>
+
 #include <stdio.h>
 
 #if HAL_RCINPUT_WITH_AP_RADIO
@@ -989,6 +991,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
 #if HAL_ADSB_ENABLED
         { MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_STATUS, MSG_UAVIONIX_ADSB_OUT_STATUS},
 #endif
+        { MAVLINK_MSG_ID_ALTITUDE,              MSG_ALTITUDE},
             };
 
     for (uint8_t i=0; i<ARRAY_SIZE(map); i++) {
@@ -5358,6 +5361,67 @@ void GCS_MAVLINK::send_received_message_deprecation_warning(const char * message
     send_text(MAV_SEVERITY_WARNING, "Received message (%s) is deprecated", message);
 }
 
+void GCS_MAVLINK::send_altitude() const
+{
+    Location loc;
+    AP_AHRS &_ahrs = AP::ahrs();
+
+    //we wait for a position from AHRS
+    if (!_ahrs.get_location(loc)) {
+        return;
+    }
+    const float altitude_amsl = global_position_int_alt() * 0.001; //meters above ground/sea level
+
+    AP_Baro &_baro = AP::baro();
+    // MAVLink specs for the altitude_monotonic value:
+    // The only guarantee on this field is that it will never be reset and is consistent within a flight.
+    // The recommended value for this field is the uncorrected barometric altitude at boot time.
+    // This altitude will also drift and vary between flights"
+    // Note: ArduPilot allows for ground calibration while disarmed so we can't guarantee (yet) that this field will not be reset!
+    const float altitude_monotonic = _baro.get_altitude();
+
+    Vector3f local_position;
+    // preset to baro altitude and if available use position NED
+    float altitude_local = altitude_monotonic;
+    if (_ahrs.get_relative_position_NED_origin(local_position)) {
+        altitude_local = -local_position.z; //meters from origin
+    }
+
+    // preset to local position and if available use home relative position
+    float altitude_relative = altitude_local;
+    if (_ahrs.home_is_set()) {
+        altitude_relative = global_position_int_relative_alt() * 0.001; //meters above home
+    }
+
+    float altitude_terrain = -1000; //mavlink specs value for unknown
+#if AP_TERRAIN_AVAILABLE
+    AP_Terrain *terrain = AP::terrain();
+    if (terrain != nullptr && terrain->enabled()) {
+        UNUSED_RESULT(terrain->height_above_terrain(altitude_terrain, true));
+    }
+#endif
+
+    float bottom_clearance = -1;    //mavlink specs value for unavailable
+    const RangeFinder *rangefinder = RangeFinder::get_singleton();
+    if (rangefinder != nullptr) {
+        const AP_RangeFinder_Backend *s = rangefinder->find_instance(ROTATION_PITCH_270);
+        if (s != nullptr) {
+            bottom_clearance = s->distance_cm()*0.01;
+        }
+    }
+
+    mavlink_msg_altitude_send(
+            chan,
+            AP_HAL::millis(),
+            altitude_monotonic,
+            altitude_amsl,
+            altitude_local,
+            altitude_relative,
+            altitude_terrain,
+            bottom_clearance
+    );
+}
+
 bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 {
     bool ret = true;
@@ -5717,6 +5781,11 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
 #endif
         break;
 
+    case MSG_ALTITUDE:
+        CHECK_PAYLOAD_SIZE(ALTITUDE);
+        send_altitude();
+        break;
+
     default:
         // try_send_message must always at some stage return true for
         // a message, or we will attempt to infinitely retry the

libraries/GCS_MAVLink/ap_message.h

@@ -82,5 +82,6 @@ enum ap_message : uint8_t {
     MSG_UAVIONIX_ADSB_OUT_STATUS,
     MSG_ATTITUDE_TARGET,
     MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE,
+    MSG_ALTITUDE,
     MSG_LAST // MSG_LAST must be the last entry in this enum
 };