mirror of https://github.com/ArduPilot/ardupilot
AP_AHRS: return estimates from state structure
this lowers CPU usage and fixes race conditions
This commit is contained in:
Andrew Tridgell
parent
fcf6cc0eff
commit
725a2227ca
|
|
@ -196,7 +196,7 @@ AP_AHRS::AP_AHRS(uint8_t flags) :
|
|||
// Copter and Sub force the use of EKF
|
||||
_ekf_flags |= AP_AHRS::FLAG_ALWAYS_USE_EKF;
|
||||
#endif
|
||||
_dcm_matrix.identity();
|
||||
state.dcm_matrix.identity();
|
||||
|
||||
// initialise the controller-to-autopilot-body trim state:
|
||||
_last_trim = _trim.get();
|
||||
|
|
@ -274,22 +274,25 @@ void AP_AHRS::update_trim_rotation_matrices()
|
|||
_rotation_vehicle_body_to_autopilot_body = _rotation_autopilot_body_to_vehicle_body.transposed();
|
||||
}
|
||||
|
||||
// return the smoothed gyro vector corrected for drift
|
||||
const Vector3f &AP_AHRS::get_gyro(void) const
|
||||
// return a Quaternion representing our current attitude in NED frame
|
||||
void AP_AHRS::get_quat_body_to_ned(Quaternion &quat) const
|
||||
{
|
||||
return _gyro_estimate;
|
||||
quat.from_rotation_matrix(get_rotation_body_to_ned());
|
||||
}
|
||||
|
||||
const Matrix3f &AP_AHRS::get_rotation_body_to_ned(void) const
|
||||
// convert a vector from body to earth frame
|
||||
Vector3f AP_AHRS::body_to_earth(const Vector3f &v) const
|
||||
{
|
||||
return _dcm_matrix;
|
||||
return get_rotation_body_to_ned() * v;
|
||||
}
|
||||
|
||||
const Vector3f &AP_AHRS::get_gyro_drift(void) const
|
||||
// convert a vector from earth to body frame
|
||||
Vector3f AP_AHRS::earth_to_body(const Vector3f &v) const
|
||||
{
|
||||
return _gyro_drift;
|
||||
return get_rotation_body_to_ned().mul_transpose(v);
|
||||
}
|
||||
|
||||
|
||||
// reset the current gyro drift estimate
|
||||
// should be called if gyro offsets are recalculated
|
||||
void AP_AHRS::reset_gyro_drift(void)
|
||||
|
|
@ -312,6 +315,32 @@ void AP_AHRS::reset_gyro_drift(void)
|
|||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
update state structure after each update()
|
||||
*/
|
||||
void AP_AHRS::update_state(void)
|
||||
{
|
||||
state.primary_IMU = _get_primary_IMU_index();
|
||||
state.primary_gyro = _get_primary_gyro_index();
|
||||
state.primary_accel = _get_primary_accel_index();
|
||||
state.primary_core = _get_primary_core_index();
|
||||
state.wind_estimate_ok = _wind_estimate(state.wind_estimate);
|
||||
state.EAS2TAS = dcm.get_EAS2TAS();
|
||||
state.airspeed_ok = _airspeed_estimate(state.airspeed);
|
||||
state.airspeed_true_ok = _airspeed_estimate_true(state.airspeed_true);
|
||||
state.airspeed_vec_ok = _airspeed_vector_true(state.airspeed_vec);
|
||||
state.quat_ok = _get_quaternion(state.quat);
|
||||
state.secondary_attitude_ok = _get_secondary_attitude(state.secondary_attitude);
|
||||
state.secondary_quat_ok = _get_secondary_quaternion(state.secondary_quat);
|
||||
state.location_ok = _get_location(state.location);
|
||||
state.secondary_pos_ok = _get_secondary_position(state.secondary_pos);
|
||||
state.ground_speed_vec = _groundspeed_vector();
|
||||
state.ground_speed = _groundspeed();
|
||||
_getCorrectedDeltaVelocityNED(state.corrected_dv, state.corrected_dv_dt);
|
||||
state.origin_ok = _get_origin(state.origin);
|
||||
state.velocity_NED_ok = _get_velocity_NED(state.velocity_NED);
|
||||
}
|
||||
|
||||
void AP_AHRS::update(bool skip_ins_update)
|
||||
{
|
||||
// periodically checks to see if we should update the AHRS
|
||||
|
|
@ -393,11 +422,11 @@ void AP_AHRS::update(bool skip_ins_update)
|
|||
update_AOA_SSA();
|
||||
|
||||
#if HAL_GCS_ENABLED
|
||||
EKFType active = active_EKF_type();
|
||||
if (active != last_active_ekf_type) {
|
||||
last_active_ekf_type = active;
|
||||
state.active_EKF = _active_EKF_type();
|
||||
if (state.active_EKF != last_active_ekf_type) {
|
||||
last_active_ekf_type = state.active_EKF;
|
||||
const char *shortname = "???";
|
||||
switch ((EKFType)active) {
|
||||
switch ((EKFType)state.active_EKF) {
|
||||
case EKFType::NONE:
|
||||
shortname = "DCM";
|
||||
break;
|
||||
|
|
@ -426,6 +455,9 @@ void AP_AHRS::update(bool skip_ins_update)
|
|||
}
|
||||
#endif // HAL_GCS_ENABLED
|
||||
|
||||
// update published state
|
||||
update_state();
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||
/*
|
||||
add timing jitter to simulate slow EKF response
|
||||
|
|
@ -448,13 +480,13 @@ void AP_AHRS::copy_estimates_from_backend_estimates(const AP_AHRS_Backend::Estim
|
|||
pitch = results.pitch_rad;
|
||||
yaw = results.yaw_rad;
|
||||
|
||||
_dcm_matrix = results.dcm_matrix;
|
||||
state.dcm_matrix = results.dcm_matrix;
|
||||
|
||||
_gyro_estimate = results.gyro_estimate;
|
||||
_gyro_drift = results.gyro_drift;
|
||||
state.gyro_estimate = results.gyro_estimate;
|
||||
state.gyro_drift = results.gyro_drift;
|
||||
|
||||
_accel_ef = results.accel_ef;
|
||||
_accel_bias = results.accel_bias;
|
||||
state.accel_ef = results.accel_ef;
|
||||
state.accel_bias = results.accel_bias;
|
||||
|
||||
update_cd_values();
|
||||
update_trig();
|
||||
|
|
@ -481,7 +513,7 @@ void AP_AHRS::update_SITL(void)
|
|||
sim.update();
|
||||
sim.get_results(sim_estimates);
|
||||
|
||||
if (active_EKF_type() == EKFType::SIM) {
|
||||
if (_active_EKF_type() == EKFType::SIM) {
|
||||
copy_estimates_from_backend_estimates(sim_estimates);
|
||||
}
|
||||
}
|
||||
|
|
@ -518,9 +550,9 @@ void AP_AHRS::update_EKF2(void)
|
|||
}
|
||||
if (_ekf2_started) {
|
||||
EKF2.UpdateFilter();
|
||||
if (active_EKF_type() == EKFType::TWO) {
|
||||
if (_active_EKF_type() == EKFType::TWO) {
|
||||
Vector3f eulers;
|
||||
EKF2.getRotationBodyToNED(_dcm_matrix);
|
||||
EKF2.getRotationBodyToNED(state.dcm_matrix);
|
||||
EKF2.getEulerAngles(eulers);
|
||||
roll = eulers.x;
|
||||
pitch = eulers.y;
|
||||
|
|
@ -537,21 +569,21 @@ void AP_AHRS::update_EKF2(void)
|
|||
|
||||
// get gyro bias for primary EKF and change sign to give gyro drift
|
||||
// Note sign convention used by EKF is bias = measurement - truth
|
||||
_gyro_drift.zero();
|
||||
EKF2.getGyroBias(_gyro_drift);
|
||||
_gyro_drift = -_gyro_drift;
|
||||
Vector3f drift;
|
||||
EKF2.getGyroBias(drift);
|
||||
state.gyro_drift = -drift;
|
||||
|
||||
// use the same IMU as the primary EKF and correct for gyro drift
|
||||
_gyro_estimate = _ins.get_gyro(primary_gyro) + _gyro_drift;
|
||||
state.gyro_estimate = _ins.get_gyro(primary_gyro) + state.gyro_drift;
|
||||
|
||||
// get z accel bias estimate from active EKF (this is usually for the primary IMU)
|
||||
float &abias = _accel_bias.z;
|
||||
float &abias = state.accel_bias.z;
|
||||
EKF2.getAccelZBias(abias);
|
||||
|
||||
// This EKF is currently using primary_imu, and abias applies to only that IMU
|
||||
Vector3f accel = _ins.get_accel(primary_accel);
|
||||
accel.z -= abias;
|
||||
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
|
||||
state.accel_ef = state.dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
|
||||
|
||||
nav_filter_status filt_state;
|
||||
EKF2.getFilterStatus(filt_state);
|
||||
|
|
@ -584,9 +616,9 @@ void AP_AHRS::update_EKF3(void)
|
|||
}
|
||||
if (_ekf3_started) {
|
||||
EKF3.UpdateFilter();
|
||||
if (active_EKF_type() == EKFType::THREE) {
|
||||
if (_active_EKF_type() == EKFType::THREE) {
|
||||
Vector3f eulers;
|
||||
EKF3.getRotationBodyToNED(_dcm_matrix);
|
||||
EKF3.getRotationBodyToNED(state.dcm_matrix);
|
||||
EKF3.getEulerAngles(eulers);
|
||||
roll = eulers.x;
|
||||
pitch = eulers.y;
|
||||
|
|
@ -604,21 +636,21 @@ void AP_AHRS::update_EKF3(void)
|
|||
|
||||
// get gyro bias for primary EKF and change sign to give gyro drift
|
||||
// Note sign convention used by EKF is bias = measurement - truth
|
||||
_gyro_drift.zero();
|
||||
EKF3.getGyroBias(-1,_gyro_drift);
|
||||
_gyro_drift = -_gyro_drift;
|
||||
Vector3f drift;
|
||||
EKF3.getGyroBias(-1, drift);
|
||||
state.gyro_drift = -drift;
|
||||
|
||||
// use the same IMU as the primary EKF and correct for gyro drift
|
||||
_gyro_estimate = _ins.get_gyro(primary_gyro) + _gyro_drift;
|
||||
state.gyro_estimate = _ins.get_gyro(primary_gyro) + state.gyro_drift;
|
||||
|
||||
// get 3-axis accel bias festimates for active EKF (this is usually for the primary IMU)
|
||||
Vector3f &abias = _accel_bias;
|
||||
Vector3f &abias = state.accel_bias;
|
||||
EKF3.getAccelBias(-1,abias);
|
||||
|
||||
// use the primary IMU for accel earth frame
|
||||
Vector3f accel = _ins.get_accel(primary_accel);
|
||||
accel -= abias;
|
||||
_accel_ef = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
|
||||
state.accel_ef = state.dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
|
||||
|
||||
nav_filter_status filt_state;
|
||||
EKF3.getFilterStatus(filt_state);
|
||||
|
|
@ -634,7 +666,7 @@ void AP_AHRS::update_external(void)
|
|||
external.update();
|
||||
external.get_results(external_estimates);
|
||||
|
||||
if (active_EKF_type() == EKFType::EXTERNAL) {
|
||||
if (_active_EKF_type() == EKFType::EXTERNAL) {
|
||||
copy_estimates_from_backend_estimates(external_estimates);
|
||||
}
|
||||
}
|
||||
|
|
@ -667,7 +699,7 @@ void AP_AHRS::reset()
|
|||
}
|
||||
|
||||
// dead-reckoning support
|
||||
bool AP_AHRS::get_location(Location &loc) const
|
||||
bool AP_AHRS::_get_location(Location &loc) const
|
||||
{
|
||||
switch (active_EKF_type()) {
|
||||
case EKFType::NONE:
|
||||
|
|
@ -720,7 +752,7 @@ float AP_AHRS::get_error_yaw(void) const
|
|||
}
|
||||
|
||||
// return a wind estimation vector, in m/s
|
||||
bool AP_AHRS::wind_estimate(Vector3f &wind) const
|
||||
bool AP_AHRS::_wind_estimate(Vector3f &wind) const
|
||||
{
|
||||
switch (active_EKF_type()) {
|
||||
case EKFType::NONE:
|
||||
|
|
@ -749,12 +781,6 @@ bool AP_AHRS::wind_estimate(Vector3f &wind) const
|
|||
}
|
||||
return false;
|
||||
}
|
||||
Vector3f AP_AHRS::wind_estimate(void) const
|
||||
{
|
||||
Vector3f ret;
|
||||
UNUSED_RESULT(wind_estimate(ret));
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
return true if a real airspeed sensor is enabled
|
||||
|
|
@ -780,7 +806,7 @@ bool AP_AHRS::airspeed_sensor_enabled(void) const
|
|||
|
||||
// return an airspeed estimate if available. return true
|
||||
// if we have an estimate
|
||||
bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
|
||||
bool AP_AHRS::_airspeed_estimate(float &airspeed_ret) const
|
||||
{
|
||||
bool ret = false;
|
||||
|
||||
|
|
@ -861,7 +887,7 @@ bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
|
|||
return ret;
|
||||
}
|
||||
|
||||
bool AP_AHRS::airspeed_estimate_true(float &airspeed_ret) const
|
||||
bool AP_AHRS::_airspeed_estimate_true(float &airspeed_ret) const
|
||||
{
|
||||
switch (active_EKF_type()) {
|
||||
case EKFType::NONE:
|
||||
|
|
@ -890,7 +916,7 @@ bool AP_AHRS::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 AP_AHRS::airspeed_vector_true(Vector3f &vec) const
|
||||
bool AP_AHRS::_airspeed_vector_true(Vector3f &vec) const
|
||||
{
|
||||
switch (active_EKF_type()) {
|
||||
case EKFType::NONE:
|
||||
|
|
@ -987,7 +1013,7 @@ bool AP_AHRS::use_compass(void)
|
|||
}
|
||||
|
||||
// return the quaternion defining the rotation from NED to XYZ (body) axes
|
||||
bool AP_AHRS::get_quaternion(Quaternion &quat) const
|
||||
bool AP_AHRS::_get_quaternion(Quaternion &quat) const
|
||||
{
|
||||
// backends always return in autopilot XYZ frame; rotate result
|
||||
// according to trim
|
||||
|
|
@ -1033,10 +1059,10 @@ bool AP_AHRS::get_quaternion(Quaternion &quat) const
|
|||
}
|
||||
|
||||
// return secondary attitude solution if available, as eulers in radians
|
||||
bool AP_AHRS::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)) {
|
||||
if (!_get_secondary_EKF_type(secondary_ekf_type)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -1086,10 +1112,10 @@ bool AP_AHRS::get_secondary_attitude(Vector3f &eulers) const
|
|||
|
||||
|
||||
// return secondary attitude solution if available, as quaternion
|
||||
bool AP_AHRS::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)) {
|
||||
if (!_get_secondary_EKF_type(secondary_ekf_type)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -1141,10 +1167,10 @@ bool AP_AHRS::get_secondary_quaternion(Quaternion &quat) const
|
|||
}
|
||||
|
||||
// return secondary position solution if available
|
||||
bool AP_AHRS::get_secondary_position(Location &loc) const
|
||||
bool AP_AHRS::_get_secondary_position(Location &loc) const
|
||||
{
|
||||
EKFType secondary_ekf_type;
|
||||
if (!get_secondary_EKF_type(secondary_ekf_type)) {
|
||||
if (!_get_secondary_EKF_type(secondary_ekf_type)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
|
|
@ -1187,7 +1213,7 @@ bool AP_AHRS::get_secondary_position(Location &loc) const
|
|||
}
|
||||
|
||||
// EKF has a better ground speed vector estimate
|
||||
Vector2f AP_AHRS::groundspeed_vector(void)
|
||||
Vector2f AP_AHRS::_groundspeed_vector(void)
|
||||
{
|
||||
Vector3f vec;
|
||||
|
||||
|
|
@ -1220,7 +1246,7 @@ Vector2f AP_AHRS::groundspeed_vector(void)
|
|||
return dcm.groundspeed_vector();
|
||||
}
|
||||
|
||||
float AP_AHRS::groundspeed(void)
|
||||
float AP_AHRS::_groundspeed(void)
|
||||
{
|
||||
switch (active_EKF_type()) {
|
||||
case EKFType::NONE:
|
||||
|
|
@ -1307,7 +1333,7 @@ bool AP_AHRS::have_inertial_nav(void) const
|
|||
|
||||
// return a ground velocity in meters/second, North/East/Down
|
||||
// order. Must only be called if have_inertial_nav() is true
|
||||
bool AP_AHRS::get_velocity_NED(Vector3f &vec) const
|
||||
bool AP_AHRS::_get_velocity_NED(Vector3f &vec) const
|
||||
{
|
||||
switch (active_EKF_type()) {
|
||||
case EKFType::NONE:
|
||||
|
|
@ -1637,7 +1663,7 @@ void AP_AHRS::get_relative_position_D_home(float &posD) const
|
|||
Location originLLH;
|
||||
float originD;
|
||||
if (!get_relative_position_D_origin(originD) ||
|
||||
!get_origin(originLLH)) {
|
||||
!_get_origin(originLLH)) {
|
||||
const auto &gps = AP::gps();
|
||||
if (_gps_use == GPSUse::EnableWithHeight &&
|
||||
gps.status() >= AP_GPS::GPS_OK_FIX_3D) {
|
||||
|
|
@ -1697,7 +1723,7 @@ AP_AHRS::EKFType AP_AHRS::ekf_type(void) const
|
|||
#endif
|
||||
}
|
||||
|
||||
AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const
|
||||
AP_AHRS::EKFType AP_AHRS::_active_EKF_type(void) const
|
||||
{
|
||||
EKFType ret = EKFType::NONE;
|
||||
|
||||
|
|
@ -1837,7 +1863,7 @@ AP_AHRS::EKFType AP_AHRS::active_EKF_type(void) const
|
|||
}
|
||||
|
||||
// get secondary EKF type. returns false if no secondary (i.e. only using DCM)
|
||||
bool AP_AHRS::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()) {
|
||||
|
|
@ -2216,7 +2242,7 @@ bool AP_AHRS::getMagOffsets(uint8_t mag_idx, Vector3f &magOffsets) const
|
|||
}
|
||||
|
||||
// Retrieves the NED delta velocity corrected
|
||||
void AP_AHRS::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
|
||||
void AP_AHRS::_getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
|
||||
{
|
||||
int8_t imu_idx = -1;
|
||||
Vector3f accel_bias;
|
||||
|
|
@ -2251,7 +2277,7 @@ void AP_AHRS::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
|
|||
ret.zero();
|
||||
AP::ins().get_delta_velocity((uint8_t)imu_idx, ret, dt);
|
||||
ret -= accel_bias*dt;
|
||||
ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret;
|
||||
ret = state.dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret;
|
||||
ret.z += GRAVITY_MSS*dt;
|
||||
}
|
||||
|
||||
|
|
@ -2565,7 +2591,7 @@ void AP_AHRS::send_ekf_status_report(GCS_MAVLINK &link) const
|
|||
}
|
||||
|
||||
// return origin for a specified EKF type
|
||||
bool AP_AHRS::get_origin(EKFType type, Location &ret) const
|
||||
bool AP_AHRS::_get_origin(EKFType type, Location &ret) const
|
||||
{
|
||||
switch (type) {
|
||||
case EKFType::NONE:
|
||||
|
|
@ -2601,12 +2627,12 @@ bool AP_AHRS::get_origin(EKFType type, Location &ret) const
|
|||
This copes with users force arming a plane that is running on DCM as
|
||||
the EKF has not fully initialised
|
||||
*/
|
||||
bool AP_AHRS::get_origin(Location &ret) const
|
||||
bool AP_AHRS::_get_origin(Location &ret) const
|
||||
{
|
||||
if (get_origin(ekf_type(), ret)) {
|
||||
if (_get_origin(ekf_type(), ret)) {
|
||||
return true;
|
||||
}
|
||||
if (hal.util->get_soft_armed() && get_origin(active_EKF_type(), ret)) {
|
||||
if (hal.util->get_soft_armed() && _get_origin(active_EKF_type(), ret)) {
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
|
|
@ -2838,7 +2864,7 @@ uint8_t AP_AHRS::get_active_airspeed_index() const
|
|||
}
|
||||
|
||||
// get the index of the current primary IMU
|
||||
uint8_t AP_AHRS::get_primary_IMU_index() const
|
||||
uint8_t AP_AHRS::_get_primary_IMU_index() const
|
||||
{
|
||||
int8_t imu = -1;
|
||||
switch (active_EKF_type()) {
|
||||
|
|
@ -2873,7 +2899,7 @@ uint8_t AP_AHRS::get_primary_IMU_index() const
|
|||
}
|
||||
|
||||
// return the index of the primary core or -1 if no primary core selected
|
||||
int8_t AP_AHRS::get_primary_core_index() const
|
||||
int8_t AP_AHRS::_get_primary_core_index() const
|
||||
{
|
||||
switch (active_EKF_type()) {
|
||||
case EKFType::NONE:
|
||||
|
|
@ -2903,7 +2929,7 @@ int8_t AP_AHRS::get_primary_core_index() const
|
|||
}
|
||||
|
||||
// get the index of the current primary accelerometer sensor
|
||||
uint8_t AP_AHRS::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();
|
||||
|
|
@ -2912,7 +2938,7 @@ uint8_t AP_AHRS::get_primary_accel_index(void) const
|
|||
}
|
||||
|
||||
// get the index of the current primary gyro sensor
|
||||
uint8_t AP_AHRS::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();
|
||||
|
|
@ -3103,6 +3129,66 @@ const EKFGSF_yaw *AP_AHRS::get_yaw_estimator(void) const
|
|||
return nullptr;
|
||||
}
|
||||
|
||||
// get current location estimate
|
||||
bool AP_AHRS::get_location(Location &loc) const
|
||||
{
|
||||
loc = state.location;
|
||||
return state.location_ok;
|
||||
}
|
||||
|
||||
// return a wind estimation vector, in m/s; returns 0,0,0 on failure
|
||||
bool AP_AHRS::wind_estimate(Vector3f &wind) const
|
||||
{
|
||||
wind = state.wind_estimate;
|
||||
return state.wind_estimate_ok;
|
||||
}
|
||||
|
||||
// return an airspeed estimate if available. return true
|
||||
// if we have an estimate
|
||||
bool AP_AHRS::airspeed_estimate(float &airspeed_ret) const
|
||||
{
|
||||
airspeed_ret = state.airspeed;
|
||||
return state.airspeed_ok;
|
||||
}
|
||||
|
||||
// return a true airspeed estimate (navigation airspeed) if
|
||||
// available. return true if we have an estimate
|
||||
bool AP_AHRS::airspeed_estimate_true(float &airspeed_ret) const
|
||||
{
|
||||
airspeed_ret = state.airspeed_true;
|
||||
return state.airspeed_true_ok;
|
||||
}
|
||||
|
||||
// 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
|
||||
{
|
||||
vec = state.airspeed_vec;
|
||||
return state.airspeed_vec_ok;
|
||||
}
|
||||
|
||||
// return the quaternion defining the rotation from NED to XYZ (body) axes
|
||||
bool AP_AHRS::get_quaternion(Quaternion &quat) const
|
||||
{
|
||||
quat = state.quat;
|
||||
return state.quat_ok;
|
||||
}
|
||||
|
||||
// returns the inertial navigation origin in lat/lon/alt
|
||||
bool AP_AHRS::get_origin(Location &ret) const
|
||||
{
|
||||
ret = state.origin;
|
||||
return state.origin_ok;
|
||||
}
|
||||
|
||||
// return a ground velocity in meters/second, North/East/Down
|
||||
// order. Must only be called if have_inertial_nav() is true
|
||||
bool AP_AHRS::get_velocity_NED(Vector3f &vec) const
|
||||
{
|
||||
vec = state.velocity_NED;
|
||||
return state.velocity_NED_ok;
|
||||
}
|
||||
|
||||
// singleton instance
|
||||
AP_AHRS *AP_AHRS::_singleton;
|
||||
|
||||
|
|
|
|||
|
|
@ -76,10 +76,10 @@ public:
|
|||
}
|
||||
|
||||
// return the smoothed gyro vector corrected for drift
|
||||
const Vector3f &get_gyro(void) const;
|
||||
const Vector3f &get_gyro(void) const { return state.gyro_estimate; }
|
||||
|
||||
// return the current drift correction integrator value
|
||||
const Vector3f &get_gyro_drift(void) const;
|
||||
const Vector3f &get_gyro_drift(void) const { return state.gyro_drift; }
|
||||
|
||||
// reset the current gyro drift estimate
|
||||
// should be called if gyro offsets are recalculated
|
||||
|
|
@ -88,7 +88,7 @@ public:
|
|||
void update(bool skip_ins_update=false);
|
||||
void reset();
|
||||
|
||||
// dead-reckoning support
|
||||
// get current location estimate
|
||||
bool get_location(Location &loc) const;
|
||||
|
||||
// get latest altitude estimate above ground level in meters and validity flag
|
||||
|
|
@ -109,7 +109,10 @@ public:
|
|||
bool get_wind_estimation_enabled() const { return wind_estimation_enabled; }
|
||||
|
||||
// return a wind estimation vector, in m/s; returns 0,0,0 on failure
|
||||
Vector3f wind_estimate() const;
|
||||
const Vector3f &wind_estimate() const { return state.wind_estimate; }
|
||||
|
||||
// return a wind estimation vector, in m/s; returns 0,0,0 on failure
|
||||
bool wind_estimate(Vector3f &wind) const;
|
||||
|
||||
// instruct DCM to update its wind estimate:
|
||||
void estimate_wind() { dcm.estimate_wind(); }
|
||||
|
|
@ -125,13 +128,13 @@ public:
|
|||
|
||||
// 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 state.EAS2TAS;
|
||||
}
|
||||
|
||||
// return an airspeed estimate if available. return true
|
||||
// if we have an estimate
|
||||
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;
|
||||
|
|
@ -168,26 +171,38 @@ public:
|
|||
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;
|
||||
bool get_secondary_attitude(Vector3f &eulers) const {
|
||||
eulers = state.secondary_attitude;
|
||||
return state.secondary_attitude_ok;
|
||||
}
|
||||
|
||||
// return secondary attitude solution if available, as quaternion
|
||||
bool get_secondary_quaternion(Quaternion &quat) const;
|
||||
bool get_secondary_quaternion(Quaternion &quat) const {
|
||||
quat = state.secondary_quat;
|
||||
return state.secondary_quat_ok;
|
||||
}
|
||||
|
||||
// return secondary position solution if available
|
||||
bool get_secondary_position(Location &loc) const;
|
||||
bool get_secondary_position(Location &loc) const {
|
||||
loc = state.secondary_pos;
|
||||
return state.secondary_pos_ok;
|
||||
}
|
||||
|
||||
// EKF has a better ground speed vector estimate
|
||||
Vector2f groundspeed_vector();
|
||||
const Vector2f &groundspeed_vector() const { return state.ground_speed_vec; }
|
||||
|
||||
// return ground speed estimate in meters/second. Used by ground vehicles.
|
||||
float groundspeed(void);
|
||||
float groundspeed(void) const { return state.ground_speed; }
|
||||
|
||||
const Vector3f &get_accel_ef() const {
|
||||
return _accel_ef;
|
||||
return state.accel_ef;
|
||||
}
|
||||
|
||||
// Retrieves the corrected NED delta velocity in use by the inertial navigation
|
||||
void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const;
|
||||
void getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const {
|
||||
ret = state.corrected_dv;
|
||||
dt = state.corrected_dv_dt;
|
||||
}
|
||||
|
||||
// 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)
|
||||
|
|
@ -343,13 +358,13 @@ 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;
|
||||
int8_t get_primary_core_index() const { return state.primary_core; }
|
||||
|
||||
// get the index of the current primary accelerometer sensor
|
||||
uint8_t get_primary_accel_index(void) const;
|
||||
uint8_t get_primary_accel_index(void) const { return state.primary_accel; }
|
||||
|
||||
// get the index of the current primary gyro sensor
|
||||
uint8_t get_primary_gyro_index(void) const;
|
||||
uint8_t get_primary_gyro_index(void) const { return state.primary_gyro; }
|
||||
|
||||
// see if EKF lane switching is possible to avoid EKF failsafe
|
||||
void check_lane_switch(void);
|
||||
|
|
@ -507,12 +522,10 @@ public:
|
|||
int32_t pitch_sensor;
|
||||
int32_t yaw_sensor;
|
||||
|
||||
const Matrix3f &get_rotation_body_to_ned(void) const;
|
||||
const Matrix3f &get_rotation_body_to_ned(void) const { return state.dcm_matrix; }
|
||||
|
||||
// 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());
|
||||
}
|
||||
void get_quat_body_to_ned(Quaternion &quat) const;
|
||||
|
||||
// get rotation matrix specifically from DCM backend (used for
|
||||
// compass calibrator)
|
||||
|
|
@ -529,14 +542,10 @@ public:
|
|||
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 get_rotation_body_to_ned() * v;
|
||||
}
|
||||
Vector3f body_to_earth(const Vector3f &v) const;
|
||||
|
||||
// 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);
|
||||
}
|
||||
Vector3f earth_to_body(const Vector3f &v) const;
|
||||
|
||||
/*
|
||||
* methods for the benefit of LUA bindings
|
||||
|
|
@ -553,7 +562,7 @@ public:
|
|||
// get_accel() vector to get best current body frame accel
|
||||
// estimate
|
||||
const Vector3f &get_accel_bias(void) const {
|
||||
return _accel_bias;
|
||||
return state.accel_bias;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -660,11 +669,7 @@ private:
|
|||
EXTERNAL = 11,
|
||||
#endif
|
||||
};
|
||||
EKFType active_EKF_type(void) const;
|
||||
|
||||
// if successful returns true and sets secondary_ekf_type to None (for DCM), EKF3 or EKF3
|
||||
// returns false if no secondary (i.e. only using DCM)
|
||||
bool get_secondary_EKF_type(EKFType &secondary_ekf_type) const;
|
||||
EKFType active_EKF_type(void) const { return state.active_EKF; }
|
||||
|
||||
bool always_use_EKF() const {
|
||||
return _ekf_flags & FLAG_ALWAYS_USE_EKF;
|
||||
|
|
@ -690,9 +695,6 @@ private:
|
|||
// update roll_sensor, pitch_sensor and yaw_sensor
|
||||
void update_cd_values(void);
|
||||
|
||||
// return origin for a specified EKF type
|
||||
bool get_origin(EKFType type, Location &ret) const;
|
||||
|
||||
// helper trig variables
|
||||
float _cos_roll{1.0f};
|
||||
float _cos_pitch{1.0f};
|
||||
|
|
@ -711,12 +713,7 @@ private:
|
|||
#endif
|
||||
|
||||
// rotation from vehicle body to NED frame
|
||||
Matrix3f _dcm_matrix;
|
||||
|
||||
Vector3f _gyro_drift;
|
||||
Vector3f _gyro_estimate;
|
||||
Vector3f _accel_ef;
|
||||
Vector3f _accel_bias;
|
||||
|
||||
const uint16_t startup_delay_ms = 1000;
|
||||
uint32_t start_time_ms;
|
||||
|
|
@ -726,7 +723,7 @@ private:
|
|||
void update_DCM();
|
||||
|
||||
// get the index of the current primary IMU
|
||||
uint8_t get_primary_IMU_index(void) const;
|
||||
uint8_t get_primary_IMU_index(void) const { return state.primary_IMU; }
|
||||
|
||||
/*
|
||||
* home-related state
|
||||
|
|
@ -799,9 +796,6 @@ private:
|
|||
*/
|
||||
bool wind_estimation_enabled;
|
||||
|
||||
// return a wind estimation vector, in m/s
|
||||
bool wind_estimate(Vector3f &wind) const WARN_IF_UNUSED;
|
||||
|
||||
/*
|
||||
* fly_forward is set by the vehicles to indicate the vehicle
|
||||
* should generally be moving in the direction of its heading.
|
||||
|
|
@ -843,6 +837,121 @@ private:
|
|||
void Write_AHRS2(void) const;
|
||||
// write POS (canonical vehicle position) message out:
|
||||
void Write_POS(void) const;
|
||||
|
||||
// return an airspeed estimate if available. return true
|
||||
// if we have an estimate
|
||||
bool _airspeed_estimate(float &airspeed_ret) const;
|
||||
|
||||
// return secondary attitude solution if available, as eulers in radians
|
||||
bool _get_secondary_attitude(Vector3f &eulers) const;
|
||||
|
||||
// return secondary attitude solution if available, as quaternion
|
||||
bool _get_secondary_quaternion(Quaternion &quat) const;
|
||||
|
||||
// get ground speed 2D
|
||||
Vector2f _groundspeed_vector(void);
|
||||
|
||||
// get active EKF type
|
||||
EKFType _active_EKF_type(void) const;
|
||||
|
||||
// return a wind estimation vector, in m/s
|
||||
bool _wind_estimate(Vector3f &wind) const WARN_IF_UNUSED;
|
||||
|
||||
// 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;
|
||||
|
||||
// return the quaternion defining the rotation from NED to XYZ (body) axes
|
||||
bool _get_quaternion(Quaternion &quat) const WARN_IF_UNUSED;
|
||||
|
||||
// return secondary position solution if available
|
||||
bool _get_secondary_position(Location &loc) const;
|
||||
|
||||
// return ground speed estimate in meters/second. Used by ground vehicles.
|
||||
float _groundspeed(void);
|
||||
|
||||
// Retrieves the corrected NED delta velocity in use by the inertial navigation
|
||||
void _getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const;
|
||||
|
||||
// returns the inertial navigation origin in lat/lon/alt
|
||||
bool _get_origin(Location &ret) const WARN_IF_UNUSED;
|
||||
|
||||
// return origin for a specified EKF type
|
||||
bool _get_origin(EKFType type, Location &ret) const;
|
||||
|
||||
// return a ground velocity in meters/second, North/East/Down
|
||||
// order. Must only be called if have_inertial_nav() is true
|
||||
bool _get_velocity_NED(Vector3f &vec) const WARN_IF_UNUSED;
|
||||
|
||||
// get secondary EKF type. returns false if no secondary (i.e. only using DCM)
|
||||
bool _get_secondary_EKF_type(EKFType &secondary_ekf_type) const;
|
||||
|
||||
// return the index of the primary core or -1 if no primary core selected
|
||||
int8_t _get_primary_core_index() const;
|
||||
|
||||
// get the index of the current primary accelerometer sensor
|
||||
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;
|
||||
|
||||
// get the index of the current primary IMU
|
||||
uint8_t _get_primary_IMU_index(void) const;
|
||||
|
||||
// get current location estimate
|
||||
bool _get_location(Location &loc) const;
|
||||
|
||||
/*
|
||||
update state structure
|
||||
*/
|
||||
void update_state(void);
|
||||
|
||||
/*
|
||||
state updated at the end of each update() call
|
||||
*/
|
||||
struct {
|
||||
EKFType active_EKF;
|
||||
uint8_t primary_IMU;
|
||||
uint8_t primary_gyro;
|
||||
uint8_t primary_accel;
|
||||
uint8_t primary_core;
|
||||
Vector3f gyro_estimate;
|
||||
Matrix3f dcm_matrix;
|
||||
Vector3f gyro_drift;
|
||||
Vector3f accel_ef;
|
||||
Vector3f accel_bias;
|
||||
Vector3f wind_estimate;
|
||||
bool wind_estimate_ok;
|
||||
float EAS2TAS;
|
||||
bool airspeed_ok;
|
||||
float airspeed;
|
||||
bool airspeed_true_ok;
|
||||
float airspeed_true;
|
||||
Vector3f airspeed_vec;
|
||||
bool airspeed_vec_ok;
|
||||
Quaternion quat;
|
||||
bool quat_ok;
|
||||
Vector3f secondary_attitude;
|
||||
bool secondary_attitude_ok;
|
||||
Quaternion secondary_quat;
|
||||
bool secondary_quat_ok;
|
||||
Location location;
|
||||
bool location_ok;
|
||||
Location secondary_pos;
|
||||
bool secondary_pos_ok;
|
||||
Vector2f ground_speed_vec;
|
||||
float ground_speed;
|
||||
Vector3f corrected_dv;
|
||||
float corrected_dv_dt;
|
||||
Location origin;
|
||||
bool origin_ok;
|
||||
Vector3f velocity_NED;
|
||||
bool velocity_NED_ok;
|
||||
} state;
|
||||
};
|
||||
|
||||
namespace AP {
|
||||
|
|
|
|||
|
|
@ -106,9 +106,9 @@ void AP_AHRS::write_video_stabilisation() const
|
|||
const struct log_Video_Stabilisation pkt {
|
||||
LOG_PACKET_HEADER_INIT(LOG_VIDEO_STABILISATION_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
gyro_x : _gyro_estimate.x,
|
||||
gyro_y : _gyro_estimate.y,
|
||||
gyro_z : _gyro_estimate.z,
|
||||
gyro_x : state.gyro_estimate.x,
|
||||
gyro_y : state.gyro_estimate.y,
|
||||
gyro_z : state.gyro_estimate.z,
|
||||
accel_x : accel.x,
|
||||
accel_y : accel.y,
|
||||
accel_z : accel.z,
|
||||
|
|
|
|||
Loading…
Reference in New Issue