diff --git a/msg/SensorBaro.msg b/msg/SensorBaro.msg index f13096d9aa9..8329e7e8a9f 100644 --- a/msg/SensorBaro.msg +++ b/msg/SensorBaro.msg @@ -1,5 +1,5 @@ # Barometer sensor -# +# # This is populated by barometer drivers and used by the EKF2 estimator. # The information is published in the `SCALED_PRESSURE_n` MAVLink messages (along with information from a corresponding `DifferentialPressure` instance). @@ -8,7 +8,7 @@ uint64 timestamp_sample # [us] Time of raw data capture uint32 device_id # [-] Unique device ID for the sensor that does not change between power cycles float32 pressure # [Pa] Static pressure measurement -float32 temperature # [degC] Temperature. +float32 temperature # [°C] Temperature. uint32 error_count # [-] Number of errors detected by driver. uint8 ORB_QUEUE_LENGTH = 4 diff --git a/msg/SensorMag.msg b/msg/SensorMag.msg index 1b5ba487edb..5e814e41d74 100644 --- a/msg/SensorMag.msg +++ b/msg/SensorMag.msg @@ -3,11 +3,11 @@ uint64 timestamp_sample uint32 device_id # unique device ID for the sensor that does not change between power cycles -float32 x # magnetic field in the FRD board frame X-axis in Gauss -float32 y # magnetic field in the FRD board frame Y-axis in Gauss -float32 z # magnetic field in the FRD board frame Z-axis in Gauss +float32 x # [Gauss] magnetic field in the FRD board frame X-axis +float32 y # [Gauss] magnetic field in the FRD board frame Y-axis +float32 z # [Gauss] magnetic field in the FRD board frame Z-axis -float32 temperature # temperature in degrees Celsius +float32 temperature # [°C] Temperature. uint32 error_count diff --git a/src/modules/simulation/gz_bridge/CMakeLists.txt b/src/modules/simulation/gz_bridge/CMakeLists.txt index 90bd2694218..28998f53dbf 100644 --- a/src/modules/simulation/gz_bridge/CMakeLists.txt +++ b/src/modules/simulation/gz_bridge/CMakeLists.txt @@ -69,6 +69,10 @@ if (gz-transport_FOUND) GZGimbal.cpp GZGimbal.hpp DEPENDS + drivers_accelerometer + drivers_gyroscope + drivers_magnetometer + drivers_rangefinder mixer_module px4_work_queue ${GZ_TRANSPORT_TARGET} diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 3bbd170282f..2818d6eb0a3 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -390,29 +390,12 @@ void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &msg) { const uint64_t timestamp = hrt_absolute_time(); - device::Device::DeviceId id{}; - id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - id.devid_s.devtype = DRV_MAG_DEVTYPE_MAGSIM; - id.devid_s.bus = 1; - - // Parameters CAL_MAGx_ID set to 0x3030C and 0x3040C in init.d-posix so only address 3 and 4 are valid for sim magnetometers (unless overwritten) - // See: https://github.com/PX4/PX4-Autopilot/blob/main/ROMFS/px4fmu_common/init.d-posix/rcS#L146-L149 - id.devid_s.address = 3; - - sensor_mag_s report{}; - report.timestamp = timestamp; - report.timestamp_sample = timestamp; - report.device_id = id.devid; - report.temperature = this->_temperature; + _px4_mag.set_temperature(_temperature); // this will be static if no airspeed sensor is on the model. // FIXME: once we're on jetty or later // The magnetometer plugin publishes in units of gauss and in a weird left handed coordinate system // https://github.com/gazebosim/gz-sim/pull/2460 - report.x = -msg.field_tesla().y(); - report.y = -msg.field_tesla().x(); - report.z = msg.field_tesla().z(); - - _sensor_mag_pub.publish(report); + _px4_mag.update(timestamp, -msg.field_tesla().y(), -msg.field_tesla().x(), msg.field_tesla().z()); } void GZBridge::airPressureCallback(const gz::msgs::FluidPressure &msg) @@ -430,7 +413,7 @@ void GZBridge::airPressureCallback(const gz::msgs::FluidPressure &msg) report.timestamp_sample = timestamp; report.device_id = id.devid; report.pressure = msg.pressure(); - report.temperature = this->_temperature; + report.temperature = _temperature; // this will be static if no airspeed sensor is on the model. _sensor_baro_pub.publish(report); } @@ -449,10 +432,9 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &msg) report.timestamp_sample = timestamp; report.device_id = id.devid; report.differential_pressure_pa = msg.diff_pressure(); // hPa to Pa; - report.temperature = static_cast(msg.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C + _temperature = static_cast(msg.temperature()) + atmosphere::kAbsoluteNullCelsius; // K to C + report.temperature = _temperature; _differential_pressure_pub.publish(report); - - this->_temperature = report.temperature; } void GZBridge::imuCallback(const gz::msgs::IMU &msg) @@ -467,42 +449,14 @@ void GZBridge::imuCallback(const gz::msgs::IMU &msg) msg.linear_acceleration().y(), msg.linear_acceleration().z())); - device::Device::DeviceId id{}; - id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - id.devid_s.devtype = DRV_IMU_DEVTYPE_SIM; - id.devid_s.bus = 1; - id.devid_s.address = 1; - - // publish accel - sensor_accel_s accel{}; - - accel.timestamp_sample = timestamp; - accel.timestamp = timestamp; - accel.device_id = id.devid; - - accel.x = accel_b.X(); - accel.y = accel_b.Y(); - accel.z = accel_b.Z(); - accel.temperature = NAN; - accel.samples = 1; - _sensor_accel_pub.publish(accel); + _px4_accel.update(timestamp, accel_b.X(), accel_b.Y(), accel_b.Z()); gz::math::Vector3d gyro_b = q_FLU_to_FRD.RotateVector(gz::math::Vector3d( msg.angular_velocity().x(), msg.angular_velocity().y(), msg.angular_velocity().z())); - // publish gyro - sensor_gyro_s gyro{}; - gyro.timestamp_sample = timestamp; - gyro.timestamp = timestamp; - gyro.device_id = id.devid; - gyro.x = gyro_b.X(); - gyro.y = gyro_b.Y(); - gyro.z = gyro_b.Z(); - gyro.temperature = NAN; - gyro.samples = 1; - _sensor_gyro_pub.publish(gyro); + _px4_gyro.update(timestamp, gyro_b.X(), gyro_b.Y(), gyro_b.Z()); } void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &msg) @@ -810,21 +764,9 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &msg) void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg) { - device::Device::DeviceId id{}; - id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; - id.devid_s.bus = 1; - id.devid_s.address = 1; - - distance_sensor_s report{}; - report.timestamp = hrt_absolute_time(); - report.device_id = id.devid; - report.min_distance = static_cast(msg.range_min()); - report.max_distance = static_cast(msg.range_max()); - report.current_distance = static_cast(msg.ranges()[0]); - report.variance = 0.0f; - report.signal_quality = -1; - report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; + _px4_rangefinder.set_min_distance(static_cast(msg.range_min())); + _px4_rangefinder.set_max_distance(static_cast(msg.range_max())); + _px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); gz::msgs::Quaternion pose_orientation = msg.world_pose().orientation(); gz::math::Quaterniond q_sensor = gz::math::Quaterniond( @@ -839,24 +781,30 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg) const gz::math::Quaterniond q_down(0, 1, 0, 0); + const float distance = static_cast(msg.ranges()[0]); + if (q_sensor.Equal(q_front, 0.03)) { - report.orientation = distance_sensor_s::ROTATION_FORWARD_FACING; + _px4_rangefinder.set_orientation(distance_sensor_s::ROTATION_FORWARD_FACING); + _px4_rangefinder.update(hrt_absolute_time(), distance); } else if (q_sensor.Equal(q_down, 0.03)) { - report.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; + _px4_rangefinder.set_orientation(distance_sensor_s::ROTATION_DOWNWARD_FACING); + _px4_rangefinder.update(hrt_absolute_time(), distance); } else if (q_sensor.Equal(q_left, 0.03)) { - report.orientation = distance_sensor_s::ROTATION_LEFT_FACING; + _px4_rangefinder.set_orientation(distance_sensor_s::ROTATION_LEFT_FACING); + _px4_rangefinder.update(hrt_absolute_time(), distance); } else { - report.orientation = distance_sensor_s::ROTATION_CUSTOM; - report.q[0] = q_sensor.W(); - report.q[1] = q_sensor.X(); - report.q[2] = q_sensor.Y(); - report.q[3] = q_sensor.Z(); + _px4_rangefinder.set_orientation(distance_sensor_s::ROTATION_CUSTOM); + const float q[4] = { + static_cast(q_sensor.W()), + static_cast(q_sensor.X()), + static_cast(q_sensor.Y()), + static_cast(q_sensor.Z()) + }; + _px4_rangefinder.update(hrt_absolute_time(), distance, -1, q, 4); } - - _distance_sensor_pub.publish(report); } void GZBridge::laserScanCallback(const gz::msgs::LaserScan &msg) diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index f821372034b..d03df89f3f9 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -45,6 +45,10 @@ #include #include #include +#include +#include +#include +#include #include #include @@ -53,11 +57,8 @@ #include #include #include -#include -#include #include #include -#include #include #include #include @@ -143,7 +144,12 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; + // simulated sensors using the general-purpose driver wrappers + PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Gyroscope _px4_gyro{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + PX4Magnetometer _px4_mag{197388}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 3, TYPE: SIMULATION + PX4Rangefinder _px4_rangefinder{10092812}; // 10092812: DRV_DIST_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; @@ -152,9 +158,6 @@ private: uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; - uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; - uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; - uORB::PublicationMulti _sensor_mag_pub{ORB_ID(sensor_mag)}; uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; uORB::PublicationMulti _optical_flow_pub{ORB_ID(sensor_optical_flow)}; @@ -176,7 +179,7 @@ private: const std::string _world_name; const std::string _model_name; - float _temperature{288.15}; // 15 degrees + float _temperature{15.0f}; // default temperature in Celsius bool _realtime_clock_set{false}; gz::transport::Node _node; diff --git a/src/modules/simulation/simulator_sih/CMakeLists.txt b/src/modules/simulation/simulator_sih/CMakeLists.txt index 9bbb190de30..1b9d3e56272 100644 --- a/src/modules/simulation/simulator_sih/CMakeLists.txt +++ b/src/modules/simulation/simulator_sih/CMakeLists.txt @@ -46,6 +46,7 @@ px4_add_module( mathlib drivers_accelerometer drivers_gyroscope + drivers_rangefinder geo ) diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index cb91df4dfd1..96ec64e8113 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -318,6 +318,8 @@ void Sih::parameters_updated() _distance_snsr_min = _sih_distance_snsr_min.get(); _distance_snsr_max = _sih_distance_snsr_max.get(); _distance_snsr_override = _sih_distance_snsr_override.get(); + _px4_rangefinder.set_min_distance(_distance_snsr_min); + _px4_rangefinder.set_max_distance(_distance_snsr_max); _T_TAU = _sih_thrust_tau.get(); @@ -679,36 +681,21 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) void Sih::send_dist_snsr(const hrt_abstime &time_now_us) { - device::Device::DeviceId device_id; - device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; - device_id.devid_s.bus = 0; - device_id.devid_s.address = 0; - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM; - - distance_sensor_s distance_sensor{}; - // distance_sensor.timestamp_sample = time_now_us; - distance_sensor.device_id = device_id.devid; - distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER; - distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING; - distance_sensor.min_distance = _distance_snsr_min; - distance_sensor.max_distance = _distance_snsr_max; - distance_sensor.signal_quality = -1; + float current_distance; if (_distance_snsr_override >= 0.f) { - distance_sensor.current_distance = _distance_snsr_override; + current_distance = _distance_snsr_override; } else { - distance_sensor.current_distance = -_lpos(2) / _q.dcm_z()(2); + current_distance = -_lpos(2) / _q.dcm_z()(2); - if (distance_sensor.current_distance > _distance_snsr_max) { + if (current_distance > _distance_snsr_max) { // this is based on lightware lw20 behaviour - distance_sensor.current_distance = UINT16_MAX / 100.f; - + current_distance = UINT16_MAX / 100.f; } } - distance_sensor.timestamp = hrt_absolute_time(); - _distance_snsr_pub.publish(distance_sensor); + _px4_rangefinder.update(hrt_absolute_time(), current_distance); } void Sih::send_ranging_beacon(const hrt_abstime &time_now_us) diff --git a/src/modules/simulation/simulator_sih/sih.hpp b/src/modules/simulation/simulator_sih/sih.hpp index 7e64672ea84..b245f35d3b4 100644 --- a/src/modules/simulation/simulator_sih/sih.hpp +++ b/src/modules/simulation/simulator_sih/sih.hpp @@ -65,6 +65,7 @@ #include // to get the real time #include #include +#include #include #include #include @@ -127,7 +128,7 @@ private: // simulated sensors PX4Accelerometer _px4_accel{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION PX4Gyroscope _px4_gyro{1310988}; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION - uORB::Publication _distance_snsr_pub{ORB_ID(distance_sensor)}; + PX4Rangefinder _px4_rangefinder{10092548}; // 10092548: DRV_DIST_DEVTYPE_SIM, BUS: 0, ADDR: 0, TYPE: SIMULATION uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; uORB::Publication _ranging_beacon_pub{ORB_ID(ranging_beacon)};