Utils clock API changes

AirLib/include/common/FrequencyLimiter.hpp

@@ -39,7 +39,6 @@ class FrequencyLimiter : UpdatableObject {
         elapsed_total_sec_ = 0;
         elapsed_interval_sec_ = 0;
         last_elapsed_interval_sec_ = 0;
-        start_timestamp_ms_ = Utils::getTimeSinceEpochMillis();
         update_count_ = 0;
         interval_complete_ = false;
         startup_complete_ = false;
@@ -112,7 +111,6 @@ class FrequencyLimiter : UpdatableObject {
     TTimeDelta elapsed_total_sec_;
     TTimeDelta elapsed_interval_sec_;
     TTimeDelta last_elapsed_interval_sec_;
-    ulong start_timestamp_ms_;
     uint update_count_;
     real_T frequency_;
     real_T startup_delay_;

AirLib/include/common/common_utils/ScheduledExecutor.hpp

@@ -46,9 +46,9 @@ class ScheduledExecutor {
         if (keep_running_) {
             keep_running_ = false;
             try {
-				if (th_.joinable()) {
-					th_.join();
-				}
+                if (th_.joinable()) {
+                    th_.join();
+                }
             }
             catch(const std::system_error& /* e */)
             { }
@@ -113,9 +113,9 @@ class ScheduledExecutor {
 
             if (period_count_ > 0) {
                 bool result = callback_(since_last_call.count());
-				if (!result) {
-					keep_running_ = result;
-				}
+                if (!result) {
+                    keep_running_ = result;
+                }
             }
 
             call_end = clock::now();

AirLib/include/common/common_utils/Utils.hpp

@@ -481,8 +481,7 @@ class Utils {
         return ptr ? ptr : "";
     }
 
-    //Unix timestamp
-    static unsigned long getTimeSinceEpochMillis(std::time_t* t = nullptr)
+    static unsigned long getUnixTimeStamp(std::time_t* t = nullptr)
     {
         std::time_t st = std::time(t);
         auto millies = static_cast<std::chrono::milliseconds>(st).count();

AirLib/include/controllers/DroneControllerCancelable.hpp

@@ -183,7 +183,7 @@ class DroneControllerCancelable : CancelableBase {
     std::string getServerDebugInfo()
     {
         //for now this method just allows to see if server was started
-        return std::to_string(Utils::getTimeSinceEpochMillis());
+        return std::to_string(Utils::getUnixTimeStamp());
     }
 
     void getStatusMessages(std::vector<std::string>& messages)

AirLib/include/controllers/rosflight/AirSimRosFlightBoard.hpp

@@ -57,12 +57,12 @@ class AirSimRosFlightBoard : public rosflight::Board {
 
     virtual uint64_t micros() override 
     {
-        return static_cast<uint64_t>(Utils::getTimeSinceEpochSecs() * 1E6);
+        return static_cast<uint64_t>(Utils::getTimeSinceEpochNanos() / 1.0E3);
     }
 
     virtual uint32_t millis() override 
     {
-        return static_cast<uint32_t>(Utils::getTimeSinceEpochSecs() * 1E3);
+        return static_cast<uint32_t>(Utils::getTimeSinceEpochNanos() / 1.0E6);
     }
 
     virtual void init_sensors(uint16_t& acc1G, float& gyro_scale, int boardVersion, const std::function<void(void)>& imu_updated_callback) override 

AirLib/include/physics/World.hpp

@@ -90,7 +90,7 @@ class World : public UpdatableContainer<UpdatableObject*> {
     }
 
     //async updater thread
-    void startAsyncUpdator(real_T period)
+    void startAsyncUpdator(double period)
     {
         executor_.initialize(std::bind(&World::worldUpdatorAsync, this), period);
         executor_.start();

AirLib/include/sensors/gps/GpsSimple.hpp

@@ -67,7 +67,7 @@ class GpsSimple : public GpsBase {
         const GroundTruth& ground_truth = getGroundTruth();
 
         //GNSS
-        output.gnss.time_utc = static_cast<long>(clock()->nowNanos() / 1E3);
+        output.gnss.time_utc = static_cast<long>(clock()->nowNanos() / 1.0E3);
         output.gnss.geo_point = ground_truth.environment->getState().geo_point;
         output.gnss.eph = eph;
         output.gnss.epv = epv;

AirLib/include/sensors/imu/ImuSimpleParams.hpp

@@ -34,7 +34,7 @@ struct ImuSimpleParams {
 
     struct Accelerometer {
         //velocity random walk (ARW)
-        real_T vrw = 0.24f * EarthUtils::Gravity / 1E3f; //mg converted to m/s^2 
+        real_T vrw = 0.24f * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2 
         //Bias Stability (tau = 800s)
         real_T tau = 800;
         real_T bias_stability = 36.0f * 1E-6f * 9.80665f; //ug converted to m/s^2

AirLib/src/controllers/DroneControllerBase.cpp

@@ -378,7 +378,7 @@ bool DroneControllerBase::setSafety(SafetyEval::SafetyViolationType enable_reaso
 
 bool DroneControllerBase::moveByManual(float vx_max, float vy_max, float z_min, DrivetrainType drivetrain, const YawMode& yaw_mode, float duration, CancelableBase& cancelable_action)
 {
-    const float kMaxMessageAge = 1, kMaxVelocity = 2, trim_duration = 1, kMinCountForTrim = 10, kMaxTrim = 100, kMaxRCValue = 10000;
+    const float kMaxMessageAge = 10 * 1E6 /* 10 ms */, kMaxVelocity = 2, trim_duration = 1, kMinCountForTrim = 10, kMaxTrim = 100, kMaxRCValue = 10000;
 
     if (duration <= 0)
         return true;

AirLib/src/controllers/MavLinkDroneController.cpp

@@ -444,7 +444,7 @@ struct MavLinkDroneController::impl {
             throw std::logic_error("Attempt to send simulated sensor messages while not in simulation mode");
 
         mavlinkcom::MavLinkHilSensor hil_sensor;
-        hil_sensor.time_usec = static_cast<uint64_t>(Utils::getTimeSinceEpochMillis() * 1000);
+        hil_sensor.time_usec = static_cast<uint64_t>(Utils::getTimeSinceEpochNanos() / 1000.0);
         hil_sensor.xacc = acceleration.x();
         hil_sensor.yacc = acceleration.y();
         hil_sensor.zacc = acceleration.z();
@@ -477,7 +477,7 @@ struct MavLinkDroneController::impl {
             throw std::logic_error("Attempt to send simulated GPS messages while not in simulation mode");
 
         mavlinkcom::MavLinkHilGps hil_gps;
-        hil_gps.time_usec = static_cast<uint64_t>(Utils::getTimeSinceEpochMillis() * 1000);
+        hil_gps.time_usec = static_cast<uint64_t>(Utils::getTimeSinceEpochNanos() / 1000.0);
         hil_gps.lat = static_cast<int32_t>(geo_point.latitude * 1E7);
         hil_gps.lon = static_cast<int32_t>(geo_point.longitude* 1E7);
         hil_gps.alt = static_cast<int32_t>(geo_point.altitude * 1000);

DroneShell/include/SimpleShell.hpp

@@ -68,6 +68,14 @@ class SimpleShell {
 				throw std::invalid_argument(Utils::stringf("expecting float value for switch '%s', but found '%s'", name.c_str(), value.c_str()));
 			}
 		}
+        TTimeDelta toTimeDelta() {
+            try {
+                return std::stod(value);
+            }
+            catch (std::exception&) {
+                throw std::invalid_argument(Utils::stringf("expecting TTimeDelta value for switch '%s', but found '%s'", name.c_str(), value.c_str()));
+            }
+        }
 		int toInt() {
 			try {
 				return std::stoi(value);

DroneShell/src/main.cpp

@@ -559,7 +559,7 @@ class BackForthByAngleCommand : public DroneCommand {
         float z = getSwitch("-z").toFloat();
         float duration = getSwitch("-duration").toFloat();
         float yaw = getSwitch("-yaw").toFloat();
-        float pause_time = getSwitch("-pause_time").toFloat();
+        TTimeDelta pause_time = getSwitch("-pause_time").toTimeDelta();
 		int iterations = getSwitch("-iterations").toInt();
         CommandContext* context = params.context;
 
@@ -598,7 +598,7 @@ class BackForthByPositionCommand : public DroneCommand {
         float length = getSwitch("-length").toFloat();
         float z = getSwitch("-z").toFloat();
         float velocity = getSwitch("-velocity").toFloat();
-        float pause_time = getSwitch("-pause_time").toFloat();
+        TTimeDelta pause_time = getSwitch("-pause_time").toTimeDelta();
         int iterations = getSwitch("-iterations").toInt();
         auto drivetrain = getDriveTrain();
         float lookahead = getSwitch("-lookahead").toFloat();
@@ -646,7 +646,7 @@ class SquareByAngleCommand : public DroneCommand {
         float z = getSwitch("-z").toFloat();
         float duration = getSwitch("-duration").toFloat();
         float yaw = getSwitch("-yaw").toFloat();
-        float pause_time = getSwitch("-pause_time").toFloat();
+        TTimeDelta pause_time = getSwitch("-pause_time").toTimeDelta();
 		int iterations = getSwitch("-iterations").toInt();
         CommandContext* context = params.context;
 
@@ -700,7 +700,7 @@ class SquareByPositionCommand : public DroneCommand {
         float length = getSwitch("-length").toFloat();
         float z = getSwitch("-z").toFloat();
         float velocity = getSwitch("-velocity").toFloat();
-        float pause_time = getSwitch("-pause_time").toFloat();
+        TTimeDelta pause_time = getSwitch("-pause_time").toTimeDelta();
 		int iterations = getSwitch("-iterations").toInt();
         auto drivetrain = getDriveTrain();
         float lookahead = getSwitch("-lookahead").toFloat();
@@ -768,7 +768,7 @@ class SquareByPathCommand : public DroneCommand {
         float length = getSwitch("-length").toFloat();
         float z = getSwitch("-z").toFloat();
         float velocity = getSwitch("-velocity").toFloat();
-        //float pause_time = getSwitch("-pause_time").toFloat();
+        //TTimeDelta pause_time = getSwitch("-pause_time").toTimeDelta();
 		int iterations = getSwitch("-iterations").toInt();
         auto drivetrain = getDriveTrain();
         float lookahead = getSwitch("-lookahead").toFloat();
@@ -827,7 +827,7 @@ class CircleByPositionCommand : public DroneCommand {
         float radius = getSwitch("-radius").toFloat();
         float z = getSwitch("-z").toFloat();
         float seg_length = getSwitch("-seg_length").toFloat();
-        float pause_time = getSwitch("-pause_time").toFloat();
+        TTimeDelta pause_time = getSwitch("-pause_time").toTimeDelta();
         float velocity = getSwitch("-velocity").toFloat();
         auto drivetrain = getDriveTrain();
         float lookahead = getSwitch("-lookahead").toFloat();
@@ -1047,7 +1047,7 @@ class GetImageCommand : public DroneCommand {
         this->addSwitch({ "-pause_time", "100", "pause time between each image in milliseconds (default 100)" });
     }
 
-    void getImages(CommandContext* context, DroneControllerBase::ImageType imageType, std::string baseName, int iterations, int pause_time)
+    void getImages(CommandContext* context, DroneControllerBase::ImageType imageType, std::string baseName, int iterations, TTimeDelta pause_time)
     {
         // group the images by the current date.
         std::string folderName = Utils::to_string(Utils::now(), "%Y-%m-%d");
@@ -1077,7 +1077,7 @@ class GetImageCommand : public DroneCommand {
 
             cout << "Image saved to: " << file_path_name << " (" << image.size() << " bytes)" << endl;
 
-            std::this_thread::sleep_for(std::chrono::milliseconds(pause_time));
+            std::this_thread::sleep_for(std::chrono::duration<double>(pause_time));
         }
 
     }
@@ -1087,7 +1087,7 @@ class GetImageCommand : public DroneCommand {
         std::string type = getSwitch("-type").value;
         std::string name = getSwitch("-name").value;
         int iterations = std::stoi(getSwitch("-iterations").value);
-        int pause_time = std::stoi(getSwitch("-pause_time").value);
+        TTimeDelta pause_time = getSwitch("-pause_time").toTimeDelta();
         CommandContext* context = params.context;
 
         DroneControllerBase::ImageType imageType;
@@ -1198,7 +1198,8 @@ int main(int argc, const char *argv[]) {
     std::cout << "DroneServer is responding." << std::endl;
 
 	std::cout << "Waiting for drone to report a valid GPS location..." << std::flush;
-	const float pause_time = 1;
+	const TTimeDelta pause_time = 1;
+
 	auto gps = command_context.client.getGpsLocation();
 	while (gps.latitude == 0 && gps.longitude == 0 && gps.altitude == 0)
 	{