publish operator altitude in geodetic frame

ThomasRigi · Mar 11, 2024 · c6a1279 · c6a1279
1 parent cb85204
commit c6a1279
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp b/src/modules/mavlink/streams/OPEN_DRONE_ID_SYSTEM.hpp
@@ -86,7 +86,8 @@ class MavlinkStreamOpenDroneIdSystem : public MavlinkStream
 				msg.area_floor = -1000;
 				msg.category_eu = MAV_ODID_CATEGORY_EU_UNDECLARED;
 				msg.class_eu = MAV_ODID_CLASS_EU_UNDECLARED;
-				msg.operator_altitude_geo = home_position.alt;
+				float wgs84_amsl_offset = vehicle_gps_position.altitude_ellipsoid_m - vehicle_gps_position.altitude_msl_m;
+				msg.operator_altitude_geo = home_position.alt + wgs84_amsl_offset;
 
 				// timestamp: 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019.
 				static uint64_t utc_offset_s = 1'546'300'800; // UTC seconds since 00:00:00 01/01/2019