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); } }
long cmsMenuExit(displayPort_t *pDisplay, const void *ptr) { if (ptr) { displayClear(pDisplay); displayWrite(pDisplay, 5, 3, "REBOOTING..."); displayResync(pDisplay); // Was max7456RefreshAll(); why at this timing? stopMotors(); stopPwmAllMotors(); delay(200); cmsTraverseGlobalExit(&menuMain); if (currentMenu->onExit) currentMenu->onExit((OSD_Entry *)NULL); // Forced exit saveConfigAndNotify(); } cmsInMenu = false; displayRelease(pDisplay); currentMenu = NULL; if (ptr) systemReset(); ENABLE_ARMING_FLAG(OK_TO_ARM); return 0; }
static void updateLEDs(void) { if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0 || armingCalibrationWasInitialised) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || (averageSystemLoadPercent > 100)) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } }
void annexCode(void) { int32_t throttleValue; // Compute ROLL PITCH and YAW command rcCommand[ROLL] = getAxisRcCommand(rcData[ROLL], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband); rcCommand[PITCH] = getAxisRcCommand(rcData[PITCH], currentControlRateProfile->rcExpo8, currentProfile->rcControlsConfig.deadband); rcCommand[YAW] = -getAxisRcCommand(rcData[YAW], currentControlRateProfile->rcYawExpo8, currentProfile->rcControlsConfig.yaw_deadband); //Compute THROTTLE command throttleValue = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); throttleValue = (uint32_t)(throttleValue - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] rcCommand[THROTTLE] = rcLookupThrottle(throttleValue); if (FLIGHT_MODE(HEADFREE_MODE)) { const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); const float cosDiff = cos_approx(radDiff); const float sinDiff = sin_approx(radDiff); const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } #if defined(NAV) if (naivationBlockArming()) { DISABLE_ARMING_FLAG(OK_TO_ARM); } #endif if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } warningLedUpdate(); } // 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); }
void mwArm(void) { 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); resetMagHoldHeading(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP); if (sharedBlackboxAndMspPort) { mspSerialReleasePortIfAllocated(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 NAV if (navigationPositionEstimateIsHealthy()) beeper(BEEPER_ARMING_GPS_FIX); else beeper(BEEPER_ARMING); #else beeper(BEEPER_ARMING); #endif return; } } if (!ARMING_FLAG(ARMED)) { beeperConfirmationBeeps(1); } }
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 mwArm(void) { if (ARMING_FLAG(OK_TO_ARM)) { if (ARMING_FLAG(ARMED)) { //led0_op(true); return; } if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) { return; } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); headFreeModeHold = heading; // led2_op(1);//drona led // led1_op(1);//drona led // led0_op(0);//drona led #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); } }
void mwArm(void) { if (ARMING_FLAG(OK_TO_ARM)) { if (ARMING_FLAG(ARMED)) { return; } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); headFreeModeHold = heading; return; } } if (!ARMING_FLAG(ARMED)) { blinkLedAndSoundBeeper(2, 255, 1); } }
void mwArm(void) { if (ARMING_FLAG(OK_TO_ARM)) { if (ARMING_FLAG(ARMED)) { return; } if (rcModeIsActive(BOXFAILSAFE)) { return; } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { serialPort_t *sharedBlackboxAndMspPort = findSharedSerialPort(FUNCTION_BLACKBOX, FUNCTION_MSP_SERVER); if (sharedBlackboxAndMspPort) { mspSerialReleasePortIfAllocated(sharedBlackboxAndMspPort); } startBlackbox(); } #endif 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 mwArm(void) { if (ARMING_FLAG(OK_TO_ARM)) { if (ARMING_FLAG(ARMED)) { return; } if (!ARMING_FLAG(PREVENT_ARMING)) { ENABLE_ARMING_FLAG(ARMED); headFreeModeHold = heading; #ifdef TELEMETRY if (feature(FEATURE_TELEMETRY)) { serialPort_t *sharedPort = findSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); while (sharedPort) { mspReleasePortIfAllocated(sharedPort); sharedPort = findNextSharedSerialPort(TELEMETRY_FUNCTION_MASK, FUNCTION_MSP); } } #endif #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 return; } } if (!ARMING_FLAG(ARMED)) { blinkLedAndSoundBeeper(2, 255, 1); } }
void annexCode(void) { if (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); float sinDiff = sin_approx(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (rcModeIsActive(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } // 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); }
static void updateLEDs(void) { if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (rcModeIsActive(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { DISABLE_ARMING_FLAG(OK_TO_ARM); } uint32_t nextBlinkMask = getArmingPreventionBlinkMask(); warningLedSetBlinkMask(nextBlinkMask); warningLedUpdate(); } }
void annexCode(void) { int32_t tmp, tmp2; tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->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 (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (rcModeIsActive(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } // 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); }
void annexCode(void) { int32_t tmp, tmp2; int32_t axis, prop1 = 0, prop2; // PITCH & ROLL only dynamic PID adjustment, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop2 = 100 - currentControlRateProfile->dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - rxConfig()->midrc), 500); if (axis == ROLL || axis == PITCH) { if (rcControlsConfig()->deadband) { if (tmp > rcControlsConfig()->deadband) { tmp -= rcControlsConfig()->deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (rcControlsConfig()->yaw_deadband) { if (tmp > rcControlsConfig()->yaw_deadband) { tmp -= rcControlsConfig()->yaw_deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -rcControlsConfig()->yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)pidProfile()->P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)pidProfile()->I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)pidProfile()->D8[axis] * prop1 / 100; // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids if (axis == YAW) { PIDweight[axis] = 100; } else { PIDweight[axis] = prop2; } if (rcData[axis] < rxConfig()->midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], rxConfig()->mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - rxConfig()->mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - rxConfig()->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 (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); float sinDiff = sin_approx(radDiff); int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!imuIsAircraftArmable(armingConfig()->max_arm_angle)) { DISABLE_ARMING_FLAG(OK_TO_ARM); debug[3] = ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } debug[3] = ARMING_FLAG(OK_TO_ARM); warningLedUpdate(); } // 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); }
void failsafeUpdateState(void) { if (!failsafeIsMonitoring()) { return; } bool receivingRxData = failsafeIsReceivingRxData(); bool armed = ARMING_FLAG(ARMED); bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); beeperMode_e beeperMode = BEEPER_SILENCE; if (!receivingRxData) { beeperMode = BEEPER_RX_LOST; } bool reprocessState; do { reprocessState = false; switch (failsafeState.phase) { case FAILSAFE_IDLE: if (armed) { // Track throttle command below minimum time if (THROTTLE_HIGH == calculateThrottleStatus(rxConfig, deadband3dThrottle)) { failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; } // Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming) if (failsafeSwitchIsOn && failsafeConfig->failsafe_kill_switch) { // KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData reprocessState = true; } else if (!receivingRxData) { if (millis() > failsafeState.throttleLowPeriod) { // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds failsafeActivate(); failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS; // require 3 seconds of valid rxData } else { failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; } reprocessState = true; } } else { // When NOT armed, show rxLinkState of failsafe switch in GUI (failsafe mode) if (failsafeSwitchIsOn) { ENABLE_FLIGHT_MODE(FAILSAFE_MODE); } else { DISABLE_FLIGHT_MODE(FAILSAFE_MODE); } // Throttle low period expired (= low long enough for JustDisarm) failsafeState.throttleLowPeriod = 0; } break; case FAILSAFE_RX_LOSS_DETECTED: if (receivingRxData) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; } else { // Stabilize, and set Throttle to specified level failsafeActivate(); } reprocessState = true; break; case FAILSAFE_LANDING: if (receivingRxData) { failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } if (armed) { failsafeApplyControlInput(); beeperMode = BEEPER_RX_LOST_LANDING; } if (failsafeShouldHaveCausedLandingByNow() || !armed) { failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData failsafeState.phase = FAILSAFE_LANDED; reprocessState = true; } break; case FAILSAFE_LANDED: ENABLE_ARMING_FLAG(PREVENT_ARMING); // To prevent accidently rearming by an intermittent rx link mwDisarm(); failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING; reprocessState = true; break; case FAILSAFE_RX_LOSS_MONITORING: // Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time. if (receivingRxData) { if (millis() > failsafeState.receivingRxDataPeriod) { // rx link is good now, when arming via ARM switch, it must be OFF first if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) { DISABLE_ARMING_FLAG(PREVENT_ARMING); failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } } } else { failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; } break; case FAILSAFE_RX_LOSS_RECOVERED: // Entering IDLE with the requirement that throttle first must be at min_check for failsafe_throttle_low_delay period. // This is to prevent that JustDisarm is activated on the next iteration. // Because that would have the effect of shutting down failsafe handling on intermittent connections. failsafeState.throttleLowPeriod = millis() + failsafeConfig->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND; failsafeState.phase = FAILSAFE_IDLE; failsafeState.active = false; DISABLE_FLIGHT_MODE(FAILSAFE_MODE); reprocessState = true; break; default: break; } } while (reprocessState); if (beeperMode != BEEPER_SILENCE) { beeper(beeperMode); } }
void tryArm(void) { if (armingConfig()->gyro_cal_on_first_arm) { gyroStartCalibration(true); } updateArmingStatus(); if (!isArmingDisabled()) { if (ARMING_FLAG(ARMED)) { return; } #ifdef USE_DSHOT if (micros() - getLastDshotBeaconCommandTimeUs() < DSHOT_BEACON_GUARD_DELAY_US) { if (tryingToArm == ARMING_DELAYED_DISARMED) { if (isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH) && IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) { tryingToArm = ARMING_DELAYED_CRASHFLIP; } else { tryingToArm = ARMING_DELAYED_NORMAL; } } return; } if (isMotorProtocolDshot() && isModeActivationConditionPresent(BOXFLIPOVERAFTERCRASH)) { if (!(IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) || (tryingToArm == ARMING_DELAYED_CRASHFLIP))) { flipOverAfterCrashMode = false; if (!feature(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_NORMAL, false); } } else { flipOverAfterCrashMode = true; #ifdef USE_RUNAWAY_TAKEOFF runawayTakeoffCheckDisabled = false; #endif if (!feature(FEATURE_3D)) { pwmWriteDshotCommand(ALL_MOTORS, getMotorCount(), DSHOT_CMD_SPIN_DIRECTION_REVERSED, false); } } } #endif ENABLE_ARMING_FLAG(ARMED); ENABLE_ARMING_FLAG(WAS_EVER_ARMED); resetTryingToArm(); #ifdef USE_ACRO_TRAINER pidAcroTrainerInit(); #endif // USE_ACRO_TRAINER if (isModeActivationConditionPresent(BOXPREARM)) { ENABLE_ARMING_FLAG(WAS_ARMED_WITH_PREARM); } imuQuaternionHeadfreeOffsetSet(); disarmAt = millis() + armingConfig()->auto_disarm_delay * 1000; // start disarm timeout, will be extended when throttle is nonzero lastArmingDisabledReason = 0; //beep to indicate arming #ifdef USE_GPS if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) { beeper(BEEPER_ARMING_GPS_FIX); } else { beeper(BEEPER_ARMING); } #else beeper(BEEPER_ARMING); #endif #ifdef USE_RUNAWAY_TAKEOFF runawayTakeoffDeactivateUs = 0; runawayTakeoffAccumulatedUs = 0; runawayTakeoffTriggerUs = 0; #endif } else { resetTryingToArm(); if (!isFirstArmingGyroCalibrationRunning()) { int armingDisabledReason = ffs(getArmingDisableFlags()); if (lastArmingDisabledReason != armingDisabledReason) { lastArmingDisabledReason = armingDisabledReason; beeperWarningBeeps(armingDisabledReason); } } } }
void annexCode(void) { int32_t tmp, tmp2; int32_t axis, prop1 = 0, prop2; static batteryState_e batteryState = BATTERY_OK; static uint8_t vbatTimer = 0; static int32_t vbatCycleTime = 0; // PITCH & ROLL only dynamic PID adjustemnt, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop2 = 100 - currentControlRateProfile->dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile->rcControlsConfig.deadband) { if (tmp > currentProfile->rcControlsConfig.deadband) { tmp -= currentProfile->rcControlsConfig.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { tmp -= currentProfile->rcControlsConfig.yaw_deadband; } else { tmp = 0; } } rcCommand[axis] = tmp * -masterConfig.yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100; if (rcData[axis] < masterConfig.rxConfig.midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.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 (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(heading - headFreeModeHold); 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 | FEATURE_CURRENT_METER)) { vbatCycleTime += cycleTime; if (!(++vbatTimer % VBATFREQ)) { if (feature(FEATURE_VBAT)) { updateBatteryVoltage(); batteryState = calculateBatteryState(); } if (feature(FEATURE_CURRENT_METER)) { updateCurrentMeter(vbatCycleTime); } vbatCycleTime = 0; } } beepcodeUpdateState(batteryState); if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating()) { LED0_TOGGLE; DISABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (ARMING_FLAG(OK_TO_ARM)) { disableWarningLed(); } else { enableWarningLed(currentTime); } updateWarningLed(currentTime); } #ifdef TELEMETRY checkTelemetryState(); #endif handleSerial(); #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTime); } #endif // 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); }
void annexCode(void) { int32_t tmp, tmp2; int32_t axis, prop1 = 0, prop2; static uint32_t vbatLastServiced = 0; static uint32_t ibatLastServiced = 0; // PITCH & ROLL only dynamic PID adjustment, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { prop2 = 100; } else { if (rcData[THROTTLE] < 2000) { prop2 = 100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcData[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (2000 - currentControlRateProfile->tpa_breakpoint); } else { prop2 = 100 - currentControlRateProfile->dynThrPID; } } for (axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile->rcControlsConfig.deadband) { if (tmp > currentProfile->rcControlsConfig.deadband) { tmp -= currentProfile->rcControlsConfig.deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = lookupPitchRollRC[tmp2] + (tmp - tmp2 * 100) * (lookupPitchRollRC[tmp2 + 1] - lookupPitchRollRC[tmp2]) / 100; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * tmp / 500; prop1 = (uint16_t)prop1 * prop2 / 100; } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { tmp -= currentProfile->rcControlsConfig.yaw_deadband; } else { tmp = 0; } } tmp2 = tmp / 100; rcCommand[axis] = (lookupYawRC[tmp2] + (tmp - tmp2 * 100) * (lookupYawRC[tmp2 + 1] - lookupYawRC[tmp2]) / 100) * -masterConfig.yaw_control_direction; prop1 = 100 - (uint16_t)currentControlRateProfile->rates[axis] * ABS(tmp) / 500; } // FIXME axis indexes into pids. use something like lookupPidIndex(rc_alias_e alias) to reduce coupling. dynP8[axis] = (uint16_t)currentProfile->pidProfile.P8[axis] * prop1 / 100; dynI8[axis] = (uint16_t)currentProfile->pidProfile.I8[axis] * prop1 / 100; dynD8[axis] = (uint16_t)currentProfile->pidProfile.D8[axis] * prop1 / 100; // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids if (axis == YAW) { PIDweight[axis] = 100; } else { PIDweight[axis] = prop2; } if (rcData[axis] < masterConfig.rxConfig.midrc) rcCommand[axis] = -rcCommand[axis]; } tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.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 (FLIGHT_MODE(HEADFREE_MODE)) { float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); float cosDiff = cos_approx(radDiff); float sinDiff = sin_approx(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)) { if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) { vbatLastServiced = currentTime; updateBattery(); } } if (feature(FEATURE_CURRENT_METER)) { int32_t ibatTimeSinceLastServiced = cmp32(currentTime, ibatLastServiced); if (ibatTimeSinceLastServiced >= IBATINTERVAL) { ibatLastServiced = currentTime; updateCurrentMeter(ibatTimeSinceLastServiced, &masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle); } } beeperUpdate(); //call periodic beeper handler if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } else { if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } } warningLedUpdate(); } #ifdef TELEMETRY telemetryCheckState(); #endif handleSerial(); #ifdef GPS if (sensors(SENSOR_GPS)) { updateGpsIndicator(currentTime); } #endif // 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); }
void annexCode(void) { int32_t tmp; for (int axis = 0; axis < 3; axis++) { tmp = MIN(ABS(rcData[axis] - masterConfig.rxConfig.midrc), 500); if (axis == ROLL || axis == PITCH) { if (currentProfile->rcControlsConfig.deadband) { if (tmp > currentProfile->rcControlsConfig.deadband) { tmp -= currentProfile->rcControlsConfig.deadband; } else { tmp = 0; } } rcCommand[axis] = rcLookupPitchRoll(tmp); } else if (axis == YAW) { if (currentProfile->rcControlsConfig.yaw_deadband) { if (tmp > currentProfile->rcControlsConfig.yaw_deadband) { tmp -= currentProfile->rcControlsConfig.yaw_deadband; } else { tmp = 0; } } rcCommand[axis] = rcLookupYaw(tmp) * -1; } if (rcData[axis] < masterConfig.rxConfig.midrc) { rcCommand[axis] = -rcCommand[axis]; } } tmp = constrain(rcData[THROTTLE], masterConfig.rxConfig.mincheck, PWM_RANGE_MAX); tmp = (uint32_t)(tmp - masterConfig.rxConfig.mincheck) * PWM_RANGE_MIN / (PWM_RANGE_MAX - masterConfig.rxConfig.mincheck); // [MINCHECK;2000] -> [0;1000] rcCommand[THROTTLE] = rcLookupThrottle(tmp); if (FLIGHT_MODE(HEADFREE_MODE)) { const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold); const float cosDiff = cos_approx(radDiff); const float sinDiff = sin_approx(radDiff); const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff; rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff; rcCommand[PITCH] = rcCommand_PITCH; } if (ARMING_FLAG(ARMED)) { LED0_ON; } else { if (IS_RC_MODE_ACTIVE(BOXARM) == 0) { ENABLE_ARMING_FLAG(OK_TO_ARM); } if (!STATE(SMALL_ANGLE)) { DISABLE_ARMING_FLAG(OK_TO_ARM); } if (isCalibrating() || isSystemOverloaded()) { warningLedFlash(); DISABLE_ARMING_FLAG(OK_TO_ARM); } #if defined(NAV) if (naivationBlockArming()) { DISABLE_ARMING_FLAG(OK_TO_ARM); } #endif if (ARMING_FLAG(OK_TO_ARM)) { warningLedDisable(); } else { warningLedFlash(); } warningLedUpdate(); } // 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); }