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

EKF: update control status and fault status getters

Browse Source 
- the fault status union exceeded 16 bits, but the getter was get_filter_fault_status(uint16_t *val)
This commit is contained in:
Daniel Agar
2020-12-10 09:13:12 -05:00
committed by GitHub
parent fbf67bdef9
commit 1541e4b414
3 changed files with 64 additions and 84 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_basics.cpp
+50 -55
View File
@@ -78,34 +78,31 @@ TEST_F(EkfBasicsTest, initialControlMode)
{
// GIVEN: reasonable static sensor data for some duration
// THEN: EKF control status should be reasonable
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
EXPECT_EQ(1, (int) control_status.flags.tilt_align);
EXPECT_EQ(1, (int) control_status.flags.yaw_align);
EXPECT_EQ(0, (int) control_status.flags.gps);
EXPECT_EQ(0, (int) control_status.flags.opt_flow);
EXPECT_EQ(1, (int) control_status.flags.mag_hdg);
EXPECT_EQ(0, (int) control_status.flags.mag_3D);
EXPECT_EQ(0, (int) control_status.flags.mag_dec);
EXPECT_EQ(0, (int) control_status.flags.in_air);
EXPECT_EQ(0, (int) control_status.flags.wind);
EXPECT_EQ(1, (int) control_status.flags.baro_hgt);
EXPECT_EQ(0, (int) control_status.flags.rng_hgt);
EXPECT_EQ(0, (int) control_status.flags.gps_hgt);
EXPECT_EQ(0, (int) control_status.flags.ev_pos);
EXPECT_EQ(0, (int) control_status.flags.ev_yaw);
EXPECT_EQ(0, (int) control_status.flags.ev_hgt);
EXPECT_EQ(0, (int) control_status.flags.fuse_beta);
EXPECT_EQ(0, (int) control_status.flags.mag_field_disturbed);
EXPECT_EQ(0, (int) control_status.flags.fixed_wing);
EXPECT_EQ(0, (int) control_status.flags.mag_fault);
EXPECT_EQ(0, (int) control_status.flags.gnd_effect);
EXPECT_EQ(0, (int) control_status.flags.rng_stuck);
EXPECT_EQ(0, (int) control_status.flags.gps_yaw);
EXPECT_EQ(0, (int) control_status.flags.mag_aligned_in_flight);
EXPECT_EQ(0, (int) control_status.flags.ev_vel);
EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z);
EXPECT_EQ(1, (int) _ekf->control_status_flags().tilt_align);
EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align);
EXPECT_EQ(0, (int) _ekf->control_status_flags().gps);
EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow);
EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_dec);
EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air);
EXPECT_EQ(0, (int) _ekf->control_status_flags().wind);
EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt);
EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_hgt);
EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_hgt);
EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_pos);
EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_yaw);
EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_hgt);
EXPECT_EQ(0, (int) _ekf->control_status_flags().fuse_beta);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_field_disturbed);
EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);
EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z);
}
TEST_F(EkfBasicsTest, convergesToZero)
@@ -135,33 +132,31 @@ TEST_F(EkfBasicsTest, gpsFusion)
_sensor_simulator.runSeconds(11);
// THEN: EKF should fuse GPS, but no other position sensor
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
EXPECT_EQ(1, (int) control_status.flags.tilt_align);
EXPECT_EQ(1, (int) control_status.flags.yaw_align);
EXPECT_EQ(1, (int) control_status.flags.gps);
EXPECT_EQ(0, (int) control_status.flags.opt_flow);
EXPECT_EQ(1, (int) control_status.flags.mag_hdg);
EXPECT_EQ(0, (int) control_status.flags.mag_3D);
EXPECT_EQ(0, (int) control_status.flags.mag_dec);
EXPECT_EQ(0, (int) control_status.flags.in_air);
EXPECT_EQ(0, (int) control_status.flags.wind);
EXPECT_EQ(1, (int) control_status.flags.baro_hgt);
EXPECT_EQ(0, (int) control_status.flags.rng_hgt);
EXPECT_EQ(0, (int) control_status.flags.gps_hgt);
EXPECT_EQ(0, (int) control_status.flags.ev_pos);
EXPECT_EQ(0, (int) control_status.flags.ev_yaw);
EXPECT_EQ(0, (int) control_status.flags.ev_hgt);
EXPECT_EQ(0, (int) control_status.flags.fuse_beta);
EXPECT_EQ(0, (int) control_status.flags.mag_field_disturbed);
EXPECT_EQ(0, (int) control_status.flags.fixed_wing);
EXPECT_EQ(0, (int) control_status.flags.mag_fault);
EXPECT_EQ(0, (int) control_status.flags.gnd_effect);
EXPECT_EQ(0, (int) control_status.flags.rng_stuck);
EXPECT_EQ(0, (int) control_status.flags.gps_yaw);
EXPECT_EQ(0, (int) control_status.flags.mag_aligned_in_flight);
EXPECT_EQ(0, (int) control_status.flags.ev_vel);
EXPECT_EQ(0, (int) control_status.flags.synthetic_mag_z);
EXPECT_EQ(1, (int) _ekf->control_status_flags().tilt_align);
EXPECT_EQ(1, (int) _ekf->control_status_flags().yaw_align);
EXPECT_EQ(1, (int) _ekf->control_status_flags().gps);
EXPECT_EQ(0, (int) _ekf->control_status_flags().opt_flow);
EXPECT_EQ(1, (int) _ekf->control_status_flags().mag_hdg);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_3D);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_dec);
EXPECT_EQ(0, (int) _ekf->control_status_flags().in_air);
EXPECT_EQ(0, (int) _ekf->control_status_flags().wind);
EXPECT_EQ(1, (int) _ekf->control_status_flags().baro_hgt);
EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_hgt);
EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_hgt);
EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_pos);
EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_yaw);
EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_hgt);
EXPECT_EQ(0, (int) _ekf->control_status_flags().fuse_beta);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_field_disturbed);
EXPECT_EQ(0, (int) _ekf->control_status_flags().fixed_wing);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_fault);
EXPECT_EQ(0, (int) _ekf->control_status_flags().gnd_effect);
EXPECT_EQ(0, (int) _ekf->control_status_flags().rng_stuck);
EXPECT_EQ(0, (int) _ekf->control_status_flags().gps_yaw);
EXPECT_EQ(0, (int) _ekf->control_status_flags().mag_aligned_in_flight);
EXPECT_EQ(0, (int) _ekf->control_status_flags().ev_vel);
EXPECT_EQ(0, (int) _ekf->control_status_flags().synthetic_mag_z);
}
TEST_F(EkfBasicsTest, accelBiasEstimation)