Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

Merge pull request #9116 from iNavFlight/dzikuvx-dsp-acc-zero

Browse source 
Acc processing with DSP
This commit is contained in:
Paweł Spychalski 2023-06-14 10:28:02 +02:00 • committed by GitHub
commit 710891f84d
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
3 changed files with 41 additions and 8 deletions
Download patch file Download diff file Expand all files Collapse all files

14
src/main/common/maths.c
View file

@ -577,4 +577,18 @@ void arm_scale_f32(
}
}
/**
* @brief Floating-point vector multiplication, equivalent of CMSIS arm_mult_f32.
*/
void arm_mult_f32(
float * pSrcA,
float * pSrcB,
float * pDst,
uint32_t blockSize)
{
for (uint32_t i = 0; i < blockSize; i++) {
pDst[i] = pSrcA[i] * pSrcB[i];
}
}
#endif

1
src/main/common/maths.h
View file

@ -206,4 +206,5 @@ float calc_length_pythagorean_3D(const float firstElement, const float secondEle
#ifdef SITL_BUILD
void arm_sub_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
void arm_scale_f32(float * pSrc, float scale, float * pDst, uint32_t blockSize);
void arm_mult_f32(float * pSrcA, float * pSrcB, float * pDst, uint32_t blockSize);
#endif

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

@ -82,6 +82,9 @@ static EXTENDED_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
static EXTENDED_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM float fAccZero[XYZ_AXIS_COUNT];
static EXTENDED_FASTRAM float fAccGain[XYZ_AXIS_COUNT];
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 5);
void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
@ -105,6 +108,17 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance)
);
}
static void updateAccCoefficients(void) {
for (uint8_t i = 0; i < XYZ_AXIS_COUNT; i++) {
//Float zero
fAccZero[i] = (float)accelerometerConfig()->accZero.raw[i];
//Float gain
fAccGain[i] = (float)accelerometerConfig()->accGain.raw[i] / 4096.0f;
}
}
static bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse)
{
accelerationSensor_e accHardware = ACC_NONE;
@ -280,6 +294,7 @@ bool accInit(uint32_t targetLooptime)
acc.accTargetLooptime = targetLooptime;
acc.accClipCount = 0;
accInitFilters();
updateAccCoefficients();
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.extremes[axis].min = 100;
@ -477,15 +492,20 @@ static void performAcclerationCalibration(void)
// saveConfigAndNotify will trigger eepromREAD and in turn call back the accelerometer gain validation
// that will set ENABLE_STATE(ACCELEROMETER_CALIBRATED) if all is good
saveConfigAndNotify();
//Recompute all coeffs
updateAccCoefficients();
}
}
}
static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const flightDynamicsTrims_t * accGain)
static void applyAccelerationZero(void)
{
accADC[X] = (accADC[X] - accZero->raw[X]) * accGain->raw[X] / 4096;
accADC[Y] = (accADC[Y] - accZero->raw[Y]) * accGain->raw[Y] / 4096;
accADC[Z] = (accADC[Z] - accZero->raw[Z]) * accGain->raw[Z] / 4096;
float tmp[XYZ_AXIS_COUNT];
//Apply zero
arm_sub_f32(accADC, fAccZero, tmp, XYZ_AXIS_COUNT);
//Apply gain
arm_mult_f32(tmp, fAccGain, accADC, XYZ_AXIS_COUNT);
}
/*
@ -493,9 +513,7 @@ static void applyAccelerationZero(const flightDynamicsTrims_t * accZero, const f
*/
void accGetMeasuredAcceleration(fpVector3_t *measuredAcc)
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
measuredAcc->v[axis] = acc.accADCf[axis] * GRAVITY_CMSS;
}
arm_scale_f32(acc.accADCf, GRAVITY_CMSS, measuredAcc->v, XYZ_AXIS_COUNT);
}
/*
@ -530,7 +548,7 @@ void accUpdate(void)
if (!ARMING_FLAG(SIMULATOR_MODE_SITL)) {
performAcclerationCalibration();
applyAccelerationZero(&accelerometerConfig()->accZero, &accelerometerConfig()->accGain);
applyAccelerationZero();
}
applySensorAlignment(accADC, accADC, acc.dev.accAlign);