static long cmsx_menuGyro_onEnter(void)
{
cmsx_gyroSync = gyroConfig()->gyroSync;
cmsx_gyroSyncDenom = gyroConfig()->gyroSyncDenominator;
cmsx_gyroLpf = gyroConfig()->gyro_lpf;
return 0;
}
static long cmsx_menuGyro_onEnter(void)
{
gyroConfig_gyro_soft_lpf_hz = gyroConfig()->gyro_soft_lpf_hz;
gyroConfig_gyro_soft_notch_hz_1 = gyroConfig()->gyro_soft_notch_hz_1;
gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
return 0;
}
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 gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config)
{
gyroSensor->gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr;
gyroSensor->gyroDev.gyroAlign = config->align;
gyroSensor->gyroDev.mpuIntExtiTag = config->extiTag;
// Must set gyro targetLooptime before gyroDev.init and initialisation of filters
gyro.targetLooptime = gyroSetSampleRate(&gyroSensor->gyroDev, gyroConfig()->gyro_hardware_lpf, gyroConfig()->gyro_sync_denom);
gyroSensor->gyroDev.hardware_lpf = gyroConfig()->gyro_hardware_lpf;
gyroSensor->gyroDev.initFn(&gyroSensor->gyroDev);
// As new gyros are supported, be sure to add them below based on whether they are subject to the overflow/inversion bug
// Any gyro not explicitly defined will default to not having built-in overflow protection as a safe alternative.
switch (gyroSensor->gyroDev.gyroHardware) {
case GYRO_NONE: // Won't ever actually get here, but included to account for all gyro types
case GYRO_DEFAULT:
case GYRO_FAKE:
case GYRO_MPU6050:
case GYRO_L3G4200D:
case GYRO_MPU3050:
case GYRO_L3GD20:
case GYRO_BMI160:
case GYRO_MPU6000:
case GYRO_MPU6500:
case GYRO_MPU9250:
gyroSensor->gyroDev.gyroHasOverflowProtection = true;
break;
case GYRO_ICM20601:
case GYRO_ICM20602:
case GYRO_ICM20608G:
case GYRO_ICM20649: // we don't actually know if this is affected, but as there are currently no flight controllers using it we err on the side of caution
case GYRO_ICM20689:
gyroSensor->gyroDev.gyroHasOverflowProtection = false;
break;
default:
gyroSensor->gyroDev.gyroHasOverflowProtection = false; // default catch for newly added gyros until proven to be unaffected
break;
}
gyroInitSensorFilters(gyroSensor);
#ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseStateInit(&gyroSensor->gyroAnalyseState, gyro.targetLooptime);
#endif
}
bool sensorsAutodetect(void)
{
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig);
UNUSED(mpuDetectionResult);
#endif
if (!detectGyro()) {
return false;
}
detectAcc(sensorSelectionConfig()->acc_hardware);
detectBaro(sensorSelectionConfig()->baro_hardware);
// Now time to init things, acc first
if (sensors(SENSOR_ACC))
acc.init();
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init(gyroConfig()->gyro_lpf);
#ifdef MAG
detectMag(sensorSelectionConfig()->mag_hardware);
#endif
reconfigureAlignment(sensorAlignmentConfig());
return true;
}
uint8_t setPidUpdateCountDown(void)
{
if (gyroConfig()->gyro_soft_lpf_hz) {
return pidConfig()->pid_process_denom - 1;
} else {
return 1;
}
}
bool sensorsAutodetect(void)
{
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig);
UNUSED(mpuDetectionResult);
#endif
if (!detectGyro()) {
return false;
}
gyro.sampleFrequencyHz = gyroConfig()->gyro_sample_hz;
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init(&gyro, gyroConfig()->gyro_lpf);
gyroInit();
// the combination of LPF and GYRO_SAMPLE_HZ may be invalid for the gyro, update the configuration to use the sample frequency that was determined for the desired LPF.
gyroConfig()->gyro_sample_hz = gyro.sampleFrequencyHz;
if (detectAcc(sensorSelectionConfig()->acc_hardware)) {
acc.acc_1G = 256; // set default
acc.init(&acc);
}
#ifdef BARO
detectBaro(sensorSelectionConfig()->baro_hardware);
#endif
#ifdef MAG
if (detectMag(sensorSelectionConfig()->mag_hardware)) {
if (!compassInit()) {
sensorsClear(SENSOR_MAG);
}
}
#endif
reconfigureAlignment(sensorAlignmentConfig());
return true;
}
bool taskPidCheck(uint32_t currentDeltaTime)
{
if (gyroReadyCounter == 0) {
return false;
}
bool shouldRunPid = false;
if (gyroConfig()->gyro_sync) {
shouldRunPid = gyroReadyCounter >= gyroConfig()->pid_process_denom;
}
shouldRunPid |= currentDeltaTime >= targetPidLooptime;
if (!shouldRunPid) {
return false;
}
return true;
}
static long cmsx_FilterPerProfileRead(void)
{
cmsx_dterm_lpf_hz = pidProfile()->dterm_lpf_hz;
cmsx_gyroSoftLpf = gyroConfig()->gyro_soft_lpf_hz;
cmsx_yaw_p_limit = pidProfile()->yaw_p_limit;
cmsx_yaw_lpf_hz = pidProfile()->yaw_lpf_hz;
return 0;
}
static void dynLpfFilterInit()
{
if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
switch (gyroConfig()->gyro_lowpass_type) {
case FILTER_PT1:
dynLpfFilter = DYN_LPF_PT1;
break;
case FILTER_BIQUAD:
dynLpfFilter = DYN_LPF_BIQUAD;
break;
default:
dynLpfFilter = DYN_LPF_NONE;
break;
}
} else {
dynLpfFilter = DYN_LPF_NONE;
}
dynLpfMin = gyroConfig()->dyn_lpf_gyro_min_hz;
dynLpfMax = gyroConfig()->dyn_lpf_gyro_max_hz;
}
static FAST_CODE_NOINLINE void handleYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
const float yawSpinResetRate = gyroConfig()->yaw_spin_threshold - 100.0f;
if (abs(gyroSensor->gyroDev.gyroADCf[Z]) < yawSpinResetRate) {
// testing whether 20ms of consecutive OK gyro yaw values is enough
if (cmpTimeUs(currentTimeUs, gyroSensor->yawSpinTimeUs) > 20000) {
gyroSensor->yawSpinDetected = false;
}
} else {
// reset the yaw spin time
gyroSensor->yawSpinTimeUs = currentTimeUs;
}
}
static void gyroInitSensorFilters(gyroSensor_t *gyroSensor)
{
#if defined(USE_GYRO_SLEW_LIMITER)
gyroInitSlewLimiter(gyroSensor);
#endif
uint16_t gyro_lowpass_hz = gyroConfig()->gyro_lowpass_hz;
#ifdef USE_DYN_LPF
if (gyroConfig()->dyn_lpf_gyro_min_hz > 0) {
gyro_lowpass_hz = gyroConfig()->dyn_lpf_gyro_min_hz;
}
#endif
gyroInitLowpassFilterLpf(
gyroSensor,
FILTER_LOWPASS,
gyroConfig()->gyro_lowpass_type,
gyro_lowpass_hz
);
gyroInitLowpassFilterLpf(
gyroSensor,
FILTER_LOWPASS2,
gyroConfig()->gyro_lowpass2_type,
gyroConfig()->gyro_lowpass2_hz
);
gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
#ifdef USE_GYRO_DATA_ANALYSE
gyroInitFilterDynamicNotch(gyroSensor);
#endif
#ifdef USE_DYN_LPF
dynLpfFilterInit();
#endif
}
FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
{
int32_t ret = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
if (gyroConfig()->checkOverflow || gyroHasOverflowProtection) {
// don't use the slew limiter if overflow checking is on or gyro is not subject to overflow bug
return ret;
}
if (abs(ret - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
// there has been a large change in value, so assume overflow has occurred and return the previous value
ret = gyroSensor->gyroDev.gyroADCRawPrevious[axis];
} else {
gyroSensor->gyroDev.gyroADCRawPrevious[axis] = ret;
}
return ret;
}
STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t gyroMovementCalibrationThreshold)
{
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
// Reset g[axis] at start of calibration
if (isOnFirstGyroCalibrationCycle(&gyroSensor->calibration)) {
gyroSensor->calibration.sum[axis] = 0.0f;
devClear(&gyroSensor->calibration.var[axis]);
// gyroZero is set to zero until calibration complete
gyroSensor->gyroDev.gyroZero[axis] = 0.0f;
}
// Sum up CALIBRATING_GYRO_TIME_US readings
gyroSensor->calibration.sum[axis] += gyroSensor->gyroDev.gyroADCRaw[axis];
devPush(&gyroSensor->calibration.var[axis], gyroSensor->gyroDev.gyroADCRaw[axis]);
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
const float stddev = devStandardDeviation(&gyroSensor->calibration.var[axis]);
// DEBUG_GYRO_CALIBRATION records the standard deviation of roll
// into the spare field - debug[3], in DEBUG_GYRO_RAW
if (axis == X) {
DEBUG_SET(DEBUG_GYRO_RAW, DEBUG_GYRO_CALIBRATION, lrintf(stddev));
}
// check deviation and startover in case the model was moved
if (gyroMovementCalibrationThreshold && stddev > gyroMovementCalibrationThreshold) {
gyroSetCalibrationCycles(gyroSensor);
return;
}
// please take care with exotic boardalignment !!
gyroSensor->gyroDev.gyroZero[axis] = gyroSensor->calibration.sum[axis] / gyroCalculateCalibratingCycles();
if (axis == Z) {
gyroSensor->gyroDev.gyroZero[axis] -= ((float)gyroConfig()->gyro_offset_yaw / 100);
}
}
}
if (isOnFinalGyroCalibrationCycle(&gyroSensor->calibration)) {
schedulerResetTaskStatistics(TASK_SELF); // so calibration cycles do not pollute tasks statistics
if (!firstArmingCalibrationWasStarted || (getArmingDisableFlags() & ~ARMING_DISABLED_CALIBRATING) == 0) {
beeper(BEEPER_GYRO_CALIBRATED);
}
}
--gyroSensor->calibration.cyclesRemaining;
}
static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor)
{
gyroSensor->notchFilterDynApplyFn = nullFilterApply;
gyroSensor->notchFilterDynApplyFn2 = nullFilterApply;
if (isDynamicFilterActive()) {
gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
if(gyroConfig()->dyn_notch_width_percent != 0) {
gyroSensor->notchFilterDynApplyFn2 = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2
}
const float notchQ = filterGetNotchQ(DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, DYNAMIC_NOTCH_DEFAULT_CUTOFF_HZ); // any defaults OK here
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilterInit(&gyroSensor->notchFilterDyn[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
biquadFilterInit(&gyroSensor->notchFilterDyn2[axis], DYNAMIC_NOTCH_DEFAULT_CENTER_HZ, gyro.targetLooptime, notchQ, FILTER_NOTCH);
}
}
}
bool shouldProcessGyro(void)
{
if (gyroConfig()->gyro_sync) {
bool sync = gyroSyncIsDataReady();
if (sync && debugMode == DEBUG_GYRO_SYNC) {
debug[1]++;
}
return sync;
}
int32_t diff = currentTime - gyroUpdateAt;
bool timeout = (diff >= 0);
return timeout;
}
bool sensorsAutodetect(void)
{
memset(&acc, 0, sizeof(acc));
memset(&gyro, 0, sizeof(gyro));
#if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) || defined(USE_ACC_MPU6050)
const extiConfig_t *extiConfig = selectMPUIntExtiConfig();
mpuDetectionResult_t *mpuDetectionResult = detectMpu(extiConfig);
UNUSED(mpuDetectionResult);
#endif
if (!detectGyro()) {
return false;
}
// this is safe because either mpu6050 or mpu3050 or lg3d20 sets it, and in case of fail, we never get here.
gyro.init(gyroConfig()->gyro_lpf);
if (detectAcc(sensorSelectionConfig()->acc_hardware)) {
acc.acc_1G = 256; // set default
acc.init(&acc);
}
#ifdef BARO
detectBaro(sensorSelectionConfig()->baro_hardware);
#endif
#ifdef MAG
if (detectMag(sensorSelectionConfig()->mag_hardware)) {
if (!compassInit()) {
sensorsClear(SENSOR_MAG);
}
}
#endif
reconfigureAlignment(sensorAlignmentConfig());
return true;
}
void dynLpfGyroUpdate(float throttle)
{
if (dynLpfFilter != DYN_LPF_NONE) {
const unsigned int cutoffFreq = fmax(dynThrottle(throttle) * dynLpfMax, dynLpfMin);
if (dynLpfFilter == DYN_LPF_PT1) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
const float gyroDt = gyro.targetLooptime * 1e-6f;
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
}
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
pt1FilterUpdateCutoff(&gyroSensor2.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
}
#else
pt1FilterUpdateCutoff(&gyroSensor1.lowpassFilter[axis].pt1FilterState, pt1FilterGain(cutoffFreq, gyroDt));
#endif
}
} else if (dynLpfFilter == DYN_LPF_BIQUAD) {
DEBUG_SET(DEBUG_DYN_LPF, 2, cutoffFreq);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
#ifdef USE_MULTI_GYRO
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
}
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2 || gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH) {
biquadFilterUpdateLPF(&gyroSensor2.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
}
#else
biquadFilterUpdateLPF(&gyroSensor1.lowpassFilter[axis].biquadFilterState, cutoffFreq, gyro.targetLooptime);
#endif
}
}
}
}
static FAST_CODE void checkForYawSpin(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
// if not in overflow mode, handle yaw spins above threshold
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->overflowDetected) {
gyroSensor->yawSpinDetected = false;
return;
}
#endif // USE_GYRO_OVERFLOW_CHECK
if (gyroSensor->yawSpinDetected) {
handleYawSpin(gyroSensor, currentTimeUs);
} else {
#ifndef SIMULATOR_BUILD
// check for spin on yaw axis only
if (abs(gyroSensor->gyroDev.gyroADCf[Z]) > gyroConfig()->yaw_spin_threshold) {
gyroSensor->yawSpinDetected = true;
gyroSensor->yawSpinTimeUs = currentTimeUs;
}
#endif // SIMULATOR_BUILD
}
}
static int32_t gyroCalculateCalibratingCycles(void)
{
return (gyroConfig()->gyroCalibrationDuration * 10000) / gyro.targetLooptime;
}
bool gyroInit(void)
{
#ifdef USE_GYRO_OVERFLOW_CHECK
if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_YAW) {
overflowAxisMask = GYRO_OVERFLOW_Z;
} else if (gyroConfig()->checkOverflow == GYRO_OVERFLOW_CHECK_ALL_AXES) {
overflowAxisMask = GYRO_OVERFLOW_X | GYRO_OVERFLOW_Y | GYRO_OVERFLOW_Z;
} else {
overflowAxisMask = 0;
}
#endif
gyroDebugMode = DEBUG_NONE;
useDualGyroDebugging = false;
switch (debugMode) {
case DEBUG_FFT:
case DEBUG_FFT_FREQ:
case DEBUG_GYRO_RAW:
case DEBUG_GYRO_SCALED:
case DEBUG_GYRO_FILTERED:
case DEBUG_DYN_LPF:
gyroDebugMode = debugMode;
break;
case DEBUG_DUAL_GYRO:
case DEBUG_DUAL_GYRO_COMBINE:
case DEBUG_DUAL_GYRO_DIFF:
case DEBUG_DUAL_GYRO_RAW:
case DEBUG_DUAL_GYRO_SCALED:
useDualGyroDebugging = true;
break;
}
firstArmingCalibrationWasStarted = false;
gyroDetectionFlags = NO_GYROS_DETECTED;
gyroToUse = gyroConfig()->gyro_to_use;
if (gyroDetectSensor(&gyroSensor1, gyroDeviceConfig(0))) {
gyroDetectionFlags |= DETECTED_GYRO_1;
}
#if defined(USE_MULTI_GYRO)
if (gyroDetectSensor(&gyroSensor2, gyroDeviceConfig(1))) {
gyroDetectionFlags |= DETECTED_GYRO_2;
}
#endif
if (gyroDetectionFlags == NO_GYROS_DETECTED) {
return false;
}
#if defined(USE_MULTI_GYRO)
if ((gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !(gyroDetectionFlags & DETECTED_BOTH_GYROS))
|| (gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & DETECTED_GYRO_1))
|| (gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & DETECTED_GYRO_2))) {
if (gyroDetectionFlags & DETECTED_GYRO_1) {
gyroToUse = GYRO_CONFIG_USE_GYRO_1;
} else {
gyroToUse = GYRO_CONFIG_USE_GYRO_2;
}
gyroConfigMutable()->gyro_to_use = gyroToUse;
}
// Only allow using both gyros simultaneously if they are the same hardware type.
if ((gyroDetectionFlags & DETECTED_BOTH_GYROS) && gyroSensor1.gyroDev.gyroHardware == gyroSensor2.gyroDev.gyroHardware) {
gyroDetectionFlags |= DETECTED_DUAL_GYROS;
} else if (gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
// If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
gyroToUse = GYRO_CONFIG_USE_GYRO_1;
gyroConfigMutable()->gyro_to_use = gyroToUse;
}
if (gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
gyroInitSensor(&gyroSensor2, gyroDeviceConfig(1));
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor2.gyroDev.gyroHasOverflowProtection;
detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor2.gyroDev.gyroHardware;
}
#endif
if (gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
gyroInitSensor(&gyroSensor1, gyroDeviceConfig(0));
gyroHasOverflowProtection = gyroHasOverflowProtection && gyroSensor1.gyroDev.gyroHasOverflowProtection;
detectedSensors[SENSOR_INDEX_GYRO] = gyroSensor1.gyroDev.gyroHardware;
}
return true;
}
void validateAndFixGyroConfig(void)
{
// Prevent invalid notch cutoff
if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
}
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
}
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
pidConfigMutable()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed
gyroConfigMutable()->gyro_sync_denom = 1;
gyroConfigMutable()->gyro_use_32khz = false;
}
if (gyroConfig()->gyro_use_32khz) {
// F1 and F3 can't handle high sample speed.
#if defined(STM32F1)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 16);
#elif defined(STM32F3)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 4);
#endif
} else {
#if defined(STM32F1)
gyroConfigMutable()->gyro_sync_denom = MAX(gyroConfig()->gyro_sync_denom, 3);
#endif
}
float samplingTime;
switch (gyroMpuDetectionResult()->sensor) {
case ICM_20649_SPI:
samplingTime = 1.0f / 9000.0f;
break;
case BMI_160_SPI:
samplingTime = 0.0003125f;
break;
default:
samplingTime = 0.000125f;
break;
}
if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) {
switch (gyroMpuDetectionResult()->sensor) {
case ICM_20649_SPI:
samplingTime = 1.0f / 1100.0f;
break;
default:
samplingTime = 0.001f;
break;
}
}
if (gyroConfig()->gyro_use_32khz) {
samplingTime = 0.00003125;
}
// check for looptime restrictions based on motor protocol. Motor times have safety margin
float motorUpdateRestriction;
switch (motorConfig()->dev.motorPwmProtocol) {
case PWM_TYPE_STANDARD:
motorUpdateRestriction = 1.0f / BRUSHLESS_MOTORS_PWM_RATE;
break;
case PWM_TYPE_ONESHOT125:
motorUpdateRestriction = 0.0005f;
break;
case PWM_TYPE_ONESHOT42:
motorUpdateRestriction = 0.0001f;
break;
#ifdef USE_DSHOT
case PWM_TYPE_DSHOT150:
motorUpdateRestriction = 0.000250f;
break;
case PWM_TYPE_DSHOT300:
motorUpdateRestriction = 0.0001f;
break;
#endif
default:
motorUpdateRestriction = 0.00003125f;
break;
}
if (motorConfig()->dev.useUnsyncedPwm) {
// Prevent overriding the max rate of motors
if ((motorConfig()->dev.motorPwmProtocol <= PWM_TYPE_BRUSHED) && (motorConfig()->dev.motorPwmProtocol != PWM_TYPE_STANDARD)) {
const uint32_t maxEscRate = lrintf(1.0f / motorUpdateRestriction);
motorConfigMutable()->dev.motorPwmRate = MIN(motorConfig()->dev.motorPwmRate, maxEscRate);
}
} else {
const float pidLooptime = samplingTime * gyroConfig()->gyro_sync_denom * pidConfig()->pid_process_denom;
if (pidLooptime < motorUpdateRestriction) {
const uint8_t minPidProcessDenom = constrain(motorUpdateRestriction / (samplingTime * gyroConfig()->gyro_sync_denom), 1, MAX_PID_PROCESS_DENOM);
pidConfigMutable()->pid_process_denom = MAX(pidConfigMutable()->pid_process_denom, minPidProcessDenom);
}
}
}
static void validateAndFixConfig(void)
{
if (!(featureConfigured(FEATURE_RX_PARALLEL_PWM) || featureConfigured(FEATURE_RX_PPM) || featureConfigured(FEATURE_RX_SERIAL) || featureConfigured(FEATURE_RX_MSP))) {
featureSet(DEFAULT_RX_FEATURE);
}
if (featureConfigured(FEATURE_RX_PPM)) {
featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_SERIAL | FEATURE_RX_MSP);
}
if (featureConfigured(FEATURE_RX_MSP)) {
featureClear(FEATURE_RX_SERIAL | FEATURE_RX_PARALLEL_PWM | FEATURE_RX_PPM);
}
if (featureConfigured(FEATURE_RX_SERIAL)) {
featureClear(FEATURE_RX_PARALLEL_PWM | FEATURE_RX_MSP | FEATURE_RX_PPM);
}
if (featureConfigured(FEATURE_RX_PARALLEL_PWM)) {
featureClear(FEATURE_RX_SERIAL | FEATURE_RX_MSP | FEATURE_RX_PPM);
}
// The retarded_arm setting is incompatible with pid_at_min_throttle because full roll causes the craft to roll over on the ground.
// The pid_at_min_throttle implementation ignores yaw on the ground, but doesn't currently ignore roll when retarded_arm is enabled.
if (armingConfig()->retarded_arm && mixerConfig()->pid_at_min_throttle) {
mixerConfig()->pid_at_min_throttle = 0;
}
if (gyroConfig()->gyro_soft_notch_hz < gyroConfig()->gyro_soft_notch_cutoff_hz) {
gyroConfig()->gyro_soft_notch_hz = gyroConfig()->gyro_soft_notch_cutoff_hz;
}
#if defined(LED_STRIP)
#if (defined(USE_SOFTSERIAL1) || defined(USE_SOFTSERIAL2))
if (featureConfigured(FEATURE_SOFTSERIAL) && (
0
#ifdef USE_SOFTSERIAL1
|| (LED_STRIP_TIMER == SOFTSERIAL_1_TIMER)
#endif
#ifdef USE_SOFTSERIAL2
|| (LED_STRIP_TIMER == SOFTSERIAL_2_TIMER)
#endif
)) {
// led strip needs the same timer as softserial
featureClear(FEATURE_LED_STRIP);
}
#endif
#if defined(TRANSPONDER) && !defined(UNIT_TEST)
if ((WS2811_DMA_TC_FLAG == TRANSPONDER_DMA_TC_FLAG) && featureConfigured(FEATURE_TRANSPONDER) && featureConfigured(FEATURE_LED_STRIP)) {
featureClear(FEATURE_LED_STRIP);
}
#endif
#endif // LED_STRIP
#if defined(CC3D)
#if defined(DISPLAY) && defined(USE_UART3)
if (featureConfigured(FEATURE_DISPLAY) && doesConfigurationUsePort(SERIAL_PORT_UART3)) {
featureClear(FEATURE_DISPLAY);
}
#endif
#if defined(SONAR) && defined(USE_SOFTSERIAL1)
if (featureConfigured(FEATURE_SONAR) && featureConfigured(FEATURE_SOFTSERIAL)) {
featureClear(FEATURE_SONAR);
}
#endif
#if defined(SONAR) && defined(USE_SOFTSERIAL1) && defined(RSSI_ADC_GPIO)
// shared pin
if ((featureConfigured(FEATURE_SONAR) + featureConfigured(FEATURE_SOFTSERIAL) + featureConfigured(FEATURE_RSSI_ADC)) > 1) {
featureClear(FEATURE_SONAR);
featureClear(FEATURE_SOFTSERIAL);
featureClear(FEATURE_RSSI_ADC);
}
#endif
#endif // CC3D
#if defined(COLIBRI_RACE)
serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP_SERVER;
if (featureConfigured(FEATURE_RX_SERIAL)) {
serialConfig()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
}
#endif
#if defined(SPRACINGF3NEO_REV) && (SPRACINGF3NEO_REV < 5)
if (featureConfigured(FEATURE_OSD) && featureConfigured(FEATURE_TRANSPONDER)) {
featureClear(FEATURE_TRANSPONDER);
}
if (featureConfigured(FEATURE_OSD) && featureConfigured(FEATURE_LED_STRIP)) {
featureClear(FEATURE_LED_STRIP);
}
#endif
if (!isSerialConfigValid(serialConfig())) {
PG_RESET_CURRENT(serialConfig);
}
#if defined(USE_VCP)
serialConfig()->portConfigs[0].functionMask = FUNCTION_MSP_SERVER;
#endif
}
void init(void)
{
drv_pwm_config_t pwm_params;
printfSupportInit();
initEEPROM();
ensureEEPROMContainsValidData();
readEEPROM();
systemState |= SYSTEM_STATE_CONFIG_LOADED;
#ifdef STM32F303
// start fpu
SCB->CPACR = (0x3 << (10*2)) | (0x3 << (11*2));
#endif
#ifdef STM32F303xC
SetSysClock();
#endif
#ifdef STM32F10X
// Configure the System clock frequency, HCLK, PCLK2 and PCLK1 prescalers
// Configure the Flash Latency cycles and enable prefetch buffer
SetSysClock(systemConfig()->emf_avoidance);
#endif
i2cSetOverclock(systemConfig()->i2c_highspeed);
systemInit();
#ifdef USE_HARDWARE_REVISION_DETECTION
detectHardwareRevision();
#endif
// Latch active features to be used for feature() in the remainder of init().
latchActiveFeatures();
// initialize IO (needed for all IO operations)
IOInitGlobal();
debugMode = debugConfig()->debug_mode;
#ifdef USE_EXTI
EXTIInit();
#endif
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_1) {
ledInit(false);
} else {
ledInit(true);
}
#else
ledInit(false);
#endif
#ifdef BEEPER
beeperConfig_t beeperConfig = {
.gpioPeripheral = BEEP_PERIPHERAL,
.gpioPin = BEEP_PIN,
.gpioPort = BEEP_GPIO,
#ifdef BEEPER_INVERTED
.gpioMode = Mode_Out_PP,
.isInverted = true
#else
.gpioMode = Mode_Out_OD,
.isInverted = false
#endif
};
#ifdef NAZE
if (hardwareRevision >= NAZE32_REV5) {
// naze rev4 and below used opendrain to PNP for buzzer. Rev5 and above use PP to NPN.
beeperConfig.gpioMode = Mode_Out_PP;
beeperConfig.isInverted = true;
}
#endif
beeperInit(&beeperConfig);
#endif
#ifdef BUTTONS
buttonsInit();
if (!isMPUSoftReset()) {
buttonsHandleColdBootButtonPresses();
}
#endif
#ifdef SPEKTRUM_BIND
if (feature(FEATURE_RX_SERIAL)) {
switch (rxConfig()->serialrx_provider) {
case SERIALRX_SPEKTRUM1024:
case SERIALRX_SPEKTRUM2048:
// Spektrum satellite binding if enabled on startup.
// Must be called before that 100ms sleep so that we don't lose satellite's binding window after startup.
// The rest of Spektrum initialization will happen later - via spektrumInit()
spektrumBind(rxConfig());
break;
}
}
#endif
delay(100);
timerInit(); // timer must be initialized before any channel is allocated
dmaInit();
serialInit(feature(FEATURE_SOFTSERIAL));
mixerInit(customMotorMixer(0));
#ifdef USE_SERVOS
mixerInitServos(customServoMixer(0));
#endif
memset(&pwm_params, 0, sizeof(pwm_params));
#ifdef SONAR
const sonarHardware_t *sonarHardware = NULL;
sonarGPIOConfig_t sonarGPIOConfig;
if (feature(FEATURE_SONAR)) {
bool usingCurrentMeterIOPins = (feature(FEATURE_AMPERAGE_METER) && batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC);
sonarHardware = sonarGetHardwareConfiguration(usingCurrentMeterIOPins);
sonarGPIOConfig.triggerGPIO = sonarHardware->trigger_gpio;
sonarGPIOConfig.triggerPin = sonarHardware->trigger_pin;
sonarGPIOConfig.echoGPIO = sonarHardware->echo_gpio;
sonarGPIOConfig.echoPin = sonarHardware->echo_pin;
pwm_params.sonarGPIOConfig = &sonarGPIOConfig;
}
#endif
// when using airplane/wing mixer, servo/motor outputs are remapped
if (mixerConfig()->mixerMode == MIXER_AIRPLANE || mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_CUSTOM_AIRPLANE)
pwm_params.airplane = true;
else
pwm_params.airplane = false;
#if defined(USE_UART2) && defined(STM32F10X)
pwm_params.useUART2 = doesConfigurationUsePort(SERIAL_PORT_UART2);
#endif
#if defined(USE_UART3)
pwm_params.useUART3 = doesConfigurationUsePort(SERIAL_PORT_UART3);
#endif
#if defined(USE_UART4)
pwm_params.useUART4 = doesConfigurationUsePort(SERIAL_PORT_UART4);
#endif
#if defined(USE_UART5)
pwm_params.useUART5 = doesConfigurationUsePort(SERIAL_PORT_UART5);
#endif
pwm_params.useVbat = feature(FEATURE_VBAT);
pwm_params.useSoftSerial = feature(FEATURE_SOFTSERIAL);
pwm_params.useParallelPWM = feature(FEATURE_RX_PARALLEL_PWM);
pwm_params.useRSSIADC = feature(FEATURE_RSSI_ADC);
pwm_params.useCurrentMeterADC = (
feature(FEATURE_AMPERAGE_METER)
&& batteryConfig()->amperageMeterSource == AMPERAGE_METER_ADC
);
pwm_params.useLEDStrip = feature(FEATURE_LED_STRIP);
pwm_params.usePPM = feature(FEATURE_RX_PPM);
pwm_params.useSerialRx = feature(FEATURE_RX_SERIAL);
#ifdef SONAR
pwm_params.useSonar = feature(FEATURE_SONAR);
#endif
#ifdef USE_SERVOS
pwm_params.useServos = isMixerUsingServos();
pwm_params.useChannelForwarding = feature(FEATURE_CHANNEL_FORWARDING);
pwm_params.servoCenterPulse = servoConfig()->servoCenterPulse;
pwm_params.servoPwmRate = servoConfig()->servo_pwm_rate;
#endif
pwm_params.useOneshot = feature(FEATURE_ONESHOT125);
pwm_params.motorPwmRate = motorConfig()->motor_pwm_rate;
pwm_params.idlePulse = calculateMotorOff();
if (pwm_params.motorPwmRate > 500)
pwm_params.idlePulse = 0; // brushed motors
pwmRxInit();
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmIOConfiguration_t *pwmIOConfiguration = pwmInit(&pwm_params);
mixerUsePWMIOConfiguration(pwmIOConfiguration);
#ifdef DEBUG_PWM_CONFIGURATION
debug[2] = pwmIOConfiguration->pwmInputCount;
debug[3] = pwmIOConfiguration->ppmInputCount;
#endif
if (!feature(FEATURE_ONESHOT125))
motorControlEnable = true;
systemState |= SYSTEM_STATE_MOTORS_READY;
#ifdef INVERTER
initInverter();
#endif
#ifdef USE_SPI
spiInit(SPI1);
spiInit(SPI2);
#ifdef STM32F303xC
#ifdef ALIENFLIGHTF3
if (hardwareRevision == AFF3_REV_2) {
spiInit(SPI3);
}
#else
spiInit(SPI3);
#endif
#endif
#endif
#ifdef USE_HARDWARE_REVISION_DETECTION
updateHardwareRevision();
#endif
#if defined(NAZE)
if (hardwareRevision == NAZE32_SP) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
} else {
serialRemovePort(SERIAL_PORT_UART3);
}
#endif
#if defined(SPRACINGF3) && defined(SONAR) && defined(USE_SOFTSERIAL2)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL2);
}
#endif
#if defined(SPRACINGF3MINI) && defined(SONAR) && defined(USE_SOFTSERIAL1)
if (feature(FEATURE_SONAR) && feature(FEATURE_SOFTSERIAL)) {
serialRemovePort(SERIAL_PORT_SOFTSERIAL1);
}
#endif
#ifdef USE_I2C
#if defined(NAZE)
if (hardwareRevision != NAZE32_SP) {
i2cInit(I2C_DEVICE);
} else {
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
}
#elif defined(CC3D)
if (!doesConfigurationUsePort(SERIAL_PORT_UART3)) {
i2cInit(I2C_DEVICE);
}
#else
i2cInit(I2C_DEVICE);
#endif
#endif
#ifdef USE_ADC
drv_adc_config_t adc_params;
adc_params.channelMask = 0;
#ifdef ADC_BATTERY
adc_params.channelMask = (feature(FEATURE_VBAT) ? ADC_CHANNEL_MASK(ADC_BATTERY) : 0);
#endif
#ifdef ADC_RSSI
adc_params.channelMask |= (feature(FEATURE_RSSI_ADC) ? ADC_CHANNEL_MASK(ADC_RSSI) : 0);
#endif
#ifdef ADC_AMPERAGE
adc_params.channelMask |= (feature(FEATURE_AMPERAGE_METER) ? ADC_CHANNEL_MASK(ADC_AMPERAGE) : 0);
#endif
#ifdef ADC_POWER_12V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_12V);
#endif
#ifdef ADC_POWER_5V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_5V);
#endif
#ifdef ADC_POWER_3V
adc_params.channelMask |= ADC_CHANNEL_MASK(ADC_POWER_3V);
#endif
#ifdef NAZE
// optional ADC5 input on rev.5 hardware
adc_params.channelMask |= (hardwareRevision >= NAZE32_REV5) ? ADC_CHANNEL_MASK(ADC_EXTERNAL) : 0;
#endif
adcInit(&adc_params);
#endif
initBoardAlignment();
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
displayInit();
}
#endif
#ifdef NAZE
if (hardwareRevision < NAZE32_REV5) {
gyroConfig()->gyro_sync = 0;
}
#endif
if (!sensorsAutodetect()) {
// if gyro was not detected due to whatever reason, we give up now.
failureMode(FAILURE_MISSING_ACC);
}
systemState |= SYSTEM_STATE_SENSORS_READY;
flashLedsAndBeep();
mspInit();
mspSerialInit();
const uint16_t pidPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz);
pidSetTargetLooptime(pidPeriodUs * gyroConfig()->pid_process_denom);
pidInitFilters(pidProfile());
#ifdef USE_SERVOS
mixerInitialiseServoFiltering(targetPidLooptime);
#endif
imuInit();
#ifdef USE_CLI
cliInit();
#endif
failsafeInit();
rxInit(modeActivationProfile()->modeActivationConditions);
#ifdef GPS
if (feature(FEATURE_GPS)) {
gpsInit();
navigationInit(pidProfile());
}
#endif
#ifdef SONAR
if (feature(FEATURE_SONAR)) {
sonarInit(sonarHardware);
}
#endif
#ifdef LED_STRIP
ledStripInit();
if (feature(FEATURE_LED_STRIP)) {
ledStripEnable();
}
#endif
#ifdef TELEMETRY
if (feature(FEATURE_TELEMETRY)) {
telemetryInit();
}
#endif
#ifdef USB_CABLE_DETECTION
usbCableDetectInit();
#endif
#ifdef TRANSPONDER
if (feature(FEATURE_TRANSPONDER)) {
transponderInit(transponderConfig()->data);
transponderEnable();
transponderStartRepeating();
systemState |= SYSTEM_STATE_TRANSPONDER_ENABLED;
}
#endif
#ifdef USE_FLASHFS
#ifdef NAZE
if (hardwareRevision == NAZE32_REV5) {
m25p16_init();
}
#elif defined(USE_FLASH_M25P16)
m25p16_init();
#endif
flashfsInit();
#endif
#ifdef USE_SDCARD
bool sdcardUseDMA = false;
sdcardInsertionDetectInit();
#ifdef SDCARD_DMA_CHANNEL_TX
#if defined(LED_STRIP) && defined(WS2811_DMA_CHANNEL)
// Ensure the SPI Tx DMA doesn't overlap with the led strip
sdcardUseDMA = !feature(FEATURE_LED_STRIP) || SDCARD_DMA_CHANNEL_TX != WS2811_DMA_CHANNEL;
#else
sdcardUseDMA = true;
#endif
#endif
sdcard_init(sdcardUseDMA);
afatfs_init();
#endif
#ifdef BLACKBOX
initBlackbox();
#endif
if (mixerConfig()->mixerMode == MIXER_GIMBAL) {
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
}
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
#ifdef BARO
baroSetCalibrationCycles(CALIBRATING_BARO_CYCLES);
#endif
// start all timers
// TODO - not implemented yet
timerStart();
ENABLE_STATE(SMALL_ANGLE);
DISABLE_ARMING_FLAG(PREVENT_ARMING);
#ifdef SOFTSERIAL_LOOPBACK
// FIXME this is a hack, perhaps add a FUNCTION_LOOPBACK to support it properly
loopbackPort = (serialPort_t*)&(softSerialPorts[0]);
if (!loopbackPort->vTable) {
loopbackPort = openSoftSerial(0, NULL, 19200, SERIAL_NOT_INVERTED);
}
serialPrint(loopbackPort, "LOOPBACK\r\n");
#endif
if (feature(FEATURE_VBAT)) {
// Now that everything has powered up the voltage and cell count be determined.
voltageMeterInit();
batteryInit();
}
if (feature(FEATURE_AMPERAGE_METER)) {
amperageMeterInit();
}
#ifdef DISPLAY
if (feature(FEATURE_DISPLAY)) {
#ifdef USE_OLED_GPS_DEBUG_PAGE_ONLY
displayShowFixedPage(PAGE_GPS);
#else
displayResetPageCycling();
displayEnablePageCycling();
#endif
}
#endif
#ifdef CJMCU
LED2_ON;
#endif
// Latch active features AGAIN since some may be modified by init().
latchActiveFeatures();
motorControlEnable = true;
systemState |= SYSTEM_STATE_READY;
}
#ifdef SOFTSERIAL_LOOPBACK
void processLoopback(void) {
if (loopbackPort) {
uint8_t bytesWaiting;
while ((bytesWaiting = serialRxBytesWaiting(loopbackPort))) {
uint8_t b = serialRead(loopbackPort);
serialWrite(loopbackPort, b);
};
}
}
#else
#define processLoopback()
#endif
void configureScheduler(void)
{
schedulerInit();
setTaskEnabled(TASK_SYSTEM, true);
uint16_t gyroPeriodUs = US_FROM_HZ(gyro.sampleFrequencyHz);
rescheduleTask(TASK_GYRO, gyroPeriodUs);
setTaskEnabled(TASK_GYRO, true);
rescheduleTask(TASK_PID, gyroPeriodUs);
setTaskEnabled(TASK_PID, true);
if (sensors(SENSOR_ACC)) {
setTaskEnabled(TASK_ACCEL, true);
}
setTaskEnabled(TASK_ATTITUDE, sensors(SENSOR_ACC));
setTaskEnabled(TASK_SERIAL, true);
#ifdef BEEPER
setTaskEnabled(TASK_BEEPER, true);
#endif
setTaskEnabled(TASK_BATTERY, feature(FEATURE_VBAT) || feature(FEATURE_AMPERAGE_METER));
setTaskEnabled(TASK_RX, true);
#ifdef GPS
setTaskEnabled(TASK_GPS, feature(FEATURE_GPS));
#endif
#ifdef MAG
setTaskEnabled(TASK_COMPASS, sensors(SENSOR_MAG));
#if defined(MPU6500_SPI_INSTANCE) && defined(USE_MAG_AK8963)
// fixme temporary solution for AK6983 via slave I2C on MPU9250
rescheduleTask(TASK_COMPASS, 1000000 / 40);
#endif
#endif
#ifdef BARO
setTaskEnabled(TASK_BARO, sensors(SENSOR_BARO));
#endif
#ifdef SONAR
setTaskEnabled(TASK_SONAR, sensors(SENSOR_SONAR));
#endif
#if defined(BARO) || defined(SONAR)
setTaskEnabled(TASK_ALTITUDE, sensors(SENSOR_BARO) || sensors(SENSOR_SONAR));
#endif
#ifdef DISPLAY
setTaskEnabled(TASK_DISPLAY, feature(FEATURE_DISPLAY));
#endif
#ifdef TELEMETRY
setTaskEnabled(TASK_TELEMETRY, feature(FEATURE_TELEMETRY));
#endif
#ifdef LED_STRIP
setTaskEnabled(TASK_LEDSTRIP, feature(FEATURE_LED_STRIP));
#endif
#ifdef TRANSPONDER
setTaskEnabled(TASK_TRANSPONDER, feature(FEATURE_TRANSPONDER));
#endif
}
