fix px4 connection for wsl 2.

AirLib/include/common/common_utils/FileSystem.hpp

@@ -104,8 +104,7 @@ class FileSystem
                 break;
         }
         if (i < 0) return "";
-        auto ui = static_cast<uint>(i);
-        return str.substr(ui, len - ui);
+        return str.substr(i, len - i);
     }
 
     static std::string getLogFolderPath(bool folder_timestamp, const std::string& parent = "")

AirLib/include/physics/PhysicsEngineBase.hpp

@@ -44,7 +44,7 @@ class PhysicsEngineBase : public UpdatableObject {
     virtual void erase_remove(TUpdatableObjectPtr obj) { 
         members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); }
 
-    virtual void setWind(const Vector3r& wind) {};
+    virtual void setWind(const Vector3r& wind) { unused(wind); };
 
 private:
     MembersContainer members_;

AirLib/include/sensors/SensorBase.hpp

@@ -59,7 +59,7 @@ class SensorBase : public UpdatableObject  {
 
 private:
     //ground truth can be shared between many sensors
-    GroundTruth ground_truth_;
+    GroundTruth ground_truth_ = { nullptr, nullptr };
     std::string name_ = "";
 };
 

AirLib/include/sensors/SensorFactory.hpp

@@ -19,28 +19,28 @@ class SensorFactory {
 public:
 
     // creates one sensor from settings
-    virtual std::unique_ptr<SensorBase> createSensorFromSettings(
+    virtual std::shared_ptr<SensorBase> createSensorFromSettings(
         const AirSimSettings::SensorSetting* sensor_setting) const
     {
         switch (sensor_setting->sensor_type) {
         case SensorBase::SensorType::Imu:
-            return std::unique_ptr<ImuSimple>(new ImuSimple(*static_cast<const AirSimSettings::ImuSetting*>(sensor_setting)));
+            return std::shared_ptr<ImuSimple>(new ImuSimple(*static_cast<const AirSimSettings::ImuSetting*>(sensor_setting)));
         case SensorBase::SensorType::Magnetometer:
-            return std::unique_ptr<MagnetometerSimple>(new MagnetometerSimple(*static_cast<const AirSimSettings::MagnetometerSetting*>(sensor_setting)));
+            return std::shared_ptr<MagnetometerSimple>(new MagnetometerSimple(*static_cast<const AirSimSettings::MagnetometerSetting*>(sensor_setting)));
         case SensorBase::SensorType::Gps:
-            return std::unique_ptr<GpsSimple>(new GpsSimple(*static_cast<const AirSimSettings::GpsSetting*>(sensor_setting)));
+            return std::shared_ptr<GpsSimple>(new GpsSimple(*static_cast<const AirSimSettings::GpsSetting*>(sensor_setting)));
         case SensorBase::SensorType::Barometer:
-            return std::unique_ptr<BarometerSimple>(new BarometerSimple(*static_cast<const AirSimSettings::BarometerSetting*>(sensor_setting)));
+            return std::shared_ptr<BarometerSimple>(new BarometerSimple(*static_cast<const AirSimSettings::BarometerSetting*>(sensor_setting)));
         default:
             throw new std::invalid_argument("Unexpected sensor type");
         }
     }
 
     // creates sensor-collection
     virtual void createSensorsFromSettings(
-        const std::map<std::string, std::unique_ptr<AirSimSettings::SensorSetting>>& sensors_settings,
+        const std::map<std::string, std::shared_ptr<AirSimSettings::SensorSetting>>& sensors_settings,
         SensorCollection& sensors,
-        vector<unique_ptr<SensorBase>>& sensor_storage) const
+        vector<shared_ptr<SensorBase>>& sensor_storage) const
     {
         for (const auto& sensor_setting_pair : sensors_settings) {
             const AirSimSettings::SensorSetting* sensor_setting = sensor_setting_pair.second.get();
@@ -49,11 +49,10 @@ class SensorFactory {
             if (sensor_setting == nullptr || !sensor_setting->enabled)
                 continue;
 
-            std::unique_ptr<SensorBase> sensor = createSensorFromSettings(sensor_setting);
+            std::shared_ptr<SensorBase> sensor = createSensorFromSettings(sensor_setting);
             if (sensor) {
-                    SensorBase* sensor_temp = sensor.get();
-                    sensor_storage.push_back(std::move(sensor));
-                    sensors.insert(sensor_temp, sensor_setting->sensor_type);
+                sensor_storage.push_back(sensor);
+                sensors.insert(sensor.get(), sensor_setting->sensor_type);
             }
         }
     }

AirLib/include/sensors/barometer/BarometerSimple.hpp

@@ -27,7 +27,7 @@ class BarometerSimple  : public BarometerBase {
         //GM process that would do random walk for pressure factor
         pressure_factor_.initialize(params_.pressure_factor_tau, params_.pressure_factor_sigma, 0);
 
-        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma);
+        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma);
         //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
 
         //initialize frequency limiter
@@ -76,7 +76,7 @@ class BarometerSimple  : public BarometerBase {
         auto altitude = ground_truth.environment->getState().geo_point.altitude;
         auto pressure = EarthUtils::getStandardPressure(altitude);
 
-        //add drift in pressure, about 10m change per hour
+        //add drift in pressure, about 10m change per hour using default settings.
         pressure_factor_.update();
         pressure += pressure * pressure_factor_.getOutput();
 

AirLib/include/sensors/barometer/BarometerSimpleParams.hpp

@@ -5,6 +5,7 @@
 #define msr_airlib_BarometerSimpleParams_hpp
 
 #include "common/Common.hpp"
+#include "common/AirSimSettings.hpp"
 
 
 namespace msr { namespace airlib {
@@ -33,14 +34,14 @@ struct BarometerSimpleParams {
 
          real_T correlated_noise_sigma = 0.27f;
          real_T correlated_noise_tau = 0.87f;
-         real_T unnorrelated_noise_sigma = 0.24f;
+         real_T uncorrelated_noise_sigma = 0.24f;
     */
 
     //Experiments for MEAS MS56112 sensor shows 0.021mbar, datasheet has resoultion of 0.027mbar @ 1024
     //http://www.te.com/commerce/DocumentDelivery/DDEController?Action=srchrtrv&DocNm=MS5611-01BA03&DocType=Data+Sheet&DocLang=English
-    real_T unnorrelated_noise_sigma = 0.027f * 100;
+    real_T uncorrelated_noise_sigma = 0.027f * 100;
     //jMavSim uses below
-    //real_T unnorrelated_noise_sigma = 0.1f;
+    //real_T uncorrelated_noise_sigma = 0.1f;
 
     //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
     real_T update_latency = 0.0f;    //sec
@@ -49,7 +50,13 @@ struct BarometerSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::BarometerSetting& settings)
     {
-        unused(settings);
+        const auto& json = settings.settings;
+        pressure_factor_sigma = json.getFloat("PressureFactorSigma", pressure_factor_sigma);
+        pressure_factor_tau = json.getFloat("PressureFactorTau", pressure_factor_tau);
+        uncorrelated_noise_sigma = json.getFloat("UncorrelatedNoiseSigma", uncorrelated_noise_sigma);
+        update_latency = json.getFloat("UpdateLatency", update_latency);
+        update_frequency = json.getFloat("UpdateFrequency", update_frequency);
+        startup_delay = json.getFloat("StartupDelay", startup_delay);
     }
 };
 

AirLib/include/sensors/distance/DistanceSimple.hpp

@@ -22,7 +22,7 @@ class DistanceSimple : public DistanceBase {
         // initialize params
         params_.initializeFromSettings(setting);
 
-        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.unnorrelated_noise_sigma);
+        uncorrelated_noise_ = RandomGeneratorGausianR(0.0f, params_.uncorrelated_noise_sigma);
         //correlated_noise_.initialize(params_.correlated_noise_tau, params_.correlated_noise_sigma, 0.0f);
 
 

AirLib/include/sensors/distance/DistanceSimpleParams.hpp

@@ -31,13 +31,13 @@ struct DistanceSimpleParams {
 
      real_T correlated_noise_sigma = 0.27f;
      real_T correlated_noise_tau = 0.87f;
-     real_T unnorrelated_noise_sigma = 0.24f;
+     real_T uncorrelated_noise_sigma = 0.24f;
 
 */
     //TODO: update sigma based on documentation, maybe as a function increasing with measured distance
-    real_T unnorrelated_noise_sigma = 0.002f * 100;
+    real_T uncorrelated_noise_sigma = 0.002f * 100;
     //jMavSim uses below
-    //real_T unnorrelated_noise_sigma = 0.1f;
+    //real_T uncorrelated_noise_sigma = 0.1f;
 
     //see PX4 param reference for EKF: https://dev.px4.io/en/advanced/parameter_reference.html
     real_T update_latency = 0.0f;    //sec
@@ -46,14 +46,17 @@ struct DistanceSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::DistanceSetting& settings)
     {
-        std::string simmode_name = AirSimSettings::singleton().simmode_name;
+        const auto& settings_json = settings.settings;
+        min_distance = settings_json.getFloat("MinDistance", min_distance);
+        max_distance = settings_json.getFloat("MaxDistance", max_distance);
+        draw_debug_points = settings_json.getBool("DrawDebugPoints", draw_debug_points);
 
-        min_distance = settings.min_distance;
-        max_distance = settings.max_distance;
+        auto position = AirSimSettings::createVectorSetting(settings_json, VectorMath::nanVector());
+        auto rotation = AirSimSettings::createRotationSetting(settings_json, AirSimSettings::Rotation::nanRotation());
 
-        draw_debug_points = settings.draw_debug_points;
+        std::string simmode_name = AirSimSettings::singleton().simmode_name;
 
-        relative_pose.position = settings.position;
+        relative_pose.position = position;
         if (std::isnan(relative_pose.position.x()))
             relative_pose.position.x() = 0;
         if (std::isnan(relative_pose.position.y()))
@@ -66,9 +69,9 @@ struct DistanceSimpleParams {
         }
 
         float pitch, roll, yaw;
-        pitch = !std::isnan(settings.rotation.pitch) ? settings.rotation.pitch : 0;
-        roll = !std::isnan(settings.rotation.roll) ? settings.rotation.roll : 0;
-        yaw = !std::isnan(settings.rotation.yaw) ? settings.rotation.yaw : 0;
+        pitch = !std::isnan(rotation.pitch) ? rotation.pitch : 0;
+        roll = !std::isnan(rotation.roll) ? rotation.roll : 0;
+        yaw = !std::isnan(rotation.yaw) ? rotation.yaw : 0;
         relative_pose.orientation = VectorMath::toQuaternion(
             Utils::degreesToRadians(pitch),   //pitch - rotation around Y axis
             Utils::degreesToRadians(roll),    //roll  - rotation around X axis

AirLib/include/sensors/gps/GpsSimpleParams.hpp

@@ -12,7 +12,7 @@ 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_final = 0.1f, epv_final = 0.1f;   // PX4 won't converge GPS sensor fusion until we get to 10% accuracty.
     real_T eph_min_3d = 3.0f, eph_min_2d = 4.0f;
 
     real_T update_latency = 0.2f;    //sec
@@ -21,7 +21,18 @@ struct GpsSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::GpsSetting& settings)
     {
-        unused(settings);
+        const auto& json = settings.settings;
+        eph_time_constant = json.getFloat("EPH_TimeConstant", eph_time_constant);
+        epv_time_constant = json.getFloat("EPV_TimeConstant", epv_time_constant);
+        eph_initial = json.getFloat("EphInitial", eph_initial);
+        epv_initial = json.getFloat("EpvInitial", epv_initial);
+        eph_final = json.getFloat("EphFinal", eph_final);
+        epv_final = json.getFloat("EpvFinal", epv_final);
+        eph_min_3d = json.getFloat("EphMin3d", eph_min_3d);
+        eph_min_2d = json.getFloat("EphMin2d", eph_min_2d);
+        update_latency = json.getFloat("UpdateLatency", update_latency);
+        update_frequency = json.getFloat("UpdateFrequency", update_frequency);
+        startup_delay = json.getFloat("StartupDelay", startup_delay);
     }
 };
 

AirLib/include/sensors/imu/ImuSimpleParams.hpp

@@ -25,7 +25,7 @@ struct ImuSimpleParams {
     For Allan Variance/Deviation plots see http://www.invensense.com/wp-content/uploads/2015/02/MPU-3300-Datasheet.pdf
     */
     struct Gyroscope {
-        //angule random walk (ARW)
+        //angular random walk (ARW)
         real_T arw = 0.30f / sqrt(3600.0f) * M_PIf / 180; //deg/sqrt(hour) converted to rad/sqrt(sec)
         //Bias Stability (tau = 500s)
         real_T tau = 500;
@@ -46,7 +46,25 @@ struct ImuSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::ImuSetting& settings)
     {
-        unused(settings);
+        const auto& json = settings.settings;
+        float arw = json.getFloat("AngularRandomWalk", Utils::nan<float>());
+        if (!std::isnan(arw)) {
+            gyro.arw = arw / sqrt(3600.0f) * M_PIf / 180; // //deg/sqrt(hour) converted to rad/sqrt(sec)
+        }
+        gyro.tau = json.getFloat("GyroBiasStabilityTau", gyro.tau);
+        float bias_stability = json.getFloat("GyroBiasStability", Utils::nan<float>());
+        if (!std::isnan(bias_stability)) {
+            gyro.bias_stability = bias_stability / 3600 * M_PIf / 180; //deg/hr converted to rad/sec
+        }
+        auto vrw = json.getFloat("VelocityRandomWalk", Utils::nan<float>());
+        if (!std::isnan(vrw)) {
+            accel.vrw = vrw * EarthUtils::Gravity / 1.0E3f; //mg converted to m/s^2 
+        }
+        accel.tau = json.getFloat("AccelBiasStabilityTau", accel.tau);
+        bias_stability = json.getFloat("AccelBiasStability", Utils::nan<float>());
+        if (!std::isnan(bias_stability)) {
+            accel.bias_stability = bias_stability * 1E-6f * 9.80665f; //ug converted to m/s^2
+        }
     }
 };
 

AirLib/include/sensors/magnetometer/MagnetometerSimpleParams.hpp

@@ -34,7 +34,15 @@ struct MagnetometerSimpleParams {
 
     void initializeFromSettings(const AirSimSettings::MagnetometerSetting& settings)
     {
-        unused(settings);
+        const auto& json = settings.settings;
+        float noise = json.getFloat("NoiseSigma", noise_sigma.x());
+        noise_sigma = Vector3r(noise, noise, noise);
+        scale_factor = json.getFloat("ScaleFactor", scale_factor);
+        float bias = json.getFloat("NoiseBias", noise_bias.x());
+        noise_bias = Vector3r(bias, bias, bias);
+        update_latency = json.getFloat("UpdateLatency", update_latency);
+        update_frequency = json.getFloat("UpdateFrequency", update_frequency);
+        startup_delay = json.getFloat("StartupDelay", startup_delay);
     }
 };
 

AirLib/include/vehicles/car/api/CarApiBase.hpp

@@ -131,11 +131,7 @@ class CarApiBase : public VehicleApiBase  {
 
     void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting)
     {
-        // use sensors from vehicle settings; if empty list, use default sensors.
-        // note that the vehicle settings completely override the default sensor "list";
-        // there is no piecemeal add/remove/update per sensor.
-        const std::map<std::string, std::unique_ptr<AirSimSettings::SensorSetting>>& sensor_settings
-            = vehicle_setting->sensors.size() > 0 ? vehicle_setting->sensors : AirSimSettings::AirSimSettings::singleton().sensor_defaults;
+        const auto& sensor_settings = vehicle_setting->sensors;
 
         sensor_factory_->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_);
     }
@@ -149,7 +145,7 @@ class CarApiBase : public VehicleApiBase  {
 
     std::shared_ptr<const SensorFactory> sensor_factory_;
     SensorCollection sensors_; //maintains sensor type indexed collection of sensors
-    vector<unique_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
+    vector<shared_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
 
 protected:
     virtual void resetImplementation() override

AirLib/include/vehicles/car/firmwares/ardurover/ArduRoverApi.hpp

@@ -142,10 +142,10 @@ class ArduRoverApi : public CarApiBase {
         }
 
         Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
-        Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
+        Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
 
         udpSocket_ = std::make_shared<mavlinkcom::UdpSocket>();
-        udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
+        udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port_local);
     }
 
 private:

AirLib/include/vehicles/multirotor/MultiRotorParams.hpp

@@ -85,11 +85,7 @@ class MultiRotorParams {
 
     void addSensorsFromSettings(const AirSimSettings::VehicleSetting* vehicle_setting)
     {
-        // use sensors from vehicle settings; if empty list, use default sensors.
-        // note that the vehicle settings completely override the default sensor "list";
-        // there is no piecemeal add/remove/update per sensor.
-        const std::map<std::string, std::unique_ptr<AirSimSettings::SensorSetting>>& sensor_settings
-            = vehicle_setting->sensors.size() > 0 ? vehicle_setting->sensors : AirSimSettings::AirSimSettings::singleton().sensor_defaults;
+        const auto& sensor_settings = vehicle_setting->sensors;
 
         getSensorFactory()->createSensorsFromSettings(sensor_settings, sensors_, sensor_storage_);
     }
@@ -412,7 +408,7 @@ class MultiRotorParams {
 private:
     Params params_;
     SensorCollection sensors_; //maintains sensor type indexed collection of sensors
-    vector<unique_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
+    vector<shared_ptr<SensorBase>> sensor_storage_; //RAII for created sensors
 };
 
 }} //namespace

AirLib/include/vehicles/multirotor/firmwares/arducopter/ArduCopterApi.hpp

@@ -318,10 +318,10 @@ class ArduCopterApi : public MultirotorApiBase {
         }
 
         Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port_, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
-        Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
+        Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip_.c_str()), Utils::kLogLevelInfo);
 
         udpSocket_ = std::make_shared<mavlinkcom::UdpSocket>();
-        udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port);
+        udpSocket_->bind(connection_info_.local_host_ip, connection_info_.control_port_local);
     }
 
 private:

AirLib/include/vehicles/multirotor/firmwares/mavlink/ArduCopterSoloApi.hpp

@@ -113,9 +113,9 @@ class ArduCopterSoloApi : public MavLinkMultirotorApi
             }
 
             Utils::log(Utils::stringf("Using UDP port %d, local IP %s, remote IP %s for sending sensor data", port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo);
-            Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo);
+            Utils::log(Utils::stringf("Using UDP port %d for receiving rotor power", connection_info_.control_port_local, connection_info_.local_host_ip.c_str(), ip.c_str()), Utils::kLogLevelInfo);
 
-            udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.control_port);
+            udpSocket_ = mavlinkcom::AdHocConnection::connectLocalUdp("ArduCopterSoloConnector", ip, connection_info_.control_port_local);
             mavlinkcom::AdHocMessageHandler handler = [this](std::shared_ptr<mavlinkcom::AdHocConnection> connection, const std::vector<uint8_t> &msg) {
                 this->rotorPowerMessageHandler(connection, msg);
             };