aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_AHRS: make AP_AHRS_DCM an AP_AHRS backend

Browse Source
This commit is contained in:
Peter Barker 2021-08-18 20:20:46 +10:00 committed by Andrew Tridgell
parent 2b402ed2d7
commit 44d56854be
7 changed files with 541 additions and 386 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

198
libraries/AP_AHRS/AP_AHRS.cpp
View File

@ -179,7 +179,6 @@ extern const AP_HAL::HAL& hal;
// constructor
AP_AHRS::AP_AHRS(uint8_t flags) :
AP_AHRS_DCM(),
_ekf_flags(flags)
{
_singleton = this;
@ -220,8 +219,8 @@ void AP_AHRS::init()
last_active_ekf_type = (EKFType)_ekf_type.get();
// init parent class
AP_AHRS_DCM::init();
// init backends
dcm.init();
#if HAL_NMEA_OUTPUT_ENABLED
_nmea_out = AP_NMEA_Output::probe();
@ -245,25 +244,16 @@ void AP_AHRS::update_trim_rotation_matrices()
// return the smoothed gyro vector corrected for drift
const Vector3f &AP_AHRS::get_gyro(void) const
{
if (active_EKF_type() == EKFType::NONE) {
return AP_AHRS_DCM::get_gyro();
}
return _gyro_estimate;
}
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();
}
return _dcm_matrix;
}
const Vector3f &AP_AHRS::get_gyro_drift(void) const
{
if (active_EKF_type() == EKFType::NONE) {
return AP_AHRS_DCM::get_gyro_drift();
}
return _gyro_drift;
}
@ -275,7 +265,7 @@ void AP_AHRS::reset_gyro_drift(void)
WITH_SEMAPHORE(_rsem);
// update DCM
AP_AHRS_DCM::reset_gyro_drift();
dcm.reset_gyro_drift();
// reset the EKF gyro bias states
#if HAL_NAVEKF2_AVAILABLE
@ -400,20 +390,45 @@ void AP_AHRS::update(bool skip_ins_update)
}
}
/*
* copy results from a backend over AP_AHRS canonical results.
* This updates member variables like roll and pitch, as well as
* updating derived values like sin_roll and sin_pitch.
*/
void AP_AHRS::copy_estimates_from_backend_estimates(const AP_AHRS_Backend::Estimates &results)
{
roll = results.roll_rad;
pitch = results.pitch_rad;
yaw = results.yaw_rad;
_dcm_matrix = results.dcm_matrix;
_gyro_estimate = results.gyro_estimate;
_gyro_drift = results.gyro_drift;
// copy earth-frame accelerometer estimates:
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_ef_ekf[i] = results.accel_ef[i];
}
_accel_ef_ekf_blended = results.accel_ef_blended;
update_cd_values();
update_trig();
}
void AP_AHRS::update_DCM()
{
// we need to restore the old DCM attitude values as these are
// used internally in DCM to calculate error values for gyro drift
// correction
roll = _dcm_attitude.x;
pitch = _dcm_attitude.y;
yaw = _dcm_attitude.z;
update_cd_values();
dcm.update();
dcm.get_results(dcm_estimates);
AP_AHRS_DCM::_update();
// we always update the vehicle's canonical roll/pitch/yaw from
// DCM. In normal operation this will usually be over-written by
// an EKF or external AHRS. This is long-held behaviour, but this
// really shouldn't be doing this.
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude = {roll, pitch, yaw};
// if (active_EKF_type() == EKFType::NONE) {
copy_estimates_from_backend_estimates(dcm_estimates);
// }
}
void AP_AHRS::update_notify_from_filter_status(const nav_filter_status &status)
@ -673,28 +688,21 @@ void AP_AHRS::update_external(void)
// accelerometer values in the earth frame in m/s/s
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);
}
return _accel_ef_ekf[i];
}
// blended accelerometer values in the earth frame in m/s/s
const Vector3f &AP_AHRS::get_accel_ef_blended(void) const
{
if (active_EKF_type() == EKFType::NONE) {
return AP_AHRS_DCM::get_accel_ef_blended();
}
return _accel_ef_ekf_blended;
}
void AP_AHRS::reset(bool recover_eulers)
void AP_AHRS::reset()
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
AP_AHRS_DCM::reset(recover_eulers);
_dcm_attitude = {roll, pitch, yaw};
dcm.reset();
#if HAL_NAVEKF2_AVAILABLE
if (_ekf2_started) {
@ -713,7 +721,7 @@ bool AP_AHRS::get_position(struct Location &loc) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
return AP_AHRS_DCM::get_position(loc);
return dcm.get_position(loc);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
@ -754,7 +762,7 @@ bool AP_AHRS::get_position(struct Location &loc) const
// fall back to position from DCM
if (!always_use_EKF()) {
return AP_AHRS_DCM::get_position(loc);
return dcm.get_position(loc);
}
return false;
}
@ -762,12 +770,12 @@ bool AP_AHRS::get_position(struct Location &loc) const
// status reporting of estimated errors
float AP_AHRS::get_error_rp(void) const
{
return AP_AHRS_DCM::get_error_rp();
return dcm.get_error_rp();
}
float AP_AHRS::get_error_yaw(void) const
{
return AP_AHRS_DCM::get_error_yaw();
return dcm.get_error_yaw();
}
// return a wind estimation vector, in m/s
@ -776,7 +784,7 @@ Vector3f AP_AHRS::wind_estimate(void) const
Vector3f wind;
switch (active_EKF_type()) {
case EKFType::NONE:
wind = AP_AHRS_DCM::wind_estimate();
wind = dcm.wind_estimate();
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -839,7 +847,7 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
Vector3f wind_vel;
switch (active_EKF_type()) {
case EKFType::NONE:
return AP_AHRS_DCM::airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SIM:
@ -852,7 +860,7 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
return AP_AHRS_DCM::airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
return dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
#endif
#if HAL_NAVEKF3_AVAILABLE
@ -884,12 +892,39 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
airspeed_ret = true_airspeed / get_EAS2TAS();
} else {
// fallback to DCM if airspeed estimate if EKF has wind but no velocity estimate
ret = AP_AHRS_DCM::airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
ret = dcm.airspeed_estimate(get_active_airspeed_index(), airspeed_ret);
}
return ret;
}
bool AP_AHRS::airspeed_estimate_true(float &airspeed_ret) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
return dcm.airspeed_estimate_true(airspeed_ret);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SIM:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
#endif
break;
}
if (!airspeed_estimate(airspeed_ret)) {
return false;
}
airspeed_ret *= get_EAS2TAS();
return true;
}
// return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable
bool AP_AHRS::airspeed_vector_true(Vector3f &vec) const
@ -920,6 +955,15 @@ bool AP_AHRS::airspeed_vector_true(Vector3f &vec) const
return false;
}
// return a synthetic airspeed estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified
// on failure.
bool AP_AHRS::synthetic_airspeed(float &ret) const
{
return dcm.synthetic_airspeed(ret);
}
// true if compass is being used
bool AP_AHRS::use_compass(void)
{
@ -947,7 +991,7 @@ bool AP_AHRS::use_compass(void)
break;
#endif
}
return AP_AHRS_DCM::use_compass();
return dcm.use_compass();
}
// return the quaternion defining the rotation from NED to XYZ (body) axes
@ -957,7 +1001,7 @@ bool AP_AHRS::get_quaternion(Quaternion &quat) const
// according to trim
switch (active_EKF_type()) {
case EKFType::NONE:
if (!AP_AHRS_DCM::get_quaternion(quat)) {
if (!dcm.get_quaternion(quat)) {
return false;
}
break;
@ -1011,7 +1055,9 @@ bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const
case EKFType::NONE:
// DCM is secondary
eulers = _dcm_attitude;
eulers[0] = dcm_estimates.roll_rad;
eulers[1] = dcm_estimates.pitch_rad;
eulers[2] = dcm_estimates.yaw_rad;
return true;
#if HAL_NAVEKF2_AVAILABLE
@ -1064,7 +1110,7 @@ bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const
case EKFType::NONE:
// DCM is secondary
if (!AP_AHRS_DCM::get_quaternion(quat)) {
if (!dcm.get_quaternion(quat)) {
return false;
}
break;
@ -1119,7 +1165,7 @@ bool AP_AHRS::get_secondary_position(struct Location &loc) const
case EKFType::NONE:
// return DCM position
AP_AHRS_DCM::get_position(loc);
dcm.get_position(loc);
return true;
#if HAL_NAVEKF2_AVAILABLE
@ -1189,7 +1235,32 @@ Vector2f AP_AHRS::groundspeed_vector(void)
}
#endif
}
return AP_AHRS_DCM::groundspeed_vector();
return dcm.groundspeed_vector();
}
float AP_AHRS::groundspeed(void)
{
switch (active_EKF_type()) {
case EKFType::NONE:
return dcm.groundspeed();
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
#endif
#if HAL_NAVEKF3_AVAILABLE
case EKFType::THREE:
#endif
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SIM:
#endif
break;
}
return groundspeed_vector().length();
}
// set the EKF's origin location in 10e7 degrees. This should only
@ -1277,7 +1348,7 @@ bool AP_AHRS::get_velocity_NED(Vector3f &vec) const
return AP::externalAHRS().get_velocity_NED(vec);
#endif
}
return AP_AHRS_DCM::get_velocity_NED(vec);
return dcm.get_velocity_NED(vec);
}
// returns the expected NED magnetic field
@ -1348,14 +1419,8 @@ bool AP_AHRS::get_mag_field_correction(Vector3f &vec) const
bool AP_AHRS::get_vert_pos_rate(float &velocity) const
{
switch (active_EKF_type()) {
case EKFType::NONE: {
Vector3f velned;
if (!AP_AHRS_DCM::get_velocity_NED(velned)) {
return false;
}
velocity = velned.z;
return true;
}
case EKFType::NONE:
return dcm.get_vert_pos_rate(velocity);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
@ -1431,7 +1496,7 @@ bool AP_AHRS::get_relative_position_NED_origin(Vector3f &vec) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
return AP_AHRS_DCM::get_relative_position_NED_origin(vec);
return dcm.get_relative_position_NED_origin(vec);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
@ -1524,7 +1589,7 @@ bool AP_AHRS::get_relative_position_NE_origin(Vector2f &posNE) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
return AP_AHRS_DCM::get_relative_position_NE_origin(posNE);
return dcm.get_relative_position_NE_origin(posNE);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
@ -1594,7 +1659,7 @@ bool AP_AHRS::get_relative_position_D_origin(float &posD) const
{
switch (active_EKF_type()) {
case EKFType::NONE:
return AP_AHRS_DCM::get_relative_position_D_origin(posD);
return dcm.get_relative_position_D_origin(posD);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
@ -1899,7 +1964,7 @@ bool AP_AHRS::healthy(void) const
// an internal processing error, but not for bad sensor data.
switch (ekf_type()) {
case EKFType::NONE:
return AP_AHRS_DCM::healthy();
return dcm.healthy();
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO: {
@ -1945,7 +2010,7 @@ bool AP_AHRS::healthy(void) const
#endif
}
return AP_AHRS_DCM::healthy();
return dcm.healthy();
}
// returns false if we fail arming checks, in which case the buffer will be populated with a failure message
@ -1963,9 +2028,10 @@ bool AP_AHRS::pre_arm_check(bool requires_position, char *failure_msg, uint8_t f
switch (ekf_type()) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKFType::SIM:
return ret;
#endif
case EKFType::NONE:
return ret;
return dcm.pre_arm_check(requires_position, failure_msg, failure_msg_len);
#if HAL_EXTERNAL_AHRS_ENABLED
case EKFType::EXTERNAL:
@ -2032,7 +2098,7 @@ bool AP_AHRS::get_filter_status(nav_filter_status &status) const
{
switch (ekf_type()) {
case EKFType::NONE:
return false;
return dcm.get_filter_status(status);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
@ -2236,7 +2302,7 @@ void AP_AHRS::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
#endif
}
if (imu_idx == -1) {
AP_AHRS_DCM::getCorrectedDeltaVelocityNED(ret, dt);
dcm.getCorrectedDeltaVelocityNED(ret, dt);
return;
}
ret.zero();
@ -2345,7 +2411,7 @@ uint32_t AP_AHRS::getLastYawResetAngle(float &yawAng)
switch (active_EKF_type()) {
case EKFType::NONE:
return 0;
return dcm.getLastYawResetAngle(yawAng);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:
@ -2517,7 +2583,7 @@ void AP_AHRS::send_ekf_status_report(mavlink_channel_t chan) const
switch (ekf_type()) {
case EKFType::NONE:
// send zero status report
mavlink_msg_ekf_status_report_send(chan, 0, 0, 0, 0, 0, 0, 0);
dcm.send_ekf_status_report(chan);
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -2567,7 +2633,7 @@ bool AP_AHRS::get_origin(Location &ret) const
{
switch (ekf_type()) {
case EKFType::NONE:
return AP_AHRS_DCM::get_origin_fallback(ret);
return dcm.get_origin(ret);
#if HAL_NAVEKF2_AVAILABLE
case EKFType::TWO:

307
libraries/AP_AHRS/AP_AHRS.h
View File

@ -53,7 +53,7 @@ class AP_AHRS_View;
#include <AP_NMEA_Output/AP_NMEA_Output.h>
class AP_AHRS : public AP_AHRS_DCM {
class AP_AHRS {
friend class AP_AHRS_View;
public:
@ -65,7 +65,7 @@ public:
AP_AHRS(uint8_t flags = 0);
// initialise
void init(void) override;
void init(void);
/* Do not allow copies */
CLASS_NO_COPY(AP_AHRS);
@ -79,29 +79,33 @@ public:
// this makes initial config easier
void update_orientation();
// allow threads to lock against AHRS update
HAL_Semaphore &get_semaphore(void) {
return _rsem;
}
// return the smoothed gyro vector corrected for drift
const Vector3f &get_gyro(void) const override;
const Matrix3f &get_rotation_body_to_ned(void) const override;
const Vector3f &get_gyro(void) const;
// return the current drift correction integrator value
const Vector3f &get_gyro_drift(void) const override;
const Vector3f &get_gyro_drift(void) const;
// reset the current gyro drift estimate
// should be called if gyro offsets are recalculated
void reset_gyro_drift() override;
void reset_gyro_drift();
void update(bool skip_ins_update=false);
void reset(bool recover_eulers = false) override;
void reset();
// dead-reckoning support
bool get_position(struct Location &loc) const override;
bool get_position(struct Location &loc) const;
// get latest altitude estimate above ground level in meters and validity flag
bool get_hagl(float &hagl) const override WARN_IF_UNUSED;
bool get_hagl(float &hagl) const WARN_IF_UNUSED;
// status reporting of estimated error
float get_error_rp() const override;
float get_error_yaw() const override;
float get_error_rp() const;
float get_error_yaw() const;
/*
* wind estimation support
@ -114,7 +118,10 @@ public:
bool get_wind_estimation_enabled() const { return wind_estimation_enabled; }
// return a wind estimation vector, in m/s
Vector3f wind_estimate() const override;
Vector3f wind_estimate() const;
// instruct DCM to update its wind estimate:
void estimate_wind() { dcm.estimate_wind(); }
// return the parameter AHRS_WIND_MAX in metres per second
uint8_t get_max_wind() const {
@ -125,40 +132,72 @@ public:
* airspeed support
*/
// get apparent to true airspeed ratio
float get_EAS2TAS(void) const {
// FIXME: make this is a method on the active backend
return dcm.get_EAS2TAS();
}
// return an airspeed estimate if available. return true
// if we have an estimate
bool airspeed_estimate(float &airspeed_ret) const override;
bool airspeed_estimate(float &airspeed_ret) const;
// return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate
bool airspeed_estimate_true(float &airspeed_ret) const;
// return estimate of true airspeed vector in body frame in m/s
// returns false if estimate is unavailable
bool airspeed_vector_true(Vector3f &vec) const override;
bool airspeed_vector_true(Vector3f &vec) const;
// return true if airspeed comes from an airspeed sensor, as
// opposed to an IMU estimate
bool airspeed_sensor_enabled(void) const {
// FIXME: make this "using_airspeed_sensor"?
return dcm.airspeed_sensor_enabled();
}
// return true if airspeed comes from a specific airspeed sensor, as
// opposed to an IMU estimate
bool airspeed_sensor_enabled(uint8_t airspeed_index) const {
// FIXME: make this a method on the active backend
return dcm.airspeed_sensor_enabled(airspeed_index);
}
// return a synthetic airspeed estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
// true if we have a synthetic airspeed. ret will not be modified
// on failure.
bool synthetic_airspeed(float &ret) const WARN_IF_UNUSED;
// true if compass is being used
bool use_compass() override;
bool use_compass();
// return the quaternion defining the rotation from NED to XYZ (body) axes
bool get_quaternion(Quaternion &quat) const override WARN_IF_UNUSED;
bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED;
// return secondary attitude solution if available, as eulers in radians
bool get_secondary_attitude(Vector3f &eulers) const override;
bool get_secondary_attitude(Vector3f &eulers) const;
// return secondary attitude solution if available, as quaternion
bool get_secondary_quaternion(Quaternion &quat) const override;
bool get_secondary_quaternion(Quaternion &quat) const;
// return secondary position solution if available
bool get_secondary_position(struct Location &loc) const override;
bool get_secondary_position(struct Location &loc) const;
// EKF has a better ground speed vector estimate
Vector2f groundspeed_vector() override;
Vector2f groundspeed_vector();
const Vector3f &get_accel_ef(uint8_t i) const override;
const Vector3f &get_accel_ef() const override;
// return ground speed estimate in meters/second. Used by ground vehicles.
float groundspeed(void);
const Vector3f &get_accel_ef(uint8_t i) const;
const Vector3f &get_accel_ef() const;
// Retrieves the corrected NED delta velocity in use by the inertial navigation
void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const override;
void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const;
// blended accelerometer values in the earth frame in m/s/s
const Vector3f &get_accel_ef_blended() const override;
const Vector3f &get_accel_ef_blended() const;
// 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)
@ -168,24 +207,24 @@ public:
// returns the inertial navigation origin in lat/lon/alt
bool get_origin(Location &ret) const WARN_IF_UNUSED;
bool have_inertial_nav() const override;
bool have_inertial_nav() const;
bool get_velocity_NED(Vector3f &vec) const override;
bool get_velocity_NED(Vector3f &vec) const;
// return the relative position NED to either home or origin
// return true if the estimate is valid
bool get_relative_position_NED_home(Vector3f &vec) const;
bool get_relative_position_NED_origin(Vector3f &vec) const override;
bool get_relative_position_NED_origin(Vector3f &vec) const;
// return the relative position NE to either home or origin
// return true if the estimate is valid
bool get_relative_position_NE_home(Vector2f &posNE) const;
bool get_relative_position_NE_origin(Vector2f &posNE) const override;
bool get_relative_position_NE_origin(Vector2f &posNE) const;
// return the relative position down to either home or origin
// baro will be used for the _home relative one if the EKF isn't
void get_relative_position_D_home(float &posD) const;
bool get_relative_position_D_origin(float &posD) const override;
bool get_relative_position_D_origin(float &posD) const;
// Get a derivative of the vertical position in m/s 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 vertical position due to the various errors that are being corrected for.
@ -201,28 +240,28 @@ public:
void writeDefaultAirSpeed(float airspeed, float uncertainty);
// Write position and quaternion data from an external navigation system
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms) override;
void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms);
// Write velocity data from an external navigation system
void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) override;
void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms);
// get speed limit
void getControlLimits(float &ekfGndSpdLimit, float &controlScaleXY) const;
float getControlScaleZ(void) const;
// is the AHRS subsystem healthy?
bool healthy() const override;
bool healthy() 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 pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override;
bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const;
// true if the AHRS has completed initialisation
bool initialised() const override;
bool initialised() const;
// return true if *DCM* yaw has been initialised
bool dcm_yaw_initialised(void) const {
return AP_AHRS_DCM::yaw_initialised();
return dcm.yaw_initialised();
}
// get_filter_status - returns filter status as a series of flags
@ -233,30 +272,30 @@ public:
bool getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const;
// check all cores providing consistent attitudes for prearm checks
bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const override;
bool attitudes_consistent(char *failure_msg, const uint8_t failure_msg_len) const;
// 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 getLastYawResetAngle(float &yawAng) override;
uint32_t getLastYawResetAngle(float &yawAng);
// return the amount of NE position change in meters due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastPosNorthEastReset(Vector2f &pos) override;
uint32_t getLastPosNorthEastReset(Vector2f &pos);
// return the amount of NE velocity change in meters/sec due to the last reset
// returns the time of the last reset or 0 if no reset has ever occurred
uint32_t getLastVelNorthEastReset(Vector2f &vel) const override;
uint32_t 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 getLastPosDownReset(float &posDelta) override;
uint32_t getLastPosDownReset(float &posDelta);
// Resets the baro so that it reads zero at the current height
// Resets the EKF height to zero
// 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 resetHeightDatum() override;
bool resetHeightDatum();
// send a EKF_STATUS_REPORT for current EKF
void send_ekf_status_report(mavlink_channel_t chan) const;
@ -264,15 +303,15 @@ public:
// get_hgt_ctrl_limit - get maximum height to be observed by the control loops in meters and a validity flag
// this is used to limit height during optical flow navigation
// it will return invalid when no limiting is required
bool get_hgt_ctrl_limit(float &limit) const override;
bool 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 set_terrain_hgt_stable(bool stable) override;
void set_terrain_hgt_stable(bool stable);
// return the innovations for the specified instance
// An out of range instance (eg -1) returns data for the primary instance
bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const override;
bool get_innovations(Vector3f &velInnov, Vector3f &posInnov, Vector3f &magInnov, float &tasInnov, float &yawInnov) const;
// returns true when the state estimates are significantly degraded by vibration
bool is_vibration_affected() const;
@ -281,17 +320,17 @@ public:
// indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accepted by the filter
// boolean false is returned if variances are not available
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const override;
bool get_variances(float &velVar, float &posVar, float &hgtVar, Vector3f &magVar, float &tasVar) const;
// get a source's velocity innovations
// returns true on success and results are placed in innovations and variances arguments
bool get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const override WARN_IF_UNUSED;
bool get_vel_innovations_and_variances_for_source(uint8_t source, Vector3f &innovations, Vector3f &variances) const WARN_IF_UNUSED;
// returns the expected NED magnetic field
bool get_mag_field_NED(Vector3f& ret) const;
// returns the estimated magnetic field offsets in body frame
bool get_mag_field_correction(Vector3f &ret) const override;
bool get_mag_field_correction(Vector3f &ret) const;
// return the index of the airspeed we should use for airspeed measurements
// with multiple airspeed sensors and airspeed affinity in EKF3, it is possible to have switched
@ -301,33 +340,33 @@ public:
uint8_t get_active_airspeed_index() const;
// return the index of the primary core or -1 if no primary core selected
int8_t get_primary_core_index() const override;
int8_t get_primary_core_index() const;
// get the index of the current primary accelerometer sensor
uint8_t get_primary_accel_index(void) const override;
uint8_t get_primary_accel_index(void) const;
// get the index of the current primary gyro sensor
uint8_t get_primary_gyro_index(void) const override;
uint8_t get_primary_gyro_index(void) const;
// see if EKF lane switching is possible to avoid EKF failsafe
void check_lane_switch(void) override;
void check_lane_switch(void);
// request EKF yaw reset to try and avoid the need for an EKF lane switch or failsafe
void request_yaw_reset(void) override;
void request_yaw_reset(void);
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
void set_posvelyaw_source_set(uint8_t source_set_idx) override;
void set_posvelyaw_source_set(uint8_t source_set_idx);
void Log_Write();
// check if non-compass sensor is providing yaw. Allows compass pre-arm checks to be bypassed
bool using_noncompass_for_yaw(void) const override;
bool using_noncompass_for_yaw(void) const;
// check if external nav is providing yaw
bool using_extnav_for_yaw(void) const override;
bool using_extnav_for_yaw(void) const;
// set and save the ALT_M_NSE parameter value
void set_alt_measurement_noise(float noise) override;
void set_alt_measurement_noise(float noise);
// active EKF type for logging
uint8_t get_active_AHRS_type(void) const {
@ -379,6 +418,23 @@ public:
const Matrix3f& get_rotation_autopilot_body_to_vehicle_body(void) const { return _rotation_autopilot_body_to_vehicle_body; }
const Matrix3f& get_rotation_vehicle_body_to_autopilot_body(void) const { return _rotation_vehicle_body_to_autopilot_body; }
// Logging functions
void Log_Write_Home_And_Origin();
void Write_AHRS2(void) const;
void Write_Attitude(const Vector3f &targets) const;
void Write_Origin(uint8_t origin_type, const Location &loc) const;
void Write_POS(void) const;
// return a smoothed and corrected gyro vector in radians/second
// using the latest ins data (which may not have been consumed by
// the EKF yet)
Vector3f get_gyro_latest(void) const;
// get yaw rate in earth frame in radians/sec
float get_yaw_rate_earth(void) const {
return get_gyro() * get_rotation_body_to_ned().c;
}
/*
* home-related functionality
*/
@ -408,7 +464,85 @@ public:
// current barometer and GPS altitudes correspond to this altitude
bool set_home(const Location &loc) WARN_IF_UNUSED;
void Log_Write_Home_And_Origin();
/*
* Attitude-related public methods and attributes:
*/
// roll/pitch/yaw euler angles, all in radians
float roll;
float pitch;
float yaw;
float get_roll() const { return roll; }
float get_pitch() const { return pitch; }
float get_yaw() const { return yaw; }
// helper trig value accessors
float cos_roll() const {
return _cos_roll;
}
float cos_pitch() const {
return _cos_pitch;
}
float cos_yaw() const {
return _cos_yaw;
}
float sin_roll() const {
return _sin_roll;
}
float sin_pitch() const {
return _sin_pitch;
}
float sin_yaw() const {
return _sin_yaw;
}
// integer Euler angles (Degrees * 100)
int32_t roll_sensor;
int32_t pitch_sensor;
int32_t yaw_sensor;
const Matrix3f &get_rotation_body_to_ned(void) const;
// return a Quaternion representing our current attitude in NED frame
void get_quat_body_to_ned(Quaternion &quat) const {
quat.from_rotation_matrix(get_rotation_body_to_ned());
}
// get rotation matrix specifically from DCM backend (used for
// compass calibrator)
const Matrix3f &get_DCM_rotation_body_to_ned(void) const {
return dcm_estimates.dcm_matrix;
}
// rotate a 2D vector from earth frame to body frame
// in result, x is forward, y is right
Vector2f earth_to_body2D(const Vector2f &ef_vector) const;
// rotate a 2D vector from earth frame to body frame
// in input, x is forward, y is right
Vector2f body_to_earth2D(const Vector2f &bf) const;
// convert a vector from body to earth frame
Vector3f body_to_earth(const Vector3f &v) const {
return v * get_rotation_body_to_ned();
}
// convert a vector from earth to body frame
Vector3f earth_to_body(const Vector3f &v) const {
return get_rotation_body_to_ned().mul_transpose(v);
}
/*
* methods for the benefit of LUA bindings
*/
// return current vibration vector for primary IMU
Vector3f get_vibration(void) const;
// return primary accels, for lua
const Vector3f &get_accel(void) const {
return AP::ins().get_accel();
}
/*
* AHRS is used as a transport for vehicle-takeoff-expected and
@ -460,11 +594,11 @@ public:
// get the view's rotation, or ROTATION_NONE
enum Rotation get_view_rotation(void) const;
protected:
private:
// optional view class
AP_AHRS_View *_view;
private:
static AP_AHRS *_singleton;
/* we modify our behaviour based on what sort of vehicle the
@ -473,6 +607,9 @@ private:
*/
VehicleClass _vehicle_class{VehicleClass::UNKNOWN};
// multi-thread access support
HAL_Semaphore _rsem;
/*
* Parameters
*/
@ -488,6 +625,19 @@ private:
*/
Matrix3f _custom_rotation;
/*
* DCM-backend parameters; it takes references to these
*/
// settable parameters
AP_Float _kp_yaw;
AP_Float _kp;
AP_Float gps_gain;
AP_Float beta;
AP_Enum<GPSUse> _gps_use;
AP_Int8 _gps_minsats;
enum class EKFType {
NONE = 0
#if HAL_NAVEKF3_AVAILABLE
@ -513,6 +663,29 @@ private:
return _ekf_flags & FLAG_ALWAYS_USE_EKF;
}
/*
* Attitude-related private methods and attributes:
*/
// calculate sin/cos of roll/pitch/yaw from rotation
void calc_trig(const Matrix3f &rot,
float &cr, float &cp, float &cy,
float &sr, float &sp, float &sy) const;
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
// should be called after _dcm_matrix is updated
void update_trig(void);
// update roll_sensor, pitch_sensor and yaw_sensor
void update_cd_values(void);
// helper trig variables
float _cos_roll{1.0f};
float _cos_pitch{1.0f};
float _cos_yaw{1.0f};
float _sin_roll;
float _sin_pitch;
float _sin_yaw;
#if HAL_NAVEKF2_AVAILABLE
void update_EKF2(void);
bool _ekf2_started;
@ -524,8 +697,7 @@ private:
// rotation from vehicle body to NED frame
Matrix3f _dcm_matrix;
Vector3f _dcm_attitude;
Vector3f _gyro_drift;
Vector3f _gyro_estimate;
Vector3f _accel_ef_ekf[INS_MAX_INSTANCES];
@ -621,6 +793,19 @@ private:
// poke AP_Notify based on values from status
void update_notify_from_filter_status(const nav_filter_status &status);
/*
* backends (and their results)
*/
AP_AHRS_DCM dcm{_kp_yaw, _kp, gps_gain, beta, _gps_use, _gps_minsats};
struct AP_AHRS_Backend::Estimates dcm_estimates;
/*
* copy results from a backend over AP_AHRS canonical results.
* This updates member variables like roll and pitch, as well as
* updating derived values like sin_roll and sin_pitch.
*/
void copy_estimates_from_backend_estimates(const AP_AHRS_Backend::Estimates &results);
#if HAL_NMEA_OUTPUT_ENABLED
class AP_NMEA_Output* _nmea_out;
#endif

17
libraries/AP_AHRS/AP_AHRS_Backend.cpp
View File

@ -23,13 +23,12 @@
extern const AP_HAL::HAL& hal;
// init sets up INS board orientation
void AP_AHRS_Backend::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_Backend::get_gyro_latest(void) const
Vector3f AP_AHRS::get_gyro_latest(void) const
{
const uint8_t primary_gyro = get_primary_gyro_index();
return AP::ins().get_gyro(primary_gyro) + get_gyro_drift();
@ -90,7 +89,7 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void)
if (gotAirspeed) {
const Vector3f wind = wind_estimate();
const Vector2f wind2d(wind.x, wind.y);
const Vector2f airspeed_vector(_cos_yaw * airspeed, _sin_yaw * airspeed);
const Vector2f airspeed_vector{cosf(yaw) * airspeed, sinf(yaw) * airspeed};
gndVelADS = airspeed_vector + wind2d;
}
@ -148,7 +147,7 @@ Vector2f AP_AHRS_DCM::groundspeed_vector(void)
/*
calculate sin and cos of roll/pitch/yaw from a body_to_ned rotation matrix
*/
void AP_AHRS_Backend::calc_trig(const Matrix3f &rot,
void AP_AHRS::calc_trig(const Matrix3f &rot,
float &cr, float &cp, float &cy,
float &sr, float &sp, float &sy) const
{
@ -194,7 +193,7 @@ void AP_AHRS_Backend::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_Backend::update_trig(void)
void AP_AHRS::update_trig(void)
{
calc_trig(get_rotation_body_to_ned(),
_cos_roll, _cos_pitch, _cos_yaw,
@ -204,7 +203,7 @@ void AP_AHRS_Backend::update_trig(void)
/*
update the centi-degree values
*/
void AP_AHRS_Backend::update_cd_values(void)
void AP_AHRS::update_cd_values(void)
{
roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100;
@ -286,14 +285,14 @@ void AP_AHRS::update_AOA_SSA(void)
}
// rotate a 2D vector from earth frame to body frame
Vector2f AP_AHRS_Backend::earth_to_body2D(const Vector2f &ef) const
Vector2f AP_AHRS::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_Backend::body_to_earth2D(const Vector2f &bf) const
Vector2f AP_AHRS::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);
@ -322,7 +321,7 @@ float AP_AHRS_Backend::get_EAS2TAS(void) const {
}
// return current vibration vector for primary IMU
Vector3f AP_AHRS_Backend::get_vibration(void) const
Vector3f AP_AHRS::get_vibration(void) const
{
return AP::ins().get_vibration_levels();
}

233
libraries/AP_AHRS/AP_AHRS_Backend.h
View File

@ -33,6 +33,12 @@ class OpticalFlow;
#define AP_AHRS_RP_P_MIN 0.05f // minimum value for AHRS_RP_P parameter
#define AP_AHRS_YAW_P_MIN 0.05f // minimum value for AHRS_YAW_P parameter
enum class GPSUse : uint8_t {
Disable = 0,
Enable = 1,
EnableWithHeight = 2,
};
class AP_AHRS_Backend
{
public:
@ -43,6 +49,20 @@ public:
// empty virtual destructor
virtual ~AP_AHRS_Backend() {}
CLASS_NO_COPY(AP_AHRS_Backend);
// structure to retrieve results from backends:
struct Estimates {
float roll_rad;
float pitch_rad;
float yaw_rad;
Matrix3f dcm_matrix;
Vector3f gyro_estimate;
Vector3f gyro_drift;
Vector3f accel_ef[INS_MAX_INSTANCES]; // must be INS_MAX_INSTANCES
Vector3f accel_ef_blended;
};
// init sets up INS board orientation
virtual void init();
@ -59,26 +79,10 @@ public:
return AP::ins().get_primary_gyro();
}
// accelerometer values in the earth frame in m/s/s
virtual const Vector3f &get_accel_ef(uint8_t i) const {
return _accel_ef[i];
}
virtual const Vector3f &get_accel_ef(void) const {
return get_accel_ef(AP::ins().get_primary_accel());
}
// blended accelerometer values in the earth frame in m/s/s
virtual const Vector3f &get_accel_ef_blended(void) const {
return _accel_ef_blended;
}
// get yaw rate in earth frame in radians/sec
float get_yaw_rate_earth(void) const {
return get_gyro() * get_rotation_body_to_ned().c;
}
// Methods
virtual void _update() = 0;
virtual void update() = 0;
virtual void get_results(Estimates &results) = 0;
// 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
@ -102,59 +106,12 @@ public:
// set position, velocity and yaw sources to either 0=primary, 1=secondary, 2=tertiary
virtual void set_posvelyaw_source_set(uint8_t source_set_idx) {}
// Euler angles (radians)
float roll;
float pitch;
float yaw;
float get_roll() const { return roll; }
float get_pitch() const { return pitch; }
float get_yaw() const { return yaw; }
// integer Euler angles (Degrees * 100)
int32_t roll_sensor;
int32_t pitch_sensor;
int32_t yaw_sensor;
// return a smoothed and corrected gyro vector in radians/second
virtual const Vector3f &get_gyro(void) const = 0;
// return primary accels, for lua
const Vector3f &get_accel(void) const {
return AP::ins().get_accel();
}
// return a smoothed and corrected gyro vector in radians/second using the latest ins data (which may not have been consumed by the EKF yet)
Vector3f get_gyro_latest(void) const;
// return the current estimate of the gyro drift
virtual const Vector3f &get_gyro_drift(void) const = 0;
// reset the current gyro drift estimate
// should be called if gyro offsets are recalculated
virtual void reset_gyro_drift(void) = 0;
// reset the current attitude, used on new IMU calibration
virtual void reset(bool recover_eulers=false) = 0;
// return the average size of the roll/pitch error estimate
// since last call
virtual float get_error_rp(void) const = 0;
// return the average size of the yaw error estimate
// since last call
virtual float get_error_yaw(void) const = 0;
// return a DCM rotation matrix representing our current attitude in NED frame
virtual const Matrix3f &get_rotation_body_to_ned(void) const = 0;
// return a Quaternion representing our current attitude in NED frame
void get_quat_body_to_ned(Quaternion &quat) const {
quat.from_rotation_matrix(get_rotation_body_to_ned());
}
// get rotation matrix specifically from DCM backend (used for compass calibrator)
virtual const Matrix3f &get_DCM_rotation_body_to_ned(void) const = 0;
virtual void reset() = 0;
// get our current position estimate. Return true if a position is available,
// otherwise false. This call fills in lat, lng and alt
@ -168,7 +125,8 @@ public:
// return an airspeed estimate if available. return true
// if we have an estimate
virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED = 0;
virtual bool airspeed_estimate(float &airspeed_ret) const WARN_IF_UNUSED { return false; }
virtual bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const { return false; }
// return a true airspeed estimate (navigation airspeed) if
// available. return true if we have an estimate
@ -219,11 +177,29 @@ public:
return false;
}
// Get a derivative of the vertical position in m/s 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 vertical position due to the various errors that are being corrected for.
virtual bool get_vert_pos_rate(float &velocity) const = 0;
// returns the estimated magnetic field offsets in body frame
virtual bool get_mag_field_correction(Vector3f &ret) const WARN_IF_UNUSED {
return false;
}
virtual bool get_mag_field_NED(Vector3f &vec) const {
return false;
}
virtual bool get_mag_offsets(uint8_t mag_idx, Vector3f &magOffsets) const {
return false;
}
//
virtual bool set_origin(const Location &loc) {
return false;
}
virtual bool get_origin(Location &ret) const = 0;
// return a position relative to origin in meters, North/East/Down
// order. This will only be accurate if have_inertial_nav() is
// true
@ -251,44 +227,9 @@ public:
// return true if we will use compass for yaw
virtual bool use_compass(void) = 0;
// helper trig value accessors
float cos_roll() const {
return _cos_roll;
}
float cos_pitch() const {
return _cos_pitch;
}
float cos_yaw() const {
return _cos_yaw;
}
float sin_roll() const {
return _sin_roll;
}
float sin_pitch() const {
return _sin_pitch;
}
float sin_yaw() const {
return _sin_yaw;
}
// return the quaternion defining the rotation from NED to XYZ (body) axes
virtual bool get_quaternion(Quaternion &quat) const WARN_IF_UNUSED = 0;
// return secondary attitude solution if available, as eulers in radians
virtual bool get_secondary_attitude(Vector3f &eulers) const WARN_IF_UNUSED {
return false;
}
// return secondary attitude solution if available, as quaternion
virtual bool get_secondary_quaternion(Quaternion &quat) const WARN_IF_UNUSED {
return false;
}
// return secondary position solution if available
virtual bool get_secondary_position(struct Location &loc) const WARN_IF_UNUSED {
return false;
}
// return true if the AHRS object supports inertial navigation,
// with very accurate position and velocity
virtual bool have_inertial_nav(void) const {
@ -302,6 +243,9 @@ public:
virtual bool initialised(void) const {
return true;
};
virtual bool started(void) const {
return initialised();
};
// 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
@ -342,6 +286,10 @@ public:
return false;
}
virtual bool get_filter_status(nav_filter_status &status) const {
return false;
}
// get_variances - provides the innovations normalised using the innovation variance where a value of 0
// indicates perfect consistency between the measurement and the EKF solution and a value of of 1 is the maximum
// inconsistency that will be accepted by the filter
@ -356,30 +304,14 @@ public:
return false;
}
virtual void send_ekf_status_report(mavlink_channel_t chan) const = 0;
// Retrieves the corrected NED delta velocity in use by the inertial navigation
virtual void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const {
ret.zero();
AP::ins().get_delta_velocity(ret, dt);
}
// rotate a 2D vector from earth frame to body frame
// in result, x is forward, y is right
Vector2f earth_to_body2D(const Vector2f &ef_vector) const;
// rotate a 2D vector from earth frame to body frame
// in input, x is forward, y is right
Vector2f body_to_earth2D(const Vector2f &bf) const;
// convert a vector from body to earth frame
Vector3f body_to_earth(const Vector3f &v) const {
return v * get_rotation_body_to_ned();
}
// convert a vector from earth to body frame
Vector3f earth_to_body(const Vector3f &v) const {
return get_rotation_body_to_ned().mul_transpose(v);
}
// get_hgt_ctrl_limit - get maximum height to be observed by the
// control loops in meters and a validity flag. It will return
// false when no limiting is required
@ -389,63 +321,4 @@ public:
// this is not related to terrain following
virtual void set_terrain_hgt_stable(bool stable) {}
// Write position and quaternion data from an external navigation system
virtual void writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms) { }
// Write velocity data from an external navigation system
virtual void writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms) { }
// return current vibration vector for primary IMU
Vector3f get_vibration(void) const;
// set and save the alt noise parameter value
virtual void set_alt_measurement_noise(float noise) {};
// allow threads to lock against AHRS update
HAL_Semaphore &get_semaphore(void) {
return _rsem;
}
// Logging to disk functions
void Write_AHRS2(void) const;
void Write_Attitude(const Vector3f &targets) const;
void Write_Origin(uint8_t origin_type, const Location &loc) const;
void Write_POS(void) const;
protected:
enum class GPSUse : uint8_t {
Disable = 0,
Enable = 1,
EnableWithHeight = 2,
};
AP_Enum<GPSUse> _gps_use;
// multi-thread access support
HAL_Semaphore _rsem;
// calculate sin/cos of roll/pitch/yaw from rotation
void calc_trig(const Matrix3f &rot,
float &cr, float &cp, float &cy,
float &sr, float &sp, float &sy) const;
// update_trig - recalculates _cos_roll, _cos_pitch, etc based on latest attitude
// should be called after _dcm_matrix is updated
void update_trig(void);
// update roll_sensor, pitch_sensor and yaw_sensor
void update_cd_values(void);
// accelerometer values in the earth frame in m/s/s
Vector3f _accel_ef[INS_MAX_INSTANCES];
Vector3f _accel_ef_blended;
// helper trig variables
float _cos_roll{1.0f};
float _cos_pitch{1.0f};
float _cos_yaw{1.0f};
float _sin_roll;
float _sin_pitch;
float _sin_yaw;
};

76
libraries/AP_AHRS/AP_AHRS_DCM.cpp
View File

@ -63,9 +63,10 @@ void AP_AHRS::load_watchdog_home()
}
}
// run a full DCM update round
void
AP_AHRS_DCM::_update()
AP_AHRS_DCM::update()
{
AP_InertialSensor &_ins = AP::ins();
@ -93,11 +94,12 @@ AP_AHRS_DCM::_update()
// paranoid check for bad values in the DCM matrix
check_matrix();
// Calculate pitch, roll, yaw for stabilization and navigation
euler_angles();
// update trig values including _cos_roll, cos_pitch
update_trig();
// calculate the euler angles and DCM matrix which will be used
// for high level navigation control. Apply trim such that a
// positive trim value results in a positive vehicle rotation
// about that axis (ie a negative offset)
_body_dcm_matrix = _dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
_body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
backup_attitude();
@ -105,6 +107,21 @@ AP_AHRS_DCM::_update()
IGNORE_RETURN(AP::ahrs().get_origin(last_origin));
}
void AP_AHRS_DCM::get_results(AP_AHRS_Backend::Estimates &results)
{
results.roll_rad = roll;
results.pitch_rad = pitch;
results.yaw_rad = yaw;
results.dcm_matrix = _body_dcm_matrix;
results.gyro_estimate = _omega;
results.gyro_drift = _omega_I;
const uint8_t to_copy = MIN(sizeof(results.accel_ef), sizeof(_accel_ef));
memcpy(results.accel_ef, _accel_ef, to_copy);
results.accel_ef_blended = _accel_ef_blended;
}
/*
backup attitude to persistent_data for use in watchdog reset
*/
@ -162,9 +179,6 @@ AP_AHRS_DCM::matrix_update(float _G_Dt)
void
AP_AHRS_DCM::reset(bool recover_eulers)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
// reset the integration terms
_omega_I.zero();
_omega_P.zero();
@ -437,8 +451,8 @@ bool AP_AHRS_DCM::use_compass(void)
// degrees and the estimated wind speed is less than 80% of the
// ground speed, then switch to GPS navigation. This will help
// prevent flyaways with very bad compass offsets
const int32_t error = labs(wrap_180_cd(yaw_sensor - AP::gps().ground_course_cd()));
if (error > 4500 && _wind.length() < AP::gps().ground_speed()*0.8f) {
const float error = fabsf(wrap_180(degrees(yaw) - AP::gps().ground_course()));
if (error > 45 && _wind.length() < AP::gps().ground_speed()*0.8f) {
if (AP_HAL::millis() - _last_consistent_heading > 2000) {
// start using the GPS for heading if the compass has been
// inconsistent with the GPS for 2 seconds
@ -727,8 +741,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
Vector3f airspeed = velocity - _wind;
// rotate vector to body frame
const Matrix3f &rot = get_rotation_body_to_ned();
airspeed = rot.mul_transpose(airspeed);
airspeed = _body_dcm_matrix.mul_transpose(airspeed);
// take positive component in X direction. This mimics a pitot
// tube
@ -913,7 +926,7 @@ AP_AHRS_DCM::drift_correction(float deltat)
if (fly_forward && _gps.status() >= AP_GPS::GPS_OK_FIX_2D &&
_gps.ground_speed() < GPS_SPEED_MIN &&
_ins.get_accel().x >= 7 &&
pitch_sensor > -3000 && pitch_sensor < 3000) {
pitch > radians(-30) && pitch < radians(30)) {
// assume we are in a launch acceleration, and reduce the
// rp gain by 50% to reduce the impact of GPS lag on
// takeoff attitude when using a catapult
@ -1013,19 +1026,6 @@ void AP_AHRS_DCM::estimate_wind(void)
}
// calculate the euler angles and DCM matrix which will be used for high level
// navigation control. Apply trim such that a positive trim value results in a
// positive vehicle rotation about that axis (ie a negative offset)
void
AP_AHRS_DCM::euler_angles(void)
{
_body_dcm_matrix = _dcm_matrix * AP::ahrs().get_rotation_vehicle_body_to_autopilot_body();
_body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
update_cd_values();
}
// return our current position estimate using
// dead-reckoning or GPS
bool AP_AHRS_DCM::get_position(struct Location &loc) const
@ -1155,6 +1155,18 @@ bool AP_AHRS_DCM::get_velocity_NED(Vector3f &vec) const
return true;
}
// Get a derivative of the vertical position in m/s 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 vertical position due to the various errors that are being corrected for.
bool AP_AHRS_DCM::get_vert_pos_rate(float &velocity) const
{
Vector3f velned;
if (!get_velocity_NED(velned)) {
return false;
}
velocity = velned.z;
return true;
}
// 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 (not used)
bool AP_AHRS_DCM::pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const
@ -1169,7 +1181,7 @@ bool AP_AHRS_DCM::pre_arm_check(bool requires_position, char *failure_msg, uint8
/*
relative-origin functions for fallback in AP_InertialNav
*/
bool AP_AHRS_DCM::get_origin_fallback(Location &ret) const
bool AP_AHRS_DCM::get_origin(Location &ret) const
{
ret = last_origin;
if (ret.is_zero()) {
@ -1182,7 +1194,7 @@ bool AP_AHRS_DCM::get_origin_fallback(Location &ret) const
bool AP_AHRS_DCM::get_relative_position_NED_origin(Vector3f &posNED) const
{
Location origin;
if (!AP_AHRS_DCM::get_origin_fallback(origin)) {
if (!AP_AHRS_DCM::get_origin(origin)) {
return false;
}
Location loc;
@ -1212,3 +1224,9 @@ bool AP_AHRS_DCM::get_relative_position_D_origin(float &posD) const
posD = posNED.z;
return true;
}
void AP_AHRS_DCM::send_ekf_status_report(mavlink_channel_t chan) const
{
// send zero status report
mavlink_msg_ekf_status_report_send(chan, 0, 0, 0, 0, 0, 0, 0);
}

88
libraries/AP_AHRS/AP_AHRS_DCM.h
View File

@ -25,8 +25,20 @@
class AP_AHRS_DCM : public AP_AHRS_Backend {
public:
AP_AHRS_DCM()
: AP_AHRS_Backend()
AP_AHRS_DCM(AP_Float &kp_yaw,
AP_Float &kp,
AP_Float &_gps_gain,
AP_Float &_beta,
AP_Enum<GPSUse> &gps_use,
AP_Int8 &gps_minsats)
: AP_AHRS_Backend(),
_kp_yaw(kp_yaw),
_kp(kp),
gps_gain(_gps_gain),
beta(_beta),
_gps_use(gps_use),
_gps_minsats(gps_minsats)
{
_dcm_matrix.identity();
}
@ -35,31 +47,14 @@ public:
AP_AHRS_DCM(const AP_AHRS_DCM &other) = delete;
AP_AHRS_DCM &operator=(const AP_AHRS_DCM&) = delete;
// return the smoothed gyro vector corrected for drift
const Vector3f &get_gyro() const override {
return _omega;
}
// return rotation matrix representing rotaton from body to earth axes
const Matrix3f &get_rotation_body_to_ned() const override {
return _body_dcm_matrix;
}
// get rotation matrix specifically from DCM backend (used for compass calibrator)
const Matrix3f &get_DCM_rotation_body_to_ned(void) const override { return _body_dcm_matrix; }
// return the current drift correction integrator value
const Vector3f &get_gyro_drift() const override {
return _omega_I;
}
// reset the current gyro drift estimate
// should be called if gyro offsets are recalculated
void reset_gyro_drift() override;
// Methods
void _update() override;
void reset(bool recover_eulers = false) override;
void update() override;
void get_results(Estimates &results) override;
void reset() override { reset(false); }
// return true if yaw has been initialised
bool yaw_initialised(void) const {
@ -70,10 +65,10 @@ public:
virtual bool get_position(struct Location &loc) const override;
// status reporting
float get_error_rp() const override {
float get_error_rp() const {
return _error_rp;
}
float get_error_yaw() const override {
float get_error_yaw() const {
return _error_yaw;
}
@ -88,7 +83,7 @@ public:
// return an airspeed estimate if available. return true
// if we have an estimate from a specific sensor index
bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const;
bool airspeed_estimate(uint8_t airspeed_index, float &airspeed_ret) const override;
// return a synthetic airspeed estimate (one derived from sensors
// other than an actual airspeed sensor), if available. return
@ -114,34 +109,44 @@ public:
bool get_velocity_NED(Vector3f &vec) const override;
// Get a derivative of the vertical position in m/s 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 vertical position due to the various errors that are being corrected for.
bool get_vert_pos_rate(float &velocity) const override;
// 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 (not used)
bool pre_arm_check(bool requires_position, char *failure_msg, uint8_t failure_msg_len) const override;
// relative-origin functions for fallback in AP_InertialNav
bool get_origin_fallback(Location &ret) const;
bool get_origin(Location &ret) const override;
bool get_relative_position_NED_origin(Vector3f &vec) const override;
bool get_relative_position_NE_origin(Vector2f &posNE) const override;
bool get_relative_position_D_origin(float &posD) const override;
protected:
// settable parameters
AP_Float _kp_yaw;
AP_Float _kp;
AP_Float gps_gain;
AP_Float beta;
AP_Int8 _gps_minsats;
void send_ekf_status_report(mavlink_channel_t chan) const override;
private:
// settable parameters
AP_Float &_kp_yaw;
AP_Float &_kp;
AP_Float &gps_gain;
AP_Float &beta;
AP_Int8 &_gps_minsats;
AP_Enum<GPSUse> &_gps_use;
// these are experimentally derived from the simulator
// with large drift levels
static constexpr float _ki = 0.0087f;
static constexpr float _ki_yaw = 0.01f;
// accelerometer values in the earth frame in m/s/s
Vector3f _accel_ef[INS_MAX_INSTANCES];
Vector3f _accel_ef_blended;
// Methods
void matrix_update(float _G_Dt);
void normalize(void);
@ -150,17 +155,26 @@ private:
void drift_correction(float deltat);
void drift_correction_yaw(void);
float yaw_error_compass(class Compass &compass);
void euler_angles(void);
bool have_gps(void) const;
bool use_fast_gains(void) const;
void backup_attitude(void);
// internal reset function. Called externally, we never reset the
// DCM matrix from the eulers. Called internally we may.
void reset(bool recover_eulers);
// primary representation of attitude of board used for all inertial calculations
Matrix3f _dcm_matrix;
// primary representation of attitude of flight vehicle body
Matrix3f _body_dcm_matrix;
// euler angles - used for recovering if the DCM
// matrix becomes ill-conditioned and watchdog storage
float roll;
float pitch;
float yaw;
Vector3f _omega_P; // accel Omega proportional correction
Vector3f _omega_yaw_P; // proportional yaw correction
Vector3f _omega_I; // Omega Integrator correction

8
libraries/AP_AHRS/AP_AHRS_Logging.cpp
View File

@ -6,7 +6,7 @@
// Write an AHRS2 packet
void AP_AHRS_Backend::Write_AHRS2() const
void AP_AHRS::Write_AHRS2() const
{
Vector3f euler;
struct Location loc;
@ -45,7 +45,7 @@ void AP_AHRS::Write_AOA_SSA(void) const
}
// Write an attitude packet
void AP_AHRS_Backend::Write_Attitude(const Vector3f &targets) const
void AP_AHRS::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_Backend::Write_Attitude(const Vector3f &targets) const
AP::logger().WriteBlock(&pkt, sizeof(pkt));
}
void AP_AHRS_Backend::Write_Origin(uint8_t origin_type, const Location &loc) const
void AP_AHRS::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_Backend::Write_Origin(uint8_t origin_type, const Location &loc) con
}
// Write a POS packet
void AP_AHRS_Backend::Write_POS() const
void AP_AHRS::Write_POS() const
{
Location loc;
if (!get_position(loc)) {