mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-01 23:34:08 +08:00
102 lines
2.2 KiB
C++
102 lines
2.2 KiB
C++
#include "sensor_simulator.h"
|
|
|
|
|
|
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)
|
|
{
|
|
setSensorDataToDefault();
|
|
setSensorRateToDefault();
|
|
startBasicSensor();
|
|
}
|
|
|
|
SensorSimulator::~SensorSimulator()
|
|
{
|
|
|
|
}
|
|
|
|
void SensorSimulator::setSensorDataToDefault()
|
|
{
|
|
_imu.setRateHz(250);
|
|
_mag.setRateHz(80);
|
|
_baro.setRateHz(80);
|
|
_gps.setRateHz(5);
|
|
_flow.setRateHz(50);
|
|
_rng.setRateHz(30);
|
|
_vio.setRateHz(30);
|
|
_airspeed.setRateHz(30); // TODO: check this rate
|
|
}
|
|
void SensorSimulator::setSensorRateToDefault()
|
|
{
|
|
_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});
|
|
_baro.setData(122.2f);
|
|
_gps.setData(_gps.getDefaultGpsData());
|
|
_flow.setData(_flow.dataAtRest());
|
|
_rng.setData(0.2f, 100);
|
|
_vio.setData(_vio.dataAtRest());
|
|
_airspeed.setData(0.0f, 0.0f);
|
|
}
|
|
void SensorSimulator::startBasicSensor()
|
|
{
|
|
_imu.start();
|
|
_mag.start();
|
|
_baro.start();
|
|
}
|
|
|
|
void SensorSimulator::runSeconds(float duration_seconds)
|
|
{
|
|
runMicroseconds( uint32_t(duration_seconds * 1e6f) );
|
|
}
|
|
|
|
void SensorSimulator::runMicroseconds(uint32_t duration)
|
|
{
|
|
// simulate in 1000us steps
|
|
const uint64_t start_time = _time;
|
|
|
|
for(;_time < start_time + (uint64_t)duration; _time+=1000)
|
|
{
|
|
updateSensors();
|
|
|
|
_ekf->update();
|
|
}
|
|
}
|
|
|
|
void SensorSimulator::updateSensors()
|
|
{
|
|
_imu.update(_time);
|
|
_mag.update(_time);
|
|
_baro.update(_time);
|
|
_gps.update(_time);
|
|
_flow.update(_time);
|
|
_rng.update(_time);
|
|
_vio.update(_time);
|
|
_airspeed.update(_time);
|
|
}
|
|
|
|
void SensorSimulator::setImuBias(Vector3f accel_bias, Vector3f gyro_bias)
|
|
{
|
|
_imu.setData(Vector3f{0.0f,0.0f,-CONSTANTS_ONE_G} + accel_bias,
|
|
Vector3f{0.0f,0.0f,0.0f} + gyro_bias);
|
|
}
|
|
|
|
void SensorSimulator::simulateOrientation(Quatf orientation)
|
|
{
|
|
const Vector3f world_sensed_gravity = {0.0f, 0.0f, -CONSTANTS_ONE_G};
|
|
const Vector3f world_mag_field = Vector3f{0.2f, 0.0f, 0.4f};
|
|
const Dcmf R_bodyToWorld(orientation);
|
|
const Vector3f sensed_gravity_body = R_bodyToWorld.transpose() * world_sensed_gravity;
|
|
const Vector3f body_mag_field = R_bodyToWorld.transpose() * world_mag_field;
|
|
|
|
_imu.setData(sensed_gravity_body, Vector3f{0.0f,0.0f,0.0f});
|
|
_mag.setData(body_mag_field);
|
|
}
|