fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08: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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Tools/sitl_gazebo

@ -1 +1 @@
Subproject commit 88ed2825628fb439055db1e2a90f11bdb7365a46
Subproject commit f78daf7cd2646bf62b2fb844bf0ce209cf7b2783

11
src/modules/simulator/simulator.h
View File

@ -198,7 +198,7 @@ private:
void send_controls();
void send_heartbeat();
void send_mavlink_message(const mavlink_message_t &aMsg);
void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &imu);
void update_sensors(const hrt_abstime &time, const mavlink_hil_sensor_t &sensors);
static void *sending_trampoline(void *);
@ -245,6 +245,15 @@ private:
WaitingForActuatorControls = 1,
WaitingForEkf2Timestamp = 2,
};
///! Enumeration to use on the bitmask in HIL_SENSOR
enum SensorSource {
ACCEL = 0x0007,
GYRO = 0x0038,
MAG = 0x01C0,
BARO = 0x1A00,
DIFF_PRESS = 0x0400
};
#endif
DEFINE_PARAMETERS(

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);
}