zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

EKF: update control status and fault status getters 

- the fault status union exceeded 16 bits, but the getter was get_filter_fault_status(uint16_t *val)
master
Daniel Agar 4 years ago committed by GitHub
parent
fbf67bdef9
commit
1541e4b414
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 64 additions and 84 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 7
      EKF/estimator_interface.h
  2. 36
      test/sensor_simulator/ekf_wrapper.cpp
  3. 105
      test/test_EKF_basics.cpp

7
EKF/estimator_interface.h
Unescape View File

@ -196,14 +196,17 @@ public: @@ -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; }

36
test/sensor_simulator/ekf_wrapper.cpp
Unescape View File

@ -62,9 +62,7 @@ void EkfWrapper::disableGpsFusion() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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() @@ -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()

105
test/test_EKF_basics.cpp
Unescape View File

@ -78,34 +78,31 @@ TEST_F(EkfBasicsTest, initialControlMode) @@ -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) @@ -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)

Write Preview
Loading…
Cancel
Save