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

GPS Yaw: wait to fuse at yaw at least once before declaring it faulty 

This fixes the cases where the yaw message from the GNSS receiver would
take more time than the vel/pos. The estimator should wait and not
immediately fall back to an other aiding source after 5sec.
If it never comes, it will never fall back, but this is ok since the
user wants to fly with GPS aiding an not with something else.
This commit is contained in:
bresch 2020-06-22 17:16:13 +02:00 committed by Mathieu Bresciani
parent 51cd63d626
commit 2bafe9df08
4 changed files with 10 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
EKF/control.cpp
View File

@ -526,6 +526,7 @@ void Ekf::controlGpsFusion()
} else {
if (ISFINITE(_gps_sample_delayed.yaw)) {
if (!_control_status.flags.gps_yaw
&& _control_status.flags.tilt_align
&& !_gps_hgt_intermittent) {
@ -548,7 +549,8 @@ void Ekf::controlGpsFusion()
// Check if the data is constantly fused by the estimator,
// if not, declare the sensor faulty and stop the fusion
// By doing this, another source of yaw aiding is allowed to start
if (isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)) {
if (isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)
&& _control_status.flags.gps_yaw) {
_is_gps_yaw_faulty = true;
stopGpsYawFusion();
}

2
test/sensor_simulator/gps.cpp
View File

@ -102,7 +102,7 @@ gps_message Gps::getDefaultGpsData()
gps_data.lat = 473566094;
gps_data.lon = 85190237;
gps_data.alt = 422056;
gps_data.yaw = 0.0f;
gps_data.yaw = NAN;
gps_data.yaw_offset = 0.0f;
gps_data.fix_type = 3;
gps_data.eph = 0.5f;

2
test/sensor_simulator/gps.h
View File

@ -66,7 +66,7 @@ public:
gps_message getDefaultGpsData();
private:
gps_message _gps_data;
gps_message _gps_data{};
Vector3f _gps_pos_rate{};
void send(uint64_t time) override;

6
test/test_EKF_gps_yaw.cpp
View File

@ -58,8 +58,10 @@ class EkfGpsHeadingTest : public ::testing::Test {
void SetUp() override
{
_ekf->init(0);
_sensor_simulator._gps.setYaw(NAN);
_sensor_simulator.runSeconds(2);
_ekf_wrapper.enableGpsFusion();
_ekf_wrapper.enableGpsHeadingFusion();
_sensor_simulator.startGps();
_sensor_simulator.runSeconds(11);
}
@ -121,11 +123,13 @@ TEST_F(EkfGpsHeadingTest, yawConvergence)
TEST_F(EkfGpsHeadingTest, fallBackToMag)
{
// GIVEN: an initial GPS yaw, not aligned with the current one
// GPS yaw is expected to arrive a bit later, first feed some NANs
// to the filter
_sensor_simulator.runSeconds(6);
float gps_heading = _ekf_wrapper.getYawAngle() + math::radians(10.f);
_sensor_simulator._gps.setYaw(gps_heading);
// WHEN: the GPS yaw fusion is activated
_ekf_wrapper.enableGpsHeadingFusion();
_sensor_simulator.runSeconds(1);
// THEN: GPS heading fusion should have started, and mag