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 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();
}
bool isCalibrating()
{
#ifdef BARO
if (sensors(SENSOR_ACC) && !isBaroCalibrationComplete()) {
return false;
}
#endif
// Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
return (!isAccelerationCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
}
void mwArm(void)
{
static bool firstArmingCalibrationWasCompleted;
if (masterConfig.gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
gyroSetCalibrationCycles();
armingCalibrationWasInitialised = true;
firstArmingCalibrationWasCompleted = true;
}
if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
#ifdef BLACKBOX
if (feature(FEATURE_BLACKBOX)) {
serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP);
if (sharedBlackboxAndMspPort) {
mspReleasePortIfAllocated(sharedBlackboxAndMspPort);
}
startBlackbox();
}
#endif
disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);
#else
beeper(BEEPER_ARMING);
#endif
return;
}
}
if (!ARMING_FLAG(ARMED)) {
beeperConfirmationBeeps(1);
}
}
static bool isCalibrating(void)
{
#ifdef USE_BARO
if (sensors(SENSOR_BARO) && !isBaroCalibrationComplete()) {
return true;
}
#endif
// Note: compass calibration is handled completely differently, outside of the main loop, see f.CALIBRATE_MAG
return (!accIsCalibrationComplete() && sensors(SENSOR_ACC)) || (!isGyroCalibrationComplete());
}
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 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 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 mwArm(void)
{
static bool firstArmingCalibrationWasCompleted;
if (armingConfig()->gyro_cal_on_first_arm && !firstArmingCalibrationWasCompleted) {
gyroSetCalibrationCycles();
armingCalibrationWasInitialised = true;
firstArmingCalibrationWasCompleted = true;
}
if (!isGyroCalibrationComplete()) return; // prevent arming before gyro is calibrated
if (ARMING_FLAG(OK_TO_ARM)) {
if (ARMING_FLAG(ARMED)) {
return;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
return;
}
if (!ARMING_FLAG(PREVENT_ARMING)) {
ENABLE_ARMING_FLAG(ARMED);
ENABLE_ARMING_FLAG(WAS_EVER_ARMED);
headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw);
disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero
//beep to indicate arming
#ifdef GPS
if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5)
beeper(BEEPER_ARMING_GPS_FIX);
else
beeper(BEEPER_ARMING);
#else
beeper(BEEPER_ARMING);
#endif
return;
}
}
if (!ARMING_FLAG(ARMED)) {
beeperConfirmationBeeps(1);
}
}
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 isFirstArmingGyroCalibrationRunning(void)
{
return firstArmingCalibrationWasStarted && !isGyroCalibrationComplete();
}
bool isImuReady(void)
{
return sensors(SENSOR_ACC) && isGyroCalibrationComplete();
}
