Sensors from JSON and more #2

AirLib/include/common/AirSimSettings.hpp

@@ -46,9 +46,9 @@ struct AirSimSettings {
         std::string camera_name;
         std::string vehicle_name;
 
-        SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene, 
+        SubwindowSetting(int window_index_val = 0, ImageType image_type_val = ImageType::Scene,
                 bool visible_val = false, const std::string& camera_name_val = "", const std::string& vehicle_name_val = "")
-            : window_index(window_index_val), image_type(image_type_val), 
+            : window_index(window_index_val), image_type(image_type_val),
               visible(visible_val), camera_name(camera_name_val), vehicle_name(vehicle_name_val)
         {
         }
@@ -197,28 +197,79 @@ struct AirSimSettings {
     };
 
     struct BarometerSetting : SensorSetting {
+        real_T qnh = EarthUtils::SeaLevelPressure / 100.0f; // hPa
+        real_T pressure_factor_sigma = 0.0365f / 20;
+        real_T pressure_factor_tau = 3600;
+        real_T unnorrelated_noise_sigma = 0.027f * 100;
+        real_T update_latency = 0.0f;    //sec
+        real_T update_frequency = 50;    //Hz
+        real_T startup_delay = 0;        //sec
     };
 
     struct ImuSetting : SensorSetting {
+        // Angular random walk (ARW)
+        real_T gyro_arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec)
+        // Bias Stability (tau = 500s)
+        real_T gyro_tau = 500;
+        real_T gyro_bias_stability = 4.6f / 3600 * M_PIf / 180; //deg/hr converted to rad/sec
+        Vector3r gyro_turn_on_bias = Vector3r::Zero(); //assume calibration is done
+
+        // Velocity random walk (VRW)
+        real_T accel_vrw = 0.24f * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2
+        // Bias Stability (tau = 800s)
+        real_T accel_tau = 800;
+        real_T accel_bias_stability = 36.0f * 1E-6f * EarthUtils::Gravity; //ug converted to m/s^2
+        Vector3r accel_turn_on_bias = Vector3r::Zero(); //assume calibration is done
     };
 
     struct GpsSetting : SensorSetting {
+        real_T eph_time_constant = 0.9f;
+        real_T epv_time_constant = 0.9f;
+        real_T eph_initial = 100.0f;
+        real_T epv_initial = 100.0f;   //initially fully diluted positions
+        real_T eph_final = 0.3f;
+        real_T epv_final = 0.4f;
+        real_T eph_min_3d = 3.0f;
+        real_T eph_min_2d = 4.0f;
+
+        real_T update_latency = 0.2f;    //sec
+        real_T update_frequency = 50;    //Hz
+        real_T startup_delay = 1;        //sec
     };
 
     struct MagnetometerSetting : SensorSetting {
+        Vector3r noise_sigma{0.005f, 0.005f, 0.005f}; //5 mgauss as per specs sheet (RMS is same as stddev) https://goo.gl/UOz6FT
+        real_T scale_factor = 1.0f;
+        Vector3r noise_bias{0.0f, 0.0f, 0.0f}; //no offset as per specsheet (zero gauss level) https://goo.gl/UOz6FT
+        float ref_update_frequency = 0.2f;    //Hz
+
+        //use dipole model if there is enough compute power available
+        bool dynamic_reference_source = true;
+        // ref_source 0==ConstantModel; 1==DipoleModel
+        int ref_source = 1;
+
+        //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
+        real_T update_latency = 0.0f;    //sec: from PX4 doc
+        real_T update_frequency = 50;    //Hz
+        real_T startup_delay = 0;        //sec
     };
 
     struct DistanceSetting : SensorSetting {
-        // shared defaults
-        real_T min_distance = 20.0f / 100; //m
-        real_T max_distance = 4000.0f / 100; //m
+        // nan means keep the default values set in components
         Vector3r position = VectorMath::nanVector();
         Rotation rotation = Rotation::nanRotation();
+
+        real_T min_distance = 20.0f / 100; //m
+        real_T max_distance = 4000.0f / 100; //m
+        real_T unnorrelated_noise_sigma = 0.002f * 100;
+        real_T update_latency = 0.0f;    //sec
+        real_T update_frequency = 50;    //Hz
+        real_T startup_delay = 0;        //sec
+
         bool draw_debug_points = false;
     };
 
     struct LidarSetting : SensorSetting {
-
         // shared defaults
         uint number_of_channels = 16;
         real_T range = 10000.0f / 100;                    // meters
@@ -238,11 +289,11 @@ struct AirSimSettings {
     };
 
     struct VehicleSetting {
-        //required
+        // Required
         std::string vehicle_name;
         std::string vehicle_type;
 
-        //optional
+        // Optional
         std::string default_vehicle_state;
         std::string pawn_path;
         bool allow_api_always = true;
@@ -252,7 +303,7 @@ struct AirSimSettings {
         bool enable_collisions = true;
         bool is_fpv_vehicle = false;
 
-        //nan means use player start
+        // nan means use player start
         Vector3r position = VectorMath::nanVector(); //in global NED
         Rotation rotation = Rotation::nanRotation();
 
@@ -267,7 +318,7 @@ struct AirSimSettings {
 
         bool use_serial = true; // false means use UDP or TCP instead
 
-        //Used to connect via HITL: needed only if use_serial = true
+        // Used to connect via HITL: needed only if use_serial = true
         std::string serial_port = "*";
         int baud_rate = 115200;
 
@@ -295,17 +346,17 @@ struct AirSimSettings {
         int qgc_ip_port = 0;
         std::string qgc_ip_address = "";
 
-        // mavlink vehicle identifiers
+        // Mavlink vehicle identifiers
         uint8_t sim_sysid = 142;
         int sim_compid = 42;
         uint8_t offboard_sysid = 134;
         int offboard_compid = 1;
         uint8_t vehicle_sysid = 135;
         int vehicle_compid = 1;
 
-        // if you want to select a specific local network adapter so you can reach certain remote machines (e.g. wifi versus ethernet)
-        // then you will want to change the LocalHostIp accordingly.  This default only works when log viewer and QGC are also on the
-        // same machine.  Whatever network you choose it has to be the same one for external
+        // If you want to select a specific local network adapter so you can reach certain remote machines (e.g. wifi versus ethernet),
+        // then you will want to change the LocalHostIp accordingly. This default only works when log viewer and QGC are also on the
+        // same machine. Whatever network you choose, it has to be the same one for external.
         std::string local_host_ip = "127.0.0.1";
 
         std::string model = "Generic";
@@ -445,7 +496,7 @@ struct AirSimSettings {
         vehicle_setting->position = pose.position;
         vehicle_setting->pawn_path = pawn_path;
 
-        VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch, 
+        VectorMath::toEulerianAngle(pose.orientation, vehicle_setting->rotation.pitch,
                                     vehicle_setting->rotation.roll, vehicle_setting->rotation.yaw);
 
         vehicles[vehicle_name] = std::move(vehicle_setting);
@@ -550,7 +601,7 @@ struct AirSimSettings {
                 physics_engine_name = "PhysX"; //this value is only informational for now
         }
     }
-    
+
     void loadLevelSettings(const Settings& settings_json)
     {
         level_name = settings_json.getString("Default Environment", "");
@@ -753,20 +804,49 @@ struct AirSimSettings {
             settings_json.getFloat("Y", default_vec.y()),
             settings_json.getFloat("Z", default_vec.z()));
     }
+
     static Rotation createRotationSetting(const Settings& settings_json, const Rotation& default_rot)
     {
         return Rotation(settings_json.getFloat("Yaw", default_rot.yaw),
             settings_json.getFloat("Pitch", default_rot.pitch),
             settings_json.getFloat("Roll", default_rot.roll));
     }
 
+    static Vector3r createGyroBiasSetting(const Settings& settings_json, const Vector3r& default_vec)
+    {
+        return Vector3r(settings_json.getFloat("GyroBias_X", default_vec.x()),
+            settings_json.getFloat("GyroBias_Y", default_vec.y()),
+            settings_json.getFloat("GyroBias_Z", default_vec.z()));
+    }
+
+    static Vector3r createAccelBiasSetting(const Settings& settings_json, const Vector3r& default_vec)
+    {
+        return Vector3r(settings_json.getFloat("AccelBias_X", default_vec.x()),
+            settings_json.getFloat("AccelBias_Y", default_vec.y()),
+            settings_json.getFloat("AccelBias_Z", default_vec.z()));
+    }
+
+    static Vector3r createNoiseSigmaSetting(const Settings& settings_json, const Vector3r& default_vec)
+    {
+        return Vector3r(settings_json.getFloat("NoiseSigma_X", default_vec.x()),
+            settings_json.getFloat("NoiseSigma_Y", default_vec.y()),
+            settings_json.getFloat("NoiseSigma_Z", default_vec.z()));
+    }
+
+    static Vector3r createNoiseBiasSetting(const Settings& settings_json, const Vector3r& default_vec)
+    {
+        return Vector3r(settings_json.getFloat("NoiseBias_X", default_vec.x()),
+            settings_json.getFloat("NoiseBias_Y", default_vec.y()),
+            settings_json.getFloat("NoiseBias_Z", default_vec.z()));
+    }
+
     static std::unique_ptr<VehicleSetting> createVehicleSetting(const std::string& simmode_name,  const Settings& settings_json,
         const std::string vehicle_name)
     {
         auto vehicle_type = Utils::toLower(settings_json.getString("VehicleType", ""));
 
         std::unique_ptr<VehicleSetting> vehicle_setting;
-        if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo 
+        if (vehicle_type == kVehicleTypePX4 || vehicle_type == kVehicleTypeArduCopterSolo
             || vehicle_type == kVehicleTypeArduCopter || vehicle_type == kVehicleTypeArduRover)
             vehicle_setting = createMavLinkVehicleSetting(settings_json);
         //for everything else we don't need derived class yet
@@ -1197,34 +1277,54 @@ struct AirSimSettings {
 
     static void initializeBarometerSetting(BarometerSetting& barometer_setting, const Settings& settings_json)
     {
-        unused(barometer_setting);
-        unused(settings_json);
-
-        //TODO: set from json as needed
+        barometer_setting.qnh = settings_json.getFloat("SeaLevelPressure", barometer_setting.qnh);
+        barometer_setting.pressure_factor_sigma = settings_json.getFloat("PressureFactorSigma", barometer_setting.pressure_factor_sigma);
+        barometer_setting.pressure_factor_tau = settings_json.getFloat("PressureFactorTau", barometer_setting.pressure_factor_tau);
+        barometer_setting.unnorrelated_noise_sigma = settings_json.getFloat("NoiseSigma", barometer_setting.unnorrelated_noise_sigma);
+        barometer_setting.update_latency = settings_json.getFloat("UpdateLatency", barometer_setting.update_latency);
+        barometer_setting.update_frequency = settings_json.getFloat("UpdateFrequency", barometer_setting.update_frequency);
+        barometer_setting.startup_delay = settings_json.getFloat("StartupDelay", barometer_setting.startup_delay);
     }
 
     static void initializeImuSetting(ImuSetting& imu_setting, const Settings& settings_json)
     {
-        unused(imu_setting);
-        unused(settings_json);
-
-        //TODO: set from json as needed
+        imu_setting.gyro_arw = settings_json.getFloat("GyroAngleRandomWalk", imu_setting.gyro_arw);
+        imu_setting.gyro_tau = settings_json.getFloat("GyroBiasTau", imu_setting.gyro_tau);
+        imu_setting.gyro_bias_stability = settings_json.getFloat("GyroBiasStability", imu_setting.gyro_bias_stability);
+        imu_setting.gyro_turn_on_bias = createGyroBiasSetting(settings_json, imu_setting.gyro_turn_on_bias);
+
+        imu_setting.accel_vrw = settings_json.getFloat("AccelerometerVelocityRandomWalk", imu_setting.accel_vrw);
+        imu_setting.accel_tau = settings_json.getFloat("AccelerometerBiasTau", imu_setting.accel_tau);
+        imu_setting.accel_bias_stability = settings_json.getFloat("AccelerometerBiasStability", imu_setting.accel_bias_stability);
+        imu_setting.accel_turn_on_bias = createAccelBiasSetting(settings_json, imu_setting.accel_turn_on_bias);
     }
 
     static void initializeGpsSetting(GpsSetting& gps_setting, const Settings& settings_json)
     {
-        unused(gps_setting);
-        unused(settings_json);
+        gps_setting.eph_time_constant = settings_json.getFloat("EphTimeConstant", gps_setting.eph_time_constant);
+        gps_setting.epv_time_constant = settings_json.getFloat("EpvTimeConstant", gps_setting.epv_time_constant);
+        gps_setting.eph_initial = settings_json.getFloat("EphInitial", gps_setting.eph_initial);
+        gps_setting.epv_initial = settings_json.getFloat("EpvInitial", gps_setting.epv_initial);
+        gps_setting.eph_final = settings_json.getFloat("EphFinal", gps_setting.eph_final);
+        gps_setting.epv_final = settings_json.getFloat("EpvFinal", gps_setting.epv_final);
+        gps_setting.eph_min_3d = settings_json.getFloat("EphMin3d", gps_setting.eph_min_3d);
+        gps_setting.eph_min_2d = settings_json.getFloat("EphMin2d", gps_setting.eph_min_2d);
+        gps_setting.update_latency = settings_json.getFloat("UpdateLatency", gps_setting.update_latency);
+        gps_setting.update_frequency = settings_json.getFloat("UpdateFrequency", gps_setting.update_frequency);
+        gps_setting.startup_delay = settings_json.getFloat("StartupDelay", gps_setting.startup_delay);
 
-        //TODO: set from json as needed
     }
 
     static void initializeMagnetometerSetting(MagnetometerSetting& magnetometer_setting, const Settings& settings_json)
     {
-        unused(magnetometer_setting);
-        unused(settings_json);
-
-        //TODO: set from json as needed
+        magnetometer_setting.noise_sigma = createNoiseSigmaSetting(settings_json, magnetometer_setting.noise_sigma);
+        magnetometer_setting.scale_factor = settings_json.getFloat("ScaleFactor", magnetometer_setting.scale_factor);
+        magnetometer_setting.dynamic_reference_source = settings_json.getBool("DynamicReferenceSource", magnetometer_setting.dynamic_reference_source);
+        magnetometer_setting.update_frequency = settings_json.getFloat("UpdateFrequency", magnetometer_setting.update_frequency);
+        magnetometer_setting.ref_source = settings_json.getInt("ReferenceSource", magnetometer_setting.ref_source);
+        magnetometer_setting.update_latency = settings_json.getFloat("UpdateLatency", magnetometer_setting.update_latency);
+        magnetometer_setting.update_frequency = settings_json.getFloat("UpdateFrequency", magnetometer_setting.update_frequency);
+        magnetometer_setting.startup_delay = settings_json.getFloat("StartupDelay", magnetometer_setting.startup_delay);
     }
 
     static void initializeDistanceSetting(DistanceSetting& distance_setting, const Settings& settings_json)
@@ -1233,6 +1333,10 @@ struct AirSimSettings {
         distance_setting.max_distance = settings_json.getFloat("MaxDistance", distance_setting.max_distance);
         distance_setting.draw_debug_points = settings_json.getBool("DrawDebugPoints", distance_setting.draw_debug_points);
 
+        distance_setting.unnorrelated_noise_sigma = settings_json.getFloat("Noise", distance_setting.unnorrelated_noise_sigma);
+        distance_setting.update_latency = settings_json.getFloat("UpdateLatency", distance_setting.update_latency);
+        distance_setting.update_frequency = settings_json.getFloat("UpdateFrequency", distance_setting.update_frequency);
+        distance_setting.startup_delay = settings_json.getFloat("StartupDelay", distance_setting.startup_delay);
         distance_setting.position = createVectorSetting(settings_json, distance_setting.position);
         distance_setting.rotation = createRotationSetting(settings_json, distance_setting.rotation);
     }

AirLib/include/sensors/barometer/BarometerSimpleParams.hpp

@@ -49,7 +49,14 @@ struct BarometerSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::BarometerSetting& settings)
     {
-        unused(settings);
+        qnh = settings.qnh;
+        pressure_factor_sigma = settings.pressure_factor_sigma;
+        pressure_factor_tau = settings.pressure_factor_tau;
+        unnorrelated_noise_sigma = settings.unnorrelated_noise_sigma;
+        update_latency = settings.update_latency;
+        update_frequency = settings.update_frequency;
+        startup_delay = settings.startup_delay;
+
     }
 };
 

AirLib/include/sensors/distance/DistanceSimpleParams.hpp

@@ -50,6 +50,10 @@ struct DistanceSimpleParams {
 
         min_distance = settings.min_distance;
         max_distance = settings.max_distance;
+        unnorrelated_noise_sigma = settings.unnorrelated_noise_sigma;
+        update_latency = settings.update_latency;
+        update_frequency = settings.update_frequency;
+        startup_delay = settings.startup_delay;
 
         draw_debug_points = settings.draw_debug_points;
 
@@ -72,7 +76,8 @@ struct DistanceSimpleParams {
         relative_pose.orientation = VectorMath::toQuaternion(
             Utils::degreesToRadians(pitch),   //pitch - rotation around Y axis
             Utils::degreesToRadians(roll),    //roll  - rotation around X axis
-            Utils::degreesToRadians(yaw));    //yaw   - rotation around Z axis
+            Utils::degreesToRadians(yaw)      //yaw   - rotation around Z axis
+        );
     }
 };
 

AirLib/include/sensors/gps/GpsSimpleParams.hpp

@@ -5,23 +5,38 @@
 #define msr_airlib_GpsSimpleParams_hpp
 
 #include "common/Common.hpp"
-
+#include "common/AirSimSettings.hpp"
 
 namespace msr { namespace airlib {
 
 struct GpsSimpleParams {
-    real_T eph_time_constant = 0.9f, epv_time_constant = 0.9f;
-    real_T eph_initial = 100.0f, epv_initial = 100.0f;   //initially fully diluted positions
-    real_T eph_final = 0.3f, epv_final = 0.4f;
-    real_T eph_min_3d = 3.0f, eph_min_2d = 4.0f;
+    real_T eph_time_constant = 0.9f;
+    real_T epv_time_constant = 0.9f;
+    real_T eph_initial = 100.0f;
+    real_T epv_initial = 100.0f;   //initially fully diluted positions
+    real_T eph_final = 0.3f;
+    real_T epv_final = 0.4f;
+    real_T eph_min_3d = 3.0f;
+    real_T eph_min_2d = 4.0f;
 
     real_T update_latency = 0.2f;    //sec
     real_T update_frequency = 50;    //Hz
     real_T startup_delay = 1;        //sec
 
     void initializeFromSettings(const AirSimSettings::GpsSetting& settings)
     {
-        unused(settings);
+        eph_time_constant = settings.eph_time_constant;
+        epv_time_constant = settings.epv_time_constant;
+        eph_initial = settings.eph_initial;
+        epv_initial = settings.epv_initial;
+        eph_final = settings.eph_final;
+        epv_final = settings.epv_final;
+        eph_min_3d = settings.eph_min_3d;
+        eph_min_2d = settings.eph_min_2d;
+
+        update_latency = settings.update_latency;
+        update_frequency = settings.update_frequency;
+        startup_delay = settings.startup_delay;
     }
 };