mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 02:34:06 +08:00
319 lines
8.0 KiB
C++
319 lines
8.0 KiB
C++
#include "sensor_simulator.h"
|
|
|
|
|
|
SensorSimulator::SensorSimulator(std::shared_ptr<Ekf> ekf):
|
|
_airspeed(ekf),
|
|
_baro(ekf),
|
|
_flow(ekf),
|
|
_gps(ekf),
|
|
_imu(ekf),
|
|
_mag(ekf),
|
|
_rng(ekf),
|
|
_vio(ekf),
|
|
_ekf{ekf}
|
|
{
|
|
setSensorRateToDefault();
|
|
setSensorDataToDefault();
|
|
startBasicSensor();
|
|
}
|
|
|
|
SensorSimulator::~SensorSimulator()
|
|
{
|
|
|
|
}
|
|
|
|
void SensorSimulator::loadSensorDataFromFile(std::string file_name)
|
|
{
|
|
std::ifstream file(file_name);
|
|
std::string line;
|
|
|
|
while (!file.eof()) {
|
|
std::string timestamp;
|
|
std::string sensor_type;
|
|
std::string sensor_data;
|
|
sensor_info sensor_sample;
|
|
|
|
getline(file, timestamp, ',');
|
|
|
|
if (!timestamp.compare("")){ // empty line at end of file
|
|
break;
|
|
}
|
|
sensor_sample.timestamp = std::stoul(timestamp);
|
|
|
|
if(_replay_data.size() > 0) {
|
|
sensor_info last_sample = _replay_data.back();
|
|
if (sensor_sample.timestamp < last_sample.timestamp)
|
|
{
|
|
std::cout << "Timestamps not sorted ascendingly" << std::endl;
|
|
exit(-1);
|
|
}
|
|
}
|
|
|
|
getline(file, sensor_type, ',');
|
|
if (!sensor_type.compare("imu")) {
|
|
sensor_sample.sensor_type = sensor_info::IMU;
|
|
} else if (!sensor_type.compare("mag")) {
|
|
sensor_sample.sensor_type = sensor_info::MAG;
|
|
|
|
} else if (!sensor_type.compare("baro")) {
|
|
sensor_sample.sensor_type = sensor_info::BARO;
|
|
|
|
} else if (!sensor_type.compare("gps")) {
|
|
sensor_sample.sensor_type = sensor_info::GPS;
|
|
|
|
} else if (!sensor_type.compare("airspeed")) {
|
|
sensor_sample.sensor_type = sensor_info::AIRSPEED;
|
|
|
|
} else if (!sensor_type.compare("range")) {
|
|
sensor_sample.sensor_type = sensor_info::RANGE;
|
|
|
|
} else if (!sensor_type.compare("flow")) {
|
|
sensor_sample.sensor_type = sensor_info::FLOW;
|
|
|
|
} else if (!sensor_type.compare("vio")) {
|
|
sensor_sample.sensor_type = sensor_info::VISION;
|
|
|
|
} else if (!sensor_type.compare("landed")) {
|
|
sensor_sample.sensor_type = sensor_info::LANDING_STATUS;
|
|
|
|
} else {
|
|
std::cout << "Sensor type in file unknown" << std::endl;
|
|
exit(-1);
|
|
}
|
|
|
|
getline(file, sensor_data);
|
|
std::stringstream ss(sensor_data);
|
|
int8_t i = 0;
|
|
while( ss.good() )
|
|
{
|
|
if(i>=10){
|
|
std::cout << "sensor data bigger than expected" << std::endl;
|
|
exit(-1);
|
|
}
|
|
std::string value_string;
|
|
getline( ss, value_string, ',' );
|
|
if(!value_string.compare("")){
|
|
continue;
|
|
}
|
|
sensor_sample.sensor_data[i] = std::stod(value_string);
|
|
i++;
|
|
}
|
|
_replay_data.emplace_back(sensor_sample);
|
|
}
|
|
file.close();
|
|
_has_replay_data = true;
|
|
}
|
|
|
|
void SensorSimulator::setSensorRateToDefault()
|
|
{
|
|
_imu.setRateHz(200);
|
|
_mag.setRateHz(80);
|
|
_baro.setRateHz(80);
|
|
_gps.setRateHz(5);
|
|
_flow.setRateHz(50);
|
|
_rng.setRateHz(30);
|
|
_vio.setRateHz(30);
|
|
_airspeed.setRateHz(100);
|
|
}
|
|
|
|
void SensorSimulator::setSensorDataToDefault()
|
|
{
|
|
_airspeed.setData(0.0f, 0.0f);
|
|
_baro.setData(122.2f);
|
|
_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());
|
|
}
|
|
|
|
void SensorSimulator::startBasicSensor()
|
|
{
|
|
_baro.start();
|
|
_imu.start();
|
|
_mag.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)
|
|
{
|
|
bool update_imu = _imu.should_send(_time);
|
|
updateSensors();
|
|
|
|
if (update_imu) {
|
|
// Update at IMU rate
|
|
_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::runReplaySeconds(float duration_seconds)
|
|
{
|
|
runReplayMicroseconds( uint32_t(duration_seconds * 1e6f) );
|
|
}
|
|
|
|
void SensorSimulator::runReplayMicroseconds(uint32_t duration)
|
|
{
|
|
if(!_has_replay_data) {
|
|
std::cout << "Can not run replay without replay data" << std::endl;
|
|
exit(-1);
|
|
}
|
|
// simulate in 1000us steps
|
|
const uint64_t start_time = _time;
|
|
|
|
for(;_time < start_time + duration; _time+=1000)
|
|
{
|
|
setSensorDataFromReplayData();
|
|
|
|
bool update_imu = _imu.should_send(_time);
|
|
updateSensors();
|
|
|
|
if(update_imu)
|
|
{
|
|
_ekf->update();
|
|
}
|
|
}
|
|
}
|
|
|
|
void SensorSimulator::setSensorDataFromReplayData()
|
|
{
|
|
if(_replay_data.size() > 0) {
|
|
sensor_info sample = _replay_data[_current_replay_data_index];
|
|
while(sample.timestamp < _time)
|
|
{
|
|
setSingleReplaySample(sample);
|
|
if(_current_replay_data_index < _replay_data.size())
|
|
{
|
|
_current_replay_data_index ++;
|
|
} else {
|
|
break;
|
|
}
|
|
sample = _replay_data[_current_replay_data_index];
|
|
}
|
|
} else {
|
|
std::cerr << "Loaded replay data empty. Likely could not load replay data" << std::endl;
|
|
exit(-1);
|
|
}
|
|
}
|
|
|
|
void SensorSimulator::setSingleReplaySample(const sensor_info& sample)
|
|
{
|
|
if (sample.sensor_type == sensor_info::IMU) {
|
|
Vector3f accel{(float) sample.sensor_data[0],
|
|
(float) sample.sensor_data[1],
|
|
(float) sample.sensor_data[2]};
|
|
Vector3f gyro{(float) sample.sensor_data[3],
|
|
(float) sample.sensor_data[4],
|
|
(float) sample.sensor_data[5]};
|
|
_imu.setData(accel, gyro);
|
|
|
|
} else if (sample.sensor_type == sensor_info::MAG) {
|
|
Vector3f mag{(float) sample.sensor_data[0],
|
|
(float) sample.sensor_data[1],
|
|
(float) sample.sensor_data[2]};
|
|
_mag.setData(mag);
|
|
|
|
} else if (sample.sensor_type == sensor_info::BARO) {
|
|
_baro.setData((float) sample.sensor_data[0]);
|
|
|
|
} else if (sample.sensor_type == sensor_info::GPS) {
|
|
_gps.setAltitude((int32_t) sample.sensor_data[0]);
|
|
_gps.setLatitude((int32_t) sample.sensor_data[1]);
|
|
_gps.setLongitude((int32_t) sample.sensor_data[2]);
|
|
_gps.setVelocity(Vector3f((float) sample.sensor_data[3],
|
|
(float) sample.sensor_data[4],
|
|
(float) sample.sensor_data[5]));
|
|
|
|
} else if (sample.sensor_type == sensor_info::AIRSPEED) {
|
|
_airspeed.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]);
|
|
|
|
} else if (sample.sensor_type == sensor_info::RANGE) {
|
|
_rng.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]);
|
|
|
|
} else if (sample.sensor_type == sensor_info::FLOW) {
|
|
flowSample flow_sample;
|
|
flow_sample.flow_xy_rad = Vector2f(sample.sensor_data[0],
|
|
sample.sensor_data[1]);
|
|
flow_sample.gyro_xyz = Vector3f(sample.sensor_data[2],
|
|
sample.sensor_data[3],
|
|
sample.sensor_data[4]);
|
|
flow_sample.quality = sample.sensor_data[5];
|
|
_flow.setData(flow_sample);
|
|
|
|
} else if (sample.sensor_type == sensor_info::VISION) {
|
|
// sensor not yet implemented
|
|
|
|
// extVisionSample vision_sample;
|
|
// vision_sample.pos;
|
|
// vision_sample.quat;
|
|
// vision_sample.vel;
|
|
// _vio.setData((float) sample.sensor_data[0], (float) sample.sensor_data[1]);
|
|
|
|
} else if (sample.sensor_type == sensor_info::LANDING_STATUS) {
|
|
bool landed = sample.sensor_data[0];
|
|
_ekf->set_in_air_status(!landed);
|
|
|
|
} else {
|
|
printf("Unknown sensor type, can not set replay sample");
|
|
exit(-1);
|
|
}
|
|
}
|
|
|
|
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,
|
|
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);
|
|
}
|