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

Refactor velocity reset (#788)

Browse Source 
* Refactor velocity reset

* Add unit tests for velocity resets

* Expand updates to vertical buffer to velocity resets outside of resetHeight

* Improve matrix library usage

* Improve naming of vertical output samples

* Fix update of output_vert_new during reset

* Improve naming of vertical output samples 2
This commit is contained in:
kritz
2020-04-08 16:39:15 +02:00
committed by GitHub
parent 47624a0f02
commit fa5a00d871
13 changed files with 259 additions and 78 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/test_EKF_gps.cpp
+25
View File
@@ -83,3 +83,28 @@ TEST_F(EkfGpsTest, gpsTimeout)
// TODO: this is not happening as expected
EXPECT_TRUE(_ekf_wrapper.isIntendingGpsFusion());
}
TEST_F(EkfGpsTest, resetToGpsVelocity)
{
// GIVEN:EKF that fuses GPS
// and has gps checks already passed
// WHEN: stopping GPS fusion
_sensor_simulator.stopGps();
_sensor_simulator.runSeconds(11);
// AND: simulate constant velocity gps samples for short time
_sensor_simulator.startGps();
const Vector3f simulated_velocity(0.5f, 1.0f, -0.3f);
_sensor_simulator._gps.setVelocity(simulated_velocity);
const uint64_t dt_us = 1e5;
_sensor_simulator._gps.stepHorizontalPositionByMeters(
Vector2f(simulated_velocity) * dt_us * 1e-6);
_sensor_simulator._gps.stepHeightByMeters(
simulated_velocity(2) * dt_us * 1e-6);
_sensor_simulator.runMicroseconds(dt_us);
// THEN: a reset to GPS velocity should be done
const Vector3f estimated_velocity = _ekf->getVelocity();
EXPECT_TRUE(isEqual(estimated_velocity, simulated_velocity, 1e-2f));
}