// Edit do read data from MPU6050 [10/14/2015 QuocTuanIT]
void sensorsRead()
{
int16_t ax = 0;
int16_t ay = 0;
int16_t az = 0;
int16_t gx = 0;
int16_t gy = 0;
int16_t gz = 0;
mpu6050_getRawData(&ax, &ay, &az, &gx, &gy, &gz);
GYRO_raw[PIT] = gx;//adcGet(ADC_GYR_X);
GYRO[PIT] = -(int16_t)(GYRO_raw[PIT] - Config.GYRO_zero[PIT]);
#if GYRO_DEADBAND > 0
if (abs(GYRO[PIT]) <= GYRO_DEADBAND) GYRO[PIT] = 0;
#endif
GYRO_raw[ROL] = gy;//adcGet(ADC_GYR_Y);
GYRO[ROL] = -(int16_t)(GYRO_raw[ROL] - Config.GYRO_zero[ROL]);
#if GYRO_DEADBAND > 0
if (abs(GYRO[ROL]) <= GYRO_DEADBAND) GYRO[ROL] = 0;
#endif
GYRO_raw[YAW] = gz;//adcGet(ADC_GYR_Z);
GYRO[YAW] = (int16_t)(GYRO_raw[YAW] - Config.GYRO_zero[YAW]);
#if GYRO_DEADBAND > 0
if (abs(GYRO[YAW]) <= GYRO_DEADBAND) GYRO[YAW] = 0;
#endif
ACC_raw[PIT] = ax;//adcGet(ADC_ACC_X);
ACC[PIT] = (int16_t)(ACC_raw[PIT] - Config.ACC_zero[PIT]);
ACC_raw[ROL] = ay;//adcGet(ADC_ACC_Y);
ACC[ROL] = (int16_t)(ACC_raw[ROL] - Config.ACC_zero[ROL]);
ACC_raw[YAW] = az;//adcGet(ADC_ACC_Z);
ACC[YAW] = (int16_t)(ACC_raw[YAW] - Config.ACC_zero[YAW]);
BATT = adcGet(ADC_VBAT) * 100 / 376;
#ifdef SIMULATOR
GYRO[0] = 100;
GYRO[1] = 100;
GYRO[2] = 100;
ACC[0] = 100;
ACC[1] = 100;
ACC[2] = 100;
#endif
}
void batterySample(void)
{
static uint8_t ind;
static uint16_t batSamples[8];
uint16_t batAccum = 0;
uint8_t i;
batSamples[(ind++) % 8] = adcGet();
for(i = 0; i < 8; ++i)
batAccum += batSamples[i];
sensorData.batteryVoltage = batteryAdcToVoltage(batAccum / 8.0f);
// TODO - add buzzer stuff
/*
if ((vbat > batteryWarningVoltage) || (vbat < cfg.vbatmincellvoltage)) { // VBAT ok, buzzer off
buzzerFreq = 0;
} else
buzzerFreq = 4; // low battery
*/
}
