Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +03:00
Code Issues Wiki Activity

Merge pull request #1573 from martinbudden/bf_gyro_efficiency

Browse source 
Improved efficiency of gyro update
This commit is contained in:
J Blackman 2016-11-19 17:08:43 +11:00 • committed by GitHub
commit 5daa6a2ef0
3 changed files with 53 additions and 55 deletions
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,

88
src/main/sensors/boardalignment.c
View file

@ -64,51 +64,53 @@ 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));
const int32_t x = src[X];
const int32_t y = src[Y];
const int32_t z = src[Z];
// 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] = x;
dest[Y] = y;
dest[Z] = z;
break;
case CW90_DEG:
dest[X] = y;
dest[Y] = -x;
dest[Z] = z;
break;
case CW180_DEG:
dest[X] = -x;
dest[Y] = -y;
dest[Z] = z;
break;
case CW270_DEG:
dest[X] = -y;
dest[Y] = x;
dest[Z] = z;
break;
case CW0_DEG_FLIP:
dest[X] = -x;
dest[Y] = y;
dest[Z] = -z;
break;
case CW90_DEG_FLIP:
dest[X] = y;
dest[Y] = x;
dest[Z] = -z;
break;
case CW180_DEG_FLIP:
dest[X] = x;
dest[Y] = -y;
dest[Z] = -z;
break;
case CW270_DEG_FLIP:
dest[X] = -y;
dest[Y] = -x;
dest[Z] = -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++) {