From 3c4d038c7c0c660909f37b429f4f583eec53ae93 Mon Sep 17 00:00:00 2001 From: JaeyoungLim Date: Thu, 1 Oct 2020 10:46:13 +0200 Subject: [PATCH] Fix initialization issues (#14) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 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 Co-authored-by: Beat Küng --- CMakeLists.txt | 3 ++- include/mavlink_interface.h | 1 + include/sensor_airspeed_plugin.h | 3 +-- include/sensor_baro_plugin.h | 9 +++---- include/sensor_imu_plugin.h | 40 +++++++++++++++++++++----------- include/sensor_mag_plugin.h | 19 +++++++++------ include/sensor_plugin.h | 6 +++-- src/actuator_plugin.cpp | 2 +- src/sensor_airspeed_plugin.cpp | 2 +- src/sensor_baro_plugin.cpp | 2 +- src/sensor_imu_plugin.cpp | 22 +----------------- src/sensor_mag_plugin.cpp | 12 +--------- src/sensor_plugin.cpp | 2 +- 13 files changed, 55 insertions(+), 68 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index c011d46..ac6dd24 100644 --- 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 index 4770a9f..2c35386 100644 --- 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 index 34dfb96..252391b 100644 --- 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 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 index a9fe9d4..5ff086f 100644 --- 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 _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 index acd2c8f..69897a3 100644 --- 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 _standard_normal_distribution; }; diff --git a/include/sensor_mag_plugin.h b/include/sensor_mag_plugin.h index c3ee6e0..d257822 100644 --- 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 _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 index c910fd2..95888a8 100644 --- 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 index 8cbabba..53b1594 100644 --- 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 index bcda741..965d5ae 100644 --- 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 index 1459338..9041eda 100644 --- 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(0.0, 1.0); } diff --git a/src/sensor_imu_plugin.cpp b/src/sensor_imu_plugin.cpp index bee6fb6..d2b1f97 100644 --- 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(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 index e6f857f..5123a17 100644 --- 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(0.0, 1.0); } diff --git a/src/sensor_plugin.cpp b/src/sensor_plugin.cpp index f3302ac..e7a5a09 100644 --- 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() {}