Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +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) 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() // PID - note this is function pointer set by setPIDController()
pidController( pidController(
&currentProfile->pidProfile, &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) void alignSensors(const int32_t *src, int32_t *dest, uint8_t rotation)
{ {
static uint32_t swap[3]; const int32_t x = src[X];
memcpy(swap, src, sizeof(swap)); const int32_t y = src[Y];
const int32_t z = src[Z];
// note src and dest may point to the same address
switch (rotation) { switch (rotation) {
default: default:
case CW0_DEG: case CW0_DEG:
dest[X] = swap[X]; dest[X] = x;
dest[Y] = swap[Y]; dest[Y] = y;
dest[Z] = swap[Z]; dest[Z] = z;
break; break;
case CW90_DEG: case CW90_DEG:
dest[X] = swap[Y]; dest[X] = y;
dest[Y] = -swap[X]; dest[Y] = -x;
dest[Z] = swap[Z]; dest[Z] = z;
break; break;
case CW180_DEG: case CW180_DEG:
dest[X] = -swap[X]; dest[X] = -x;
dest[Y] = -swap[Y]; dest[Y] = -y;
dest[Z] = swap[Z]; dest[Z] = z;
break; break;
case CW270_DEG: case CW270_DEG:
dest[X] = -swap[Y]; dest[X] = -y;
dest[Y] = swap[X]; dest[Y] = x;
dest[Z] = swap[Z]; dest[Z] = z;
break; break;
case CW0_DEG_FLIP: case CW0_DEG_FLIP:
dest[X] = -swap[X]; dest[X] = -x;
dest[Y] = swap[Y]; dest[Y] = y;
dest[Z] = -swap[Z]; dest[Z] = -z;
break; break;
case CW90_DEG_FLIP: case CW90_DEG_FLIP:
dest[X] = swap[Y]; dest[X] = y;
dest[Y] = swap[X]; dest[Y] = x;
dest[Z] = -swap[Z]; dest[Z] = -z;
break; break;
case CW180_DEG_FLIP: case CW180_DEG_FLIP:
dest[X] = swap[X]; dest[X] = x;
dest[Y] = -swap[Y]; dest[Y] = -y;
dest[Z] = -swap[Z]; dest[Z] = -z;
break; break;
case CW270_DEG_FLIP: case CW270_DEG_FLIP:
dest[X] = -swap[Y]; dest[X] = -y;
dest[Y] = -swap[X]; dest[Y] = -x;
dest[Z] = -swap[Z]; dest[Z] = -z;
break; break;
} }
if (!standardBoardAlignment) 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) void gyroUpdate(void)
{ {
int16_t gyroADCRaw[XYZ_AXIS_COUNT]; int16_t gyroADCRaw[XYZ_AXIS_COUNT];
@ -173,9 +166,9 @@ void gyroUpdate(void)
return; return;
} }
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { gyroADC[X] = gyroADCRaw[X];
gyroADC[axis] = gyroADCRaw[axis]; gyroADC[Y] = gyroADCRaw[Y];
} gyroADC[Z] = gyroADCRaw[Z];
alignSensors(gyroADC, gyroADC, gyroAlign); alignSensors(gyroADC, gyroADC, gyroAlign);
@ -183,7 +176,9 @@ void gyroUpdate(void)
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold); performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
} }
applyGyroZero(); gyroADC[X] -= gyroZero[X];
gyroADC[Y] -= gyroZero[Y];
gyroADC[Z] -= gyroZero[Z];
if (gyroSoftLpfHz) { if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {