Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 18:27:05 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

ekf2: move gps helpers to gps_control.cpp

Browse Source
This commit is contained in:
Daniel Agar
2023-02-21 11:06:58 -05:00
parent 9ec3f30ae1
commit 3a317ec18c
2 changed files with 57 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/ekf_helper.cpp
-57
View File
@@ -1181,63 +1181,6 @@ void Ekf::loadMagCovData()
P(18, 18) = _saved_mag_ef_d_variance; P(18, 18) = _saved_mag_ef_d_variance;
} }
void Ekf::stopGpsFusion()
{
if (_control_status.flags.gps) {
stopGpsPosFusion();
stopGpsVelFusion();
_control_status.flags.gps = false;
}
if (_control_status.flags.gps_yaw) {
stopGpsYawFusion();
}
// We do not need to know the true North anymore
// EV yaw can start again
_inhibit_ev_yaw_use = false;
}
void Ekf::stopGpsPosFusion()
{
if (_control_status.flags.gps) {
ECL_INFO("stopping GPS position fusion");
_control_status.flags.gps = false;
resetEstimatorAidStatus(_aid_src_gnss_pos);
}
}
void Ekf::stopGpsVelFusion()
{
ECL_INFO("stopping GPS velocity fusion");
resetEstimatorAidStatus(_aid_src_gnss_vel);
}
void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
{
if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
ECL_INFO("starting GPS yaw fusion");
_control_status.flags.yaw_align = true;
_control_status.flags.mag_dec = false;
stopEvYawFusion();
stopMagHdgFusion();
stopMag3DFusion();
_control_status.flags.gps_yaw = true;
}
}
void Ekf::stopGpsYawFusion()
{
if (_control_status.flags.gps_yaw) {
ECL_INFO("stopping GPS yaw fusion");
_control_status.flags.gps_yaw = false;
resetEstimatorAidStatus(_aid_src_gnss_yaw);
}
}
void Ekf::stopAuxVelFusion() void Ekf::stopAuxVelFusion()
{ {
ECL_INFO("stopping aux vel fusion"); ECL_INFO("stopping aux vel fusion");
src/modules/ekf2/EKF/gps_control.cpp
+57
View File
@@ -380,3 +380,60 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi
_control_status.flags.yaw_align = false; _control_status.flags.yaw_align = false;
} }
} }
void Ekf::stopGpsFusion()
{
if (_control_status.flags.gps) {
stopGpsPosFusion();
stopGpsVelFusion();
_control_status.flags.gps = false;
}
if (_control_status.flags.gps_yaw) {
stopGpsYawFusion();
}
// We do not need to know the true North anymore
// EV yaw can start again
_inhibit_ev_yaw_use = false;
}
void Ekf::stopGpsPosFusion()
{
if (_control_status.flags.gps) {
ECL_INFO("stopping GPS position fusion");
_control_status.flags.gps = false;
resetEstimatorAidStatus(_aid_src_gnss_pos);
}
}
void Ekf::stopGpsVelFusion()
{
ECL_INFO("stopping GPS velocity fusion");
resetEstimatorAidStatus(_aid_src_gnss_vel);
}
void Ekf::startGpsYawFusion(const gpsSample &gps_sample)
{
if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) {
ECL_INFO("starting GPS yaw fusion");
_control_status.flags.yaw_align = true;
_control_status.flags.mag_dec = false;
stopEvYawFusion();
stopMagHdgFusion();
stopMag3DFusion();
_control_status.flags.gps_yaw = true;
}
}
void Ekf::stopGpsYawFusion()
{
if (_control_status.flags.gps_yaw) {
ECL_INFO("stopping GPS yaw fusion");
_control_status.flags.gps_yaw = false;
resetEstimatorAidStatus(_aid_src_gnss_yaw);
}
}