// Note: That mwii div by 4 stuff (done there on gyro readout) isn't bad at all to get rid of noise / jitter the easy way.
// The idea is preserved but accuracy is improved without introducing jitter.
// gyroData is seen by main pid controller. gyroADC is used in IMU for vector-rotation.
static void GYRO_Common(void)
{
int16_t i, tmp; // uint8 for "i" is shorter on 8Bit
for (i = 0; i < 3; i++)
{
gyroADC[i] = gyroADC[i] - gyroZero[i];
tmp = (int16_t)(gyroADC[i] * 0.3125f);
gyroData[i] = (float)tmp * 3.2f;
gyroADC[i] *= GyroScale16; // gyroADC delivered in 16 * rad/s
}
alignBoardyaw(gyroData);
alignBoardyaw(gyroADC);
}
void Mag_getADC(void)
{
static uint32_t Lasttime = 0;
uint8_t i;
uint32_t TimeNow = micros();
if (calibratingM) Mag_Calibration(); // Calibrates saves and resets
if ((TimeNow - Lasttime) < 14000) return; // Do 70Hz in normal operation
Lasttime = TimeNow;
Mag_getRawADC_With_Gain(); // Read mag sensor with orientation correction do nothing more for now
for (i = 0; i < 3; i++) magADCfloat[i] -= cfg.magZero[i]; // Adjust by BIAS (gathered by user calibration)
alignBoardyaw(magADCfloat);
HaveNewMag = true;
}
static void ACC_Common(void)
{
uint8_t axis;
if (calibratingA)
{
cfg.acc_calibrated = 0; // Mark ACC not calibrated
Gyro_Calibrate(); // Recal Gyro as well to have fresh data (warm chip) to save along acc offsets
if (!GyroCalCompromised) Acc_Calibrate(); // Proceed if copter is still. Dead end (Freeze, Calibrate, Save, Reset)
else calibratingA = false; // Shaky copter during ACC Calibration is not tolerated
}
else
{
for (axis = 0; axis < 3; axis++)
{
if(cfg.acc_calibrated) accADC[axis] -= cfg.accZero[axis];
else accADC[axis] = 1.0f;
}
alignBoardyaw(accADC);
}
}
static void GYRO_Common(void)
{
uint16_t i;
for (i = 0; i < 3; i++) gyroADC[i] -= gyroZero[i];
alignBoardyaw(gyroADC);
}
