mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 16:55:36 +03:00
297609d4c3
This resulted in the removal of duplicate logic, duplicate code and the removal of a temporary buffer per-driver. The NAZE specific sensor alignment is now contained within the core code instead of in the drivers. The sensor alignment is determines by the sensor initialisation code. The alignment of sensor readings is now performed once and only by the appropriate sensor code, see usages of alignSensors(). The acc/gyro/compass driver code is now more reusable since it has no dependencies on the main code.
73 lines
2 KiB
C
73 lines
2 KiB
C
#include "board.h"
|
|
#include "mw.h"
|
|
|
|
#include "common/axis.h"
|
|
|
|
int16_t magADC[XYZ_AXIS_COUNT];
|
|
sensor_align_e magAlign = 0;
|
|
#ifdef MAG
|
|
static uint8_t magInit = 0;
|
|
|
|
void Mag_init(void)
|
|
{
|
|
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
|
|
LED1_ON;
|
|
hmc5883lInit();
|
|
LED1_OFF;
|
|
magInit = 1;
|
|
}
|
|
|
|
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
|
|
hmc5883lRead(magADC);
|
|
alignSensors(magADC, magADC, magAlign);
|
|
|
|
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
|
|
}
|
|
copyCurrentProfileToProfileSlot(mcfg.current_profile);
|
|
writeEEPROM();
|
|
readEEPROMAndNotify();
|
|
}
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
#endif
|