Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-09 01:18:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_InertialSensor: fixed orientation of batch sampled data

Browse Source 
we need to rotate the data so that FFT displays match the configured
orientation
This commit is contained in:
Andrew Tridgell 2023-08-28 12:02:12 +10:00
parent 0ebf9e7af5
commit 2688848cd1
1 changed files with 11 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
View File

@ -662,23 +662,32 @@ void AP_InertialSensor_Backend::_notify_new_delta_velocity(uint8_t instance, con
} }
void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &accel) void AP_InertialSensor_Backend::_notify_new_accel_sensor_rate_sample(uint8_t instance, const Vector3f &_accel)
{ {
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
if (!_imu.batchsampler.doing_sensor_rate_logging()) { if (!_imu.batchsampler.doing_sensor_rate_logging()) {
return; return;
} }
// get batch sampling in correct orientation
Vector3f accel = _accel;
accel.rotate(_imu._accel_orientation[instance]);
_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, AP_HAL::micros64(), accel); _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_ACCEL, AP_HAL::micros64(), accel);
#endif #endif
} }
void AP_InertialSensor_Backend::_notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &gyro) void AP_InertialSensor_Backend::_notify_new_gyro_sensor_rate_sample(uint8_t instance, const Vector3f &_gyro)
{ {
#if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED #if AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED
if (!_imu.batchsampler.doing_sensor_rate_logging()) { if (!_imu.batchsampler.doing_sensor_rate_logging()) {
return; return;
} }
// get batch sampling in correct orientation
Vector3f gyro = _gyro;
gyro.rotate(_imu._gyro_orientation[instance]);
_imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, AP_HAL::micros64(), gyro); _imu.batchsampler.sample(instance, AP_InertialSensor::IMU_SENSOR_TYPE_GYRO, AP_HAL::micros64(), gyro);
#endif #endif
} }