Sub: publish filtered alt in mav msgs

ArduPilot · Apr 8, 2024 · e4115ef · e4115ef
1 parent 4cdca46
commit e4115ef
Show file tree

Hide file tree

Showing 4 changed files with 67 additions and 11 deletions.
diff --git a/ArduSub/ArduSub.cpp b/ArduSub/ArduSub.cpp
@@ -380,4 +380,54 @@ void Sub::stats_update(void)
 }
 #endif
 
+// get the altitude relative to the home position or the ekf origin
+float Sub::get_alt_rel() const
+{
+    if (!ap.depth_sensor_present) {
+        return 0;
+    }
+
+    // get relative position
+    float posD;
+    if (ahrs.get_relative_position_D_origin(posD)) {
+        if (ahrs.home_is_set()) {
+            // adjust to the home position
+            auto home = ahrs.get_home();
+            posD -= static_cast<float>(home.alt) * 0.01f;
+        }
+    } else {
+        // fall back to the barometer reading
+        posD = -AP::baro().get_altitude();
+    }
+
+    // convert down to up
+    return -posD;
+}
+
+// get the altitude above mean sea level
+float Sub::get_alt_msl() const
+{
+    if (!ap.depth_sensor_present) {
+        return 0;
+    }
+
+    Location origin;
+    if (!ahrs.get_origin(origin)) {
+        return 0;
+    }
+
+    // get relative position
+    float posD;
+    if (!ahrs.get_relative_position_D_origin(posD)) {
+        // fall back to the barometer reading
+        posD = -AP::baro().get_altitude();
+    }
+
+    // add in the ekf origin altitude
+    posD -= static_cast<float>(origin.alt) * 0.01f;
+
+    // convert down to up
+    return -posD;
+}
+
 AP_HAL_MAIN_CALLBACKS(&sub);
diff --git a/ArduSub/GCS_Mavlink.cpp b/ArduSub/GCS_Mavlink.cpp
@@ -93,6 +93,11 @@ int16_t GCS_MAVLINK_Sub::vfr_hud_throttle() const
     return (int16_t)(sub.motors.get_throttle() * 100);
 }
 
+float GCS_MAVLINK_Sub::vfr_hud_alt() const
+{
+    return sub.get_alt_msl();
+}
+
 // Work around to get temperature sensor data out
 void GCS_MAVLINK_Sub::send_scaled_pressure3()
 {
@@ -842,25 +847,23 @@ uint64_t GCS_MAVLINK_Sub::capabilities() const
         );
 }
 
-MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_int_t &packet) {
+MAV_RESULT GCS_MAVLINK_Sub::handle_flight_termination(const mavlink_command_int_t &packet)
+{
     if (packet.param1 > 0.5f) {
         sub.arming.disarm(AP_Arming::Method::TERMINATION);
         return MAV_RESULT_ACCEPTED;
     }
     return MAV_RESULT_FAILED;
 }
 
-int32_t GCS_MAVLINK_Sub::global_position_int_alt() const {
-    if (!sub.ap.depth_sensor_present) {
-        return 0;
-    }
-    return GCS_MAVLINK::global_position_int_alt();
+int32_t GCS_MAVLINK_Sub::global_position_int_alt() const
+{
+    return static_cast<int32_t>(sub.get_alt_msl() * 1000.0f);
 }
-int32_t GCS_MAVLINK_Sub::global_position_int_relative_alt() const {
-    if (!sub.ap.depth_sensor_present) {
-        return 0;
-    }
-    return GCS_MAVLINK::global_position_int_relative_alt();
+
+int32_t GCS_MAVLINK_Sub::global_position_int_relative_alt() const
+{
+    return static_cast<int32_t>(sub.get_alt_rel() * 1000.0f);
 }
 
 #if HAL_HIGH_LATENCY2_ENABLED

diff --git a/ArduSub/GCS_Mavlink.h b/ArduSub/GCS_Mavlink.h
@@ -51,6 +51,7 @@ class GCS_MAVLINK_Sub : public GCS_MAVLINK {
     MAV_STATE vehicle_system_status() const override;
 
     int16_t vfr_hud_throttle() const override;
+    float vfr_hud_alt() const override;
 
     MAV_RESULT handle_MAV_CMD_CONDITION_YAW(const mavlink_command_int_t &packet);
     MAV_RESULT handle_MAV_CMD_MISSION_START(const mavlink_command_int_t &packet);

diff --git a/ArduSub/Sub.h b/ArduSub/Sub.h
@@ -426,6 +426,8 @@ class Sub : public AP_Vehicle {
     bool set_home_to_current_location(bool lock) WARN_IF_UNUSED;
     bool set_home(const Location& loc, bool lock) WARN_IF_UNUSED;
     bool far_from_EKF_origin(const Location& loc);
+    float get_alt_rel() const WARN_IF_UNUSED;
+    float get_alt_msl() const WARN_IF_UNUSED;
     void exit_mission();
     bool verify_loiter_unlimited();
     bool verify_loiter_time();