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