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

Add unit tests and update ekf_helper setEkfGlobalOrigin() method to allow for condition when current position is unitialized.

Browse Source
This commit is contained in:
mcsauder
2021-02-12 11:20:08 -07:00
committed by Daniel Agar
parent 63f64b57c1
commit 164e53bad3
6 changed files with 210 additions and 128 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/sensor_simulator/sensor_simulator.cpp
+34 -15
View File
@@ -2,15 +2,15 @@
SensorSimulator::SensorSimulator(std::shared_ptr<Ekf> ekf):
_ekf{ekf},
_imu(ekf),
_mag(ekf),
_baro(ekf),
_gps(ekf),
_flow(ekf),
_rng(ekf),
_vio(ekf),
_airspeed(ekf)
_airspeed(ekf),
_baro(ekf),
_flow(ekf),
_gps(ekf),
_imu(ekf),
_mag(ekf),
_rng(ekf),
_vio(ekf),
_ekf{ekf}
{
setSensorRateToDefault();
setSensorDataToDefault();
@@ -115,23 +115,24 @@ void SensorSimulator::setSensorRateToDefault()
_vio.setRateHz(30);
_airspeed.setRateHz(100);
}
void SensorSimulator::setSensorDataToDefault()
{
_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G},
Vector3f{0.0f,0.0f,0.0f});
_mag.setData(Vector3f{0.2f, 0.0f, 0.4f});
_airspeed.setData(0.0f, 0.0f);
_baro.setData(122.2f);
_gps.setData(_gps.getDefaultGpsData());
_flow.setData(_flow.dataAtRest());
_gps.setData(_gps.getDefaultGpsData());
_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G}, Vector3f{0.0f,0.0f,0.0f});
_mag.setData(Vector3f{0.2f, 0.0f, 0.4f});
_rng.setData(0.2f, 100);
_vio.setData(_vio.dataAtRest());
_airspeed.setData(0.0f, 0.0f);
}
void SensorSimulator::startBasicSensor()
{
_baro.start();
_imu.start();
_mag.start();
_baro.start();
}
void SensorSimulator::runSeconds(float duration_seconds)
@@ -280,6 +281,24 @@ void SensorSimulator::setSingleReplaySample(const sensor_info& sample)
}
}
void SensorSimulator::setGpsLatitude(const double latitude)
{
int32_t lat = static_cast<int32_t>(latitude * 1e7);
_gps.setLatitude(lat);
}
void SensorSimulator::setGpsLongitude(const double longitude)
{
int32_t lon = static_cast<int32_t>(longitude * 1e7);
_gps.setLongitude(lon);
}
void SensorSimulator::setGpsAltitude(const float altitude)
{
int32_t alt = static_cast<int32_t>(altitude * 1e3);
_gps.setAltitude(alt);
}
void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)
{
_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G} + accel_bias,