void Ekf::controlFusionModes()
{
// Store the status to enable change detection
_control_status_prev.value = _control_status.value;
// Get the magnetic declination
calcMagDeclination();
// monitor the tilt alignment
if (!_control_status.flags.tilt_align) {
// whilst we are aligning the tilt, monitor the variances
Vector3f angle_err_var_vec = calcRotVecVariances();
// Once the tilt variances have reduced to equivalent of 3deg uncertainty, re-set the yaw and magnetic field states
// and declare the tilt alignment complete
if ((angle_err_var_vec(0) + angle_err_var_vec(1)) < sq(0.05235f)) {
_control_status.flags.tilt_align = true;
_control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag);
ECL_INFO("EKF alignment complete");
}
}
// check for arrival of new sensor data at the fusion time horizon
_gps_data_ready = _gps_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed);
_mag_data_ready = _mag_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_mag_sample_delayed);
_baro_data_ready = _baro_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_baro_sample_delayed);
_range_data_ready = _range_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_range_sample_delayed)
&& (_R_to_earth(2, 2) > 0.7071f);
_flow_data_ready = _flow_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_flow_sample_delayed)
&& (_R_to_earth(2, 2) > 0.7071f);
_ev_data_ready = _ext_vision_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_ev_sample_delayed);
_tas_data_ready = _airspeed_buffer.pop_first_older_than(_imu_sample_delayed.time_us, &_airspeed_sample_delayed);
// check for height sensor timeouts and reset and change sensor if necessary
controlHeightSensorTimeouts();
// control use of observations for aiding
controlMagFusion();
controlExternalVisionFusion();
controlOpticalFlowFusion();
controlGpsFusion();
controlBaroFusion();
controlRangeFinderFusion();
controlAirDataFusion();
// for efficiency, fusion of direct state observations for position ad velocity is performed sequentially
// in a single function using sensor data from multiple sources (GPS, external vision, baro, range finder, etc)
controlVelPosFusion();
}
void Ekf::controlHeightAiding()
{
// check for height sensor timeouts and reset and change sensor if necessary
controlHeightSensorTimeouts();
// Control the source of height measurements for the main filter
// do not switch to a sensor if it is unhealthy or the data is stale
if ((_params.vdist_sensor_type == VDIST_SENSOR_BARO) &&
!_baro_hgt_faulty &&
(((_imu_sample_delayed.time_us - _baro_sample_delayed.time_us) < 2 * BARO_MAX_INTERVAL) || _control_status.flags.baro_hgt)) {
_control_status.flags.baro_hgt = true;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_GPS) &&
!_gps_hgt_faulty &&
(((_imu_sample_delayed.time_us - _gps_sample_delayed.time_us) < 2 * GPS_MAX_INTERVAL) || _control_status.flags.gps_hgt)) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = true;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_RANGE) &&
!_rng_hgt_faulty &&
(((_imu_sample_delayed.time_us - _range_sample_delayed.time_us) < 2 * RNG_MAX_INTERVAL) || _control_status.flags.rng_hgt)) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = true;
_control_status.flags.ev_hgt = false;
} else if ((_params.vdist_sensor_type == VDIST_SENSOR_EV) &&
(((_imu_sample_delayed.time_us - _ev_sample_delayed.time_us) < 2 * EV_MAX_INTERVAL) || _control_status.flags.ev_hgt)) {
_control_status.flags.baro_hgt = false;
_control_status.flags.gps_hgt = false;
_control_status.flags.rng_hgt = false;
_control_status.flags.ev_hgt = true;
}
}
