Merge pull request #3158 from kemen209/fix-bug-#8

unified the simulation time
microsoft · Feb 10, 2021 · 0096b5e · 0096b5e
2 parents 4be4d1e + 6fc4eeb
commit 0096b5e
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 3 deletions.
diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp
@@ -28,7 +28,7 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi
             const auto& imu_output = getImuData("");
 
             SensorMessage packet;
-            packet.timestamp = Utils::getTimeSinceEpochNanos() / 1000;
+            packet.timestamp = clock()->nowNanos() / 1000;
             packet.latitude = gps_output.gnss.geo_point.latitude;
             packet.longitude = gps_output.gnss.geo_point.longitude;
             packet.altitude = gps_output.gnss.geo_point.altitude;

diff --git a/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp b/AirLib/include/vehicles/multirotor/firmwares/mavlink/MavLinkMultirotorApi.hpp
@@ -1339,7 +1339,7 @@ class MavLinkMultirotorApi : public MultirotorApiBase
         if (!is_simulation_mode_)
             throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode");
 
-        auto now = static_cast<uint64_t>(Utils::getTimeSinceEpochNanos() / 1000.0);
+        auto now = clock()->nowNanos() / 1000;
         if (lock_step_enabled_) {
             if (last_hil_sensor_time_ + 100000 < now) {
                 // if 100 ms passes then something is terribly wrong, reset lockstep mode
@@ -1401,8 +1401,8 @@ class MavLinkMultirotorApi : public MultirotorApiBase
             throw std::logic_error("Attempt to send simulated distance sensor messages while not in simulation mode");
 
         mavlinkcom::MavLinkDistanceSensor distance_sensor;
-        distance_sensor.time_boot_ms = static_cast<uint32_t>(Utils::getTimeSinceEpochNanos() / 1000000.0);
 
+        distance_sensor.time_boot_ms = static_cast<uint32_t>(clock()->nowNanos() / 1000000);
         distance_sensor.min_distance = static_cast<uint16_t>(min_distance);
         distance_sensor.max_distance = static_cast<uint16_t>(max_distance);
         distance_sensor.current_distance = static_cast<uint16_t>(current_distance);