From b450a96698431d2675bbe5c827e200fb551dfa39 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Tue, 20 Jul 2021 23:04:20 +1000 Subject: [PATCH] AP_AHRS: shuffle AP_AHRS classes --- libraries/AP_AHRS/AP_AHRS.cpp | 54 ++--- libraries/AP_AHRS/AP_AHRS.h | 37 +--- libraries/AP_AHRS/AP_AHRS_DCM.h | 6 +- libraries/AP_AHRS/AP_AHRS_Logging.cpp | 10 +- libraries/AP_AHRS/AP_AHRS_NavEKF.cpp | 189 ++++++++++-------- libraries/AP_AHRS/AP_AHRS_NavEKF.h | 34 +++- .../AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp | 4 +- 7 files changed, 166 insertions(+), 168 deletions(-) diff --git a/libraries/AP_AHRS/AP_AHRS.cpp b/libraries/AP_AHRS/AP_AHRS.cpp index f9a6fae83f..b580243e81 100644 --- a/libraries/AP_AHRS/AP_AHRS.cpp +++ b/libraries/AP_AHRS/AP_AHRS.cpp @@ -165,7 +165,7 @@ const AP_Param::GroupInfo AP_AHRS::var_info[] = { }; // init sets up INS board orientation -void AP_AHRS::init() +void AP_AHRS_Backend::init() { update_orientation(); @@ -175,14 +175,14 @@ void AP_AHRS::init() } // return a smoothed and corrected gyro vector using the latest ins data (which may not have been consumed by the EKF yet) -Vector3f AP_AHRS::get_gyro_latest(void) const +Vector3f AP_AHRS_Backend::get_gyro_latest(void) const { const uint8_t primary_gyro = get_primary_gyro_index(); return AP::ins().get_gyro(primary_gyro) + get_gyro_drift(); } // set_trim -void AP_AHRS::set_trim(const Vector3f &new_trim) +void AP_AHRS_Backend::set_trim(const Vector3f &new_trim) { Vector3f trim; trim.x = constrain_float(new_trim.x, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT)); @@ -191,7 +191,7 @@ void AP_AHRS::set_trim(const Vector3f &new_trim) } // add_trim - adjust the roll and pitch trim up to a total of 10 degrees -void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom) +void AP_AHRS_Backend::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom) { Vector3f trim = _trim.get(); @@ -209,7 +209,7 @@ void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_ } // Set the board mounting orientation, may be called while disarmed -void AP_AHRS::update_orientation() +void AP_AHRS_Backend::update_orientation() { const enum Rotation orientation = (enum Rotation)_board_orientation.get(); if (orientation != ROTATION_CUSTOM) { @@ -227,7 +227,7 @@ void AP_AHRS::update_orientation() } // return a ground speed estimate in m/s -Vector2f AP_AHRS::groundspeed_vector(void) +Vector2f AP_AHRS_Backend::groundspeed_vector(void) { // Generate estimate of ground speed vector using air data system Vector2f gndVelADS; @@ -296,7 +296,7 @@ Vector2f AP_AHRS::groundspeed_vector(void) /* calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix */ -void AP_AHRS::calc_trig(const Matrix3f &rot, +void AP_AHRS_Backend::calc_trig(const Matrix3f &rot, float &cr, float &cp, float &cy, float &sr, float &sp, float &sy) const { @@ -342,7 +342,7 @@ void AP_AHRS::calc_trig(const Matrix3f &rot, // update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude // should be called after _dcm_matrix is updated -void AP_AHRS::update_trig(void) +void AP_AHRS_Backend::update_trig(void) { if (_last_trim != _trim.get()) { _last_trim = _trim.get(); @@ -358,7 +358,7 @@ void AP_AHRS::update_trig(void) /* update the centi-degree values */ -void AP_AHRS::update_cd_values(void) +void AP_AHRS_Backend::update_cd_values(void) { roll_sensor = degrees(roll) * 100; pitch_sensor = degrees(pitch) * 100; @@ -393,7 +393,7 @@ AP_AHRS_View *AP_AHRS::create_view(enum Rotation rotation, float pitch_trim_deg) * "ANGLE OF ATTACK AND SIDESLIP ESTIMATION USING AN INERTIAL REFERENCE PLATFORM" by * JOSEPH E. ZEIS, JR., CAPTAIN, USAF */ -void AP_AHRS::update_AOA_SSA(void) +void AP_AHRS_Backend::update_AOA_SSA(void) { #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) const uint32_t now = AP_HAL::millis(); @@ -440,35 +440,35 @@ void AP_AHRS::update_AOA_SSA(void) } // return current AOA -float AP_AHRS::getAOA(void) +float AP_AHRS_Backend::getAOA(void) { update_AOA_SSA(); return _AOA; } // return calculated SSA -float AP_AHRS::getSSA(void) +float AP_AHRS_Backend::getSSA(void) { update_AOA_SSA(); return _SSA; } // rotate a 2D vector from earth frame to body frame -Vector2f AP_AHRS::earth_to_body2D(const Vector2f &ef) const +Vector2f AP_AHRS_Backend::earth_to_body2D(const Vector2f &ef) const { return Vector2f(ef.x * _cos_yaw + ef.y * _sin_yaw, -ef.x * _sin_yaw + ef.y * _cos_yaw); } // rotate a 2D vector from earth frame to body frame -Vector2f AP_AHRS::body_to_earth2D(const Vector2f &bf) const +Vector2f AP_AHRS_Backend::body_to_earth2D(const Vector2f &bf) const { return Vector2f(bf.x * _cos_yaw - bf.y * _sin_yaw, bf.x * _sin_yaw + bf.y * _cos_yaw); } // log ahrs home and EKF origin -void AP_AHRS::Log_Write_Home_And_Origin() +void AP_AHRS_Backend::Log_Write_Home_And_Origin() { AP_Logger *logger = AP_Logger::get_singleton(); if (logger == nullptr) { @@ -485,11 +485,11 @@ void AP_AHRS::Log_Write_Home_And_Origin() } // get apparent to true airspeed ratio -float AP_AHRS::get_EAS2TAS(void) const { +float AP_AHRS_Backend::get_EAS2TAS(void) const { return AP::baro().get_EAS2TAS(); } -void AP_AHRS::update_nmea_out() +void AP_AHRS_Backend::update_nmea_out() { #if !HAL_MINIMIZE_FEATURES if (_nmea_out != nullptr) { @@ -499,18 +499,18 @@ void AP_AHRS::update_nmea_out() } // return current vibration vector for primary IMU -Vector3f AP_AHRS::get_vibration(void) const +Vector3f AP_AHRS_Backend::get_vibration(void) const { return AP::ins().get_vibration_levels(); } -void AP_AHRS::set_takeoff_expected(bool b) +void AP_AHRS_Backend::set_takeoff_expected(bool b) { _flags.takeoff_expected = b; takeoff_expected_start_ms = AP_HAL::millis(); } -void AP_AHRS::set_touchdown_expected(bool b) +void AP_AHRS_Backend::set_touchdown_expected(bool b) { _flags.touchdown_expected = b; touchdown_expected_start_ms = AP_HAL::millis(); @@ -519,7 +519,7 @@ void AP_AHRS::set_touchdown_expected(bool b) /* update takeoff/touchdown flags */ -void AP_AHRS::update_flags(void) +void AP_AHRS_Backend::update_flags(void) { const uint32_t timeout_ms = 1000; if (_flags.takeoff_expected && AP_HAL::millis() - takeoff_expected_start_ms > timeout_ms) { @@ -529,15 +529,3 @@ void AP_AHRS::update_flags(void) _flags.touchdown_expected = false; } } - -// singleton instance -AP_AHRS *AP_AHRS::_singleton; - -namespace AP { - -AP_AHRS &ahrs() -{ - return *AP_AHRS::get_singleton(); -} - -} diff --git a/libraries/AP_AHRS/AP_AHRS.h b/libraries/AP_AHRS/AP_AHRS.h index ebea920427..8bcd6acdd3 100644 --- a/libraries/AP_AHRS/AP_AHRS.h +++ b/libraries/AP_AHRS/AP_AHRS.h @@ -43,26 +43,17 @@ enum AHRS_VehicleClass : uint8_t { }; -// forward declare view class -class AP_AHRS_View; - -class AP_AHRS +class AP_AHRS_Backend { public: - friend class AP_AHRS_View; // Constructor - AP_AHRS() : + AP_AHRS_Backend() : _vehicle_class(AHRS_VEHICLE_UNKNOWN), _cos_roll(1.0f), _cos_pitch(1.0f), _cos_yaw(1.0f) { - _singleton = this; - - // load default values from var_info table - AP_Param::setup_object_defaults(this, var_info); - // base the ki values by the sensors maximum drift // rate. _gyro_drift_limit = AP::ins().get_gyro_drift_rate(); @@ -76,12 +67,7 @@ public: } // empty virtual destructor - virtual ~AP_AHRS() {} - - // get singleton instance - static AP_AHRS *get_singleton() { - return _singleton; - } + virtual ~AP_AHRS_Backend() {} // init sets up INS board orientation virtual void init(); @@ -543,9 +529,6 @@ public: AP::ins().get_delta_velocity(ret, dt); } - // create a view - AP_AHRS_View *create_view(enum Rotation rotation, float pitch_trim_deg=0); - // return calculated AOA float getAOA(void); @@ -601,9 +584,6 @@ public: // active AHRS type for logging virtual uint8_t get_active_AHRS_type(void) const { return 0; } - // for holding parameters - static const struct AP_Param::GroupInfo var_info[]; - // Logging to disk functions void Write_AHRS2(void) const; void Write_AOA_SSA(void); // should be const? but it calls update functions @@ -711,15 +691,11 @@ protected: // which accelerometer instance is active uint8_t _active_accel_instance; - // optional view class - AP_AHRS_View *_view; - // AOA and SSA float _AOA, _SSA; uint32_t _last_AOA_update_ms; private: - static AP_AHRS *_singleton; AP_NMEA_Output* _nmea_out; @@ -727,11 +703,4 @@ private: uint32_t touchdown_expected_start_ms; }; -#include "AP_AHRS_DCM.h" #include "AP_AHRS_NavEKF.h" - -namespace AP { - AP_AHRS &ahrs(); - // ahrs_navekf only exists to avoid code churn - AP_AHRS_NavEKF &ahrs_navekf(); -}; diff --git a/libraries/AP_AHRS/AP_AHRS_DCM.h b/libraries/AP_AHRS/AP_AHRS_DCM.h index 84ae8addb7..577043fec6 100644 --- a/libraries/AP_AHRS/AP_AHRS_DCM.h +++ b/libraries/AP_AHRS/AP_AHRS_DCM.h @@ -21,10 +21,12 @@ * */ -class AP_AHRS_DCM : public AP_AHRS { +#include "AP_AHRS.h" + +class AP_AHRS_DCM : public AP_AHRS_Backend { public: AP_AHRS_DCM() - : AP_AHRS() + : AP_AHRS_Backend() , _error_rp(1.0f) , _error_yaw(1.0f) , _mag_earth(1, 0) diff --git a/libraries/AP_AHRS/AP_AHRS_Logging.cpp b/libraries/AP_AHRS/AP_AHRS_Logging.cpp index 532a02699d..89b205f77a 100644 --- a/libraries/AP_AHRS/AP_AHRS_Logging.cpp +++ b/libraries/AP_AHRS/AP_AHRS_Logging.cpp @@ -6,7 +6,7 @@ // Write an AHRS2 packet -void AP_AHRS::Write_AHRS2() const +void AP_AHRS_Backend::Write_AHRS2() const { Vector3f euler; struct Location loc; @@ -32,7 +32,7 @@ void AP_AHRS::Write_AHRS2() const } // Write AOA and SSA -void AP_AHRS::Write_AOA_SSA(void) +void AP_AHRS_Backend::Write_AOA_SSA(void) { const struct log_AOA_SSA aoa_ssa{ LOG_PACKET_HEADER_INIT(LOG_AOA_SSA_MSG), @@ -45,7 +45,7 @@ void AP_AHRS::Write_AOA_SSA(void) } // Write an attitude packet -void AP_AHRS::Write_Attitude(const Vector3f &targets) const +void AP_AHRS_Backend::Write_Attitude(const Vector3f &targets) const { const struct log_Attitude pkt{ LOG_PACKET_HEADER_INIT(LOG_ATTITUDE_MSG), @@ -63,7 +63,7 @@ void AP_AHRS::Write_Attitude(const Vector3f &targets) const AP::logger().WriteBlock(&pkt, sizeof(pkt)); } -void AP_AHRS::Write_Origin(uint8_t origin_type, const Location &loc) const +void AP_AHRS_Backend::Write_Origin(uint8_t origin_type, const Location &loc) const { const struct log_ORGN pkt{ LOG_PACKET_HEADER_INIT(LOG_ORGN_MSG), @@ -77,7 +77,7 @@ void AP_AHRS::Write_Origin(uint8_t origin_type, const Location &loc) const } // Write a POS packet -void AP_AHRS::Write_POS() const +void AP_AHRS_Backend::Write_POS() const { Location loc; if (!get_position(loc)) { diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp index 9e2b43279d..54628459e0 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.cpp @@ -37,19 +37,24 @@ extern const AP_HAL::HAL& hal; // constructor -AP_AHRS_NavEKF::AP_AHRS_NavEKF(uint8_t flags) : +AP_AHRS::AP_AHRS(uint8_t flags) : AP_AHRS_DCM(), _ekf_flags(flags) { + _singleton = this; + + // load default values from var_info table + AP_Param::setup_object_defaults(this, var_info); + #if APM_BUILD_TYPE(APM_BUILD_ArduCopter) || APM_BUILD_TYPE(APM_BUILD_ArduSub) // Copter and Sub force the use of EKF - _ekf_flags |= AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF; + _ekf_flags |= AP_AHRS::FLAG_ALWAYS_USE_EKF; #endif _dcm_matrix.identity(); } // init sets up INS board orientation -void AP_AHRS_NavEKF::init() +void AP_AHRS::init() { // EKF1 is no longer supported - handle case where it is selected if (_ekf_type.get() == 1) { @@ -74,7 +79,7 @@ void AP_AHRS_NavEKF::init() } // return the smoothed gyro vector corrected for drift -const Vector3f &AP_AHRS_NavEKF::get_gyro(void) const +const Vector3f &AP_AHRS::get_gyro(void) const { if (active_EKF_type() == EKFType::NONE) { return AP_AHRS_DCM::get_gyro(); @@ -82,7 +87,7 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro(void) const return _gyro_estimate; } -const Matrix3f &AP_AHRS_NavEKF::get_rotation_body_to_ned(void) const +const Matrix3f &AP_AHRS::get_rotation_body_to_ned(void) const { if (active_EKF_type() == EKFType::NONE) { return AP_AHRS_DCM::get_rotation_body_to_ned(); @@ -90,7 +95,7 @@ const Matrix3f &AP_AHRS_NavEKF::get_rotation_body_to_ned(void) const return _dcm_matrix; } -const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const +const Vector3f &AP_AHRS::get_gyro_drift(void) const { if (active_EKF_type() == EKFType::NONE) { return AP_AHRS_DCM::get_gyro_drift(); @@ -100,7 +105,7 @@ const Vector3f &AP_AHRS_NavEKF::get_gyro_drift(void) const // reset the current gyro drift estimate // should be called if gyro offsets are recalculated -void AP_AHRS_NavEKF::reset_gyro_drift(void) +void AP_AHRS::reset_gyro_drift(void) { // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); @@ -117,7 +122,7 @@ void AP_AHRS_NavEKF::reset_gyro_drift(void) #endif } -void AP_AHRS_NavEKF::update(bool skip_ins_update) +void AP_AHRS::update(bool skip_ins_update) { // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); @@ -209,7 +214,7 @@ void AP_AHRS_NavEKF::update(bool skip_ins_update) } } -void AP_AHRS_NavEKF::update_DCM(bool skip_ins_update) +void AP_AHRS::update_DCM(bool skip_ins_update) { // we need to restore the old DCM attitude values as these are // used internally in DCM to calculate error values for gyro drift @@ -226,7 +231,7 @@ void AP_AHRS_NavEKF::update_DCM(bool skip_ins_update) } #if HAL_NAVEKF2_AVAILABLE -void AP_AHRS_NavEKF::update_EKF2(void) +void AP_AHRS::update_EKF2(void) { if (!_ekf2_started) { // wait 1 second for DCM to output a valid tilt error estimate @@ -307,7 +312,7 @@ void AP_AHRS_NavEKF::update_EKF2(void) #endif #if HAL_NAVEKF3_AVAILABLE -void AP_AHRS_NavEKF::update_EKF3(void) +void AP_AHRS::update_EKF3(void) { if (!_ekf3_started) { // wait 1 second for DCM to output a valid tilt error estimate @@ -389,7 +394,7 @@ void AP_AHRS_NavEKF::update_EKF3(void) #endif #if CONFIG_HAL_BOARD == HAL_BOARD_SITL -void AP_AHRS_NavEKF::update_SITL(void) +void AP_AHRS::update_SITL(void) { if (_sitl == nullptr) { _sitl = AP::sitl(); @@ -449,7 +454,7 @@ void AP_AHRS_NavEKF::update_SITL(void) #endif // CONFIG_HAL_BOARD #if HAL_EXTERNAL_AHRS_ENABLED -void AP_AHRS_NavEKF::update_external(void) +void AP_AHRS::update_external(void) { AP::externalAHRS().update(); @@ -479,7 +484,7 @@ void AP_AHRS_NavEKF::update_external(void) #endif // HAL_EXTERNAL_AHRS_ENABLED // accelerometer values in the earth frame in m/s/s -const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const +const Vector3f &AP_AHRS::get_accel_ef(uint8_t i) const { if (active_EKF_type() == EKFType::NONE) { return AP_AHRS_DCM::get_accel_ef(i); @@ -488,7 +493,7 @@ const Vector3f &AP_AHRS_NavEKF::get_accel_ef(uint8_t i) const } // blended accelerometer values in the earth frame in m/s/s -const Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const +const Vector3f &AP_AHRS::get_accel_ef_blended(void) const { if (active_EKF_type() == EKFType::NONE) { return AP_AHRS_DCM::get_accel_ef_blended(); @@ -496,7 +501,7 @@ const Vector3f &AP_AHRS_NavEKF::get_accel_ef_blended(void) const return _accel_ef_ekf_blended; } -void AP_AHRS_NavEKF::reset(bool recover_eulers) +void AP_AHRS::reset(bool recover_eulers) { // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); @@ -516,7 +521,7 @@ void AP_AHRS_NavEKF::reset(bool recover_eulers) } // dead-reckoning support -bool AP_AHRS_NavEKF::get_position(struct Location &loc) const +bool AP_AHRS::get_position(struct Location &loc) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -567,18 +572,18 @@ bool AP_AHRS_NavEKF::get_position(struct Location &loc) const } // status reporting of estimated errors -float AP_AHRS_NavEKF::get_error_rp(void) const +float AP_AHRS::get_error_rp(void) const { return AP_AHRS_DCM::get_error_rp(); } -float AP_AHRS_NavEKF::get_error_yaw(void) const +float AP_AHRS::get_error_yaw(void) const { return AP_AHRS_DCM::get_error_yaw(); } // return a wind estimation vector, in m/s -Vector3f AP_AHRS_NavEKF::wind_estimate(void) const +Vector3f AP_AHRS::wind_estimate(void) const { Vector3f wind; switch (active_EKF_type()) { @@ -616,7 +621,7 @@ Vector3f AP_AHRS_NavEKF::wind_estimate(void) const // return an airspeed estimate if available. return true // if we have an estimate -bool AP_AHRS_NavEKF::airspeed_estimate(float &airspeed_ret) const +bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const { bool ret = false; if (airspeed_sensor_enabled()) { @@ -688,7 +693,7 @@ bool AP_AHRS_NavEKF::airspeed_estimate(float &airspeed_ret) const // return estimate of true airspeed vector in body frame in m/s // returns false if estimate is unavailable -bool AP_AHRS_NavEKF::airspeed_vector_true(Vector3f &vec) const +bool AP_AHRS::airspeed_vector_true(Vector3f &vec) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -717,7 +722,7 @@ bool AP_AHRS_NavEKF::airspeed_vector_true(Vector3f &vec) const } // true if compass is being used -bool AP_AHRS_NavEKF::use_compass(void) +bool AP_AHRS::use_compass(void) { switch (active_EKF_type()) { case EKFType::NONE: @@ -747,7 +752,7 @@ bool AP_AHRS_NavEKF::use_compass(void) } // return the quaternion defining the rotation from NED to XYZ (body) axes -bool AP_AHRS_NavEKF::get_quaternion(Quaternion &quat) const +bool AP_AHRS::get_quaternion(Quaternion &quat) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -782,7 +787,7 @@ bool AP_AHRS_NavEKF::get_quaternion(Quaternion &quat) const } // return secondary attitude solution if available, as eulers in radians -bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const +bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const { EKFType secondary_ekf_type; if (!get_secondary_EKF_type(secondary_ekf_type)) { @@ -835,7 +840,7 @@ bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers) const // return secondary attitude solution if available, as quaternion -bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const +bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const { EKFType secondary_ekf_type; if (!get_secondary_EKF_type(secondary_ekf_type)) { @@ -881,7 +886,7 @@ bool AP_AHRS_NavEKF::get_secondary_quaternion(Quaternion &quat) const } // return secondary position solution if available -bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) const +bool AP_AHRS::get_secondary_position(struct Location &loc) const { EKFType secondary_ekf_type; if (!get_secondary_EKF_type(secondary_ekf_type)) { @@ -927,7 +932,7 @@ bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc) const } // EKF has a better ground speed vector estimate -Vector2f AP_AHRS_NavEKF::groundspeed_vector(void) +Vector2f AP_AHRS::groundspeed_vector(void) { Vector3f vec; @@ -968,7 +973,7 @@ Vector2f AP_AHRS_NavEKF::groundspeed_vector(void) // set the EKF's origin location in 10e7 degrees. This should only // be called when the EKF has no absolute position reference (i.e. GPS) // from which to decide the origin on its own -bool AP_AHRS_NavEKF::set_origin(const Location &loc) +bool AP_AHRS::set_origin(const Location &loc) { WITH_SEMAPHORE(_rsem); #if HAL_NAVEKF2_AVAILABLE @@ -1010,14 +1015,14 @@ bool AP_AHRS_NavEKF::set_origin(const Location &loc) } // return true if inertial navigation is active -bool AP_AHRS_NavEKF::have_inertial_nav(void) const +bool AP_AHRS::have_inertial_nav(void) const { return active_EKF_type() != EKFType::NONE; } // return a ground velocity in meters/second, North/East/Down // order. Must only be called if have_inertial_nav() is true -bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const +bool AP_AHRS::get_velocity_NED(Vector3f &vec) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -1054,7 +1059,7 @@ bool AP_AHRS_NavEKF::get_velocity_NED(Vector3f &vec) const } // returns the expected NED magnetic field -bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const +bool AP_AHRS::get_mag_field_NED(Vector3f &vec) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -1085,7 +1090,7 @@ bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const } // returns the estimated magnetic field offsets in body frame -bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const +bool AP_AHRS::get_mag_field_correction(Vector3f &vec) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -1118,7 +1123,7 @@ bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const // Get a derivative of the vertical position which is kinematically consistent with the vertical position is required by some control loops. // This is different to the vertical velocity from the EKF which is not always consistent with the verical position due to the various errors that are being corrected for. -bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) const +bool AP_AHRS::get_vert_pos_rate(float &velocity) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -1156,7 +1161,7 @@ bool AP_AHRS_NavEKF::get_vert_pos_rate(float &velocity) const } // get latest height above ground level estimate in metres and a validity flag -bool AP_AHRS_NavEKF::get_hagl(float &height) const +bool AP_AHRS::get_hagl(float &height) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -1194,7 +1199,7 @@ bool AP_AHRS_NavEKF::get_hagl(float &height) const // return a relative ground position to the origin in meters // North/East/Down order. -bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const +bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -1268,7 +1273,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_origin(Vector3f &vec) const // return a relative ground position to the home in meters // North/East/Down order. -bool AP_AHRS_NavEKF::get_relative_position_NED_home(Vector3f &vec) const +bool AP_AHRS::get_relative_position_NED_home(Vector3f &vec) const { Location originLLH; Vector3f originNED; @@ -1287,7 +1292,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NED_home(Vector3f &vec) const // write a relative ground position estimate to the origin in meters, North/East order // return true if estimate is valid -bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const +bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -1336,7 +1341,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const // return a relative ground position to the home in meters // North/East order. -bool AP_AHRS_NavEKF::get_relative_position_NE_home(Vector2f &posNE) const +bool AP_AHRS::get_relative_position_NE_home(Vector2f &posNE) const { Location originLLH; Vector2f originNE; @@ -1357,7 +1362,7 @@ bool AP_AHRS_NavEKF::get_relative_position_NE_home(Vector2f &posNE) const // write a relative ground position to the origin in meters, Down // return true if the estimate is valid -bool AP_AHRS_NavEKF::get_relative_position_D_origin(float &posD) const +bool AP_AHRS::get_relative_position_D_origin(float &posD) const { switch (active_EKF_type()) { case EKFType::NONE: @@ -1409,7 +1414,7 @@ bool AP_AHRS_NavEKF::get_relative_position_D_origin(float &posD) const // write a relative ground position to home in meters, Down // will use the barometer if the EKF isn't available -void AP_AHRS_NavEKF::get_relative_position_D_home(float &posD) const +void AP_AHRS::get_relative_position_D_home(float &posD) const { Location originLLH; float originD; @@ -1426,7 +1431,7 @@ void AP_AHRS_NavEKF::get_relative_position_D_home(float &posD) const canonicalise _ekf_type, forcing it to be 0, 2 or 3 type 1 has been deprecated */ -AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::ekf_type(void) const +AP_AHRS::EKFType AP_AHRS::ekf_type(void) const { EKFType type = (EKFType)_ekf_type.get(); switch (type) { @@ -1468,7 +1473,7 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::ekf_type(void) const #endif } -AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const +AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const { EKFType ret = EKFType::NONE; @@ -1605,7 +1610,7 @@ AP_AHRS_NavEKF::EKFType AP_AHRS_NavEKF::active_EKF_type(void) const } // get secondary EKF type. returns false if no secondary (i.e. only using DCM) -bool AP_AHRS_NavEKF::get_secondary_EKF_type(EKFType &secondary_ekf_type) const +bool AP_AHRS::get_secondary_EKF_type(EKFType &secondary_ekf_type) const { switch (active_EKF_type()) { @@ -1655,7 +1660,7 @@ bool AP_AHRS_NavEKF::get_secondary_EKF_type(EKFType &secondary_ekf_type) const /* check if the AHRS subsystem is healthy */ -bool AP_AHRS_NavEKF::healthy(void) const +bool AP_AHRS::healthy(void) const { // If EKF is started we switch away if it reports unhealthy. This could be due to bad // sensor data. If EKF reversion is inhibited, we only switch across if the EKF encounters @@ -1713,7 +1718,7 @@ bool AP_AHRS_NavEKF::healthy(void) const // returns false if we fail arming checks, in which case the buffer will be populated with a failure message // requires_position should be true if horizontal position configuration should be checked -bool AP_AHRS_NavEKF::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const +bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const { bool ret = true; if (!healthy()) { @@ -1760,7 +1765,7 @@ bool AP_AHRS_NavEKF::pre_arm_check(bool requires_position, char *failure_msg, ui } // true if the AHRS has completed initialisation -bool AP_AHRS_NavEKF::initialised(void) const +bool AP_AHRS::initialised(void) const { switch (ekf_type()) { case EKFType::NONE: @@ -1791,7 +1796,7 @@ bool AP_AHRS_NavEKF::initialised(void) const }; // get_filter_status : returns filter status as a series of flags -bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const +bool AP_AHRS::get_filter_status(nav_filter_status &status) const { switch (ekf_type()) { case EKFType::NONE: @@ -1834,7 +1839,7 @@ bool AP_AHRS_NavEKF::get_filter_status(nav_filter_status &status) const } // write optical flow data to EKF -void AP_AHRS_NavEKF::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset) +void AP_AHRS::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset) { #if HAL_NAVEKF2_AVAILABLE EKF2.writeOptFlowMeas(rawFlowQuality, rawFlowRates, rawGyroRates, msecFlowMeas, posOffset); @@ -1845,7 +1850,7 @@ void AP_AHRS_NavEKF::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vecto } // write body frame odometry measurements to the EKF -void AP_AHRS_NavEKF::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset) +void AP_AHRS::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset) { #if HAL_NAVEKF3_AVAILABLE EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, delay_ms, posOffset); @@ -1853,7 +1858,7 @@ void AP_AHRS_NavEKF::writeBodyFrameOdom(float quality, const Vector3f &delPos, } // Write position and quaternion data from an external navigation system -void AP_AHRS_NavEKF::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms) +void AP_AHRS::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms) { #if HAL_NAVEKF2_AVAILABLE EKF2.writeExtNavData(pos, quat, posErr, angErr, timeStamp_ms, delay_ms, resetTime_ms); @@ -1864,7 +1869,7 @@ void AP_AHRS_NavEKF::writeExtNavData(const Vector3f &pos, const Quaternion &quat } // Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available. -void AP_AHRS_NavEKF::writeDefaultAirSpeed(float airspeed, float uncertainty) +void AP_AHRS::writeDefaultAirSpeed(float airspeed, float uncertainty) { #if HAL_NAVEKF2_AVAILABLE EKF2.writeDefaultAirSpeed(airspeed); @@ -1875,7 +1880,7 @@ void AP_AHRS_NavEKF::writeDefaultAirSpeed(float airspeed, float uncertainty) } // Write velocity data from an external navigation system -void AP_AHRS_NavEKF::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) +void AP_AHRS::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) { #if HAL_NAVEKF2_AVAILABLE EKF2.writeExtNavVelData(vel, err, timeStamp_ms, delay_ms); @@ -1886,7 +1891,7 @@ void AP_AHRS_NavEKF::writeExtNavVelData(const Vector3f &vel, float err, uint32_t } // get speed limit -void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const +void AP_AHRS::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVelGainScaler) const { switch (ekf_type()) { case EKFType::NONE: @@ -1921,7 +1926,7 @@ void AP_AHRS_NavEKF::getEkfControlLimits(float &ekfGndSpdLimit, float &ekfNavVel // get compass offset estimates // true if offsets are valid -bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const +bool AP_AHRS::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const { switch (ekf_type()) { case EKFType::NONE: @@ -1952,7 +1957,7 @@ bool AP_AHRS_NavEKF::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const } // Retrieves the NED delta velocity corrected -void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const +void AP_AHRS::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const { int8_t imu_idx = -1; Vector3f accel_bias; @@ -1992,7 +1997,7 @@ void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) cons } // check all cores providing consistent attitudes for prearm checks -bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const +bool AP_AHRS::attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const { // get primary attitude source's attitude as quaternion Quaternion primary_quat; @@ -2085,7 +2090,7 @@ bool AP_AHRS_NavEKF::attitudes_consistent(char *failure_msg, const uint8_t failu // return the amount of yaw angle change due to the last yaw angle reset in radians // returns the time of the last yaw angle reset or 0 if no reset has ever occurred -uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng) +uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng) { switch (ekf_type()) { @@ -2116,7 +2121,7 @@ uint32_t AP_AHRS_NavEKF::getLastYawResetAngle(float &yawAng) // return the amount of NE position change in metres due to the last reset // returns the time of the last reset or 0 if no reset has ever occurred -uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos) +uint32_t AP_AHRS::getLastPosNorthEastReset(Vector2f &pos) { switch (ekf_type()) { @@ -2147,7 +2152,7 @@ uint32_t AP_AHRS_NavEKF::getLastPosNorthEastReset(Vector2f &pos) // return the amount of NE velocity change in metres/sec due to the last reset // returns the time of the last reset or 0 if no reset has ever occurred -uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const +uint32_t AP_AHRS::getLastVelNorthEastReset(Vector2f &vel) const { switch (ekf_type()) { @@ -2179,7 +2184,7 @@ uint32_t AP_AHRS_NavEKF::getLastVelNorthEastReset(Vector2f &vel) const // return the amount of vertical position change due to the last reset in meters // returns the time of the last reset or 0 if no reset has ever occurred -uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta) +uint32_t AP_AHRS::getLastPosDownReset(float &posDelta) { switch (ekf_type()) { case EKFType::NONE: @@ -2212,7 +2217,7 @@ uint32_t AP_AHRS_NavEKF::getLastPosDownReset(float &posDelta) // Adjusts the EKf origin height so that the EKF height + origin height is the same as before // Returns true if the height datum reset has been performed // If using a range finder for height no reset is performed and it returns false -bool AP_AHRS_NavEKF::resetHeightDatum(void) +bool AP_AHRS::resetHeightDatum(void) { // support locked access functions to AHRS data WITH_SEMAPHORE(_rsem); @@ -2257,7 +2262,7 @@ bool AP_AHRS_NavEKF::resetHeightDatum(void) } // send a EKF_STATUS_REPORT for current EKF -void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) const +void AP_AHRS::send_ekf_status_report(mavlink_channel_t chan) const { switch (ekf_type()) { case EKFType::NONE: @@ -2308,7 +2313,7 @@ void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan) const // passes a reference to the location of the inertial navigation origin // in WGS-84 coordinates // returns a boolean true when the inertial navigation origin has been set -bool AP_AHRS_NavEKF::get_origin(Location &ret) const +bool AP_AHRS::get_origin(Location &ret) const { switch (ekf_type()) { case EKFType::NONE: @@ -2352,7 +2357,7 @@ bool AP_AHRS_NavEKF::get_origin(Location &ret) const // get_hgt_ctrl_limit - get maximum height to be observed by the control loops in metres and a validity flag // this is used to limit height during optical flow navigation // it will return false when no limiting is required -bool AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const +bool AP_AHRS::get_hgt_ctrl_limit(float& limit) const { switch (ekf_type()) { case EKFType::NONE: @@ -2384,7 +2389,7 @@ bool AP_AHRS_NavEKF::get_hgt_ctrl_limit(float& limit) const // Set to true if the terrain underneath is stable enough to be used as a height reference // this is not related to terrain following -void AP_AHRS_NavEKF::set_terrain_hgt_stable(bool stable) +void AP_AHRS::set_terrain_hgt_stable(bool stable) { // avoid repeatedly setting variable in NavEKF objects to prevent // spurious event logging @@ -2414,7 +2419,7 @@ void AP_AHRS_NavEKF::set_terrain_hgt_stable(bool stable) // get_location - updates the provided location with the latest calculated location // returns true on success (i.e. the EKF knows it's latest position), false on failure -bool AP_AHRS_NavEKF::get_location(struct Location &loc) const +bool AP_AHRS::get_location(struct Location &loc) const { switch (ekf_type()) { case EKFType::NONE: @@ -2446,7 +2451,7 @@ bool AP_AHRS_NavEKF::get_location(struct Location &loc) const // return the innovations for the primariy EKF // boolean false is returned if innovations are not available -bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const +bool AP_AHRS::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const { switch (ekf_type()) { case EKFType::NONE: @@ -2488,7 +2493,7 @@ bool AP_AHRS_NavEKF::get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vec // indicates prefect consistency between the measurement and the EKF solution and a value of of 1 is the maximum // inconsistency that will be accpeted by the filter // boolean false is returned if variances are not available -bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const +bool AP_AHRS::get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const { switch (ekf_type()) { case EKFType::NONE: @@ -2532,7 +2537,7 @@ bool AP_AHRS_NavEKF::get_variances(float &velVar, float &posVar, float &hgtVar, // get a source's velocity innovations. source should be from 0 to 7 (see AP_NavEKF_Source::SourceXY) // returns true on success and results are placed in innovations and variances arguments -bool AP_AHRS_NavEKF::get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const +bool AP_AHRS::get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const { switch (ekf_type()) { case EKFType::NONE: @@ -2566,7 +2571,7 @@ bool AP_AHRS_NavEKF::get_vel_innovations_and_variances_for_source(uint8_t source return false; } -bool AP_AHRS_NavEKF::getGpsGlitchStatus() const +bool AP_AHRS::getGpsGlitchStatus() const { nav_filter_status ekf_status {}; if (!get_filter_status(ekf_status)) { @@ -2576,7 +2581,7 @@ bool AP_AHRS_NavEKF::getGpsGlitchStatus() const } //get the index of the active airspeed sensor, wrt the primary core -uint8_t AP_AHRS_NavEKF::get_active_airspeed_index() const +uint8_t AP_AHRS::get_active_airspeed_index() const { // we only have affinity for EKF3 as of now #if HAL_NAVEKF3_AVAILABLE @@ -2593,7 +2598,7 @@ uint8_t AP_AHRS_NavEKF::get_active_airspeed_index() const } // get the index of the current primary IMU -uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const +uint8_t AP_AHRS::get_primary_IMU_index() const { int8_t imu = -1; switch (ekf_type()) { @@ -2627,13 +2632,13 @@ uint8_t AP_AHRS_NavEKF::get_primary_IMU_index() const } // get earth-frame accel vector for primary IMU -const Vector3f &AP_AHRS_NavEKF::get_accel_ef() const +const Vector3f &AP_AHRS::get_accel_ef() const { return get_accel_ef(get_primary_accel_index()); } // return the index of the primary core or -1 if no primary core selected -int8_t AP_AHRS_NavEKF::get_primary_core_index() const +int8_t AP_AHRS::get_primary_core_index() const { switch (active_EKF_type()) { case EKFType::NONE: @@ -2663,7 +2668,7 @@ int8_t AP_AHRS_NavEKF::get_primary_core_index() const } // get the index of the current primary accelerometer sensor -uint8_t AP_AHRS_NavEKF::get_primary_accel_index(void) const +uint8_t AP_AHRS::get_primary_accel_index(void) const { if (ekf_type() != EKFType::NONE) { return get_primary_IMU_index(); @@ -2672,7 +2677,7 @@ uint8_t AP_AHRS_NavEKF::get_primary_accel_index(void) const } // get the index of the current primary gyro sensor -uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const +uint8_t AP_AHRS::get_primary_gyro_index(void) const { if (ekf_type() != EKFType::NONE) { return get_primary_IMU_index(); @@ -2681,7 +2686,7 @@ uint8_t AP_AHRS_NavEKF::get_primary_gyro_index(void) const } // see if EKF lane switching is possible to avoid EKF failsafe -void AP_AHRS_NavEKF::check_lane_switch(void) +void AP_AHRS::check_lane_switch(void) { switch (active_EKF_type()) { case EKFType::NONE: @@ -2712,7 +2717,7 @@ void AP_AHRS_NavEKF::check_lane_switch(void) } // request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe -void AP_AHRS_NavEKF::request_yaw_reset(void) +void AP_AHRS::request_yaw_reset(void) { switch (active_EKF_type()) { case EKFType::NONE: @@ -2743,14 +2748,14 @@ void AP_AHRS_NavEKF::request_yaw_reset(void) } // set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary -void AP_AHRS_NavEKF::set_posvelyaw_source_set(uint8_t source_set_idx) +void AP_AHRS::set_posvelyaw_source_set(uint8_t source_set_idx) { #if HAL_NAVEKF3_AVAILABLE EKF3.setPosVelYawSourceSet(source_set_idx); #endif } -void AP_AHRS_NavEKF::Log_Write() +void AP_AHRS::Log_Write() { #if HAL_NAVEKF2_AVAILABLE EKF2.Log_Write(); @@ -2760,13 +2765,8 @@ void AP_AHRS_NavEKF::Log_Write() #endif } -AP_AHRS_NavEKF &AP::ahrs_navekf() -{ - return static_cast(*AP_AHRS::get_singleton()); -} - // check whether compass can be bypassed for arming check in case when external navigation data is available -bool AP_AHRS_NavEKF::is_ext_nav_used_for_yaw(void) const +bool AP_AHRS::is_ext_nav_used_for_yaw(void) const { switch (active_EKF_type()) { #if HAL_NAVEKF2_AVAILABLE @@ -2791,7 +2791,7 @@ bool AP_AHRS_NavEKF::is_ext_nav_used_for_yaw(void) const } // set and save the alt noise parameter value -void AP_AHRS_NavEKF::set_alt_measurement_noise(float noise) +void AP_AHRS::set_alt_measurement_noise(float noise) { #if HAL_NAVEKF2_AVAILABLE EKF2.set_baro_alt_noise(noise); @@ -2800,3 +2800,16 @@ void AP_AHRS_NavEKF::set_alt_measurement_noise(float noise) EKF3.set_baro_alt_noise(noise); #endif } + + +// singleton instance +AP_AHRS *AP_AHRS::_singleton; + +namespace AP { + +AP_AHRS &ahrs() +{ + return *AP_AHRS::get_singleton(); +} + +} diff --git a/libraries/AP_AHRS/AP_AHRS_NavEKF.h b/libraries/AP_AHRS/AP_AHRS_NavEKF.h index 0160777846..1824c30cde 100644 --- a/libraries/AP_AHRS/AP_AHRS_NavEKF.h +++ b/libraries/AP_AHRS/AP_AHRS_NavEKF.h @@ -44,24 +44,34 @@ #include #include // definitions shared by inertial and ekf nav filters +#include "AP_AHRS_DCM.h" + +// forward declare view class +class AP_AHRS_View; #define AP_AHRS_NAVEKF_SETTLE_TIME_MS 20000 // time in milliseconds the ekf needs to settle after being started -class AP_AHRS_NavEKF : public AP_AHRS_DCM { +class AP_AHRS : public AP_AHRS_DCM { + friend class AP_AHRS_View; public: + enum Flags { FLAG_ALWAYS_USE_EKF = 0x1, }; // Constructor - AP_AHRS_NavEKF(uint8_t flags = 0); + AP_AHRS(uint8_t flags = 0); // initialise void init(void) override; /* Do not allow copies */ - AP_AHRS_NavEKF(const AP_AHRS_NavEKF &other) = delete; - AP_AHRS_NavEKF &operator=(const AP_AHRS_NavEKF&) = delete; + CLASS_NO_COPY(AP_AHRS); + + // get singleton instance + static AP_AHRS *get_singleton() { + return _singleton; + } // return the smoothed gyro vector corrected for drift const Vector3f &get_gyro(void) const override; @@ -303,7 +313,19 @@ public: NavEKF3 EKF3; #endif + // for holding parameters + static const struct AP_Param::GroupInfo var_info[]; + + // create a view + AP_AHRS_View *create_view(enum Rotation rotation, float pitch_trim_deg=0); + +protected: + // optional view class + AP_AHRS_View *_view; + private: + static AP_AHRS *_singleton; + enum class EKFType { NONE = 0 #if HAL_NAVEKF3_AVAILABLE @@ -377,3 +399,7 @@ private: void update_external(void); #endif }; + +namespace AP { + AP_AHRS &ahrs(); +}; diff --git a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp index 4f4f6ae747..521d6a7e29 100644 --- a/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp +++ b/libraries/AP_AHRS/examples/AHRS_Test/AHRS_Test.cpp @@ -25,7 +25,7 @@ static AP_Logger logger{logger_bitmask}; class DummyVehicle : public AP_Vehicle { public: - AP_AHRS_NavEKF ahrs{AP_AHRS_NavEKF::FLAG_ALWAYS_USE_EKF}; + AP_AHRS ahrs{AP_AHRS::FLAG_ALWAYS_USE_EKF}; bool set_mode(const uint8_t new_mode, const ModeReason reason) override { return true; }; uint8_t get_mode() const override { return 1; }; void get_scheduler_tasks(const AP_Scheduler::Task *&tasks, uint8_t &task_count, uint32_t &log_bit) override {}; @@ -43,7 +43,7 @@ static DummyVehicle vehicle; // choose which AHRS system to use // AP_AHRS_DCM ahrs = AP_AHRS_DCM::create(barometer, gps); -AP_AHRS_NavEKF &ahrs = vehicle.ahrs; +auto &ahrs = vehicle.ahrs; void setup(void) {