diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 0df7d2ff49..e9f6a6e3f6 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -63,6 +63,10 @@ px4_add_module( module.yaml DEPENDS airspeed + drivers_accelerometer + drivers_barometer + drivers_gyroscope + drivers_magnetometer git_mavlink_v2 conversion git_ecl diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 65a2114596..2f10a84751 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -80,6 +80,14 @@ using matrix::wrap_2pi; +MavlinkReceiver::~MavlinkReceiver() +{ + delete _px4_accel; + delete _px4_baro; + delete _px4_gyro; + delete _px4_mag; +} + MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : ModuleParams(nullptr), _mavlink(parent), @@ -2085,57 +2093,70 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* gyro */ { - sensor_gyro_s gyro{}; + if (_px4_gyro == nullptr) { + // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION + _px4_gyro = new PX4Gyroscope(2294028); - gyro.timestamp = timestamp; - gyro.x = imu.xgyro; - gyro.y = imu.ygyro; - gyro.z = imu.zgyro; - gyro.temperature = imu.temperature; + if (_px4_gyro == nullptr) { + PX4_ERR("PX4Gyroscope alloc failed"); + } + } - _gyro_pub.publish(gyro); + if (_px4_gyro != nullptr) { + _px4_gyro->set_temperature(imu.temperature); + _px4_gyro->update(timestamp, imu.xgyro, imu.ygyro, imu.zgyro); + } } /* accelerometer */ { - sensor_accel_s accel{}; + if (_px4_accel == nullptr) { + // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_accel = new PX4Accelerometer(1311244); - accel.timestamp = timestamp; - accel.x = imu.xacc; - accel.y = imu.yacc; - accel.z = imu.zacc; - accel.temperature = imu.temperature; + if (_px4_accel == nullptr) { + PX4_ERR("PX4Accelerometer alloc failed"); + } + } - _accel_pub.publish(accel); + if (_px4_accel != nullptr) { + _px4_accel->set_temperature(imu.temperature); + _px4_accel->update(timestamp, imu.xacc, imu.yacc, imu.zacc); + } } /* magnetometer */ { - sensor_mag_s mag{}; + if (_px4_mag == nullptr) { + // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + _px4_mag = new PX4Magnetometer(197388); - mag.timestamp = timestamp; - mag.x_raw = imu.xmag * 1000.0f; - mag.y_raw = imu.ymag * 1000.0f; - mag.z_raw = imu.zmag * 1000.0f; - mag.x = imu.xmag; - mag.y = imu.ymag; - mag.z = imu.zmag; + if (_px4_mag == nullptr) { + PX4_ERR("PX4Magnetometer alloc failed"); + } + } - _mag_pub.publish(mag); + if (_px4_mag != nullptr) { + _px4_mag->set_temperature(imu.temperature); + _px4_mag->update(timestamp, imu.xmag, imu.ymag, imu.zmag); + } } /* baro */ { - sensor_baro_s baro{}; + if (_px4_baro == nullptr) { + // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + _px4_baro = new PX4Barometer(6620172); - baro.timestamp = timestamp; - baro.pressure = imu.abs_pressure; - baro.temperature = imu.temperature; + if (_px4_baro == nullptr) { + PX4_ERR("PX4Barometer alloc failed"); + } + } - /* fake device ID */ - baro.device_id = 1234567; - - _baro_pub.publish(baro); + if (_px4_baro != nullptr) { + _px4_baro->set_temperature(imu.temperature); + _px4_baro->update(timestamp, imu.abs_pressure); + } } /* battery status */ @@ -2498,28 +2519,36 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) /* accelerometer */ { - sensor_accel_s accel{}; + if (_px4_accel == nullptr) { + // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + _px4_accel = new PX4Accelerometer(1311244); - accel.timestamp = timestamp; - accel.x = hil_state.xacc; - accel.y = hil_state.yacc; - accel.z = hil_state.zacc; - accel.temperature = 25.0f; + if (_px4_accel == nullptr) { + PX4_ERR("PX4Accelerometer alloc failed"); + } + } - _accel_pub.publish(accel); + if (_px4_accel != nullptr) { + // accel in mG + _px4_accel->set_scale(CONSTANTS_ONE_G / 1000.0f); + _px4_accel->update(timestamp, hil_state.xacc, hil_state.yacc, hil_state.zacc); + } } /* gyroscope */ { - sensor_gyro_s gyro{}; + if (_px4_gyro == nullptr) { + // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION + _px4_gyro = new PX4Gyroscope(2294028); - gyro.timestamp = timestamp; - gyro.x = hil_state.rollspeed; - gyro.y = hil_state.pitchspeed; - gyro.z = hil_state.yawspeed; - gyro.temperature = 25.0f; + if (_px4_gyro == nullptr) { + PX4_ERR("PX4Gyroscope alloc failed"); + } + } - _gyro_pub.publish(gyro); + if (_px4_gyro != nullptr) { + _px4_gyro->update(timestamp, hil_state.rollspeed, hil_state.pitchspeed, hil_state.yawspeed); + } } /* battery status */ diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 98acb8ab1d..3e0dab395a 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -47,6 +47,10 @@ #include "mavlink_parameters.h" #include "mavlink_timesync.h" +#include +#include +#include +#include #include #include #include @@ -79,11 +83,6 @@ #include #include #include -#include -#include -#include -#include -#include #include #include #include @@ -107,7 +106,7 @@ class MavlinkReceiver : public ModuleParams { public: MavlinkReceiver(Mavlink *parent); - ~MavlinkReceiver() = default; + ~MavlinkReceiver() override; /** * Start the receiver thread @@ -258,10 +257,6 @@ private: uORB::PublicationMulti _manual_pub{ORB_ID(manual_control_setpoint), ORB_PRIO_LOW}; uORB::PublicationMulti _ping_pub{ORB_ID(ping), ORB_PRIO_LOW}; uORB::PublicationMulti _radio_status_pub{ORB_ID(radio_status), ORB_PRIO_LOW}; - uORB::PublicationMulti _accel_pub{ORB_ID(sensor_accel), ORB_PRIO_LOW}; - uORB::PublicationMulti _baro_pub{ORB_ID(sensor_baro), ORB_PRIO_LOW}; - uORB::PublicationMulti _gyro_pub{ORB_ID(sensor_gyro), ORB_PRIO_LOW}; - uORB::PublicationMulti _mag_pub{ORB_ID(sensor_mag), ORB_PRIO_LOW}; // ORB publications (queue length > 1) uORB::PublicationQueued _gps_inject_data_pub{ORB_ID(gps_inject_data)}; @@ -277,6 +272,12 @@ private: uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)}; uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; + // hil_sensor and hil_state_quaternion + PX4Accelerometer *_px4_accel{nullptr}; + PX4Barometer *_px4_baro{nullptr}; + PX4Gyroscope *_px4_gyro{nullptr}; + PX4Magnetometer *_px4_mag{nullptr}; + static constexpr unsigned int MOM_SWITCH_COUNT{8}; uint8_t _mom_switch_pos[MOM_SWITCH_COUNT] {}; uint16_t _mom_switch_state{0}; diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 7c1c7ac538..db0a9322e2 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -92,8 +92,8 @@ Sih *Sih::instantiate(int argc, char *argv[]) Sih::Sih() : ModuleParams(nullptr), - _loop_perf(perf_alloc(PC_ELAPSED, "sih_execution")), - _sampling_perf(perf_alloc(PC_ELAPSED, "sih_sampling")) + _loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": execution")), + _sampling_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": sampling")) { } @@ -333,34 +333,20 @@ void Sih::reconstruct_sensors_signals() void Sih::send_IMU() { // gyro - { - static constexpr float scaling = 1000.0f; - _px4_gyro.set_scale(1 / scaling); - _px4_gyro.set_temperature(T1_C); - _px4_gyro.update(_now, _gyro(0) * scaling, _gyro(1) * scaling, _gyro(2) * scaling); - } + _px4_gyro.set_temperature(T1_C); + _px4_gyro.update(_now, _gyro(0), _gyro(1), _gyro(2)); // accel - { - static constexpr float scaling = 1000.0f; - _px4_accel.set_scale(1 / scaling); - _px4_accel.set_temperature(T1_C); - _px4_accel.update(_now, _acc(0) * scaling, _acc(1) * scaling, _acc(2) * scaling); - } + _px4_accel.set_temperature(T1_C); + _px4_accel.update(_now, _acc(0), _acc(1), _acc(2)); // magnetometer - { - static constexpr float scaling = 1000.0f; - _px4_mag.set_scale(1 / scaling); - _px4_mag.set_temperature(T1_C); - _px4_mag.update(_now, _mag(0) * scaling, _mag(1) * scaling, _mag(2) * scaling); - } + _px4_mag.set_temperature(T1_C); + _px4_mag.update(_now, _mag(0), _mag(1), _mag(2)); // baro - { - _px4_baro.set_temperature(_baro_temp_c); - _px4_baro.update(_now, _baro_p_mBar); - } + _px4_baro.set_temperature(_baro_temp_c); + _px4_baro.update(_now, _baro_p_mBar); } void Sih::send_gps() diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 103152434e..e58d7de02d 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -195,26 +195,20 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor // gyro if (!_param_sim_gyro_block.get()) { - static constexpr float scaling = 1000.0f; - _px4_gyro.set_scale(1 / scaling); _px4_gyro.set_temperature(imu.temperature); - _px4_gyro.update(time, imu.xgyro * scaling, imu.ygyro * scaling, imu.zgyro * scaling); + _px4_gyro.update(time, imu.xgyro, imu.ygyro, imu.zgyro); } // accel if (!_param_sim_accel_block.get()) { - static constexpr float scaling = 1000.0f; - _px4_accel.set_scale(1 / scaling); _px4_accel.set_temperature(imu.temperature); - _px4_accel.update(time, imu.xacc * scaling, imu.yacc * scaling, imu.zacc * scaling); + _px4_accel.update(time, imu.xacc, imu.yacc, imu.zacc); } // magnetometer if (!_param_sim_mag_block.get()) { - static constexpr float scaling = 1000.0f; - _px4_mag.set_scale(1 / scaling); _px4_mag.set_temperature(imu.temperature); - _px4_mag.update(time, imu.xmag * scaling, imu.ymag * scaling, imu.zmag * scaling); + _px4_mag.update(time, imu.xmag, imu.ymag, imu.zmag); } // baro