fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 23:04:07 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor start of gps into separate function

Browse Source
This commit is contained in:
kamilritz 2020-08-06 14:41:35 +02:00 committed by Mathieu Bresciani
parent 7eb2b08eed
commit 310b989c9a
3 changed files with 16 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
EKF/control.cpp
View File

@ -514,16 +514,7 @@ void Ekf::controlGpsFusion()
// If the heading is valid start using gps aiding
if (_control_status.flags.yaw_align) {
resetHorizontalPositionToGps();
// when adding with optical flow,
// velocity reset is not necessary
if (!_control_status.flags.opt_flow) {
resetVelocityToGps();
}
ECL_INFO_TIMESTAMPED("starting GPS fusion");
_control_status.flags.gps = true;
startGpsFusion();
}
}

3
EKF/ekf.h
View File

@ -847,10 +847,9 @@ private:
return sensor_timestamp + acceptance_interval > _time_last_imu;
}
void startGpsFusion();
void stopGpsFusion();
void stopGpsPosFusion();
void stopGpsVelFusion();
void startGpsYawFusion();

14
EKF/ekf_helper.cpp
View File

@ -1530,6 +1530,20 @@ void Ekf::loadMagCovData()
P.slice<2, 2>(16, 16) = _saved_mag_ef_covmat;
}
void Ekf::startGpsFusion()
{
resetHorizontalPositionToGps();
// when using optical flow,
// velocity reset is not necessary
if (!_control_status.flags.opt_flow) {
resetVelocityToGps();
}
ECL_INFO_TIMESTAMPED("starting GPS fusion");
_control_status.flags.gps = true;
}
void Ekf::stopGpsFusion()
{
stopGpsPosFusion();