rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Improved efficiency of gyro update

This commit is contained in:
Martin Budden 2016-11-15 22:39:22 +00:00
parent 0afecc4740
commit 8480570510
3 changed files with 55 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/main/fc/mw.c
View File

@ -688,7 +688,8 @@ void processRx(uint32_t currentTime)
void subTaskPidController(void)
{
const uint32_t startTime = micros();
uint32_t startTime;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
// PID - note this is function pointer set by setPIDController()
pidController(
&currentProfile->pidProfile,

90
src/main/sensors/boardalignment.c
View File

@ -64,51 +64,55 @@ static void alignBoard(int32_t *vec)
void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation)
{
static uint32_t swap[3];
memcpy(swap, src, sizeof(swap));
int32_t x;
// note src and dest may point to the same address
switch (rotation) {
default:
case CW0_DEG:
dest[X] = swap[X];
dest[Y] = swap[Y];
dest[Z] = swap[Z];
break;
case CW90_DEG:
dest[X] = swap[Y];
dest[Y] = -swap[X];
dest[Z] = swap[Z];
break;
case CW180_DEG:
dest[X] = -swap[X];
dest[Y] = -swap[Y];
dest[Z] = swap[Z];
break;
case CW270_DEG:
dest[X] = -swap[Y];
dest[Y] = swap[X];
dest[Z] = swap[Z];
break;
case CW0_DEG_FLIP:
dest[X] = -swap[X];
dest[Y] = swap[Y];
dest[Z] = -swap[Z];
break;
case CW90_DEG_FLIP:
dest[X] = swap[Y];
dest[Y] = swap[X];
dest[Z] = -swap[Z];
break;
case CW180_DEG_FLIP:
dest[X] = swap[X];
dest[Y] = -swap[Y];
dest[Z] = -swap[Z];
break;
case CW270_DEG_FLIP:
dest[X] = -swap[Y];
dest[Y] = -swap[X];
dest[Z] = -swap[Z];
break;
default:
case CW0_DEG:
dest[X] = src[X];
dest[Y] = src[Y];
dest[Z] = src[Z];
break;
case CW90_DEG:
x = src[X];
dest[X] = src[Y];
dest[Y] = -x;
dest[Z] = src[Z];
break;
case CW180_DEG:
dest[X] = -src[X];
dest[Y] = -src[Y];
dest[Z] = src[Z];
break;
case CW270_DEG:
x = src[X];
dest[X] = -src[Y];
dest[Y] = x;
dest[Z] = src[Z];
break;
case CW0_DEG_FLIP:
dest[X] = -src[X];
dest[Y] = src[Y];
dest[Z] = -src[Z];
break;
case CW90_DEG_FLIP:
x = src[X];
dest[X] = src[Y];
dest[Y] = x;
dest[Z] = -src[Z];
break;
case CW180_DEG_FLIP:
dest[X] = src[X];
dest[Y] = -src[Y];
dest[Z] = -src[Z];
break;
case CW270_DEG_FLIP:
x = src[X];
dest[X] = -src[Y];
dest[Y] = -x;
dest[Z] = -src[Z];
break;
}
if (!standardBoardAlignment)

17
src/main/sensors/gyro.c
View File

@ -157,13 +157,6 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
}
static void applyGyroZero(void)
{
for (int axis = 0; axis < 3; axis++) {
gyroADC[axis] -= gyroZero[axis];
}
}
void gyroUpdate(void)
{
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
@ -173,9 +166,9 @@ void gyroUpdate(void)
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = gyroADCRaw[axis];
}
gyroADC[X] = gyroADCRaw[X];
gyroADC[Y] = gyroADCRaw[Y];
gyroADC[Z] = gyroADCRaw[Z];
alignSensors(gyroADC, gyroADC, gyroAlign);
@ -183,7 +176,9 @@ void gyroUpdate(void)
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();
gyroADC[X] -= gyroZero[X];
gyroADC[Y] -= gyroZero[Y];
gyroADC[Z] -= gyroZero[Z];
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {