Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-26 01:35:41 +03:00
Code Issues Wiki Activity

Changed gyro init and read functions to take a gyro_t* parameter.

Browse source 
Scaled gyro values to degrees per second in gyroUpdate.
This commit is contained in:
Martin Budden 2016-11-19 14:11:03 +00:00
parent 55b32740d9
commit 590e569375
28 changed files with 136 additions and 124 deletions
Download patch file Download diff file Expand all files Collapse all files

36
src/main/sensors/gyro.c
View file

@ -39,7 +39,7 @@
gyro_t gyro; // gyro access functions
sensor_align_e gyroAlign = 0;
int32_t gyroADC[XYZ_AXIS_COUNT];
static int32_t gyroADC[XYZ_AXIS_COUNT];
float gyroADCf[XYZ_AXIS_COUNT];
static int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
@ -174,41 +174,39 @@ static void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
void gyroUpdate(void)
{
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
if (!gyro.read(&gyro)) {
return;
}
gyroADC[X] = gyroADCRaw[X];
gyroADC[Y] = gyroADCRaw[Y];
gyroADC[Z] = gyroADCRaw[Z];
gyro.dataReady = false;
gyroADC[X] = gyro.gyroADCRaw[X];
gyroADC[Y] = gyro.gyroADCRaw[Y];
gyroADC[Z] = gyro.gyroADCRaw[Z];
alignSensors(gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
const bool calibrationComplete = isGyroCalibrationComplete();
if (!calibrationComplete) {
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
gyroADC[X] -= gyroZero[X];
gyroADC[Y] -= gyroZero[Y];
gyroADC[Z] -= gyroZero[Z];
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] -= gyroZero[axis];
// scale gyro output to degrees per second
gyroADCf[axis] = (float)gyroADC[axis] * gyro.scale;
if (debugMode == DEBUG_GYRO)
debug[axis] = gyroADC[axis];
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf[axis]));
gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], (float) gyroADC[axis]);
gyroADCf[axis] = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf[axis]);
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf[axis]));
gyroADCf[axis] = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf[axis]);
gyroADCf[axis] = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
if (!calibrationComplete) {
gyroADC[axis] = lrintf(gyroADCf[axis] / gyro.scale);
}
}
}