void AP_AHRS_NavEKF::reset(bool recover_eulers)
{
AP_AHRS_DCM::reset(recover_eulers);
_dcm_attitude(roll, pitch, yaw);
if (_ekf2_started) {
_ekf2_started = EKF2.InitialiseFilter();
}
if (_ekf3_started) {
_ekf3_started = EKF3.InitialiseFilter();
}
}
// reset the current attitude, used on new IMU calibration
void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, const float &_yaw)
{
AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw);
_dcm_attitude(roll, pitch, yaw);
if (_ekf2_started) {
_ekf2_started = EKF2.InitialiseFilter();
}
if (_ekf3_started) {
_ekf3_started = EKF3.InitialiseFilter();
}
}
void AP_AHRS_NavEKF::reset(bool recover_eulers)
{
AP_AHRS_DCM::reset(recover_eulers);
_dcm_attitude(roll, pitch, yaw);
if (ekf1_started) {
ekf1_started = EKF1.InitialiseFilterBootstrap();
}
if (ekf2_started) {
ekf2_started = EKF2.InitialiseFilter();
}
}
// reset the current attitude, used on new IMU calibration
void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, const float &_yaw)
{
AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw);
_dcm_attitude(roll, pitch, yaw);
#if AP_AHRS_WITH_EKF1
if (ekf1_started) {
ekf1_started = EKF1.InitialiseFilterBootstrap();
}
#endif
if (ekf2_started) {
ekf2_started = EKF2.InitialiseFilter();
}
}
// reset the current attitude, used on new IMU calibration
void AP_AHRS_NavEKF::reset_attitude(const float &_roll, const float &_pitch, const float &_yaw)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
AP_AHRS_DCM::reset_attitude(_roll, _pitch, _yaw);
_dcm_attitude(roll, pitch, yaw);
if (_ekf2_started) {
_ekf2_started = EKF2.InitialiseFilter();
}
if (_ekf3_started) {
_ekf3_started = EKF3.InitialiseFilter();
}
}
void AP_AHRS_NavEKF::reset(bool recover_eulers)
{
// support locked access functions to AHRS data
WITH_SEMAPHORE(_rsem);
AP_AHRS_DCM::reset(recover_eulers);
_dcm_attitude(roll, pitch, yaw);
if (_ekf2_started) {
_ekf2_started = EKF2.InitialiseFilter();
}
if (_ekf3_started) {
_ekf3_started = EKF3.InitialiseFilter();
}
}
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(void)
{
AP_AHRS_DCM::update();
// keep DCM attitude available for get_secondary_attitude()
_dcm_attitude(roll, pitch, yaw);
if (!ekf_started) {
// if we have a compass set and GPS lock we can start the EKF
if (get_compass() && get_gps()->status() >= GPS::GPS_OK_FIX_3D) {
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;
roll_sensor = degrees(roll) * 100;
pitch_sensor = degrees(pitch) * 100;
yaw_sensor = degrees(yaw) * 100;
if (yaw_sensor < 0)
yaw_sensor += 36000;
update_trig();
}
}
}
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(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];
}
}
}
}