Merge pull request #2294 from mavlink/pr-unix-time

telemetry and telemetry_server: fix Unix epoch time
mavlink · May 8, 2024 · c742d40 · c742d40
2 parents 9860fd7 + d64468e
commit c742d40
Show file tree

Hide file tree

Showing 3 changed files with 28 additions and 9 deletions.
diff --git a/examples/autopilot_server/autopilot_server.cpp b/examples/autopilot_server/autopilot_server.cpp
@@ -141,6 +141,9 @@ int main(int argc, char** argv)
             telemServer.publish_position(position, velocity, heading);
             telemServer.publish_position_velocity_ned(positionVelocityNed);
             telemServer.publish_raw_gps(rawGps, gpsInfo);
+
+            // Just a silly test.
+            telemServer.publish_unix_epoch_time(42);
         }
     });
 
@@ -247,6 +250,11 @@ int main(int argc, char** argv)
         std::cout << "Altitude: " << position.relative_altitude_m << " m" << std::endl;
     });
 
+    // Set up callback to monitor Unix time
+    telemetry.subscribe_unix_epoch_time([](uint64_t time_us) {
+        std::cout << "Unix epoch time: " << time_us << " us" << std::endl;
+    });
+
     // Check if vehicle is ready to arm
     while (telemetry.health_all_ok() != true) {
         std::cout << "Vehicle is getting ready to arm" << std::endl;

diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp
@@ -151,7 +151,7 @@ void TelemetryImpl::init()
         this);
 
     _system_impl->register_mavlink_message_handler(
-        MAVLINK_MSG_ID_UTM_GLOBAL_POSITION,
+        MAVLINK_MSG_ID_SYSTEM_TIME,
         [this](const mavlink_message_t& message) { process_unix_epoch_time(message); },
         this);
 
@@ -399,7 +399,7 @@ Telemetry::Result TelemetryImpl::set_rate_scaled_pressure(double rate_hz)
 Telemetry::Result TelemetryImpl::set_rate_unix_epoch_time(double rate_hz)
 {
     return telemetry_result_from_command_result(
-        _system_impl->set_msg_rate(MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, rate_hz));
+        _system_impl->set_msg_rate(MAVLINK_MSG_ID_SYSTEM_TIME, rate_hz));
 }
 
 Telemetry::Result TelemetryImpl::set_rate_altitude(double rate_hz)
@@ -1352,10 +1352,10 @@ void TelemetryImpl::process_rc_channels(const mavlink_message_t& message)
 
 void TelemetryImpl::process_unix_epoch_time(const mavlink_message_t& message)
 {
-    mavlink_utm_global_position_t utm_global_position;
-    mavlink_msg_utm_global_position_decode(&message, &utm_global_position);
+    mavlink_system_time_t system_time;
+    mavlink_msg_system_time_decode(&message, &system_time);
 
-    set_unix_epoch_time_us(utm_global_position.time);
+    set_unix_epoch_time_us(system_time.time_unix_usec);
 
     std::lock_guard<std::mutex> lock(_subscription_mutex);
     _unix_epoch_time_subscriptions.queue(

diff --git a/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp b/src/mavsdk/plugins/telemetry_server/telemetry_server_impl.cpp
@@ -392,10 +392,21 @@ TelemetryServer::Result TelemetryServerImpl::publish_raw_imu(TelemetryServer::Im
 
 TelemetryServer::Result TelemetryServerImpl::publish_unix_epoch_time(uint64_t time_us)
 {
-    UNUSED(time_us);
-
-    // TODO :)
-    return {};
+    return _server_component_impl->queue_message(
+               [&](MavlinkAddress mavlink_address, uint8_t channel) {
+                   mavlink_message_t message;
+                   mavlink_msg_system_time_pack_chan(
+                       mavlink_address.system_id,
+                       mavlink_address.component_id,
+                       channel,
+                       &message,
+                       time_us,
+                       0 // TODO: add timestamping in general
+                   );
+                   return message;
+               }) ?
+               TelemetryServer::Result::Success :
+               TelemetryServer::Result::Unsupported;
 }
 
 TelemetryServer::Result TelemetryServerImpl::publish_sys_status(