mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_AHRS: shuffle AP_AHRS classes
This commit is contained in:
Peter Barker
Peter Barker
parent
5c7d5a048f
commit
b450a96698
@ -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();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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();
|
||||
};
|
||||
|
||||
@ -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)
|
||||
|
||||
@ -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)) {
|
||||
|
||||
@ -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_NavEKF&>(*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();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -44,24 +44,34 @@
|
||||
#include <AP_NavEKF3/AP_NavEKF3.h>
|
||||
#include <AP_NavEKF/AP_Nav_Common.h> // 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();
|
||||
};
|
||||
|
||||
@ -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)
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user