From 0ccaa5bcba4833511f5a8464dc2e9509d40ea147 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 13 Dec 2017 12:06:11 +1100 Subject: [PATCH] AP_AHRS: removed create() method for objects See discussion here: https://github.com/ArduPilot/ardupilot/issues/7331 we were getting some uninitialised variables. While it only showed up in AP_SbusOut, it means we can't be sure it won't happen on other objects, so safest to remove the approach Thanks to assistance from Lucas, Peter and Francisco --- libraries/AP_AHRS/AP_AHRS_DCM.h | 69 +++++++++---------- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 17 +---- .../AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp | 22 +++--- 3 files changed, 45 insertions(+), 63 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 5ceb530a1b..67418f351b 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -23,11 +23,38 @@ class AP_AHRS_DCM : public AP_AHRS { public: - static AP_AHRS_DCM create(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) { - return AP_AHRS_DCM{ins, baro, gps}; - } + AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) + : AP_AHRS(ins, baro, gps) + , _omega_I_sum_time(0.0f) + , _renorm_val_sum(0.0f) + , _renorm_val_count(0) + , _error_rp(1.0f) + , _error_yaw(1.0f) + , _gps_last_update(0) + , _ra_deltat(0.0f) + , _ra_sum_start(0) + , _last_declination(0.0f) + , _mag_earth(1, 0) + , _have_gps_lock(false) + , _last_lat(0) + , _last_lng(0) + , _position_offset_north(0.0f) + , _position_offset_east(0.0f) + , _have_position(false) + , _last_wind_time(0) + , _last_airspeed(0.0f) + , _last_consistent_heading(0) + , _imu1_weight(0.5f) + , _last_failure_ms(0) + , _last_startup_ms(0) + { + _dcm_matrix.identity(); - constexpr AP_AHRS_DCM(AP_AHRS_DCM &&other) = default; + // these are experimentally derived from the simulator + // with large drift levels + _ki = 0.0087f; + _ki_yaw = 0.01f; + } /* Do not allow copies */ AP_AHRS_DCM(const AP_AHRS_DCM &other) = delete; @@ -93,40 +120,6 @@ public: // time that the AHRS has been up uint32_t uptime_ms() const override; -protected: - AP_AHRS_DCM(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps) - : AP_AHRS(ins, baro, gps) - , _omega_I_sum_time(0.0f) - , _renorm_val_sum(0.0f) - , _renorm_val_count(0) - , _error_rp(1.0f) - , _error_yaw(1.0f) - , _gps_last_update(0) - , _ra_deltat(0.0f) - , _ra_sum_start(0) - , _last_declination(0.0f) - , _mag_earth(1, 0) - , _have_gps_lock(false) - , _last_lat(0) - , _last_lng(0) - , _position_offset_north(0.0f) - , _position_offset_east(0.0f) - , _have_position(false) - , _last_wind_time(0) - , _last_airspeed(0.0f) - , _last_consistent_heading(0) - , _imu1_weight(0.5f) - , _last_failure_ms(0) - , _last_startup_ms(0) - { - _dcm_matrix.identity(); - - // these are experimentally derived from the simulator - // with large drift levels - _ki = 0.0087f; - _ki_yaw = 0.01f; - } - private: float _ki; float _ki_yaw; diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index ca5399e2ba..0681902aa9 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -43,15 +43,9 @@ public: FLAG_ALWAYS_USE_EKF = 0x1, }; - static AP_AHRS_NavEKF create(AP_InertialSensor &ins, - AP_Baro &baro, - AP_GPS &gps, - NavEKF2 &_EKF2, NavEKF3 &_EKF3, - Flags flags = FLAG_NONE) { - return AP_AHRS_NavEKF{ins, baro, gps, _EKF2, _EKF3, flags}; - } - - constexpr AP_AHRS_NavEKF(AP_AHRS_NavEKF &&other) = default; + // Constructor + AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, + NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE); /* Do not allow copies */ AP_AHRS_NavEKF(const AP_AHRS_NavEKF &other) = delete; @@ -302,10 +296,5 @@ private: uint32_t _last_body_odm_update_ms = 0; void update_SITL(void); #endif - -private: - // Constructor - AP_AHRS_NavEKF(AP_InertialSensor &ins, AP_Baro &baro, AP_GPS &gps, - NavEKF2 &_EKF2, NavEKF3 &_EKF3, Flags flags = FLAG_NONE); }; #endif diff --git a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp index 6cd66af3da..1b81f71266 100644 --- a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp +++ b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp @@ -14,22 +14,22 @@ void loop(); const AP_HAL::HAL& hal = AP_HAL::get_HAL(); -static AP_BoardConfig board_config = AP_BoardConfig::create(); -static AP_InertialSensor ins = AP_InertialSensor::create(); +static AP_BoardConfig board_config; +static AP_InertialSensor ins; -static Compass compass = Compass::create(); +static Compass compass; -static AP_GPS gps = AP_GPS::create(); -static AP_Baro barometer = AP_Baro::create(); -static AP_SerialManager serial_manager = AP_SerialManager::create(); +static AP_GPS gps; +static AP_Baro barometer; +static AP_SerialManager serial_manager; class DummyVehicle { public: - RangeFinder sonar = RangeFinder::create(serial_manager, ROTATION_PITCH_270); - NavEKF2 EKF2 = NavEKF2::create(&ahrs, barometer, sonar); - NavEKF3 EKF3 = NavEKF3::create(&ahrs, barometer, sonar); - AP_AHRS_NavEKF ahrs = AP_AHRS_NavEKF::create(ins, barometer, gps, EKF2, EKF3, - AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF); + RangeFinder sonar{serial_manager, ROTATION_PITCH_270}; + NavEKF2 EKF2{&ahrs, barometer, sonar}; + NavEKF3 EKF3{&ahrs, barometer, sonar}; + AP_AHRS_NavEKF ahrs{ins, barometer, gps, EKF2, EKF3, + AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; }; static DummyVehicle vehicle;