// returns the expected NED magnetic field
bool AP_AHRS_NavEKF::get_mag_field_NED(Vector3f &vec) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE2:
default:
EKF2.getMagNED(-1,vec);
return true;
case EKF_TYPE3:
EKF3.getMagNED(-1,vec);
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return false;
#endif
}
}
// dead-reckoning support
bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
{
Vector3f ned_pos;
Location origin;
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return AP_AHRS_DCM::get_position(loc);
case EKF_TYPE2:
if (EKF2.getLLH(loc) && EKF2.getPosD(-1,ned_pos.z) && EKF2.getOriginLLH(-1,origin)) {
// fixup altitude using relative position from EKF origin
loc.alt = origin.alt - ned_pos.z*100;
return true;
}
break;
case EKF_TYPE3:
if (EKF3.getLLH(loc) && EKF3.getPosD(-1,ned_pos.z) && EKF3.getOriginLLH(-1,origin)) {
// fixup altitude using relative position from EKF origin
loc.alt = origin.alt - ned_pos.z*100;
return true;
}
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL: {
const struct SITL::sitl_fdm &fdm = _sitl->state;
memset(&loc, 0, sizeof(loc));
loc.lat = fdm.latitude * 1e7;
loc.lng = fdm.longitude * 1e7;
loc.alt = fdm.altitude*100;
return true;
}
#endif
default:
break;
}
return AP_AHRS_DCM::get_position(loc);
}
// get latest height above ground level estimate in metres and a validity flag
bool AP_AHRS_NavEKF::get_hagl(float &height) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE2:
default:
return EKF2.getHAGL(height);
case EKF_TYPE3:
return EKF3.getHAGL(height);
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL: {
const struct SITL::sitl_fdm &fdm = _sitl->state;
height = fdm.altitude - get_home().alt*0.01f;
return true;
}
#endif
}
}
// send a EKF_STATUS_REPORT for current EKF
void AP_AHRS_NavEKF::send_ekf_status_report(mavlink_channel_t chan)
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
// send zero status report
mavlink_msg_ekf_status_report_send(chan, 0, 0, 0, 0, 0, 0);
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
// send zero status report
mavlink_msg_ekf_status_report_send(chan, 0, 0, 0, 0, 0, 0);
break;
#endif
case EKF_TYPE2:
return EKF2.send_status_report(chan);
case EKF_TYPE3:
return EKF3.send_status_report(chan);
}
}
// 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
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE2:
default:
EKF2.getVelNED(-1,vec);
return true;
case EKF_TYPE3:
EKF3.getVelNED(-1,vec);
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
const struct SITL::sitl_fdm &fdm = _sitl->state;
vec = Vector3f(fdm.speedN, fdm.speedE, fdm.speedD);
return true;
#endif
}
}
// returns the estimated magnetic field offsets in body frame
bool AP_AHRS_NavEKF::get_mag_field_correction(Vector3f &vec) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1:
EKF1.getMagXYZ(vec);
return true;
#endif
case EKF_TYPE2:
default:
EKF2.getMagXYZ(-1,vec);
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
return false;
#endif
}
}
// return secondary attitude solution if available, as eulers in radians
bool AP_AHRS_NavEKF::get_secondary_attitude(Vector3f &eulers)
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
// EKF is secondary
#if AP_AHRS_WITH_EKF1
EKF1.getEulerAngles(eulers);
return ekf1_started;
#else
EKF2.getEulerAngles(-1, eulers);
return ekf2_started;
#endif
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1:
#endif
case EKF_TYPE2:
default:
// DCM is secondary
eulers = _dcm_attitude;
return true;
}
}
// return secondary position solution if available
bool AP_AHRS_NavEKF::get_secondary_position(struct Location &loc)
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
// EKF is secondary
#if AP_AHRS_WITH_EKF1
EKF1.getLLH(loc);
return ekf1_started;
#else
EKF2.getLLH(loc);
return ekf2_started;
#endif
#if AP_AHRS_WITH_EKF1
case EKF_TYPE1:
#endif
case EKF_TYPE2:
default:
// return DCM position
AP_AHRS_DCM::get_position(loc);
return true;
}
}
// dead-reckoning support
bool AP_AHRS_NavEKF::get_position(struct Location &loc) const
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return AP_AHRS_DCM::get_position(loc);
case EKF_TYPE2:
if (EKF2.getLLH(loc)) {
return true;
}
break;
case EKF_TYPE3:
if (EKF3.getLLH(loc)) {
return true;
}
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL: {
if (_sitl) {
const struct SITL::sitl_fdm &fdm = _sitl->state;
loc = {};
loc.lat = fdm.latitude * 1e7;
loc.lng = fdm.longitude * 1e7;
loc.alt = fdm.altitude*100;
return true;
}
break;
}
#endif
default:
break;
}
return AP_AHRS_DCM::get_position(loc);
}
// 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
{
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
return false;
case EKF_TYPE2:
default:
velocity = EKF2.getPosDownDerivative(-1);
return true;
case EKF_TYPE3:
velocity = EKF3.getPosDownDerivative(-1);
return true;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL: {
const struct SITL::sitl_fdm &fdm = _sitl->state;
velocity = fdm.speedD;
return true;
}
#endif
}
}
// return a wind estimation vector, in m/s
Vector3f AP_AHRS_NavEKF::wind_estimate(void)
{
Vector3f wind;
switch (active_EKF_type()) {
case EKF_TYPE_NONE:
wind = AP_AHRS_DCM::wind_estimate();
break;
case EKF_TYPE1:
EKF1.getWind(wind);
break;
case EKF_TYPE2:
EKF2.getWind(-1,wind);
break;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
case EKF_TYPE_SITL:
wind.zero();
break;
#endif
}
return wind;
}
// return true if inertial navigation is active
bool AP_AHRS_NavEKF::have_inertial_nav(void) const
{
return active_EKF_type() != EKF_TYPE_NONE;
}
void AP_AHRS_NavEKF::update_EKF2(void)
{
if (!ekf2_started) {
// wait 1 second for DCM to output a valid tilt error estimate
if (start_time_ms == 0) {
start_time_ms = AP_HAL::millis();
}
if (AP_HAL::millis() - start_time_ms > startup_delay_ms || force_ekf) {
ekf2_started = EKF2.InitialiseFilter();
if (force_ekf) {
return;
}
}
}
if (ekf2_started) {
EKF2.UpdateFilter();
if (active_EKF_type() == EKF_TYPE2) {
Vector3f eulers;
EKF2.getRotationBodyToNED(_dcm_matrix);
EKF2.getEulerAngles(-1,eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
update_cd_values();
update_trig();
// keep _gyro_bias for get_gyro_drift()
EKF2.getGyroBias(-1,_gyro_bias);
_gyro_bias = -_gyro_bias;
// calculate corrected gryo estimate for get_gyro()
_gyro_estimate.zero();
uint8_t healthy_count = 0;
for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
if (_ins.get_gyro_health(i) && healthy_count < 2 && _ins.use_gyro(i)) {
_gyro_estimate += _ins.get_gyro(i);
healthy_count++;
}
}
if (healthy_count > 1) {
_gyro_estimate /= healthy_count;
}
_gyro_estimate += _gyro_bias;
float abias;
EKF2.getAccelZBias(-1,abias);
// This EKF uses the primary IMU
// Eventually we will run a separate instance of the EKF for each IMU and do the selection and blending of EKF outputs upstream
// update _accel_ef_ekf
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
Vector3f accel = _ins.get_accel(i);
if (i==_ins.get_primary_accel()) {
accel.z -= abias;
}
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * accel;
}
}
_accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()];
}
}
}
void AP_AHRS_NavEKF::update_EKF1(void)
{
#if AP_AHRS_WITH_EKF1
if (!ekf1_started) {
// wait 1 second for DCM to output a valid tilt error estimate
if (start_time_ms == 0) {
start_time_ms = AP_HAL::millis();
}
// slight extra delay on EKF1 to prioritise EKF2 for memory
if (AP_HAL::millis() - start_time_ms > startup_delay_ms + 100U || force_ekf) {
ekf1_started = EKF1.InitialiseFilterDynamic();
if (force_ekf) {
return;
}
}
}
if (ekf1_started) {
EKF1.UpdateFilter();
if (active_EKF_type() == EKF_TYPE1) {
Vector3f eulers;
EKF1.getRotationBodyToNED(_dcm_matrix);
EKF1.getEulerAngles(eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
update_cd_values();
update_trig();
// keep _gyro_bias for get_gyro_drift()
EKF1.getGyroBias(_gyro_bias);
_gyro_bias = -_gyro_bias;
// calculate corrected gryo estimate for get_gyro()
_gyro_estimate.zero();
uint8_t healthy_count = 0;
for (uint8_t i=0; i<_ins.get_gyro_count(); i++) {
if (_ins.get_gyro_health(i) && healthy_count < 2 && _ins.use_gyro(i)) {
_gyro_estimate += _ins.get_gyro(i);
healthy_count++;
}
}
if (healthy_count > 1) {
_gyro_estimate /= healthy_count;
}
_gyro_estimate += _gyro_bias;
float abias1, abias2;
EKF1.getAccelZBias(abias1, abias2);
// update _accel_ef_ekf
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
Vector3f accel = _ins.get_accel(i);
if (i==0) {
accel.z -= abias1;
} else if (i==1) {
accel.z -= abias2;
}
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * accel;
}
}
if(_ins.use_accel(0) && _ins.use_accel(1)) {
float IMU1_weighting;
EKF1.getIMU1Weighting(IMU1_weighting);
_accel_ef_ekf_blended = _accel_ef_ekf[0] * IMU1_weighting + _accel_ef_ekf[1] * (1.0f-IMU1_weighting);
} else {
_accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()];
}
}
}
#endif
}
void AP_AHRS_NavEKF::update_EKF3(void)
{
if (!_ekf3_started) {
// wait 1 second for DCM to output a valid tilt error estimate
if (start_time_ms == 0) {
start_time_ms = AP_HAL::millis();
}
if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf) {
_ekf3_started = EKF3.InitialiseFilter();
if (_force_ekf) {
return;
}
}
}
if (_ekf3_started) {
EKF3.UpdateFilter();
if (active_EKF_type() == EKF_TYPE3) {
Vector3f eulers;
EKF3.getRotationBodyToNED(_dcm_matrix);
EKF3.getEulerAngles(-1,eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
update_cd_values();
update_trig();
const AP_InertialSensor &_ins = AP::ins();
// Use the primary EKF to select the primary gyro
const int8_t primary_imu = EKF3.getPrimaryCoreIMUIndex();
// 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;
// calculate corrected gyro estimate for get_gyro()
_gyro_estimate.zero();
if (primary_imu == -1) {
// the primary IMU is undefined so use an uncorrected default value from the INS library
_gyro_estimate = _ins.get_gyro();
} else {
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins.get_gyro(primary_imu) + _gyro_drift;
}
// get 3-axis accel bias festimates for active EKF (this is usually for the primary IMU)
Vector3f abias;
EKF3.getAccelBias(-1,abias);
// This EKF uses the primary IMU
// Eventually we will run a separate instance of the EKF for each IMU and do the selection and blending of EKF outputs upstream
// update _accel_ef_ekf
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
Vector3f accel = _ins.get_accel(i);
if (i==_ins.get_primary_accel()) {
accel -= abias;
}
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * accel;
}
}
_accel_ef_ekf_blended = _accel_ef_ekf[_ins.get_primary_accel()];
nav_filter_status filt_state;
EKF3.getFilterStatus(-1,filt_state);
AP_Notify::flags.gps_fusion = filt_state.flags.using_gps; // Drives AP_Notify flag for usable GPS.
AP_Notify::flags.gps_glitching = filt_state.flags.gps_glitching;
AP_Notify::flags.have_pos_abs = filt_state.flags.horiz_pos_abs;
}
}
}
void AP_AHRS_NavEKF::update_EKF2(void)
{
if (!_ekf2_started) {
// wait 1 second for DCM to output a valid tilt error estimate
if (start_time_ms == 0) {
start_time_ms = AP_HAL::millis();
}
if (AP_HAL::millis() - start_time_ms > startup_delay_ms || _force_ekf) {
_ekf2_started = EKF2.InitialiseFilter();
if (_force_ekf) {
return;
}
}
}
if (_ekf2_started) {
EKF2.UpdateFilter();
if (active_EKF_type() == EKF_TYPE2) {
Vector3f eulers;
EKF2.getRotationBodyToNED(_dcm_matrix);
EKF2.getEulerAngles(-1,eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
update_cd_values();
update_trig();
// Use the primary EKF to select the primary gyro
const int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
const AP_InertialSensor &_ins = AP::ins();
// 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(-1,_gyro_drift);
_gyro_drift = -_gyro_drift;
// calculate corrected gyro estimate for get_gyro()
_gyro_estimate.zero();
if (primary_imu == -1) {
// the primary IMU is undefined so use an uncorrected default value from the INS library
_gyro_estimate = _ins.get_gyro();
} else {
// use the same IMU as the primary EKF and correct for gyro drift
_gyro_estimate = _ins.get_gyro(primary_imu) + _gyro_drift;
}
// get z accel bias estimate from active EKF (this is usually for the primary IMU)
float abias = 0;
EKF2.getAccelZBias(-1,abias);
// This EKF is currently using primary_imu, and abias applies to only that IMU
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
Vector3f accel = _ins.get_accel(i);
if (i == primary_imu) {
accel.z -= abias;
}
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
}
_accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()];
nav_filter_status filt_state;
EKF2.getFilterStatus(-1,filt_state);
AP_Notify::flags.gps_fusion = filt_state.flags.using_gps; // Drives AP_Notify flag for usable GPS.
AP_Notify::flags.gps_glitching = filt_state.flags.gps_glitching;
AP_Notify::flags.have_pos_abs = filt_state.flags.horiz_pos_abs;
}
}
}
void AP_AHRS_NavEKF::update_EKF2(void)
{
if (!ekf2_started) {
// wait 1 second for DCM to output a valid tilt error estimate
if (start_time_ms == 0) {
start_time_ms = AP_HAL::millis();
}
if (AP_HAL::millis() - start_time_ms > startup_delay_ms || force_ekf) {
ekf2_started = EKF2.InitialiseFilter();
if (force_ekf) {
return;
}
}
}
if (ekf2_started) {
EKF2.UpdateFilter();
if (active_EKF_type() == EKF_TYPE2) {
Vector3f eulers;
EKF2.getRotationBodyToNED(_dcm_matrix);
EKF2.getEulerAngles(-1,eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
update_cd_values();
update_trig();
// keep _gyro_bias for get_gyro_drift()
_gyro_bias.zero();
EKF2.getGyroBias(-1,_gyro_bias);
_gyro_bias = -_gyro_bias;
// calculate corrected gryo estimate for get_gyro()
_gyro_estimate.zero();
// the gyro bias applies only to the IMU associated with the primary EKF2
// core
int8_t primary_imu = EKF2.getPrimaryCoreIMUIndex();
if (primary_imu == -1) {
_gyro_estimate = _ins.get_gyro();
} else {
_gyro_estimate = _ins.get_gyro(primary_imu);
}
_gyro_estimate += _gyro_bias;
float abias = 0;
EKF2.getAccelZBias(-1,abias);
// This EKF is currently using primary_imu, and abias applies to only that IMU
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
Vector3f accel = _ins.get_accel(i);
if (i == primary_imu) {
accel.z -= abias;
}
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
}
_accel_ef_ekf_blended = _accel_ef_ekf[primary_imu>=0?primary_imu:_ins.get_primary_accel()];
}
}
}
