void loop(void) { static uint32_t loopTime; #ifdef BARO static bool haveProcessedAnnexCodeOnce = false; #endif updateRx(); if (shouldProcessRx(currentTime)) { processRx(); #ifdef BARO // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. if (haveProcessedAnnexCodeOnce) { if (sensors(SENSOR_BARO)) { updateAltHoldState(); } } #endif } else { // not processing rx this iteration executePeriodicTasks(); } currentTime = micros(); if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + masterConfig.looptime; computeIMU(¤tProfile.accelerometerTrims, masterConfig.mixerConfiguration); // Measure loop rate just after reading the sensors currentTime = micros(); cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; annexCode(); #ifdef BARO haveProcessedAnnexCodeOnce = true; #endif #ifdef AUTOTUNE updateAutotuneState(); #endif #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #ifdef BARO if (sensors(SENSOR_BARO)) { if (f.BARO_MODE) { updateAltHold(); } debug[0] = rcCommand[THROTTLE]; } #endif if (currentProfile.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile.throttle_correction_value); } #ifdef GPS if (sensors(SENSOR_GPS)) { if ((f.GPS_HOME_MODE || f.GPS_HOLD_MODE) && f.GPS_FIX_HOME) { updateGpsStateForHomeAndHoldMode(); } } #endif // PID - note this is function pointer set by setPIDController() pid_controller( ¤tProfile.pidProfile, ¤tProfile.controlRateConfig, masterConfig.max_angle_inclination, ¤tProfile.accelerometerTrims ); mixTable(); writeServos(); writeMotors(); } #ifdef TELEMETRY if (!cliMode && feature(FEATURE_TELEMETRY)) { handleTelemetry(); } #endif #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { updateLedStrip(); } #endif }
void annexCode(void) { static uint32_t calibratedAccTime; int32_t tmp, tmp2; int32_t axis, prop1, prop2; static uint8_t buzzerFreq; // delay between buzzer ring // vbat shit static uint8_t vbatTimer = 0; static int32_t vbatRaw = 0; static int32_t amperageRaw = 0; static int64_t mAhdrawnRaw = 0; static int32_t vbatCycleTime = 0; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE] < cfg.tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)cfg.dynThrPID * (rcData[THROTTLE] - cfg.tpa_breakpoint) / (2000 - cfg.tpa_breakpoint); } else { prop2 = 100 - cfg.dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = min(abs(rcData[axis] - mcfg.midrc), 500); if (axis != 2) { // ROLL & PITCH if (cfg.deadband) { if (tmp > cfg.deadband) { tmp -= cfg.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)cfg.rollPitchRate * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else { // YAW if (cfg.yawdeadband) { if (tmp > cfg.yawdeadband) { tmp -= cfg.yawdeadband; } else { tmp = 0; } } rcCommand[axis] = tmp * -mcfg.yaw_control_direction; prop1 = 100 - (uint16_t)cfg.yawRate * abs(tmp) / 500; } dynP8[axis] = (uint16_t)cfg.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)cfg.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)cfg.D8[axis] * prop1 / 100; if (rcData[axis] < mcfg.midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], mcfg.mincheck, 2000); tmp = (uint32_t)(tmp - mcfg.mincheck) * 1000 / (2000 - mcfg.mincheck); // [MINCHECK;2000] -> [0;1000] tmp2 = tmp / 100; rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE] if (f.HEADFREE_MODE) { float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f; float cosDiff = cosf(radDiff); float sinDiff = sinf(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (feature(FEATURE_VBAT)) { vbatCycleTime += cycleTime; if (!(++vbatTimer % VBATFREQ)) { vbatRaw -= vbatRaw / 8; vbatRaw += adcGetChannel(ADC_BATTERY); vbat = batteryAdcToVoltage(vbatRaw / 8); if (mcfg.power_adc_channel > 0) { amperageRaw -= amperageRaw / 8; amperageRaw += adcGetChannel(ADC_EXTERNAL_CURRENT); amperage = currentSensorToCentiamps(amperageRaw / 8); mAhdrawnRaw += (amperage * vbatCycleTime) / 1000; mAhdrawn = mAhdrawnRaw / (3600 * 100); vbatCycleTime = 0; } } if ((vbat > batteryWarningVoltage) || (vbat < mcfg.vbatmincellvoltage)) { // VBAT ok, buzzer off buzzerFreq = 0; } else buzzerFreq = 4; // low battery } buzzer(buzzerFreq); // external buzzer routine that handles buzzer events globally now if ((calibratingA > 0 && sensors(SENSOR_ACC)) || (calibratingG > 0)) { // Calibration phasis LED0_TOGGLE; } else { if (f.ACC_CALIBRATED) LED0_OFF; if (f.ARMED) LED0_ON; #ifndef CJMCU checkTelemetryState(); #endif } #ifdef LEDRING if (feature(FEATURE_LED_RING)) { static uint32_t LEDTime; if ((int32_t)(currentTime - LEDTime) >= 0) { LEDTime = currentTime + 50000; ledringState(); } } #endif if ((int32_t)(currentTime - calibratedAccTime) >= 0) { if (!f.SMALL_ANGLE) { f.ACC_CALIBRATED = 0; // the multi uses ACC and is not calibrated or is too much inclinated LED0_TOGGLE; calibratedAccTime = currentTime + 500000; } else { f.ACC_CALIBRATED = 1; } } serialCom(); #ifndef CJMCU if (!cliMode && feature(FEATURE_TELEMETRY)) { handleTelemetry(); } #endif if (sensors(SENSOR_GPS)) { static uint32_t GPSLEDTime; if ((int32_t)(currentTime - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) { GPSLEDTime = currentTime + 150000; LED1_TOGGLE; } } // Read out gyro temperature. can use it for something somewhere. maybe get MCU temperature instead? lots of fun possibilities. if (gyro.temperature) gyro.temperature(&telemTemperature1); else { // TODO MCU temp } }
void executePeriodicTasks(void) { static int periodicTaskIndex = 0; switch (periodicTaskIndex++) { #ifdef MAG case UPDATE_COMPASS_TASK: if (sensors(SENSOR_MAG)) { updateCompass(&masterConfig.magZero); } break; #endif #ifdef BARO case UPDATE_BARO_TASK: if (sensors(SENSOR_BARO)) { baroUpdate(currentTime); } break; #endif #if defined(BARO) || defined(SONAR) case CALCULATE_ALTITUDE_TASK: #if defined(BARO) && !defined(SONAR) if (sensors(SENSOR_BARO) && isBaroReady()) { #endif #if defined(BARO) && defined(SONAR) if ((sensors(SENSOR_BARO) && isBaroReady()) || sensors(SENSOR_SONAR)) { #endif #if !defined(BARO) && defined(SONAR) if (sensors(SENSOR_SONAR)) { #endif calculateEstimatedAltitude(currentTime); } break; #endif #ifdef SONAR case UPDATE_SONAR_TASK: if (sensors(SENSOR_SONAR)) { sonarUpdate(); } break; #endif #ifdef DISPLAY case UPDATE_DISPLAY_TASK: if (feature(FEATURE_DISPLAY)) { updateDisplay(); } break; #endif } if (periodicTaskIndex >= PERIODIC_TASK_COUNT) { periodicTaskIndex = 0; } } void processRx(void) { calculateRxChannelsAndUpdateFailsafe(currentTime); // in 3D mode, we need to be able to disarm by switch at any time if (feature(FEATURE_3D)) { if (!IS_RC_MODE_ACTIVE(BOXARM)) mwDisarm(); } updateRSSI(currentTime); if (feature(FEATURE_FAILSAFE)) { if (currentTime > FAILSAFE_POWER_ON_DELAY_US && !failsafeIsEnabled()) { failsafeEnable(); } failsafeUpdateState(); } throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); if (throttleStatus == THROTTLE_LOW) { pidResetErrorAngle(); pidResetErrorGyro(); } // When armed and motors aren't spinning, disarm board after delay so users without buzzer won't lose fingers. // mixTable constrains motor commands, so checking throttleStatus is enough if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING) && masterConfig.auto_disarm_delay != 0 && isUsingSticksForArming()) { if (throttleStatus == THROTTLE_LOW) { if ((int32_t)(disarmAt - millis()) < 0) // delay is over mwDisarm(); } else { disarmAt = millis() + masterConfig.auto_disarm_delay * 1000; // extend delay } } processRcStickPositions(&masterConfig.rxConfig, throttleStatus, masterConfig.retarded_arm, masterConfig.disarm_kill_switch); if (feature(FEATURE_INFLIGHT_ACC_CAL)) { updateInflightCalibrationState(); } updateActivatedModes(currentProfile->modeActivationConditions); if (!cliMode) { updateAdjustmentStates(currentProfile->adjustmentRanges); processRcAdjustments(currentControlRateProfile, &masterConfig.rxConfig); } bool canUseHorizonMode = true; if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafeHasTimerElapsed())) && (sensors(SENSOR_ACC))) { // bumpless transfer to Level mode canUseHorizonMode = false; if (!FLIGHT_MODE(ANGLE_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(ANGLE_MODE); } } else { DISABLE_FLIGHT_MODE(ANGLE_MODE); // failsafe support } if (IS_RC_MODE_ACTIVE(BOXHORIZON) && canUseHorizonMode) { DISABLE_FLIGHT_MODE(ANGLE_MODE); if (!FLIGHT_MODE(HORIZON_MODE)) { pidResetErrorAngle(); ENABLE_FLIGHT_MODE(HORIZON_MODE); } } else { DISABLE_FLIGHT_MODE(HORIZON_MODE); } if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { LED1_ON; } else { LED1_OFF; } #ifdef MAG if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { if (IS_RC_MODE_ACTIVE(BOXMAG)) { if (!FLIGHT_MODE(MAG_MODE)) { ENABLE_FLIGHT_MODE(MAG_MODE); magHold = heading; } } else { DISABLE_FLIGHT_MODE(MAG_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { headFreeModeHold = heading; // acquire new heading } } #endif #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsWaypointsAndMode(); } #endif if (IS_RC_MODE_ACTIVE(BOXPASSTHRU)) { ENABLE_FLIGHT_MODE(PASSTHRU_MODE); } else { DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } } void loop(void) { static uint32_t loopTime; #if defined(BARO) || defined(SONAR) static bool haveProcessedAnnexCodeOnce = false; #endif updateRx(); if (shouldProcessRx(currentTime)) { processRx(); #ifdef BARO // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. if (haveProcessedAnnexCodeOnce) { if (sensors(SENSOR_BARO)) { updateAltHoldState(); } } #endif #ifdef SONAR // the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data. if (haveProcessedAnnexCodeOnce) { if (sensors(SENSOR_SONAR)) { updateSonarAltHoldState(); } } #endif } else { // not processing rx this iteration executePeriodicTasks(); // if GPS feature is enabled, gpsThread() will be called at some intervals to check for stuck // hardware, wrong baud rates, init GPS if needed, etc. Don't use SENSOR_GPS here as gpsThread() can and will // change this based on available hardware #ifdef GPS if (feature(FEATURE_GPS)) { gpsThread(); } #endif } currentTime = micros(); if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + masterConfig.looptime; imuUpdate(¤tProfile->accelerometerTrims, masterConfig.mixerMode); // Measure loop rate just after reading the sensors currentTime = micros(); cycleTime = (int32_t)(currentTime - previousTime); previousTime = currentTime; annexCode(); #if defined(BARO) || defined(SONAR) haveProcessedAnnexCodeOnce = true; #endif #ifdef AUTOTUNE updateAutotuneState(); #endif #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #if defined(BARO) || defined(SONAR) if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(&masterConfig.airplaneConfig); } } #endif // If we're armed, at minimum throttle, and we do arming via the // sticks, do not process yaw input from the rx. We do this so the // motors do not spin up while we are trying to arm or disarm. if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck) { rcCommand[YAW] = 0; } if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(currentProfile->throttle_correction_value); } #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif // PID - note this is function pointer set by setPIDController() pid_controller( ¤tProfile->pidProfile, currentControlRateProfile, masterConfig.max_angle_inclination, ¤tProfile->accelerometerTrims, &masterConfig.rxConfig ); mixTable(); #ifdef USE_SERVOS filterServos(); writeServos(); #endif writeMotors(); #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif } #ifdef TELEMETRY if (!cliMode && feature(FEATURE_TELEMETRY)) { handleTelemetry(); } #endif #ifdef LED_STRIP if (feature(FEATURE_LED_STRIP)) { updateLedStrip(); } #endif }