diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 274f10c3e9..d8ca75e806 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1048,6 +1048,15 @@ Sensors::accel_poll(struct sensor_combined_s &raw) raw.accelerometer_m_s2[1] = vect(1); raw.accelerometer_m_s2[2] = vect(2); + math::Vector<3> vect_int(accel_report.x_integral, accel_report.y_integral, accel_report.z_integral); + vect_int = _board_rotation * vect_int; + + raw.accelerometer_integral_m_s[0] = vect_int(0); + raw.accelerometer_integral_m_s[1] = vect_int(1); + raw.accelerometer_integral_m_s[2] = vect_int(2); + + raw.accelerometer_integral_dt = accel_report.integral_dt; + raw.accelerometer_raw[0] = accel_report.x_raw; raw.accelerometer_raw[1] = accel_report.y_raw; raw.accelerometer_raw[2] = accel_report.z_raw; @@ -1125,6 +1134,15 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) raw.gyro_rad_s[1] = vect(1); raw.gyro_rad_s[2] = vect(2); + math::Vector<3> vect_int(gyro_report.x_integral, gyro_report.y_integral, gyro_report.z_integral); + vect_int = _board_rotation * vect_int; + + raw.gyro_integral_rad[0] = vect_int(0); + raw.gyro_integral_rad[1] = vect_int(1); + raw.gyro_integral_rad[2] = vect_int(2); + + raw.gyro_integral_dt = gyro_report.integral_dt; + raw.gyro_raw[0] = gyro_report.x_raw; raw.gyro_raw[1] = gyro_report.y_raw; raw.gyro_raw[2] = gyro_report.z_raw; @@ -2169,9 +2187,6 @@ Sensors::task_main() /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vcontrol_mode_sub, 200); - /* rate limit gyro to 250 Hz (the gyro signal is lowpassed accordingly earlier) */ - orb_set_interval(_gyro_sub, 4); - /* * do advertisements */