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
}
