void AP_AHRS_NavEKF::update_SITL(void)
{
if (_sitl == nullptr) {
_sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
if (_sitl == nullptr) {
return;
}
}
const struct SITL::sitl_fdm &fdm = _sitl->state;
if (active_EKF_type() == EKF_TYPE_SITL) {
roll = radians(fdm.rollDeg);
pitch = radians(fdm.pitchDeg);
yaw = radians(fdm.yawDeg);
fdm.quaternion.rotation_matrix(_dcm_matrix);
update_cd_values();
update_trig();
_gyro_drift.zero();
_gyro_estimate = Vector3f(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
const Vector3f accel(fdm.xAccel,
fdm.yAccel,
fdm.zAccel);
_accel_ef_ekf[i] = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * accel;
}
_accel_ef_ekf_blended = _accel_ef_ekf[0];
}
if (_sitl->odom_enable) {
// use SITL states to write body frame odometry data at 20Hz
uint32_t timeStamp_ms = AP_HAL::millis();
if (timeStamp_ms - _last_body_odm_update_ms > 50) {
const float quality = 100.0f;
const Vector3f posOffset(0.0f, 0.0f, 0.0f);
const float delTime = 0.001f * (timeStamp_ms - _last_body_odm_update_ms);
_last_body_odm_update_ms = timeStamp_ms;
timeStamp_ms -= (timeStamp_ms - _last_body_odm_update_ms)/2; // correct for first order hold average delay
Vector3f delAng(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
delAng *= delTime;
// rotate earth velocity into body frame and calculate delta position
Matrix3f Tbn;
Tbn.from_euler(radians(fdm.rollDeg),radians(fdm.pitchDeg),radians(fdm.yawDeg));
const Vector3f earth_vel(fdm.speedN,fdm.speedE,fdm.speedD);
const Vector3f delPos = Tbn.transposed() * (earth_vel * delTime);
// write to EKF
EKF3.writeBodyFrameOdom(quality, delPos, delAng, delTime, timeStamp_ms, posOffset);
}
}
}
// Retrieves the NED delta velocity corrected
void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
{
const EKF_TYPE type = active_EKF_type();
if (type == EKF_TYPE2 || type == EKF_TYPE3) {
int8_t imu_idx = 0;
Vector3f accel_bias;
if (type == EKF_TYPE2) {
accel_bias.zero();
imu_idx = EKF2.getPrimaryCoreIMUIndex();
EKF2.getAccelZBias(-1,accel_bias.z);
} else if (type == EKF_TYPE3) {
imu_idx = EKF3.getPrimaryCoreIMUIndex();
EKF3.getAccelBias(-1,accel_bias);
}
if (imu_idx == -1) {
// should never happen, call parent implementation in this scenario
AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt);
return;
}
ret.zero();
const AP_InertialSensor &_ins = AP::ins();
_ins.get_delta_velocity((uint8_t)imu_idx, ret);
dt = _ins.get_delta_velocity_dt((uint8_t)imu_idx);
ret -= accel_bias*dt;
ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret;
ret.z += GRAVITY_MSS*dt;
} else {
AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt);
}
}
// Retrieves the NED delta velocity corrected
void AP_AHRS_NavEKF::getCorrectedDeltaVelocityNED(Vector3f& ret, float& dt) const
{
if (ekf_type() == 2) {
uint8_t imu_idx = EKF2.getPrimaryCoreIMUIndex();
float accel_z_bias;
EKF2.getAccelZBias(-1,accel_z_bias);
ret.zero();
_ins.get_delta_velocity(imu_idx, ret);
dt = _ins.get_delta_velocity_dt(imu_idx);
ret.z -= accel_z_bias*dt;
ret = _dcm_matrix * get_rotation_autopilot_body_to_vehicle_body() * ret;
ret.z += GRAVITY_MSS*dt;
} else {
AP_AHRS::getCorrectedDeltaVelocityNED(ret, dt);
}
}
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()];
}
}
}
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 * get_rotation_autopilot_body_to_vehicle_body() * 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
}
