diff --git a/src/modules/sih/CMakeLists.txt b/src/modules/sih/CMakeLists.txt index 5b41b047ca..23ebf62b91 100644 --- a/src/modules/sih/CMakeLists.txt +++ b/src/modules/sih/CMakeLists.txt @@ -41,4 +41,8 @@ px4_add_module( sih.cpp DEPENDS mathlib + drivers_accelerometer + drivers_barometer + drivers_gyroscope + drivers_magnetometer ) diff --git a/src/modules/sih/sih.cpp b/src/modules/sih/sih.cpp index 16f6029e6f..dd366aa5a9 100644 --- a/src/modules/sih/sih.cpp +++ b/src/modules/sih/sih.cpp @@ -46,6 +46,7 @@ #include #include // to get PWM flags +#include #include // to get the HIL status #include @@ -283,28 +284,6 @@ void Sih::init_variables() void Sih::init_sensors() { - - _sensor_accel.device_id=1; - _sensor_accel.error_count=0; - _sensor_accel.integral_dt=0; - _sensor_accel.temperature=T1_C; - _sensor_accel.scaling=0.0f; - - _sensor_gyro.device_id=1; - _sensor_gyro.error_count=0; - _sensor_gyro.integral_dt=0; - _sensor_gyro.temperature=T1_C; - _sensor_gyro.scaling=0.0f; - - _sensor_mag.device_id=1; - _sensor_mag.error_count=0; - _sensor_mag.temperature=T1_C; - _sensor_mag.scaling=0.0f; - _sensor_mag.is_external=false; - - _sensor_baro.error_count=0; - _sensor_baro.device_id=1; - _vehicle_gps_pos.fix_type=3; // 3D fix _vehicle_gps_pos.satellites_used=8; _vehicle_gps_pos.heading=NAN; @@ -410,44 +389,35 @@ void Sih::reconstruct_sensors_signals() void Sih::send_IMU() { - _sensor_accel.timestamp=_now; - _sensor_accel.x=_acc(0); - _sensor_accel.y=_acc(1); - _sensor_accel.z=_acc(2); - if (_sensor_accel_pub != nullptr) { - orb_publish(ORB_ID(sensor_accel), _sensor_accel_pub, &_sensor_accel); - } else { - _sensor_accel_pub = orb_advertise(ORB_ID(sensor_accel), &_sensor_accel); - } + // 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); + } - _sensor_gyro.timestamp=_now; - _sensor_gyro.x=_gyro(0); - _sensor_gyro.y=_gyro(1); - _sensor_gyro.z=_gyro(2); - if (_sensor_gyro_pub != nullptr) { - orb_publish(ORB_ID(sensor_gyro), _sensor_gyro_pub, &_sensor_gyro); - } else { - _sensor_gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &_sensor_gyro); - } + // 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); + } - _sensor_mag.timestamp=_now; - _sensor_mag.x=_mag(0); - _sensor_mag.y=_mag(1); - _sensor_mag.z=_mag(2); - if (_sensor_mag_pub != nullptr) { - orb_publish(ORB_ID(sensor_mag), _sensor_mag_pub, &_sensor_mag); - } else { - _sensor_mag_pub = orb_advertise(ORB_ID(sensor_mag), &_sensor_mag); - } + // 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); + } - _sensor_baro.timestamp=_now; - _sensor_baro.pressure=_baro_p_mBar; - _sensor_baro.temperature=_baro_temp_c; - if (_sensor_baro_pub != nullptr) { - orb_publish(ORB_ID(sensor_baro), _sensor_baro_pub, &_sensor_baro); - } else { - _sensor_baro_pub = orb_advertise(ORB_ID(sensor_baro), &_sensor_baro); - } + // baro + { + _px4_baro.set_temperature(_baro_temp_c); + _px4_baro.update(_now, _baro_p_mBar); + } } void Sih::send_gps() @@ -501,7 +471,7 @@ void Sih::publish_sih() } else { _att_gt_sub = orb_advertise(ORB_ID(vehicle_attitude_groundtruth), &_att_gt); } -} +} float Sih::generate_wgn() // generate white Gaussian noise sample with std=1 { diff --git a/src/modules/sih/sih.hpp b/src/modules/sih/sih.hpp index 0dde73782c..a185668bb2 100644 --- a/src/modules/sih/sih.hpp +++ b/src/modules/sih/sih.hpp @@ -41,17 +41,15 @@ #include // math::radians, #include // to get the physical constants #include // to get the real time +#include +#include +#include +#include #include - #include -#include -#include -#include -#include -#include -#include #include // to publish groundtruth #include // to publish groundtruth +#include extern "C" __EXPORT int sih_main(int argc, char *argv[]); @@ -98,18 +96,12 @@ private: void parameters_update_poll(); void parameters_updated(); - // to publish the sensor baro - struct sensor_baro_s _sensor_baro {}; - orb_advert_t _sensor_baro_pub{nullptr}; - // to publish the sensor mag - struct sensor_mag_s _sensor_mag {}; - orb_advert_t _sensor_mag_pub{nullptr}; - // to publish the sensor gyroscope - struct sensor_gyro_s _sensor_gyro {}; - orb_advert_t _sensor_gyro_pub{nullptr}; - // to publish the sensor accelerometer - struct sensor_accel_s _sensor_accel {}; - orb_advert_t _sensor_accel_pub{nullptr}; + // simulated sensor instances + PX4Accelerometer _px4_accel{ 1311244, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 1311244: DRV_ACC_DEVTYPE_ACCELSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Gyroscope _px4_gyro{ 2294028, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 2294028: DRV_GYR_DEVTYPE_GYROSIM, BUS: 1, ADDR: 2, TYPE: SIMULATION + PX4Magnetometer _px4_mag{ 197388, ORB_PRIO_DEFAULT, ROTATION_NONE }; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 3, ADDR: 1, TYPE: SIMULATION + PX4Barometer _px4_baro{ 6620172, ORB_PRIO_DEFAULT }; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION + // to publish the gps position struct vehicle_gps_position_s _vehicle_gps_pos {}; orb_advert_t _vehicle_gps_pos_pub{nullptr};