Fix initialization issues (#14)

* Fix initialization issues Some variables were not being initialized, causing issues at runtime * Add EIGEN_MAKE_ALIGNED_OPERATOR_NEW This addressed the issue of eigen that is mentioned in https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html * Move initializations to headerfile This commit moves initializations to the header file * Update include/sensor_airspeed_plugin.h Co-authored-by: Beat Küng <[email protected]> Co-authored-by: Beat Küng <[email protected]>
Auterion · Oct 1, 2020 · 3c4d038 · 3c4d038
1 parent 85031df
commit 3c4d038
Show file tree

Hide file tree

Showing 13 changed files with 55 additions and 68 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -57,7 +57,8 @@ endif()
 
 add_definitions(-DJSBSIM_ROOT_DIR="${CMAKE_CURRENT_SOURCE_DIR}")
 add_definitions(
-    -Wno-address-of-packed-member   # MAVLink annoyances
+	-Wno-address-of-packed-member   # MAVLink annoyances
+	--warn-uninitialized
 )
 
 include_directories(include)

diff --git a/include/mavlink_interface.h b/include/mavlink_interface.h
@@ -122,6 +122,7 @@ struct Gps {
 
 class MavlinkInterface {
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   MavlinkInterface();
   ~MavlinkInterface();
   void pollForMAVLinkMessages();

diff --git a/include/sensor_airspeed_plugin.h b/include/sensor_airspeed_plugin.h
@@ -54,7 +54,6 @@ class SensorAirspeedPlugin : public SensorPlugin {
   double getAirspeed();
   double getAirTemperature();
 
-  std::default_random_engine _random_generator;
   std::normal_distribution<double> standard_normal_distribution_;
-  double diff_pressure_stddev_;
+  double diff_pressure_stddev_{0.01};
 };
diff --git a/include/sensor_baro_plugin.h b/include/sensor_baro_plugin.h
@@ -55,13 +55,10 @@ class SensorBaroPlugin : public SensorPlugin {
   float getPressureAltitude();
   float getAirPressure();
 
-  std::default_random_engine _random_generator;
   std::normal_distribution<double> _standard_normal_distribution;
 
   // state variables for baro pressure sensor random noise generator
-  double _baro_rnd_y2;
-  bool _baro_rnd_use_last;
-
-  double _baro_drift_pa_per_sec;
-  double _baro_drift_pa;
+  double _baro_rnd_y2{0.};
+  bool _baro_rnd_use_last{false};
+  double _baro_drift_pa{0.};
 };
diff --git a/include/sensor_imu_plugin.h b/include/sensor_imu_plugin.h
@@ -44,8 +44,21 @@
 #include "common.h"
 #include "sensor_plugin.h"
 
+// Default values for use with ADIS16448 IMU
+static constexpr double kDefaultAdisGyroscopeNoiseDensity = 2.0 * 35.0 / 3600.0 / 180.0 * M_PI;
+static constexpr double kDefaultAdisGyroscopeRandomWalk = 2.0 * 4.0 / 3600.0 / 180.0 * M_PI;
+static constexpr double kDefaultAdisGyroscopeBiasCorrelationTime = 1.0e+3;
+static constexpr double kDefaultAdisGyroscopeTurnOnBiasSigma = 0.5 / 180.0 * M_PI;
+static constexpr double kDefaultAdisAccelerometerNoiseDensity = 2.0 * 2.0e-3;
+static constexpr double kDefaultAdisAccelerometerRandomWalk = 2.0 * 3.0e-3;
+static constexpr double kDefaultAdisAccelerometerBiasCorrelationTime = 300.0;
+static constexpr double kDefaultAdisAccelerometerTurnOnBiasSigma = 20.0e-3 * 9.8;
+// Earth's gravity in Zurich (lat=+47.3667degN, lon=+8.5500degE, h=+500m, WGS84)
+static constexpr double kDefaultGravityMagnitude = 9.8068;
+
 class SensorImuPlugin : public SensorPlugin {
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   SensorImuPlugin(JSBSim::FGFDMExec* jsbsim);
   ~SensorImuPlugin();
   SensorData::Imu getData();
@@ -56,30 +69,29 @@ class SensorImuPlugin : public SensorPlugin {
   void addNoise(Eigen::Vector3d* linear_acceleration, Eigen::Vector3d* angular_velocity, const double dt);
 
   /// Gyroscope noise density (two-sided spectrum) [rad/s/sqrt(Hz)]
-  double gyroscope_noise_density;
+  double gyroscope_noise_density{kDefaultAdisGyroscopeNoiseDensity};
   /// Gyroscope bias random walk [rad/s/s/sqrt(Hz)]
-  double gyroscope_random_walk;
+  double gyroscope_random_walk{kDefaultAdisGyroscopeRandomWalk};
   /// Gyroscope bias correlation time constant [s]
-  double gyroscope_bias_correlation_time;
+  double gyroscope_bias_correlation_time{kDefaultAdisGyroscopeBiasCorrelationTime};
   /// Gyroscope turn on bias standard deviation [rad/s]
-  double gyroscope_turn_on_bias_sigma;
+  double gyroscope_turn_on_bias_sigma{kDefaultAdisGyroscopeTurnOnBiasSigma};
   /// Accelerometer noise density (two-sided spectrum) [m/s^2/sqrt(Hz)]
-  double accelerometer_noise_density;
+  double accelerometer_noise_density{kDefaultAdisAccelerometerNoiseDensity};
   /// Accelerometer bias random walk. [m/s^2/s/sqrt(Hz)]
-  double accelerometer_random_walk;
+  double accelerometer_random_walk{kDefaultAdisAccelerometerRandomWalk};
   /// Accelerometer bias correlation time constant [s]
-  double accelerometer_bias_correlation_time;
+  double accelerometer_bias_correlation_time{kDefaultAdisAccelerometerBiasCorrelationTime};
   /// Accelerometer turn on bias standard deviation [m/s^2]
-  double accelerometer_turn_on_bias_sigma;
+  double accelerometer_turn_on_bias_sigma{kDefaultAdisAccelerometerTurnOnBiasSigma};
   /// Norm of the gravitational acceleration [m/s^2]
-  double gravity_magnitude;
+  double gravity_magnitude{kDefaultGravityMagnitude};
 
-  Eigen::Vector3d _gyroscope_bias;
-  Eigen::Vector3d _accelerometer_bias;
+  Eigen::Vector3d _gyroscope_bias{Eigen::Vector3d::Zero()};
+  Eigen::Vector3d _accelerometer_bias{Eigen::Vector3d::Zero()};
 
-  Eigen::Vector3d _gyroscope_turn_on_bias;
-  Eigen::Vector3d _accelerometer_turn_on_bias;
+  Eigen::Vector3d _gyroscope_turn_on_bias{Eigen::Vector3d::Zero()};
+  Eigen::Vector3d _accelerometer_turn_on_bias{Eigen::Vector3d::Zero()};
 
-  std::default_random_engine _random_generator;
   std::normal_distribution<double> _standard_normal_distribution;
 };
diff --git a/include/sensor_mag_plugin.h b/include/sensor_mag_plugin.h
@@ -45,8 +45,16 @@
 #include "geo_mag_declination.h"
 #include "sensor_plugin.h"
 
+static constexpr auto kDefaultPubRate = 100.0;  // [Hz]. Note: corresponds to most of the mag devices supported in PX4
+
+// Default values for use with ADIS16448 IMU
+static constexpr auto kDefaultNoiseDensity = 0.4 * 1e-3;     // [gauss / sqrt(hz)]
+static constexpr auto kDefaultRandomWalk = 6.4 * 1e-6;       // [gauss * sqrt(hz)]
+static constexpr auto kDefaultBiasCorrelationTime = 6.0e+2;  // [s]
+
 class SensorMagPlugin : public SensorPlugin {
  public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
   SensorMagPlugin(JSBSim::FGFDMExec* jsbsim);
   ~SensorMagPlugin();
   SensorData::Magnetometer getData();
@@ -55,13 +63,10 @@ class SensorMagPlugin : public SensorPlugin {
   Eigen::Vector3d getMagFromJSBSim();
   void addNoise(Eigen::Vector3d* magnetic_field, const double dt);
 
-  std::default_random_engine _random_generator;
   std::normal_distribution<double> _standard_normal_distribution;
-  double _noise_density;
-  double _random_walk;
-  double _bias_correlation_time;
-
-  Eigen::Vector3d _bias;
+  double _noise_density{kDefaultNoiseDensity};
+  double _random_walk{kDefaultRandomWalk};
+  double _bias_correlation_time{kDefaultBiasCorrelationTime};
 
-  double _last_sim_time;
+  Eigen::Vector3d _bias{Eigen::Vector3d::Zero()};
 };
diff --git a/include/sensor_plugin.h b/include/sensor_plugin.h
@@ -55,6 +55,8 @@ class SensorPlugin {
 
  protected:
   JSBSim::FGFDMExec *_sim_ptr;
-  double _last_sim_time;
-  double _update_rate;
+  double _last_sim_time{0.};
+  double _update_rate{0.};
+
+  std::default_random_engine _random_generator;
 };
diff --git a/src/actuator_plugin.cpp b/src/actuator_plugin.cpp
@@ -41,7 +41,7 @@
 
 #include "actuator_plugin.h"
 
-ActuatorPlugin::ActuatorPlugin(JSBSim::FGFDMExec *jsbsim) : _sim_ptr(jsbsim) {}
+ActuatorPlugin::ActuatorPlugin(JSBSim::FGFDMExec *jsbsim) : _sim_ptr(jsbsim), _last_sim_time(0.0) {}
 
 ActuatorPlugin::~ActuatorPlugin() {}
 

diff --git a/src/sensor_airspeed_plugin.cpp b/src/sensor_airspeed_plugin.cpp
@@ -42,7 +42,7 @@
 #include "sensor_airspeed_plugin.h"
 
 SensorAirspeedPlugin::SensorAirspeedPlugin(JSBSim::FGFDMExec *jsbsim)
-    : SensorPlugin(jsbsim), diff_pressure_stddev_(0.01f) {}
+    : SensorPlugin(jsbsim) {}
 
 SensorAirspeedPlugin::~SensorAirspeedPlugin() {}
 

diff --git a/src/sensor_baro_plugin.cpp b/src/sensor_baro_plugin.cpp
@@ -42,7 +42,7 @@
 #include "sensor_baro_plugin.h"
 
 SensorBaroPlugin::SensorBaroPlugin(JSBSim::FGFDMExec *jsbsim)
-    : SensorPlugin(jsbsim), _baro_rnd_y2(0.0), _baro_rnd_use_last(false), _baro_drift_pa(0.0) {
+    : SensorPlugin(jsbsim) {
   _standard_normal_distribution = std::normal_distribution<double>(0.0, 1.0);
 }
 

diff --git a/src/sensor_imu_plugin.cpp b/src/sensor_imu_plugin.cpp
@@ -41,29 +41,9 @@
 
 #include "sensor_imu_plugin.h"
 
-// Default values for use with ADIS16448 IMU
-static constexpr double kDefaultAdisGyroscopeNoiseDensity = 2.0 * 35.0 / 3600.0 / 180.0 * M_PI;
-static constexpr double kDefaultAdisGyroscopeRandomWalk = 2.0 * 4.0 / 3600.0 / 180.0 * M_PI;
-static constexpr double kDefaultAdisGyroscopeBiasCorrelationTime = 1.0e+3;
-static constexpr double kDefaultAdisGyroscopeTurnOnBiasSigma = 0.5 / 180.0 * M_PI;
-static constexpr double kDefaultAdisAccelerometerNoiseDensity = 2.0 * 2.0e-3;
-static constexpr double kDefaultAdisAccelerometerRandomWalk = 2.0 * 3.0e-3;
-static constexpr double kDefaultAdisAccelerometerBiasCorrelationTime = 300.0;
-static constexpr double kDefaultAdisAccelerometerTurnOnBiasSigma = 20.0e-3 * 9.8;
-// Earth's gravity in Zurich (lat=+47.3667degN, lon=+8.5500degE, h=+500m, WGS84)
-static constexpr double kDefaultGravityMagnitude = 9.8068;
 
 SensorImuPlugin::SensorImuPlugin(JSBSim::FGFDMExec* jsbsim)
-    : SensorPlugin(jsbsim),
-      gyroscope_noise_density(kDefaultAdisGyroscopeNoiseDensity),
-      gyroscope_random_walk(kDefaultAdisGyroscopeRandomWalk),
-      gyroscope_bias_correlation_time(kDefaultAdisGyroscopeBiasCorrelationTime),
-      gyroscope_turn_on_bias_sigma(kDefaultAdisGyroscopeTurnOnBiasSigma),
-      accelerometer_noise_density(kDefaultAdisAccelerometerNoiseDensity),
-      accelerometer_random_walk(kDefaultAdisAccelerometerRandomWalk),
-      accelerometer_bias_correlation_time(kDefaultAdisAccelerometerBiasCorrelationTime),
-      accelerometer_turn_on_bias_sigma(kDefaultAdisAccelerometerTurnOnBiasSigma),
-      gravity_magnitude(kDefaultGravityMagnitude) {
+    : SensorPlugin(jsbsim) {
   _standard_normal_distribution = std::normal_distribution<double>(0.0, 1.0);
 
   double sigma_bon_g = gyroscope_turn_on_bias_sigma;

diff --git a/src/sensor_mag_plugin.cpp b/src/sensor_mag_plugin.cpp
@@ -41,18 +41,8 @@
 
 #include "sensor_mag_plugin.h"
 
-static constexpr auto kDefaultPubRate = 100.0;  // [Hz]. Note: corresponds to most of the mag devices supported in PX4
-
-// Default values for use with ADIS16448 IMU
-static constexpr auto kDefaultNoiseDensity = 0.4 * 1e-3;     // [gauss / sqrt(hz)]
-static constexpr auto kDefaultRandomWalk = 6.4 * 1e-6;       // [gauss * sqrt(hz)]
-static constexpr auto kDefaultBiasCorrelationTime = 6.0e+2;  // [s]
-
 SensorMagPlugin::SensorMagPlugin(JSBSim::FGFDMExec* jsbsim)
-    : SensorPlugin(jsbsim),
-      _noise_density(kDefaultNoiseDensity),
-      _random_walk(kDefaultRandomWalk),
-      _bias_correlation_time(kDefaultBiasCorrelationTime) {
+    : SensorPlugin(jsbsim) {
   _standard_normal_distribution = std::normal_distribution<double>(0.0, 1.0);
 }
 

diff --git a/src/sensor_plugin.cpp b/src/sensor_plugin.cpp
@@ -41,7 +41,7 @@
 
 #include "sensor_plugin.h"
 
-SensorPlugin::SensorPlugin(JSBSim::FGFDMExec *jsbsim) : _sim_ptr(jsbsim), _update_rate(0.0) {}
+SensorPlugin::SensorPlugin(JSBSim::FGFDMExec *jsbsim) : _sim_ptr(jsbsim) {}
 
 SensorPlugin::~SensorPlugin() {}