UAV-develop/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-07-27 00:48:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

simulation/gz_bridge: ignition with no lockstep (#20561)

Browse Source 
When using ignition SITL simulation with NO_LOCKSTEP, the SITL PX4 fails to update the IMU data from Ignition Gazebo.

The timestamp for the IMU data is taken from the ignition message:

 - In LOCKSTEP mode the clock from the ignition simulation and the one from PX4 SITL are synchronized, hence everything works fine
 - In NO_LOCKSTEP mode, those clocks are not synchronized anymore, so the timestamp for the IMU data should not be the one from Ignition but the current time in PX4 SITL when receiving the message.
This commit is contained in:
dsix-ls2n
2022-11-22 17:26:11 +01:00
committed by GitHub
parent 55b454a8a5
commit 2833832968
1 changed files with 28 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

33
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

@@ -282,7 +282,6 @@ int GZBridge::task_spawn(int argc, char *argv[])
#else
return PX4_OK;
#endif // ENABLE_LOCKSTEP_SCHEDULER
//return PX4_OK;
@@ -353,14 +352,19 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
// publish accel
sensor_accel_s sensor_accel{};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
sensor_accel.timestamp_sample = time_us;
sensor_accel.timestamp = time_us;
#else
sensor_accel.timestamp_sample = hrt_absolute_time();
sensor_accel.timestamp = hrt_absolute_time();
#endif
sensor_accel.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
sensor_accel.x = accel_b.X();
sensor_accel.y = accel_b.Y();
sensor_accel.z = accel_b.Z();
sensor_accel.temperature = NAN;
sensor_accel.samples = 1;
sensor_accel.timestamp = time_us; // hrt_absolute_time();
_sensor_accel_pub.publish(sensor_accel);
@@ -371,14 +375,19 @@ void GZBridge::imuCallback(const ignition::msgs::IMU &imu)
// publish gyro
sensor_gyro_s sensor_gyro{};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
sensor_gyro.timestamp_sample = time_us;
sensor_gyro.timestamp = time_us;
#else
sensor_gyro.timestamp_sample = hrt_absolute_time();
sensor_gyro.timestamp = hrt_absolute_time();
#endif
sensor_gyro.device_id = 1310988; // 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION
sensor_gyro.x = gyro_b.X();
sensor_gyro.y = gyro_b.Y();
sensor_gyro.z = gyro_b.Z();
sensor_gyro.temperature = NAN;
sensor_gyro.samples = 1;
sensor_gyro.timestamp = time_us; // hrt_absolute_time();
_sensor_gyro_pub.publish(sensor_gyro);
pthread_mutex_unlock(&_mutex);
@@ -430,7 +439,11 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
// publish attitude groundtruth
vehicle_attitude_s vehicle_attitude_groundtruth{};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
vehicle_attitude_groundtruth.timestamp_sample = time_us;
#else
vehicle_attitude_groundtruth.timestamp_sample = hrt_absolute_time();
#endif
vehicle_attitude_groundtruth.q[0] = q_nb.W();
vehicle_attitude_groundtruth.q[1] = q_nb.X();
vehicle_attitude_groundtruth.q[2] = q_nb.Y();
@@ -441,8 +454,11 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
// publish angular velocity groundtruth
const matrix::Eulerf euler{matrix::Quatf(vehicle_attitude_groundtruth.q)};
vehicle_angular_velocity_s vehicle_angular_velocity_groundtruth{};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
vehicle_angular_velocity_groundtruth.timestamp_sample = time_us;
#else
vehicle_angular_velocity_groundtruth.timestamp_sample = hrt_absolute_time();
#endif
const matrix::Vector3f angular_velocity = (euler - _euler_prev) / dt;
_euler_prev = euler;
angular_velocity.copyTo(vehicle_angular_velocity_groundtruth.xyz);
@@ -455,8 +471,11 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
}
vehicle_local_position_s local_position_groundtruth{};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
local_position_groundtruth.timestamp_sample = time_us;
#else
local_position_groundtruth.timestamp_sample = hrt_absolute_time();
#endif
// position ENU -> NED
const matrix::Vector3d position{pose_position.y(), pose_position.x(), -pose_position.z()};
const matrix::Vector3d velocity{(position - _position_prev) / dt};
@@ -486,7 +505,11 @@ void GZBridge::poseInfoCallback(const ignition::msgs::Pose_V &pose)
if (_pos_ref.isInitialized()) {
// publish position groundtruth
vehicle_global_position_s global_position_groundtruth{};
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
global_position_groundtruth.timestamp_sample = time_us;
#else
global_position_groundtruth.timestamp_sample = hrt_absolute_time();
#endif
_pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y,
global_position_groundtruth.lat, global_position_groundtruth.lon);