diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index bb6bedd386d1..3bf41266bef1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -13,9 +13,8 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 +param set-default EKF2_MAG_CHECK 0 param set-default CA_AIRFRAME 0 param set-default CA_ROTOR_COUNT 4 diff --git a/Tools/simulation/gz/models/x500/model.sdf b/Tools/simulation/gz/models/x500/model.sdf index 39269e00c9cd..07dd5b20173b 100644 --- a/Tools/simulation/gz/models/x500/model.sdf +++ b/Tools/simulation/gz/models/x500/model.sdf @@ -231,6 +231,30 @@ 1 250 + + 1 + 10 + + + + + 0.0 + 0.06 + + + + + 0.0 + 0.158 + + + + + + + 1 + 10 + true diff --git a/Tools/simulation/gz/worlds/default.sdf b/Tools/simulation/gz/worlds/default.sdf index 31cb3b63ed30..d9c90d9f5446 100644 --- a/Tools/simulation/gz/worlds/default.sdf +++ b/Tools/simulation/gz/worlds/default.sdf @@ -11,9 +11,20 @@ + ogre2 + + + EARTH_WGS84 + ENU + 47.397742 + 8.545594 + 488.0 + 0 + + diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 885ae5f27322..4ed646a91d0c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -35,6 +35,7 @@ #include +#include #include #include @@ -172,6 +173,24 @@ int GZBridge::init() return PX4_ERROR; } + // GPS: /world/$WORLD/model/$MODEL/link/base_link/sensor/navsat_sensor/navsat + std::string navsat_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/base_link/sensor/navsat_sensor/navsat"; + + if (!_node.Subscribe(navsat_topic, &GZBridge::gpsCallback, this)) { + PX4_ERR("failed to subscribe to %s", navsat_topic.c_str()); + return PX4_ERROR; + } + + // magnetometer: /world/$WORLD/model/$MODEL/link/base_link/sensor/magnetometer_sensor/magnetometer + std::string magnetometer_topic = "/world/" + _world_name + "/model/" + _model_name + + "/link/base_link/sensor/magnetometer_sensor/magnetometer"; + + if (!_node.Subscribe(magnetometer_topic, &GZBridge::magnetometerCallback, this)) { + PX4_ERR("failed to subscribe to %s", magnetometer_topic.c_str()); + return PX4_ERROR; + } + if (!_mixing_interface_esc.init(_model_name)) { PX4_ERR("failed to init ESC output"); return PX4_ERROR; @@ -380,7 +399,7 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION report.differential_pressure_pa = static_cast(air_speed_value); // hPa to Pa; report.temperature = static_cast(air_speed.temperature()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C - report.timestamp = hrt_absolute_time();; + report.timestamp = hrt_absolute_time(); _differential_pressure_pub.publish(report); this->_temperature = report.temperature; @@ -389,6 +408,90 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) } #endif +///////////////////////////////////////////////// +void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &magnetometer) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_node_mutex); + + gz::math::Vector3d mag(magnetometer.field_tesla().x(), + magnetometer.field_tesla().y(), + magnetometer.field_tesla().z()); + gz::math::Quaternion q(0.0, 0.0, -1.57); + mag = q.RotateVector(mag); + + _px4_mag.update(hrt_absolute_time(), + mag.X(), mag.Y(), mag.Z()); + + pthread_mutex_unlock(&_node_mutex); +} + +///////////////////////////////////////////////// +void GZBridge::gpsCallback(const gz::msgs::NavSat &navsat) +{ + if (hrt_absolute_time() == 0) { + return; + } + + pthread_mutex_lock(&_node_mutex); + + const uint64_t time_us = (navsat.header().stamp().sec() * 1000000) + + (navsat.header().stamp().nsec() / 1000); + + sensor_gps_s sensor_gps{}; + + // device id + 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_GPS_DEVTYPE_SIM; + + // fix + sensor_gps.fix_type = 3; // 3D fix + sensor_gps.s_variance_m_s = 0.5f; + sensor_gps.c_variance_rad = 0.1f; + sensor_gps.eph = 0.9f; + sensor_gps.epv = 1.78f; + sensor_gps.hdop = 0.7f; + sensor_gps.vdop = 1.1f; + + sensor_gps.timestamp_sample = time_us; + sensor_gps.time_utc_usec = 0; + sensor_gps.device_id = device_id.devid; + sensor_gps.lat = roundf(navsat.latitude_deg() * 1e7); // Latitude in 1E-7 degrees + sensor_gps.lon = roundf(navsat.longitude_deg() * 1e7); // Longitude in 1E-7 degrees + sensor_gps.alt = roundf(navsat.altitude() * 1000); // Altitude in 1E-3 meters above MSL, (millimetres) + + sensor_gps.alt_ellipsoid = sensor_gps.alt; + sensor_gps.noise_per_ms = 0; + sensor_gps.jamming_indicator = 0; + sensor_gps.vel_m_s = sqrtf( + navsat.velocity_east() * navsat.velocity_east() + + navsat.velocity_north() * navsat.velocity_north()); // GPS ground speed, (metres/sec) + sensor_gps.vel_n_m_s = navsat.velocity_north(); + sensor_gps.vel_e_m_s = navsat.velocity_east(); + sensor_gps.vel_d_m_s = navsat.velocity_up(); + sensor_gps.cog_rad = atan2(navsat.velocity_east(), + navsat.velocity_north()); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) + + sensor_gps.timestamp_time_relative = 0; + sensor_gps.heading = NAN; + sensor_gps.heading_offset = NAN; + sensor_gps.heading_accuracy = 0; + sensor_gps.automatic_gain_control = 0; + sensor_gps.jamming_state = 0; + sensor_gps.vel_ned_valid = true; + sensor_gps.satellites_used = 13; + sensor_gps.timestamp = hrt_absolute_time(); + _sensor_gps_pub.publish(sensor_gps); + + pthread_mutex_unlock(&_node_mutex); +} + void GZBridge::imuCallback(const gz::msgs::IMU &imu) { if (hrt_absolute_time() == 0) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 42ac2ae82885..f6e78337f799 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -55,13 +56,16 @@ #include #include #include +#include #include #include #include -#include #include +#include +#include +#include using namespace time_literals; @@ -99,6 +103,8 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void barometerCallback(const gz::msgs::FluidPressure &air_pressure); void imuCallback(const gz::msgs::IMU &imu); void poseInfoCallback(const gz::msgs::Pose_V &pose); + void gpsCallback(const gz::msgs::NavSat &navsat); + void magnetometerCallback(const gz::msgs::Magnetometer &magnetometer); // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -108,11 +114,14 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + PX4Magnetometer _px4_mag{197388, ROTATION_NONE}; // 197388: DRV_MAG_DEVTYPE_MAGSIM, BUS: 1, ADDR: 1, TYPE: SIMULATION + GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex};