From 4a7821e1640f40d4ea4c686ffa414f6fa7eaa36e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 23 Feb 2023 18:30:53 +0100 Subject: [PATCH 1/8] Gazebo simulation: Used Gazebo GPS plugin MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../init.d-posix/airframes/4001_gz_x500 | 2 +- Tools/simulation/gz/models/x500/model.sdf | 44 +++++++++++ Tools/simulation/gz/worlds/default.sdf | 11 +++ src/modules/simulation/gz_bridge/GZBridge.cpp | 74 ++++++++++++++++++- src/modules/simulation/gz_bridge/GZBridge.hpp | 8 +- 5 files changed, 135 insertions(+), 4 deletions(-) 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..7aa981555fc5 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -13,7 +13,7 @@ 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_GPSSIM 0 param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 diff --git a/Tools/simulation/gz/models/x500/model.sdf b/Tools/simulation/gz/models/x500/model.sdf index 39269e00c9cd..734c83dde5b7 100644 --- a/Tools/simulation/gz/models/x500/model.sdf +++ b/Tools/simulation/gz/models/x500/model.sdf @@ -231,6 +231,50 @@ 1 250 + + 1 + 10 + + + + + 0.0 + 0.06 + + + + + 0.0 + 0.158 + + + + + + + 1 + 250 + + + + 0.0 + 0.02 + + + + + 0.0 + 0.02 + + + + + 0.0 + 0.03 + + + + true diff --git a/Tools/simulation/gz/worlds/default.sdf b/Tools/simulation/gz/worlds/default.sdf index 31cb3b63ed30..a41436725200 100644 --- a/Tools/simulation/gz/worlds/default.sdf +++ b/Tools/simulation/gz/worlds/default.sdf @@ -11,9 +11,20 @@ + ogre2 + + + EARTH_WGS84 + ENU + 0 + 0 + 0 + 0 + + diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 885ae5f27322..c3c6b91843dd 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,14 @@ 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::navsatCallback, this)) { + PX4_ERR("failed to subscribe to %s", navsat_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 +389,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 +398,69 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) } #endif +///////////////////////////////////////////////// +void GZBridge::navsatCallback(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_east(); + sensor_gps.vel_e_m_s = navsat.velocity_north(); + sensor_gps.vel_d_m_s = navsat.velocity_up(); + sensor_gps.cog_rad = atan2(navsat.velocity_north(), + navsat.velocity_east()); // 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..d898afbc5694 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -55,13 +55,15 @@ #include #include #include +#include #include #include #include -#include #include +#include +#include using namespace time_literals; @@ -99,6 +101,7 @@ 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 navsatCallback(const gz::msgs::NavSat &_msg); // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -108,7 +111,8 @@ 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)}; From 8e9456b9074eabef1210f191c06a5def0964238a Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 23 Feb 2023 18:35:06 +0100 Subject: [PATCH 2/8] Removed magnetometer MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- Tools/simulation/gz/models/x500/model.sdf | 24 ----------------------- 1 file changed, 24 deletions(-) diff --git a/Tools/simulation/gz/models/x500/model.sdf b/Tools/simulation/gz/models/x500/model.sdf index 734c83dde5b7..3648af4ab266 100644 --- a/Tools/simulation/gz/models/x500/model.sdf +++ b/Tools/simulation/gz/models/x500/model.sdf @@ -251,30 +251,6 @@ - - 1 - 250 - - - - 0.0 - 0.02 - - - - - 0.0 - 0.02 - - - - - 0.0 - 0.03 - - - - true From 7767b9503df3198f99517c89176f36482c833a0c Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 23 Feb 2023 18:48:17 +0100 Subject: [PATCH 3/8] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../init.d-posix/airframes/4001_gz_x500 | 2 -- src/modules/simulation/gz_bridge/GZBridge.cpp | 12 ++++++------ src/modules/simulation/gz_bridge/GZBridge.hpp | 2 +- 3 files changed, 7 insertions(+), 9 deletions(-) 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 7aa981555fc5..c1723335b070 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 0 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default CA_AIRFRAME 0 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index c3c6b91843dd..cc50c2d8d390 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -176,7 +176,7 @@ int GZBridge::init() // 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::navsatCallback, this)) { + if (!_node.Subscribe(navsat_topic, &GZBridge::gpsCallback, this)) { PX4_ERR("failed to subscribe to %s", navsat_topic.c_str()); return PX4_ERROR; } @@ -399,7 +399,7 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed) #endif ///////////////////////////////////////////////// -void GZBridge::navsatCallback(const gz::msgs::NavSat &navsat) +void GZBridge::gpsCallback(const gz::msgs::NavSat &navsat) { if (hrt_absolute_time() == 0) { return; @@ -441,11 +441,11 @@ void GZBridge::navsatCallback(const gz::msgs::NavSat &navsat) 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_east(); - sensor_gps.vel_e_m_s = navsat.velocity_north(); + 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_north(), - navsat.velocity_east()); // Course over ground (NOT heading, but direction of movement), -PI..PI, (radians) + 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; diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index d898afbc5694..c01acf2f0042 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -101,7 +101,7 @@ 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 navsatCallback(const gz::msgs::NavSat &_msg); + void gpsCallback(const gz::msgs::NavSat &_msg); // Subscriptions uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; From 35f42747b65b881471c417b5bf5e359948e5381b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 23 Feb 2023 19:33:36 +0100 Subject: [PATCH 4/8] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/modules/simulation/gz_bridge/GZBridge.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index cc50c2d8d390..c5f7580a7f5b 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -175,7 +175,7 @@ int GZBridge::init() // 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"; + "/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; @@ -439,13 +439,13 @@ void GZBridge::gpsCallback(const gz::msgs::NavSat &navsat) 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) + 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) + 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; From abc123087be9ea2065d651fb3a9a3620d175f76e Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 24 Feb 2023 11:14:11 +0100 Subject: [PATCH 5/8] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/modules/simulation/gz_bridge/GZBridge.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index c5f7580a7f5b..76572b5e7932 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -175,7 +175,8 @@ int GZBridge::init() // 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"; + "/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; From 7a55bf9a1673cedfc603f56985c14dbce0e629d8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 24 Feb 2023 13:44:58 +0100 Subject: [PATCH 6/8] Added magnetometer MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- .../init.d-posix/airframes/4001_gz_x500 | 2 -- Tools/simulation/gz/models/x500/model.sdf | 4 +++ Tools/simulation/gz/worlds/default.sdf | 1 + src/modules/simulation/gz_bridge/GZBridge.cpp | 30 +++++++++++++++++++ src/modules/simulation/gz_bridge/GZBridge.hpp | 7 ++++- 5 files changed, 41 insertions(+), 3 deletions(-) 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 c1723335b070..cb7e267fc034 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_MAGSIM 1 - 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 3648af4ab266..07dd5b20173b 100644 --- a/Tools/simulation/gz/models/x500/model.sdf +++ b/Tools/simulation/gz/models/x500/model.sdf @@ -251,6 +251,10 @@ + + 1 + 10 + true diff --git a/Tools/simulation/gz/worlds/default.sdf b/Tools/simulation/gz/worlds/default.sdf index a41436725200..b25547825d52 100644 --- a/Tools/simulation/gz/worlds/default.sdf +++ b/Tools/simulation/gz/worlds/default.sdf @@ -12,6 +12,7 @@ + ogre2 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 76572b5e7932..f78174b6a91c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -182,6 +182,15 @@ int GZBridge::init() 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; @@ -399,6 +408,27 @@ 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) { diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index c01acf2f0042..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 @@ -64,6 +65,7 @@ #include #include #include +#include using namespace time_literals; @@ -101,7 +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 &_msg); + 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}; @@ -117,6 +120,8 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S 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}; From d2945bcf60876f22eb279a8dd91448bf13844cb3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Fri, 24 Feb 2023 14:00:45 +0100 Subject: [PATCH 7/8] make linters happy MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- src/modules/simulation/gz_bridge/GZBridge.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index f78174b6a91c..4ed646a91d0c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -418,13 +418,13 @@ void GZBridge::magnetometerCallback(const gz::msgs::Magnetometer &magnetometer) pthread_mutex_lock(&_node_mutex); gz::math::Vector3d mag(magnetometer.field_tesla().x(), - magnetometer.field_tesla().y(), - magnetometer.field_tesla().z()); + 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()); + mag.X(), mag.Y(), mag.Z()); pthread_mutex_unlock(&_node_mutex); } From 56a6c17444b529a34bb767f6d1ef9e86b0110ebe Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Mon, 27 Feb 2023 13:44:05 +0100 Subject: [PATCH 8/8] Added feedback MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Cordero --- ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 | 3 +++ Tools/simulation/gz/worlds/default.sdf | 7 +++---- 2 files changed, 6 insertions(+), 4 deletions(-) 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 cb7e267fc034..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,6 +13,9 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} param set-default SIM_GZ_EN 1 +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/worlds/default.sdf b/Tools/simulation/gz/worlds/default.sdf index b25547825d52..d9c90d9f5446 100644 --- a/Tools/simulation/gz/worlds/default.sdf +++ b/Tools/simulation/gz/worlds/default.sdf @@ -12,7 +12,6 @@ - ogre2 @@ -20,9 +19,9 @@ EARTH_WGS84 ENU - 0 - 0 - 0 + 47.397742 + 8.545594 + 488.0 0