Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +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

10
src/main/sensors/acceleration.c
View file

@ -314,6 +314,7 @@ bool accInit(uint32_t targetLooptime)
acc.dev.acc_1G = 256; // set default
acc.dev.initFn(&acc.dev);
acc.accTargetLooptime = targetLooptime;
acc.accClipCount = 0;
accInitFilters();
if (accelerometerConfig()->acc_align != ALIGN_DEFAULT) {
@ -541,7 +542,7 @@ void accUpdate(void)
// Before filtering check for clipping and vibration levels
if (ABS(acc.accADCf[X]) > ACC_CLIPPING_THRESHOLD_G || ABS(acc.accADCf[Y]) > ACC_CLIPPING_THRESHOLD_G || ABS(acc.accADCf[Z]) > ACC_CLIPPING_THRESHOLD_G) {
debug[3]++;
acc.accClipCount++;
}
// Calculate vibration levels
@ -552,8 +553,6 @@ void accUpdate(void)
// calc difference from this sample and 5hz filtered value, square and filter at 2hz
const float accDiff = acc.accADCf[axis] - accFloorFilt;
acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
debug[axis] = acc.accVibeSq[axis] * 100;
}
// Filter acceleration
@ -588,6 +587,11 @@ float accGetVibrationLevel(void)
return sqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
}
uint32_t accGetClipCount(void)
{
return acc.accClipCount;
}
void accSetCalibrationValues(void)
{
if ((accelerometerConfig()->accZero.raw[X] == 0) && (accelerometerConfig()->accZero.raw[Y] == 0) && (accelerometerConfig()->accZero.raw[Z] == 0) &&