mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-22 07:53:33 +08:00
ekf2: move GPS control logic to separate source file
This commit is contained in:
bresch
committed by
Mathieu Bresciani
Mathieu Bresciani
parent
689ab12845
commit
8aae39e82a
@@ -519,196 +519,6 @@ void Ekf::resetOnGroundMotionForOpticalFlowChecks()
|
||||
_time_good_motion_us = _imu_sample_delayed.time_us;
|
||||
}
|
||||
|
||||
void Ekf::controlGpsFusion()
|
||||
{
|
||||
if (!(_params.fusion_mode & MASK_USE_GPS)) {
|
||||
stopGpsFusion();
|
||||
return;
|
||||
}
|
||||
|
||||
// Check for new GPS data that has fallen behind the fusion time horizon
|
||||
if (_gps_data_ready) {
|
||||
|
||||
const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6);
|
||||
const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6);
|
||||
|
||||
controlGpsYawFusion(gps_checks_passing, gps_checks_failing);
|
||||
|
||||
// Determine if we should use GPS aiding for velocity and horizontal position
|
||||
// To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently
|
||||
const bool mandatory_conditions_passing = _control_status.flags.tilt_align
|
||||
&& _control_status.flags.yaw_align
|
||||
&& _NED_origin_initialised;
|
||||
const bool continuing_conditions_passing = mandatory_conditions_passing
|
||||
&& !gps_checks_failing;
|
||||
const bool starting_conditions_passing = continuing_conditions_passing
|
||||
&& gps_checks_passing;
|
||||
|
||||
if (_control_status.flags.gps) {
|
||||
if (mandatory_conditions_passing) {
|
||||
if (continuing_conditions_passing
|
||||
|| !isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
fuseGpsVelPos();
|
||||
|
||||
// We are relying on aiding to constrain drift so after a specified time
|
||||
// with no aiding we need to do something
|
||||
const bool do_vel_pos_reset = hasHorizontalAidingTimedOut()
|
||||
|| isTimedOut(_time_last_hor_pos_fuse, 2 * _params.reset_timeout_max);
|
||||
|
||||
/* Logic controlling the reset of navigation filter yaw to the EKF-GSF estimate to recover from loss of
|
||||
navigation casued by a bad yaw estimate.
|
||||
|
||||
A rapid reset to the EKF-GSF estimate is performed after a recent takeoff if horizontal velocity
|
||||
innovation checks fail. This enables recovery from a bad yaw estimate. After 30 seconds from takeoff,
|
||||
different test criteria are used that take longer to trigger and reduce false positives. A reset is
|
||||
not performed if the fault condition was present before flight to prevent triggering due to GPS glitches
|
||||
or other sensor errors.
|
||||
|
||||
The yaw reset to the EKF-GSF estimate can be requested externally at any time during flight.
|
||||
|
||||
The total number of resets allowed per boot cycle is limited.
|
||||
|
||||
The minimum time interval between resets to the EKF-GSF estimate is limited to allow the EKF-GSF time
|
||||
to improve its estimate if the previous reset was not successful.
|
||||
|
||||
A reset is not performed when getting GPS back after a significant period of no data because the timeout
|
||||
could have been caused by bad GPS.
|
||||
*/
|
||||
const bool is_recent_takeoff_nav_failure = isTimedOut(_time_last_hor_vel_fuse, _params.EKFGSF_reset_delay)
|
||||
&& (_time_last_hor_vel_fuse > _time_last_on_ground_us)
|
||||
&& isRecentTakeoff();
|
||||
|
||||
|
||||
const bool is_inflight_nav_failure = _control_status.flags.in_air &&
|
||||
do_vel_pos_reset &&
|
||||
(_time_last_hor_vel_fuse > _time_last_on_ground_us) &&
|
||||
(_time_last_hor_pos_fuse > _time_last_on_ground_us);
|
||||
|
||||
if (is_recent_takeoff_nav_failure || is_inflight_nav_failure){
|
||||
const bool is_yaw_failure = !isVelStateAlignedWithObs();
|
||||
const bool was_gps_signal_lost = isTimedOut(_time_prev_gps_us, 1000000);
|
||||
|
||||
if (is_yaw_failure
|
||||
&& !was_gps_signal_lost
|
||||
&& _ekfgsf_yaw_reset_count < _params.EKFGSF_reset_count_limit) {
|
||||
|
||||
_do_ekfgsf_yaw_reset = true;
|
||||
_ekfgsf_yaw_reset_count++;
|
||||
|
||||
} else {
|
||||
|
||||
// use GPS velocity data to check and correct yaw angle if a FW vehicle
|
||||
if (_control_status.flags.fixed_wing && _control_status.flags.in_air) {
|
||||
// if flying a fixed wing aircraft, do a complete reset that includes yaw
|
||||
_control_status.flags.mag_aligned_in_flight = realignYawGPS();
|
||||
}
|
||||
|
||||
_warning_events.flags.gps_fusion_timout = true;
|
||||
ECL_WARN("GPS fusion timeout - resetting");
|
||||
_velpos_reset_request = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
stopGpsFusion();
|
||||
_warning_events.flags.gps_quality_poor = true;
|
||||
ECL_WARN("GPS quality poor - stopping use");
|
||||
}
|
||||
|
||||
} else { // mandatory conditions are not passing
|
||||
stopGpsFusion();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (starting_conditions_passing) {
|
||||
// If the heading is not aligned, reset the yaw and magnetic field states
|
||||
// Do not use external vision for yaw if using GPS because yaw needs to be
|
||||
// defined relative to an NED reference frame
|
||||
if (_control_status.flags.ev_yaw
|
||||
|| _mag_inhibit_yaw_reset_req
|
||||
|| _mag_yaw_reset_req) {
|
||||
|
||||
_mag_yaw_reset_req = true;
|
||||
|
||||
// Stop the vision for yaw fusion and do not allow it to start again
|
||||
stopEvYawFusion();
|
||||
_inhibit_ev_yaw_use = true;
|
||||
|
||||
} else {
|
||||
startGpsFusion();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const bool align_yaw_using_gsf = !_control_status.flags.yaw_align
|
||||
&& (_params.mag_fusion_type == MAG_FUSE_TYPE_NONE);
|
||||
_do_ekfgsf_yaw_reset = _do_ekfgsf_yaw_reset || align_yaw_using_gsf;
|
||||
|
||||
// Process yaw estimator reset request
|
||||
if (_do_ekfgsf_yaw_reset
|
||||
&& isTimedOut(_ekfgsf_yaw_reset_time, 5000000)){
|
||||
if (resetYawToEKFGSF()) {
|
||||
_ekfgsf_yaw_reset_time = _time_last_imu;
|
||||
_time_last_hor_pos_fuse = _time_last_imu;
|
||||
_time_last_hor_vel_fuse = _time_last_imu;
|
||||
|
||||
_do_ekfgsf_yaw_reset = false;
|
||||
_velpos_reset_request = false; // included in yaw reset
|
||||
}
|
||||
}
|
||||
|
||||
if (_velpos_reset_request) {
|
||||
resetVelocity();
|
||||
resetHorizontalPosition();
|
||||
_velpos_reset_request = false;
|
||||
|
||||
// Reset the timeout counters
|
||||
_time_last_hor_pos_fuse = _time_last_imu;
|
||||
_time_last_hor_vel_fuse = _time_last_imu;
|
||||
}
|
||||
|
||||
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) {
|
||||
stopGpsFusion();
|
||||
_warning_events.flags.gps_data_stopped = true;
|
||||
ECL_WARN("GPS data stopped");
|
||||
|
||||
} else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6)
|
||||
&& isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) {
|
||||
// Handle the case where we are fusing another position source along GPS,
|
||||
// stop waiting for GPS after 1 s of lost signal
|
||||
stopGpsFusion();
|
||||
_warning_events.flags.gps_data_stopped_using_alternate = true;
|
||||
ECL_WARN("GPS data stopped, using only EV, OF or air data");
|
||||
}
|
||||
//TODO: move that resetHorizontalPosition to EV
|
||||
/* // Handle the case where we are using GPS and another source of aiding and GPS is failing checks */
|
||||
/* if (_control_status.flags.gps && gps_checks_failing && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { */
|
||||
/* stopGpsFusion(); */
|
||||
|
||||
/* // Reset position state to external vision if we are going to use absolute values */
|
||||
/* if (_control_status.flags.ev_pos && !(_params.fusion_mode & MASK_ROTATE_EV)) { */
|
||||
/* resetHorizontalPosition(); */
|
||||
/* } */
|
||||
|
||||
/* _warning_events.flags.gps_quality_poor = true; */
|
||||
/* ECL_WARN("GPS quality poor - stopping use"); */
|
||||
/* } */
|
||||
}
|
||||
|
||||
bool Ekf::hasHorizontalAidingTimedOut() const
|
||||
{
|
||||
return isTimedOut(_time_last_hor_pos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_delpos_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_hor_vel_fuse, _params.reset_timeout_max)
|
||||
&& isTimedOut(_time_last_of_fuse, _params.reset_timeout_max);
|
||||
}
|
||||
|
||||
bool Ekf::isRecentTakeoff() const
|
||||
{
|
||||
return _control_status.flags.in_air
|
||||
&& !isTimedOut(_time_last_on_ground_us, 30000000);
|
||||
}
|
||||
|
||||
void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing)
|
||||
{
|
||||
if (!(_params.fusion_mode & MASK_USE_GPSYAW)
|
||||
|
||||
Reference in New Issue
Block a user