From 0a7399ae7d689badc582ad6f42790a0d75e2d11e Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 16 Jul 2018 10:29:20 +1000 Subject: [PATCH] AC_PrecLand: use AHRS singleton, remove pointless initialisations --- libraries/AC_PrecLand/AC_PrecLand.cpp | 19 +++++++------------ libraries/AC_PrecLand/AC_PrecLand.h | 5 +---- .../AC_PrecLand/AC_PrecLand_Companion.cpp | 10 ---------- libraries/AC_PrecLand/AC_PrecLand_Companion.h | 2 +- libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp | 4 +--- libraries/AC_PrecLand/AC_PrecLand_SITL.cpp | 10 ++-------- libraries/AC_PrecLand/AC_PrecLand_SITL.h | 2 +- 7 files changed, 13 insertions(+), 39 deletions(-) diff --git a/libraries/AC_PrecLand/AC_PrecLand.cpp b/libraries/AC_PrecLand/AC_PrecLand.cpp index 1e71f1cb82..652eff1262 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand.cpp @@ -97,18 +97,10 @@ const AP_Param::GroupInfo AC_PrecLand::var_info[] = { // Note that the Vector/Matrix constructors already implicitly zero // their values. // -AC_PrecLand::AC_PrecLand(const AP_AHRS_NavEKF& ahrs) : - _ahrs(ahrs), - _last_update_ms(0), - _target_acquired(false), - _last_backend_los_meas_ms(0), - _backend(nullptr) +AC_PrecLand::AC_PrecLand() { // set parameters to defaults AP_Param::setup_object_defaults(this, var_info); - - // other initialisation - _backend_state.healthy = false; } // init - perform any required initialisation of backends @@ -158,6 +150,7 @@ void AC_PrecLand::update(float rangefinder_alt_cm, bool rangefinder_alt_valid) { // append current velocity and attitude correction into history buffer struct inertial_data_frame_s inertial_data_newest; + const AP_AHRS_NavEKF &_ahrs = AP::ahrs_navekf(); _ahrs.getCorrectedDeltaVelocityNED(inertial_data_newest.correctedVehicleDeltaVelocityNED, inertial_data_newest.dt); inertial_data_newest.Tbn = _ahrs.get_rotation_body_to_ned(); Vector3f curr_vel; @@ -191,7 +184,7 @@ bool AC_PrecLand::get_target_position_cm(Vector2f& ret) return false; } Vector2f curr_pos; - if (!_ahrs.get_relative_position_NE_origin(curr_pos)) { + if (!AP::ahrs().get_relative_position_NE_origin(curr_pos)) { return false; } ret.x = (_target_pos_rel_out_NE.x + curr_pos.x) * 100.0f; // m to cm @@ -288,7 +281,7 @@ void AC_PrecLand::run_estimator(float rangefinder_alt_m, bool rangefinder_alt_va // Update if a new LOS measurement is available if (construct_pos_meas_using_rangefinder(rangefinder_alt_m, rangefinder_alt_valid)) { - float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*_ahrs.get_gyro().length()) + 0.02f); + float xy_pos_var = sq(_target_pos_rel_meas_NED.z*(0.01f + 0.01f*AP::ahrs().get_gyro().length()) + 0.02f); if (!target_acquired()) { // reset filter state if (inertial_data_delayed.inertialNavVelocityValid) { @@ -371,7 +364,7 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m, } // Compute camera position relative to IMU - Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(_ahrs.get_primary_accel_index()); + Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(AP::ahrs().get_primary_accel_index()); Vector3f cam_pos_ned = inertial_data_delayed.Tbn * (_cam_offset.get() - accel_body_offset); // Compute target position relative to IMU @@ -396,6 +389,8 @@ void AC_PrecLand::run_output_prediction() _target_pos_rel_out_NE.y += _target_vel_rel_out_NE.y * inertial_data.dt; } + const AP_AHRS &_ahrs = AP::ahrs(); + const Matrix3f& Tbn = _inertial_history.peek(_inertial_history.size()-1).Tbn; Vector3f accel_body_offset = AP::ins().get_imu_pos_offset(_ahrs.get_primary_accel_index()); diff --git a/libraries/AC_PrecLand/AC_PrecLand.h b/libraries/AC_PrecLand/AC_PrecLand.h index cc18bd20ee..ed2ccb2858 100644 --- a/libraries/AC_PrecLand/AC_PrecLand.h +++ b/libraries/AC_PrecLand/AC_PrecLand.h @@ -25,7 +25,7 @@ class AC_PrecLand friend class AC_PrecLand_SITL; public: - AC_PrecLand(const AP_AHRS_NavEKF& ahrs); + AC_PrecLand(); /* Do not allow copies */ AC_PrecLand(const AC_PrecLand &other) = delete; @@ -114,9 +114,6 @@ private: // results are stored in_target_pos_rel_out_NE, _target_vel_rel_out_NE void run_output_prediction(); - // references to inertial nav and ahrs libraries - const AP_AHRS_NavEKF& _ahrs; - // parameters AP_Int8 _enabled; // enabled/disabled and behaviour AP_Int8 _type; // precision landing sensor type diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp index 05c2122950..a0fb965230 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.cpp @@ -3,16 +3,6 @@ extern const AP_HAL::HAL& hal; -// Constructor -AC_PrecLand_Companion::AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) - : AC_PrecLand_Backend(frontend, state), - _timestamp_us(0), - _distance_to_target(0.0f), - _have_los_meas(false), - _los_meas_time_ms(0) -{ -} - // perform any required initialisation of backend void AC_PrecLand_Companion::init() { diff --git a/libraries/AC_PrecLand/AC_PrecLand_Companion.h b/libraries/AC_PrecLand/AC_PrecLand_Companion.h index 5b45e03e09..8fe755af1d 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_Companion.h +++ b/libraries/AC_PrecLand/AC_PrecLand_Companion.h @@ -13,7 +13,7 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend { public: // Constructor - AC_PrecLand_Companion(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state); + using AC_PrecLand_Backend::AC_PrecLand_Backend; // perform any required initialisation of backend void init() override; diff --git a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp index bc014a3e23..15dd93f50e 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_IRLock.cpp @@ -6,9 +6,7 @@ extern const AP_HAL::HAL& hal; // Constructor AC_PrecLand_IRLock::AC_PrecLand_IRLock(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) : AC_PrecLand_Backend(frontend, state), - irlock(), - _have_los_meas(false), - _los_meas_time_ms(0) + irlock() { } diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp index 504953d74e..82f7103333 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.cpp @@ -7,12 +7,6 @@ extern const AP_HAL::HAL& hal; #include -// Constructor -AC_PrecLand_SITL::AC_PrecLand_SITL(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state) - : AC_PrecLand_Backend(frontend, state) -{ -} - // init - perform initialisation of this backend void AC_PrecLand_SITL::init() { @@ -28,7 +22,7 @@ void AC_PrecLand_SITL::update() // get new sensor data; we always point home Vector3f home; - if (! _frontend._ahrs.get_relative_position_NED_home(home)) { + if (! AP::ahrs().get_relative_position_NED_home(home)) { _state.healthy = false; return; } @@ -37,7 +31,7 @@ void AC_PrecLand_SITL::update() } _state.healthy = true; - const Matrix3f &body_to_ned = _frontend._ahrs.get_rotation_body_to_ned(); + const Matrix3f &body_to_ned = AP::ahrs().get_rotation_body_to_ned(); _los_meas_body = body_to_ned.mul_transpose(-home); _los_meas_body /= _los_meas_body.length(); diff --git a/libraries/AC_PrecLand/AC_PrecLand_SITL.h b/libraries/AC_PrecLand/AC_PrecLand_SITL.h index a062f7127c..2b1f7abab7 100644 --- a/libraries/AC_PrecLand/AC_PrecLand_SITL.h +++ b/libraries/AC_PrecLand/AC_PrecLand_SITL.h @@ -14,7 +14,7 @@ class AC_PrecLand_SITL : public AC_PrecLand_Backend public: // Constructor - AC_PrecLand_SITL(const AC_PrecLand& frontend, AC_PrecLand::precland_state& state); + using AC_PrecLand_Backend::AC_PrecLand_Backend; // perform any required initialisation of backend void init() override;