yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

simulator: make sensor update rate dependent of the HIL_SENSOR bitmask

Browse Source
This commit is contained in:
TSC21
2020-02-25 10:58:30 +00:00
committed by Nuno Marques
parent e6ab5fc2a8
commit bcb6056a12
3 changed files with 28 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
src/modules/simulator/simulator_mavlink.cpp
View File

@@ -191,43 +191,39 @@ void Simulator::send_controls()
}
}
void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu)
void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors)
{
if ((imu.fields_updated & 0x1FFF) != 0x1FFF) {
PX4_DEBUG("All sensor fields in mavlink HIL_SENSOR packet not updated. Got %08x", imu.fields_updated);
}
// gyro
if (!_param_sim_gyro_block.get()) {
_px4_gyro.set_temperature(imu.temperature);
_px4_gyro.update(time, imu.xgyro, imu.ygyro, imu.zgyro);
if ((sensors.fields_updated & SensorSource::GYRO) == SensorSource::GYRO && !_param_sim_gyro_block.get()) {
_px4_gyro.set_temperature(sensors.temperature);
_px4_gyro.update(time, sensors.xgyro, sensors.ygyro, sensors.zgyro);
}
// accel
if (!_param_sim_accel_block.get()) {
_px4_accel.set_temperature(imu.temperature);
_px4_accel.update(time, imu.xacc, imu.yacc, imu.zacc);
if ((sensors.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL && !_param_sim_accel_block.get()) {
_px4_accel.set_temperature(sensors.temperature);
_px4_accel.update(time, sensors.xacc, sensors.yacc, sensors.zacc);
}
// magnetometer
if (!_param_sim_mag_block.get()) {
_px4_mag.set_temperature(imu.temperature);
_px4_mag.update(time, imu.xmag, imu.ymag, imu.zmag);
if ((sensors.fields_updated & SensorSource::MAG) == SensorSource::MAG && !_param_sim_mag_block.get()) {
_px4_mag.set_temperature(sensors.temperature);
_px4_mag.update(time, sensors.xmag, sensors.ymag, sensors.zmag);
}
// baro
if (!_param_sim_baro_block.get()) {
_px4_baro.set_temperature(imu.temperature);
_px4_baro.update(time, imu.abs_pressure);
if ((sensors.fields_updated & SensorSource::BARO) == SensorSource::BARO && !_param_sim_baro_block.get()) {
_px4_baro.set_temperature(sensors.temperature);
_px4_baro.update(time, sensors.abs_pressure);
}
// differential pressure
if (!_param_sim_dpres_block.get()) {
if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_param_sim_dpres_block.get()) {
differential_pressure_s report{};
report.timestamp = time;
report.temperature = imu.temperature;
report.differential_pressure_filtered_pa = imu.diff_pressure * 100.0f; // convert from millibar to bar;
report.differential_pressure_raw_pa = imu.diff_pressure * 100.0f; // convert from millibar to bar;
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;
_differential_pressure_pub.publish(report);
}