dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf2: move controlGpsYawFusion() control.cpp -> gps_control.cpp

This commit is contained in:
Daniel Agar 2023-02-21 09:57:06 -05:00
parent b66e15c4b9
commit d840f7f39f
2 changed files with 87 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

87
src/modules/ekf2/EKF/control.cpp
View File

@ -218,93 +218,6 @@ void Ekf::controlFusionModes(const imuSample &imu_delayed)
updateDeadReckoningStatus();
}
void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
{
if (!(_params.gnss_ctrl & GnssCtrl::YAW)
|| _control_status.flags.gps_yaw_fault) {
stopGpsYawFusion();
return;
}
updateGpsYaw(gps_sample);
const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
if (is_new_data_available) {
const bool continuing_conditions_passing = !gps_checks_failing;
const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
const bool starting_conditions_passing = continuing_conditions_passing
&& _control_status.flags.tilt_align
&& gps_checks_passing
&& !is_gps_yaw_data_intermittent
&& !_gps_intermittent;
if (_control_status.flags.gps_yaw) {
if (continuing_conditions_passing) {
fuseGpsYaw();
const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
if (is_fusion_failing) {
if (_nb_gps_yaw_reset_available > 0) {
// Data seems good, attempt a reset
resetYawToGps(gps_sample.yaw);
if (_control_status.flags.in_air) {
_nb_gps_yaw_reset_available--;
}
} else if (starting_conditions_passing) {
// Data seems good, but previous reset did not fix the issue
// something else must be wrong, declare the sensor faulty and stop the fusion
_control_status.flags.gps_yaw_fault = true;
stopGpsYawFusion();
} else {
// A reset did not fix the issue but all the starting checks are not passing
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
stopGpsYawFusion();
}
// TODO: should we give a new reset credit when the fusion does not fail for some time?
}
} else {
// Stop GPS yaw fusion but do not declare it faulty
stopGpsYawFusion();
}
} else {
if (starting_conditions_passing) {
// Try to activate GPS yaw fusion
startGpsYawFusion(gps_sample);
if (_control_status.flags.gps_yaw) {
_nb_gps_yaw_reset_available = 1;
}
}
}
} else if (_control_status.flags.gps_yaw && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
// No yaw data in the message anymore. Stop until it comes back.
stopGpsYawFusion();
}
// Before takeoff, we do not want to continue to rely on the current heading
// if we had to stop the fusion
if (!_control_status.flags.in_air
&& !_control_status.flags.gps_yaw
&& _control_status_prev.flags.gps_yaw) {
_control_status.flags.yaw_align = false;
}
}
void Ekf::controlAirDataFusion()
{
// control activation and initialisation/reset of wind states required for airspeed fusion

87
src/modules/ekf2/EKF/gps_control.cpp
View File

@ -262,3 +262,90 @@ bool Ekf::isYawFailure() const
return fabsf(yaw_error) > math::radians(25.f);
}
void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passing, bool gps_checks_failing)
{
if (!(_params.gnss_ctrl & GnssCtrl::YAW)
|| _control_status.flags.gps_yaw_fault) {
stopGpsYawFusion();
return;
}
updateGpsYaw(gps_sample);
const bool is_new_data_available = PX4_ISFINITE(gps_sample.yaw);
if (is_new_data_available) {
const bool continuing_conditions_passing = !gps_checks_failing;
const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 2 * GNSS_YAW_MAX_INTERVAL);
const bool starting_conditions_passing = continuing_conditions_passing
&& _control_status.flags.tilt_align
&& gps_checks_passing
&& !is_gps_yaw_data_intermittent
&& !_gps_intermittent;
if (_control_status.flags.gps_yaw) {
if (continuing_conditions_passing) {
fuseGpsYaw();
const bool is_fusion_failing = isTimedOut(_aid_src_gnss_yaw.time_last_fuse, _params.reset_timeout_max);
if (is_fusion_failing) {
if (_nb_gps_yaw_reset_available > 0) {
// Data seems good, attempt a reset
resetYawToGps(gps_sample.yaw);
if (_control_status.flags.in_air) {
_nb_gps_yaw_reset_available--;
}
} else if (starting_conditions_passing) {
// Data seems good, but previous reset did not fix the issue
// something else must be wrong, declare the sensor faulty and stop the fusion
_control_status.flags.gps_yaw_fault = true;
stopGpsYawFusion();
} else {
// A reset did not fix the issue but all the starting checks are not passing
// This could be a temporary issue, stop the fusion without declaring the sensor faulty
stopGpsYawFusion();
}
// TODO: should we give a new reset credit when the fusion does not fail for some time?
}
} else {
// Stop GPS yaw fusion but do not declare it faulty
stopGpsYawFusion();
}
} else {
if (starting_conditions_passing) {
// Try to activate GPS yaw fusion
startGpsYawFusion(gps_sample);
if (_control_status.flags.gps_yaw) {
_nb_gps_yaw_reset_available = 1;
}
}
}
} else if (_control_status.flags.gps_yaw && !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, _params.reset_timeout_max)) {
// No yaw data in the message anymore. Stop until it comes back.
stopGpsYawFusion();
}
// Before takeoff, we do not want to continue to rely on the current heading
// if we had to stop the fusion
if (!_control_status.flags.in_air
&& !_control_status.flags.gps_yaw
&& _control_status_prev.flags.gps_yaw) {
_control_status.flags.yaw_align = false;
}
}