// 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); }