From 18074dec5adf08aef920464813d895f607f539dd Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Tue, 29 Mar 2022 16:02:01 +0300 Subject: [PATCH] simulator: fix conversion from hPa to Pa Signed-off-by: RomanBapst --- src/modules/simulator/simulator.h | 2 ++ src/modules/simulator/simulator_mavlink.cpp | 6 +++--- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 8cdd807672a..c4ce824f18f 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -161,6 +161,8 @@ private: static Simulator *_instance; + static constexpr float hPa2Pa = 100.0f; // hectopascal to pascal multiplier + // simulated sensor instances static constexpr uint8_t ACCEL_COUNT_MAX = 3; PX4Accelerometer _px4_accel[ACCEL_COUNT_MAX] { diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ac1d9bb1d6f..83f1ff9a39b 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -328,7 +328,7 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_baro_blocked) { if (!_baro_stuck) { - _last_baro_pressure = sensors.abs_pressure; + _last_baro_pressure = sensors.abs_pressure * hPa2Pa; // convert hPa to Pa _last_baro_temperature = sensors.temperature; } @@ -354,8 +354,8 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor differential_pressure_s report{}; report.timestamp = time; report.temperature = _sensors_temperature; - report.differential_pressure_filtered_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar; - report.differential_pressure_raw_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar; + report.differential_pressure_filtered_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa; + report.differential_pressure_raw_pa = sensors.diff_pressure * hPa2Pa; // convert hPa to Pa; _differential_pressure_pub.publish(report); }