Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 09:45:33 +03:00
Code Issues Wiki Activity

Debug vibration

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2018-05-13 21:25:06 +10:00
parent ce3abc0ad3
commit c74905cbfd
5 changed files with 26 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

15
src/main/flight/imu.c
View file

@ -548,6 +548,19 @@ void imuUpdateAccelerometer(void)
#endif
}
void imuCheckVibrationLevels(void)
{
fpVector3_t accVibeLevels;
accGetVibrationLevels(&accVibeLevels);
const uint32_t accClipCount = accGetClipCount();
DEBUG_SET(DEBUG_VIBE, 0, accVibeLevels.x * 100);
DEBUG_SET(DEBUG_VIBE, 1, accVibeLevels.y * 100);
DEBUG_SET(DEBUG_VIBE, 2, accVibeLevels.z * 100);
DEBUG_SET(DEBUG_VIBE, 3, accClipCount);
}
void imuUpdateAttitude(timeUs_t currentTimeUs)
{
/* Calculate dT */
@ -560,6 +573,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
if (!hilActive) {
gyroGetMeasuredRotationRate(&imuMeasuredRotationBF); // Calculate gyro rate in body frame in rad/s
accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s
imuCheckVibrationLevels();
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
}
else {
@ -569,6 +583,7 @@ void imuUpdateAttitude(timeUs_t currentTimeUs)
#else
gyroGetMeasuredRotationRate(&imuMeasuredRotationBF); // Calculate gyro rate in body frame in rad/s
accGetMeasuredAcceleration(&imuMeasuredAccelBF); // Calculate accel in body frame in cm/s/s
imuCheckVibrationLevels();
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
#endif
} else {