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);
}
}
}
void AP_AHRS_NavEKF::update_SITL(void)
{
if (_sitl == nullptr) {
_sitl = (SITL::SITL *)AP_Param::find_object("SIM_");
}
if (_sitl && active_EKF_type() == EKF_TYPE_SITL) {
const struct SITL::sitl_fdm &fdm = _sitl->state;
roll = radians(fdm.rollDeg);
pitch = radians(fdm.pitchDeg);
yaw = radians(fdm.yawDeg);
_dcm_matrix.from_euler(roll, pitch, yaw);
update_cd_values();
update_trig();
_gyro_bias.zero();
_gyro_estimate = Vector3f(radians(fdm.rollRate),
radians(fdm.pitchRate),
radians(fdm.yawRate));
for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
_accel_ef_ekf[i] = Vector3f(fdm.xAccel,
fdm.yAccel,
fdm.zAccel);
}
_accel_ef_ekf_blended = _accel_ef_ekf[0];
}
}
// 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 * get_rotation_vehicle_body_to_autopilot_body();
_body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
update_cd_values();
}
// 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;
_body_dcm_matrix.rotateXYinv(_trim);
_body_dcm_matrix.to_euler(&roll, &pitch, &yaw);
update_cd_values();
}
void AP_AHRS_NavEKF::update_DCM(void)
{
// 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();
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude(roll, pitch, yaw);
}
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(void)
{
// 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();
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude(roll, pitch, yaw);
if (!ekf_started) {
// wait 10 seconds
if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis();
}
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
ekf_started = true;
EKF.InitialiseFilterDynamic();
}
}
if (ekf_started) {
EKF.UpdateFilter();
EKF.getRotationBodyToNED(_dcm_matrix);
if (using_EKF()) {
Vector3f eulers;
EKF.getEulerAngles(eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
update_cd_values();
update_trig();
// keep _gyro_bias for get_gyro_drift()
EKF.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)) {
_gyro_estimate += _ins.get_gyro(i);
healthy_count++;
}
}
if (healthy_count > 1) {
_gyro_estimate /= healthy_count;
}
_gyro_estimate += _gyro_bias;
// update _accel_ef_ekf
for (uint8_t i=0; i<_ins.get_accel_count(); i++) {
if (_ins.get_accel_health(i)) {
_accel_ef_ekf[i] = _dcm_matrix * _ins.get_accel(i);
}
}
// update _accel_ef_ekf_blended
EKF.getAccelNED(_accel_ef_ekf_blended);
}
}
}
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(void)
{
// 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();
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude(roll, pitch, yaw);
if (!ekf_started) {
// wait 1 second for DCM to output a valid tilt error estimate
if (start_time_ms == 0) {
start_time_ms = hal.scheduler->millis();
}
if (hal.scheduler->millis() - start_time_ms > startup_delay_ms) {
ekf_started = EKF.InitialiseFilterDynamic();
}
}
if (ekf_started) {
EKF.UpdateFilter();
EKF.getRotationBodyToNED(_dcm_matrix);
if (using_EKF()) {
Vector3f eulers;
EKF.getEulerAngles(eulers);
roll = eulers.x;
pitch = eulers.y;
yaw = eulers.z;
update_cd_values();
update_trig();
// keep _gyro_bias for get_gyro_drift()
EKF.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)) {
_gyro_estimate += _ins.get_gyro(i);
healthy_count++;
}
}
if (healthy_count > 1) {
_gyro_estimate /= healthy_count;
}
_gyro_estimate += _gyro_bias;
float abias1, abias2;
EKF.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.get_accel_health(0) && _ins.get_accel_health(1)) {
float IMU1_weighting;
EKF.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[0];
}
}
}
}
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()];
}
}
}
