fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-13 22:27:35 +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
EKF/estimator_interface.h
+5 -2
View File
@@ -196,14 +196,17 @@ public:
}
// get EKF mode status
void get_control_mode(uint32_t *val) const { *val = _control_status.value; }
const filter_control_status_u &control_status() const { return _control_status; }
const decltype(filter_control_status_u::flags) &control_status_flags() const { return _control_status.flags; }
const filter_control_status_u &control_status_prev() const { return _control_status_prev; }
const decltype(filter_control_status_u::flags) &control_status_prev_flags() const { return _control_status_prev.flags; }
// get EKF internal fault status
void get_filter_fault_status(uint16_t *val) const { *val = _fault_status.value; }
const fault_status_u &fault_status() const { return _fault_status; }
const decltype(fault_status_u::flags) &fault_status_flags() const { return _fault_status.flags; }
const innovation_fault_status_u &innov_check_fail_status() const { return _innov_check_fail_status; }
const decltype(innovation_fault_status_u::flags) &innov_check_fail_status_flags() const { return _innov_check_fail_status.flags; }
bool isVehicleAtRest() const { return _control_status.flags.vehicle_at_rest; }
test/sensor_simulator/ekf_wrapper.cpp
+9 -27
View File
@@ -62,9 +62,7 @@ void EkfWrapper::disableGpsFusion()
bool EkfWrapper::isIntendingGpsFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.gps;
return _ekf->control_status_flags().gps;
}
void EkfWrapper::enableGpsHeadingFusion()
@@ -79,9 +77,7 @@ void EkfWrapper::disableGpsHeadingFusion()
bool EkfWrapper::isIntendingGpsHeadingFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.gps_yaw;
return _ekf->control_status_flags().gps_yaw;
}
void EkfWrapper::enableFlowFusion()
@@ -96,9 +92,7 @@ void EkfWrapper::disableFlowFusion()
bool EkfWrapper::isIntendingFlowFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.opt_flow;
return _ekf->control_status_flags().opt_flow;
}
void EkfWrapper::enableExternalVisionPositionFusion()
@@ -113,9 +107,7 @@ void EkfWrapper::disableExternalVisionPositionFusion()
bool EkfWrapper::isIntendingExternalVisionPositionFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.ev_pos;
return _ekf->control_status_flags().ev_pos;
}
void EkfWrapper::enableExternalVisionVelocityFusion()
@@ -130,9 +122,7 @@ void EkfWrapper::disableExternalVisionVelocityFusion()
bool EkfWrapper::isIntendingExternalVisionVelocityFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.ev_vel;
return _ekf->control_status_flags().ev_vel;
}
void EkfWrapper::enableExternalVisionHeadingFusion()
@@ -147,9 +137,7 @@ void EkfWrapper::disableExternalVisionHeadingFusion()
bool EkfWrapper::isIntendingExternalVisionHeadingFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.ev_yaw;
return _ekf->control_status_flags().ev_yaw;
}
void EkfWrapper::enableExternalVisionAlignment()
@@ -164,23 +152,17 @@ void EkfWrapper::disableExternalVisionAlignment()
bool EkfWrapper::isIntendingMagHeadingFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.mag_hdg;
return _ekf->control_status_flags().mag_hdg;
}
bool EkfWrapper::isIntendingMag3DFusion() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.mag_3D;
return _ekf->control_status_flags().mag_3D;
}
bool EkfWrapper::isWindVelocityEstimated() const
{
filter_control_status_u control_status;
_ekf->get_control_mode(&control_status.value);
return control_status.flags.wind;
return _ekf->control_status_flags().wind;
}
void EkfWrapper::enableTerrainRngFusion()
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)