diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index f99444f9..27bb6ba3 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -26,7 +26,7 @@ STEER_RATIO = 15. pm = messaging.PubMaster(['roadCameraState', 'roadCameraBuffer', 'wideRoadCameraState', - 'wideRoadCameraBuffer', 'sensorEvents', 'can', "gpsLocationExternal"]) + 'wideRoadCameraBuffer','accelerometer', 'gyroscope', 'can', "gpsLocationExternal"]) sm = messaging.SubMaster(['carControl', 'controlsState']) def parse_args(add_args=None): @@ -105,18 +105,26 @@ def _cam_callback(self, image, frame_id, fram_size, intrinsics, state_topic, buf pm.send(buffer_topic, dat) def imu_callback(imu, vehicle_state): - vehicle_state.bearing_deg = math.degrees(imu.compass) - dat = messaging.new_message('sensorEvents', 2) - dat.sensorEvents[0].sensor = 4 - dat.sensorEvents[0].type = 0x10 - dat.sensorEvents[0].init('acceleration') - dat.sensorEvents[0].acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z] - # copied these numbers from locationd - dat.sensorEvents[1].sensor = 5 - dat.sensorEvents[1].type = 0x10 - dat.sensorEvents[1].init('gyroUncalibrated') - dat.sensorEvents[1].gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z] - pm.send('sensorEvents', dat) + # send 5x since 'sensor_tick' doesn't seem to work. limited by the world tick? + for _ in range(5): + vehicle_state.bearing_deg = math.degrees(imu.compass) + dat = messaging.new_message('accelerometer') + dat.accelerometer.sensor = 4 + dat.accelerometer.type = 0x10 + dat.accelerometer.timestamp = dat.logMonoTime # TODO: use the IMU timestamp + dat.accelerometer.init('acceleration') + dat.accelerometer.acceleration.v = [imu.accelerometer.x, imu.accelerometer.y, imu.accelerometer.z] + pm.send('accelerometer', dat) + + # copied these numbers from locationd + dat = messaging.new_message('gyroscope') + dat.gyroscope.sensor = 5 + dat.gyroscope.type = 0x10 + dat.gyroscope.timestamp = dat.logMonoTime # TODO: use the IMU timestamp + dat.gyroscope.init('gyroUncalibrated') + dat.gyroscope.gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z] + pm.send('gyroscope', dat) + time.sleep(0.01) def panda_state_function(vs: VehicleState, exit_event: threading.Event): pm = messaging.PubMaster(['pandaStates'])