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

ekf2_test: add test case for accel clipping handling

Browse Source
This commit is contained in:
bresch
2022-07-28 14:37:38 +02:00
committed by Daniel Agar
parent 375753eba8
commit 88ac5ea210
3 changed files with 40 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/test/sensor_simulator/imu.cpp
+10
View File
@@ -22,6 +22,9 @@ void Imu::send(uint64_t time)
imu_sample.delta_ang = _gyro_data * imu_sample.delta_ang_dt;
imu_sample.delta_vel_dt = dt;
imu_sample.delta_vel = _accel_data * imu_sample.delta_vel_dt;
imu_sample.delta_vel_clipping[0] = _is_accel_clipping[0];
imu_sample.delta_vel_clipping[1] = _is_accel_clipping[1];
imu_sample.delta_vel_clipping[2] = _is_accel_clipping[2];
_ekf->setIMUData(imu_sample);
}
@@ -42,5 +45,12 @@ void Imu::setGyroData(const Vector3f &gyro)
_gyro_data = gyro;
}
void Imu::setAccelClipping(bool x, bool y, bool z)
{
_is_accel_clipping[0] = x;
_is_accel_clipping[1] = y;
_is_accel_clipping[2] = z;
}
} // namespace sensor
} // namespace sensor_simulator
src/modules/ekf2/test/sensor_simulator/imu.h
+2
View File
@@ -54,6 +54,7 @@ public:
void setData(const Vector3f &accel, const Vector3f &gyro);
void setAccelData(const Vector3f &accel);
void setGyroData(const Vector3f &gyro);
void setAccelClipping(bool x, bool y, bool z);
bool moving()
{
@@ -63,6 +64,7 @@ public:
private:
Vector3f _accel_data;
Vector3f _gyro_data;
bool _is_accel_clipping[3] {};
void send(uint64_t time) override;
src/modules/ekf2/test/test_EKF_accelerometer.cpp
+28
View File
@@ -118,6 +118,13 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroOnly)
// WHEN: there is only one source of vertical aiding
// THEN: the estimator cannot know which one is wrong
EXPECT_FALSE(_ekf->fault_status_flags().bad_acc_vertical);
// BUT WHEN: the accelerometer also reports clipping on the Z axis
_sensor_simulator._imu.setAccelClipping(false, false, true);
_sensor_simulator.runSeconds(2);
// THEN: a single source is enough to detect a bad acceleration
EXPECT_TRUE(_ekf->fault_status_flags().bad_acc_vertical);
}
TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroGnssVel)
@@ -137,6 +144,13 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroGnssVel)
// THEN: the bad vertical acceleration is detected
EXPECT_TRUE(_ekf->fault_status_flags().bad_acc_vertical);
// AND WHEN: the accelerometer also reports clipping on the Z axis
_sensor_simulator._imu.setAccelClipping(false, false, true);
_sensor_simulator.runSeconds(2);
// THEN: the bad vertical acceleration is still detected
EXPECT_TRUE(_ekf->fault_status_flags().bad_acc_vertical);
}
TEST_F(EkfAccelerometerTest, imuFallingDetectionGnssOnly)
@@ -159,6 +173,13 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionGnssOnly)
// THEN: the bad vertical acceleration is not detected because both sources are of the same type
EXPECT_FALSE(_ekf->fault_status_flags().bad_acc_vertical);
// BUT WHEN: the accelerometer also reports clipping on the Z axis
_sensor_simulator._imu.setAccelClipping(false, false, true);
_sensor_simulator.runSeconds(2);
// THEN: a single source is enough to detect a bad acceleration
EXPECT_TRUE(_ekf->fault_status_flags().bad_acc_vertical);
}
TEST_F(EkfAccelerometerTest, imuFallingDetectionBaroRange)
@@ -223,4 +244,11 @@ TEST_F(EkfAccelerometerTest, imuFallingDetectionEvVelHgt)
// THEN: the bad vertical acceleration is not detected because both sources are of the same type
EXPECT_FALSE(_ekf->fault_status_flags().bad_acc_vertical);
// BUT WHEN: the accelerometer also reports clipping on the Z axis
_sensor_simulator._imu.setAccelClipping(false, false, true);
_sensor_simulator.runSeconds(2);
// THEN: a single source is enough to detect a bad acceleration
EXPECT_TRUE(_ekf->fault_status_flags().bad_acc_vertical);
}