static void Gyro_getRawRot(void)
{
int16_t gyroADC16[3], i;
gyro.read(gyroADC16); // range: +/- 8192; +/- 2000 deg/sec
if (cfg.align[ALIGN_GYRO][0]) alignSensors(ALIGN_GYRO, gyroADC16); else gyro.align(gyroADC16);
for (i = 0; i < 3; i++) gyroADC[i] = (float)gyroADC16[i];
}
static void ACC_getRawRot(void)
{
int16_t accADC16[3], i;
acc.read(accADC16);
if (cfg.align[ALIGN_ACCEL][0]) alignSensors(ALIGN_ACCEL, accADC16); else acc.align(accADC16);
for (i = 0; i < 3; i++) accADC[i] = (float)accADC16[i];
}
void gyroUpdate(void)
{
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.dev.read(&gyro.dev)) {
return;
}
// Prepare a copy of int32_t gyroADC for mangling to prevent overflow
gyro.gyroADC[X] = gyro.dev.gyroADCRaw[X];
gyro.gyroADC[Y] = gyro.dev.gyroADCRaw[Y];
gyro.gyroADC[Z] = gyro.dev.gyroADCRaw[Z];
if (gyroConfig->gyro_soft_lpf_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyro.gyroADC[axis] = lrintf(biquadFilterApply(&gyroFilterLPF[axis], (float)gyro.gyroADC[axis]));
}
}
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
gyro.gyroADC[X] -= gyroZero[X];
gyro.gyroADC[Y] -= gyroZero[Y];
gyro.gyroADC[Z] -= gyroZero[Z];
alignSensors(gyro.gyroADC, gyro.dev.gyroAlign);
}
void updateAccelerationReadings(void)
{
if (!acc.read(accADCRaw)) {
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) accADC[axis] = accADCRaw[axis];
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (int axis = 0; axis < 3; axis++) {
biquadFilterInit(&accFilterState[axis], accLpfCutHz, 0);
}
accFilterInitialised = true;
}
}
if (accFilterInitialised) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accADC[axis] = lrintf(biquadFilterApply(&accFilterState[axis], (float) accADC[axis]));
}
}
}
if (!isAccelerationCalibrationComplete()) {
performAcclerationCalibration();
}
applyAccelerationZero(accZero, accGain);
alignSensors(accADC, accADC, accAlign);
}
void gyroUpdate(void)
{
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int axis;
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;
}
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_GYRO) debug[axis] = gyroADC[axis];
gyroADC[axis] = gyroADCRaw[axis];
}
alignSensors(gyroADC, gyroADC, gyroAlign);
if (gyroLpfCutFreq) {
if (!gyroFilterStateIsSet) initGyroFilterCoefficients(); /* initialise filter coefficients */
if (gyroFilterStateIsSet) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroFilterState[axis]));
}
}
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();
}
void GETMPU6050(void)
{
int16_t accADC16[3];
int16_t gyroADC16[3];
uint8_t i;
MPU6050ReadAllShit(accADC16, &GyroTempC100, gyroADC16);
if (cfg.align[ALIGN_ACCEL][0]) alignSensors(ALIGN_ACCEL, accADC16); else acc.align(accADC16);
if (cfg.align[ALIGN_GYRO][0]) alignSensors(ALIGN_GYRO, gyroADC16); else gyro.align(gyroADC16);
for (i = 0; i < 3; i++)
{
accADC[i] = (float)accADC16[i];
gyroADC[i] = (float)gyroADC16[i];
}
ACC_Common();
GYRO_Common();
}
static void adxl345Read(int16_t *accelData)
{
uint8_t buf[8];
int16_t data[3];
if (useFifo) {
int32_t x = 0;
int32_t y = 0;
int32_t z = 0;
uint8_t i = 0;
uint8_t samples_remaining;
do {
i++;
i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 8, buf);
x += (int16_t)(buf[0] + (buf[1] << 8));
y += (int16_t)(buf[2] + (buf[3] << 8));
z += (int16_t)(buf[4] + (buf[5] << 8));
samples_remaining = buf[7] & 0x7F;
} while ((i < 32) && (samples_remaining > 0));
data[0] = x / i;
data[1] = y / i;
data[2] = z / i;
acc_samples = i;
} else {
i2cRead(ADXL345_ADDRESS, ADXL345_DATA_OUT, 6, buf);
data[0] = buf[0] + (buf[1] << 8);
data[1] = buf[2] + (buf[3] << 8);
data[2] = buf[4] + (buf[5] << 8);
}
alignSensors(data, accelData, accAlign);
}
void hmc5883lRead(int16_t *magData) // Read aligned BTW: The 5883 sends in that order: X Z Y
{
uint8_t buf[6];
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
magData[0] = (int16_t)(((uint16_t)buf[0] << 8) | (uint16_t)buf[1]);
magData[1] = (int16_t)(((uint16_t)buf[2] << 8) | (uint16_t)buf[3]);
magData[2] = (int16_t)(((uint16_t)buf[4] << 8) | (uint16_t)buf[5]);
alignSensors(ALIGN_MAG, magData);
}
bool mpu6000Read(int16_t *datag, int16_t *dataa)
{
int16_t dataBuffer[3];
uint8_t buf[14];
uint8_t Read_OK;
ENABLE_MPU6000;
buf[0]=spiTransferByte2(MPU6000_INT_STATUS | 0x80);
buf[1]=spiTransferByte2(0xFF);
DISABLE_MPU6000;
delayMicroseconds(5);
//debug[0] = buf[1];
//debug[1] = buf[1] & BIT_RAW_RDY_INT;
if ((buf[1] & BIT_RAW_RDY_INT) == 0){
//debug[2]=99;
return false;
}
ENABLE_MPU6000;
spiTransferByte2(MPU6000_ACCEL_XOUT_H | 0x80);
Read_OK = spiTransfer2(buf, NULL, 14);
DISABLE_MPU6000;
//Gyro
if (Read_OK==1){
dataBuffer[X] = (int16_t)((buf[8] << 8) | buf[9]) / 4;
dataBuffer[Y] = (int16_t)((buf[10] << 8) | buf[11]) / 4;
dataBuffer[Z] = (int16_t)((buf[12] << 8) | buf[13]) / 4;
alignSensors(dataBuffer, datag, gyroAlign);
//Acc
dataBuffer[X] = (int16_t)((buf[0] << 8) | buf[1]);
dataBuffer[Y] = (int16_t)((buf[2] << 8) | buf[3]);
dataBuffer[Z] = (int16_t)((buf[4] << 8) | buf[5]);
alignSensors(dataBuffer, dataa, accAlign);
return true;
}
else
return false;
}
static FAST_CODE FAST_CODE_NOINLINE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
return;
}
gyroSensor->gyroDev.dataReady = false;
if (isGyroSensorCalibrationComplete(gyroSensor)) {
// move 16-bit gyro data into 32-bit variables to avoid overflows in calculations
#if defined(USE_GYRO_SLEW_LIMITER)
gyroSensor->gyroDev.gyroADC[X] = gyroSlewLimiter(gyroSensor, X) - gyroSensor->gyroDev.gyroZero[X];
gyroSensor->gyroDev.gyroADC[Y] = gyroSlewLimiter(gyroSensor, Y) - gyroSensor->gyroDev.gyroZero[Y];
gyroSensor->gyroDev.gyroADC[Z] = gyroSlewLimiter(gyroSensor, Z) - gyroSensor->gyroDev.gyroZero[Z];
#else
gyroSensor->gyroDev.gyroADC[X] = gyroSensor->gyroDev.gyroADCRaw[X] - gyroSensor->gyroDev.gyroZero[X];
gyroSensor->gyroDev.gyroADC[Y] = gyroSensor->gyroDev.gyroADCRaw[Y] - gyroSensor->gyroDev.gyroZero[Y];
gyroSensor->gyroDev.gyroADC[Z] = gyroSensor->gyroDev.gyroADCRaw[Z] - gyroSensor->gyroDev.gyroZero[Z];
#endif
alignSensors(gyroSensor->gyroDev.gyroADC, gyroSensor->gyroDev.gyroAlign);
} else {
performGyroCalibration(gyroSensor, gyroConfig()->gyroMovementCalibrationThreshold);
// still calibrating, so no need to further process gyro data
return;
}
if (gyroDebugMode == DEBUG_NONE) {
filterGyro(gyroSensor);
} else {
filterGyroDebug(gyroSensor);
}
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroConfig()->checkOverflow && !gyroHasOverflowProtection) {
checkForOverflow(gyroSensor, currentTimeUs);
}
#endif
#ifdef USE_YAW_SPIN_RECOVERY
if (gyroConfig()->yaw_spin_recovery) {
checkForYawSpin(gyroSensor, currentTimeUs);
}
#endif
#ifdef USE_GYRO_DATA_ANALYSE
if (isDynamicFilterActive()) {
gyroDataAnalyse(&gyroSensor->gyroAnalyseState, gyroSensor->notchFilterDyn, gyroSensor->notchFilterDyn2);
}
#endif
#if (!defined(USE_GYRO_OVERFLOW_CHECK) && !defined(USE_YAW_SPIN_RECOVERY))
UNUSED(currentTimeUs);
#endif
}
static void mpu6050AccRead(int16_t *accData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(MPU6050_ADDRESS, MPU_RA_ACCEL_XOUT_H, 6, buf);
data[0] = (int16_t)((buf[0] << 8) | buf[1]);
data[1] = (int16_t)((buf[2] << 8) | buf[3]);
data[2] = (int16_t)((buf[4] << 8) | buf[5]);
alignSensors(data, accData, accAlign);
}
static void mpu6050GyroRead(int16_t *gyroData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(MPU6050_ADDRESS, MPU_RA_GYRO_XOUT_H, 6, buf);
data[0] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[1] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[2] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}
// Read 3 gyro values into user-provided buffer. No overrun checking is done.
static void l3g4200dRead(int16_t *gyroData)
{
uint8_t buf[6];
int16_t data[3];
i2cRead(L3G4200D_ADDRESS, L3G4200D_AUTOINCR | L3G4200D_GYRO_OUT, 6, buf);
data[X] = (int16_t)((buf[0] << 8) | buf[1]) / 4;
data[Y] = (int16_t)((buf[2] << 8) | buf[3]) / 4;
data[Z] = (int16_t)((buf[4] << 8) | buf[5]) / 4;
alignSensors(data, gyroData, gyroAlign);
}
int compassGetADC(flightDynamicsTrims_t *magZero)
{
static uint32_t t, tCal = 0;
static flightDynamicsTrims_t magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax;
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++) {
magZero->raw[axis] = 0;
magZeroTempMin.raw[axis] = magADC[axis];
magZeroTempMax.raw[axis] = magADC[axis];
}
f.CALIBRATE_MAG = 0;
}
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 ((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.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
}
saveAndReloadCurrentProfileToCurrentProfileSlot();
}
}
return 1;
}
void hmc5883lRead(int16_t *magData)
{
uint8_t buf[6];
int16_t mag[3];
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
// During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
// After calibration is done, magGain is set to calculated gain values.
mag[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
mag[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
mag[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
alignSensors(mag, magData, magAlign);
}
void gyroUpdate(void)
{
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = gyroADCRaw[axis];
}
alignSensors(gyroADC, gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
performGyroCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();
if (gyroSoftLpfHz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_GYRO)
debug[axis] = gyroADC[axis];
if (gyroSoftLpfType == FILTER_BIQUAD)
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]);
else if (gyroSoftLpfType == FILTER_PT1)
gyroADCf[axis] = pt1FilterApply4(&gyroFilterPt1[axis], (float) gyroADC[axis], gyroSoftLpfHz, gyroDt);
else
gyroADCf[axis] = firFilterDenoiseUpdate(&gyroDenoiseState[axis], (float) gyroADC[axis]);
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);
if (gyroSoftNotchHz1)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_1[axis], gyroADCf[axis]);
if (gyroSoftNotchHz2)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch_2[axis], gyroADCf[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++)
gyroADCf[axis] = gyroADC[axis];
}
}
void gyroGetADC(void)
{
// FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time.
// range: +/- 8192; +/- 2000 deg/sec
gyro.read(gyroADC);
alignSensors(gyroADC, gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();
}
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
acc.read(accADC);
alignSensors(accADC, accADC, accAlign);
if (!isAccelerationCalibrationComplete()) {
performAcclerationCalibration(rollAndPitchTrims);
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
performInflightAccelerationCalibration(rollAndPitchTrims);
}
applyAccelerationTrims(accelerationTrims);
}
void compassUpdate(uint32_t currentTime, flightDynamicsTrims_t *magZero)
{
static uint32_t tCal = 0;
static flightDynamicsTrims_t magZeroTempMin;
static flightDynamicsTrims_t magZeroTempMax;
magDev.read(&magDev, magADCRaw);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
mag.magADC[axis] = magADCRaw[axis];
}
alignSensors(mag.magADC, magDev.magAlign);
if (STATE(CALIBRATE_MAG)) {
tCal = currentTime;
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = 0;
magZeroTempMin.raw[axis] = mag.magADC[axis];
magZeroTempMax.raw[axis] = mag.magADC[axis];
}
DISABLE_STATE(CALIBRATE_MAG);
}
if (magInit) { // we apply offset only once mag calibration is done
mag.magADC[X] -= magZero->raw[X];
mag.magADC[Y] -= magZero->raw[Y];
mag.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 (int axis = 0; axis < 3; axis++) {
if (mag.magADC[axis] < magZeroTempMin.raw[axis])
magZeroTempMin.raw[axis] = mag.magADC[axis];
if (mag.magADC[axis] > magZeroTempMax.raw[axis])
magZeroTempMax.raw[axis] = mag.magADC[axis];
}
} else {
tCal = 0;
for (int axis = 0; axis < 3; axis++) {
magZero->raw[axis] = (magZeroTempMin.raw[axis] + magZeroTempMax.raw[axis]) / 2; // Calculate offsets
}
saveConfigAndNotify();
}
}
}
void gyroUpdate(void)
{
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;
}
gyroADC[X] = gyroADCRaw[X];
gyroADC[Y] = gyroADCRaw[Y];
gyroADC[Z] = gyroADCRaw[Z];
alignSensors(gyroADC, gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig()->gyroMovementCalibrationThreshold);
}
gyroADC[X] -= gyroZero[X];
gyroADC[Y] -= gyroZero[Y];
gyroADC[Z] -= gyroZero[Z];
if (gyroConfig()->gyro_soft_lpf_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_GYRO)
debug[axis] = gyroADC[axis];
if (gyroConfig()->gyro_soft_type == FILTER_BIQUAD)
gyroADCf[axis] = biquadFilterApply(&gyroFilterLPF[axis], (float) gyroADC[axis]);
else
gyroADCf[axis] = pt1FilterApply(&gyroFilterPt1[axis], (float) gyroADC[axis]);
if (debugMode == DEBUG_NOTCH)
debug[axis] = lrintf(gyroADCf[axis]);
if (gyroConfig()->gyro_soft_notch_hz)
gyroADCf[axis] = biquadFilterApply(&gyroFilterNotch[axis], gyroADCf[axis]);
gyroADC[axis] = lrintf(gyroADCf[axis]);
}
} else {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADCf[axis] = gyroADC[axis];
}
}
}
void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
{
int16_t accADCRaw[XYZ_AXIS_COUNT];
if (!acc.read(accADCRaw)) {
return;
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
if (debugMode == DEBUG_ACCELEROMETER) debug[axis] = accADCRaw[axis];
accSmooth[axis] = accADCRaw[axis];
}
if (accLpfCutHz) {
if (!accFilterInitialised) {
if (accTargetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInitLPF(&accFilter[axis], accLpfCutHz, accTargetLooptime);
}
accFilterInitialised = true;
}
}
if (accFilterInitialised) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
accSmooth[axis] = lrintf(biquadFilterApply(&accFilter[axis], (float)accSmooth[axis]));
}
}
}
alignSensors(accSmooth, accSmooth, accAlign);
if (!isAccelerationCalibrationComplete()) {
performAcclerationCalibration(rollAndPitchTrims);
}
if (feature(FEATURE_INFLIGHT_ACC_CAL)) {
performInflightAccelerationCalibration(rollAndPitchTrims);
}
applyAccelerationTrims(accelerationTrims);
}
void gyroUpdate(void)
{
int8_t axis;
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADCRaw)) {
return;
}
// Prepare a copy of int32_t gyroADC for mangling to prevent overflow
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = gyroADCRaw[axis];
if (gyroLpfCutHz) {
if (!gyroFilterInitialised) {
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
for (axis = 0; axis < 3; axis++) {
filterInitBiQuad(gyroLpfCutHz, &gyroFilterState[axis], 0);
}
gyroFilterInitialised = true;
}
}
if (gyroFilterInitialised) {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] = lrintf(filterApplyBiQuad((float) gyroADC[axis], &gyroFilterState[axis]));
}
}
}
alignSensors(gyroADC, gyroADC, gyroAlign);
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();
}
void gyroUpdate(void)
{
// range: +/- 8192; +/- 2000 deg/sec
if (!gyro.read(gyroADC)) {
return;
}
alignSensors(gyroADC, gyroADC, gyroAlign);
if (gyroLpfCutFreq) {
if (!gyroFilterStateIsSet) {
initGyroFilterCoefficients();
} else {
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) gyroADC[axis] = lrintf(applyBiQuadFilter((float) gyroADC[axis], &gyroBiQuadState[axis]));
}
}
if (!isGyroCalibrationComplete()) {
performAcclerationCalibration(gyroConfig->gyroMovementCalibrationThreshold);
}
applyGyroZero();
}
bool readHMC5983(int16_t *data)
{
uint8_t buf[6];
int16_t dataBuffer[3];
setSPIdivisor(8); // 4.5 MHz SPI clock
ENABLE_HMC5983;
spiTransferByte(HMC5983_DATA_X_MSB_REG + 0x80 + 0x40);
spiTransfer(buf, NULL, 6);
DISABLE_HMC5983;
dataBuffer[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
dataBuffer[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
dataBuffer[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
alignSensors(dataBuffer, data, magAlign);
// check for valid data
if (dataBuffer[X] == -4096 || dataBuffer[Y] == -4096 || dataBuffer[Z] == -4096) {
return false;
} else {
return true;
}
}
