dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

mavlink: HIL_SENSOR receiver respect fields_updated 

- keep in sync with the simulator module
This commit is contained in:
Daniel Agar 2020-06-17 10:31:40 -04:00
parent be2081a2a2
commit 6afa7e4368
2 changed files with 52 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

75
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -2120,80 +2120,91 @@ MavlinkReceiver::get_message_interval(int msgId)
void
MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
{
mavlink_hil_sensor_t imu;
mavlink_msg_hil_sensor_decode(msg, &imu);
mavlink_hil_sensor_t hil_sensor;
mavlink_msg_hil_sensor_decode(msg, &hil_sensor);
const uint64_t timestamp = hrt_absolute_time();
/* airspeed */
{
airspeed_s airspeed{};
// temperature only updated with baro
float temperature = NAN;
float ias = calc_IAS(imu.diff_pressure * 1e2f);
float scale = 1.0f; //assume no instrument or pitot placement errors
float eas = calc_EAS_from_IAS(ias, scale);
float tas = calc_TAS_from_EAS(eas, imu.abs_pressure * 100, imu.temperature);
airspeed.timestamp = timestamp;
airspeed.indicated_airspeed_m_s = ias;
airspeed.true_airspeed_m_s = tas;
_airspeed_pub.publish(airspeed);
if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) {
temperature = hil_sensor.temperature;
}
/* gyro */
{
// gyro
if ((hil_sensor.fields_updated & SensorSource::GYRO) == SensorSource::GYRO) {
if (_px4_gyro == nullptr) {
// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_gyro = new PX4Gyroscope(1311244);
}
if (_px4_gyro != nullptr) {
_px4_gyro->set_temperature(imu.temperature);
_px4_gyro->update(timestamp, imu.xgyro, imu.ygyro, imu.zgyro);
if (PX4_ISFINITE(temperature)) {
_px4_gyro->set_temperature(temperature);
}
_px4_gyro->update(timestamp, hil_sensor.xgyro, hil_sensor.ygyro, hil_sensor.zgyro);
}
}
/* accelerometer */
{
// accelerometer
if ((hil_sensor.fields_updated & SensorSource::ACCEL) == SensorSource::ACCEL) {
if (_px4_accel == nullptr) {
// 1311244: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
_px4_accel = new PX4Accelerometer(1311244);
}
if (_px4_accel != nullptr) {
_px4_accel->set_temperature(imu.temperature);
_px4_accel->update(timestamp, imu.xacc, imu.yacc, imu.zacc);
if (PX4_ISFINITE(temperature)) {
_px4_accel->set_temperature(temperature);
}
_px4_accel->update(timestamp, hil_sensor.xacc, hil_sensor.yacc, hil_sensor.zacc);
}
}
/* magnetometer */
{
// magnetometer
if ((hil_sensor.fields_updated & SensorSource::MAG) == SensorSource::MAG) {
if (_px4_mag == nullptr) {
// 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION
_px4_mag = new PX4Magnetometer(197388);
}
if (_px4_mag != nullptr) {
_px4_mag->set_temperature(imu.temperature);
_px4_mag->update(timestamp, imu.xmag, imu.ymag, imu.zmag);
if (PX4_ISFINITE(temperature)) {
_px4_mag->set_temperature(temperature);
}
_px4_mag->update(timestamp, hil_sensor.xmag, hil_sensor.ymag, hil_sensor.zmag);
}
}
/* baro */
{
// baro
if ((hil_sensor.fields_updated & SensorSource::BARO) == SensorSource::BARO) {
if (_px4_baro == nullptr) {
// 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
_px4_baro = new PX4Barometer(6620172);
}
if (_px4_baro != nullptr) {
_px4_baro->set_temperature(imu.temperature);
_px4_baro->update(timestamp, imu.abs_pressure);
_px4_baro->set_temperature(hil_sensor.temperature);
_px4_baro->update(timestamp, hil_sensor.abs_pressure);
}
}
/* battery status */
// differential pressure
if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) {
differential_pressure_s report{};
report.timestamp = timestamp;
report.temperature = hil_sensor.temperature;
report.differential_pressure_filtered_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
report.differential_pressure_raw_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar;
_differential_pressure_pub.publish(report);
}
// battery status
{
battery_status_s hil_battery_status{};

10
src/modules/mavlink/mavlink_receiver.h
View File

@ -59,7 +59,6 @@
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cellular_status.h>
#include <uORB/topics/collision_report.h>
@ -67,6 +66,7 @@
#include <uORB/topics/debug_key_value.h>
#include <uORB/topics/debug_value.h>
#include <uORB/topics/debug_vect.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/follow_target.h>
#include <uORB/topics/gps_inject_data.h>
@ -237,6 +237,7 @@ private:
uORB::Publication<debug_key_value_s> _debug_key_value_pub{ORB_ID(debug_key_value)};
uORB::Publication<debug_value_s> _debug_value_pub{ORB_ID(debug_value)};
uORB::Publication<debug_vect_s> _debug_vect_pub{ORB_ID(debug_vect)};
uORB::Publication<differential_pressure_s> _differential_pressure_pub{ORB_ID(differential_pressure)};
uORB::Publication<follow_target_s> _follow_target_pub{ORB_ID(follow_target)};
uORB::Publication<irlock_report_s> _irlock_report_pub{ORB_ID(irlock_report)};
uORB::Publication<landing_target_pose_s> _landing_target_pose_pub{ORB_ID(landing_target_pose)};
@ -283,6 +284,13 @@ private:
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
// hil_sensor and hil_state_quaternion
enum SensorSource {
ACCEL = 0b111,
GYRO = 0b111000,
MAG = 0b111000000,
BARO = 0b1101000000000,
DIFF_PRESS = 0b10000000000
};
PX4Accelerometer *_px4_accel{nullptr};
PX4Barometer *_px4_baro{nullptr};
PX4Gyroscope *_px4_gyro{nullptr};