diff --git a/src/modules/simulator/simulator.cpp b/src/modules/simulator/simulator.cpp index f0345ba036..f8c8fcec0f 100644 --- a/src/modules/simulator/simulator.cpp +++ b/src/modules/simulator/simulator.cpp @@ -86,6 +86,11 @@ bool Simulator::getGPSSample(uint8_t *buf, int len) return _gps.copyData(buf, len); } +bool Simulator::getAirspeedSample(uint8_t *buf, int len) +{ + return _airspeed.copyData(buf, len); +} + void Simulator::write_MPU_data(void *buf) { _mpu.writeData(buf); } @@ -106,6 +111,10 @@ void Simulator::write_gps_data(void *buf) { _gps.writeData(buf); } +void Simulator::write_airspeed_data(void *buf) { + _airspeed.writeData(buf); +} + int Simulator::start(int argc, char *argv[]) { int ret = 0; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index dbb3d5b783..af4b8a746c 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -94,6 +94,13 @@ struct RawBaroData { }; #pragma pack(pop) +#pragma pack(push, 1) +struct RawAirspeedData { + float temperature; + float diff_pressure; +}; +#pragma pack(pop) + #pragma pack(push, 1) struct RawGPSData { int32_t lat; @@ -191,12 +198,14 @@ public: bool getMPUReport(uint8_t *buf, int len); bool getBaroSample(uint8_t *buf, int len); bool getGPSSample(uint8_t *buf, int len); + bool getAirspeedSample(uint8_t *buf, int len); void write_MPU_data(void *buf); void write_accel_data(void *buf); void write_mag_data(void *buf); void write_baro_data(void *buf); void write_gps_data(void *buf); + void write_airspeed_data(void *buf); bool isInitialized() { return _initialized; } @@ -207,6 +216,7 @@ private: _baro(1), _mag(1), _gps(1), + _airspeed(1), _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), @@ -238,6 +248,7 @@ private: simulator::Report _baro; simulator::Report _mag; simulator::Report _gps; + simulator::Report _airspeed; // uORB publisher handlers orb_advert_t _accel_pub; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ec3a7c8a24..ddbc79db76 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -187,11 +187,17 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu) { RawBaroData baro; // calculate air pressure from altitude (valid for low altitude) - baro.pressure = PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt; + baro.pressure = (PRESS_GROUND - GRAVITY * DENSITY * imu->pressure_alt) / 100.0f; // convert from Pa to mbar baro.altitude = imu->pressure_alt; baro.temperature = imu->temperature; write_baro_data((void *)&baro); + + RawAirspeedData airspeed; + airspeed.temperature = imu->temperature; + airspeed.diff_pressure = imu->diff_pressure; + + write_airspeed_data((void *)&airspeed); } void Simulator::update_gps(mavlink_hil_gps_t *gps_sim) {