diff --git a/msg/sensor_combined.msg b/msg/sensor_combined.msg index 8a7045f9d7..f474900a34 100644 --- a/msg/sensor_combined.msg +++ b/msg/sensor_combined.msg @@ -7,11 +7,11 @@ # gyro timstamp is equal to the timestamp of the message float32[3] gyro_integral_rad # delta angle in the NED body frame in rad in the integration time frame -uint64 gyro_integral_dt # delta time for gyro integral in us +uint32 gyro_integral_dt # delta time for gyro integral in us uint64 accelerometer_timestamp # Accelerometer timestamp float32[3] accelerometer_integral_m_s # velocity in NED body frame, in m/s^2 -uint64 accelerometer_integral_dt # delta time for accel integral in us +uint32 accelerometer_integral_dt # delta time for accel integral in us uint64 magnetometer_timestamp # Magnetometer timestamp float32[3] magnetometer_ga # Magnetic field in NED body frame, in Gauss diff --git a/src/modules/ekf2_replay/ekf2_replay_main.cpp b/src/modules/ekf2_replay/ekf2_replay_main.cpp index b92014081f..a29a21774d 100644 --- a/src/modules/ekf2_replay/ekf2_replay_main.cpp +++ b/src/modules/ekf2_replay/ekf2_replay_main.cpp @@ -378,7 +378,7 @@ void Ekf2Replay::setEstimatorInput(uint8_t *data, uint8_t type) parseMessage(data, dest_ptr, type); _sensors.timestamp = replay_part1.time_ref; _sensors.gyro_integral_dt = replay_part1.gyro_integral_dt; - _sensors.accelerometer_integral_dt = replay_part1.accelerometer_integral_dt; + _sensors.accelerometer_integral_dt = (uint32_t)replay_part1.accelerometer_integral_dt; _sensors.magnetometer_timestamp = replay_part1.magnetometer_timestamp; _sensors.baro_timestamp = replay_part1.baro_timestamp; _sensors.gyro_integral_rad[0] = replay_part1.gyro_integral_x_rad; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 658998d879..a268d9b24b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1076,7 +1076,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) _last_sensor_data[i].accelerometer_integral_m_s[1] = vect_int(1); _last_sensor_data[i].accelerometer_integral_m_s[2] = vect_int(2); - _last_sensor_data[i].accelerometer_integral_dt = accel_report.integral_dt; + _last_sensor_data[i].accelerometer_integral_dt = (uint32_t)accel_report.integral_dt; float dt = accel_report.integral_dt / 1.e6f; sensor_value = vect_int / dt; @@ -1093,7 +1093,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) } _last_sensor_data[i].accelerometer_integral_dt = - accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp; + (uint32_t)(accel_report.timestamp - _last_sensor_data[i].accelerometer_timestamp); float dt = _last_sensor_data[i].accelerometer_integral_dt / 1.e6f; _last_sensor_data[i].accelerometer_integral_m_s[0] = vect_val(0) * dt; _last_sensor_data[i].accelerometer_integral_m_s[1] = vect_val(1) * dt; @@ -1149,7 +1149,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) _last_sensor_data[i].gyro_integral_rad[1] = vect_int(1); _last_sensor_data[i].gyro_integral_rad[2] = vect_int(2); - _last_sensor_data[i].gyro_integral_dt = gyro_report.integral_dt; + _last_sensor_data[i].gyro_integral_dt = (uint32_t)gyro_report.integral_dt; float dt = gyro_report.integral_dt / 1.e6f; sensor_value = vect_int / dt; @@ -1166,7 +1166,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) } _last_sensor_data[i].gyro_integral_dt = - gyro_report.timestamp - _last_sensor_data[i].timestamp; + (uint32_t)(gyro_report.timestamp - _last_sensor_data[i].timestamp); float dt = _last_sensor_data[i].gyro_integral_dt / 1.e6f; _last_sensor_data[i].gyro_integral_rad[0] = vect_val(0) * dt; _last_sensor_data[i].gyro_integral_rad[1] = vect_val(1) * dt;