void sensorsInit(void) { zeroSensorAccumulators(); // TODO allow user to select hardware if there are multiple choices if(mpu6050Detect(&gyro, &accel, cfg.mpu6050Scale)) { sensorsSet(SENSOR_ACC); } else if(!mpu3050Detect(&gyro)) { failureMode(3); } if(adxl345Detect(&accel)) { sensorsSet(SENSOR_ACC); } // At the moment we will do this after mpu6050 and overrride mpu6050 accel if detected if(mma8452Detect(&accel)) { sensorsSet(SENSOR_ACC); } gyro.init(); if(sensorsGet(SENSOR_ACC)) accel.init(); if(hmc5883Detect(&mag)) { mag.init(); sensorsSet(SENSOR_MAG); } #ifdef SONAR if(feature(FEATURE_PPM);) {
void Mag_init(void) { // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) LED1_ON; mag.init(mcfg.mag_align); LED1_OFF; magInit = 1; }
void compassInit(void) { // initialize and calibration. turn on led during mag calibration (calibration routine blinks it) LED1_ON; mag.init(); LED1_OFF; magInit = 1; }
void magSample(void) { uint8_t i; mag.read(sensorData.mag); for(i = 0; i < 3; ++i) sensorData.magAccum[i] += sensorData.mag[i] - cfg.magBias[i]; sensorData.magSamples++; }
int Mag_getADC(void) { static uint32_t t, tCal = 0; static int16_t magZeroTempMin[3]; static int16_t magZeroTempMax[3]; uint32_t axis; if ((int32_t)(currentTime - t) < 0) return 0; //each read is spaced by 100ms t = currentTime + 100000; // Read mag sensor mag.read(magADC); if (f.CALIBRATE_MAG) { tCal = t; for (axis = 0; axis < 3; axis++) { mcfg.magZero[axis] = 0; magZeroTempMin[axis] = magADC[axis]; magZeroTempMax[axis] = magADC[axis]; } f.CALIBRATE_MAG = 0; } if (magInit) { // we apply offset only once mag calibration is done magADC[X] -= mcfg.magZero[X]; magADC[Y] -= mcfg.magZero[Y]; magADC[Z] -= mcfg.magZero[Z]; } if (tCal != 0) { if ((t - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions LED0_TOGGLE; for (axis = 0; axis < 3; axis++) { if (magADC[axis] < magZeroTempMin[axis]) magZeroTempMin[axis] = magADC[axis]; if (magADC[axis] > magZeroTempMax[axis]) magZeroTempMax[axis] = magADC[axis]; } } else { tCal = 0; for (axis = 0; axis < 3; axis++) mcfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets writeEEPROM(1, true); } } return 1; }
void updateCompass(flightDynamicsTrims_t *magZero) { static uint32_t tCal = 0; static flightDynamicsTrims_t magZeroTempMin; static flightDynamicsTrims_t magZeroTempMax; uint32_t axis; mag.read(magADCRaw); for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) magADC[axis] = magADCRaw[axis]; // int32_t copy to work with alignSensors(magADC, magADC, magAlign); if (STATE(CALIBRATE_MAG)) { tCal = currentTime; for (axis = 0; axis < 3; axis++) { magZero->raw[axis] = 0; magZeroTempMin.raw[axis] = magADC[axis]; magZeroTempMax.raw[axis] = magADC[axis]; } DISABLE_STATE(CALIBRATE_MAG); } if (magInit) { // we apply offset only once mag calibration is done magADC[X] -= magZero->raw[X]; magADC[Y] -= magZero->raw[Y]; magADC[Z] -= magZero->raw[Z]; } if (tCal != 0) { if ((currentTime - tCal) < 30000000) { // 30s: you have 30s to turn the multi in all directions LED0_TOGGLE; for (axis = 0; axis < 3; axis++) { if (magADC[axis] < magZeroTempMin.raw[axis]) magZeroTempMin.raw[axis] = magADC[axis]; if (magADC[axis] > magZeroTempMax.raw[axis]) magZeroTempMax.raw[axis] = magADC[axis]; } } else { tCal = 0; for (axis = 0; axis < 3; axis++) { magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets } saveConfigAndNotify(); } } }