static void taskUpdateRxMain(timeUs_t currentTimeUs) { if (!processRx(currentTimeUs)) { return; } static timeUs_t lastRxTimeUs; currentRxRefreshRate = constrain(currentTimeUs - lastRxTimeUs, 1000, 30000); lastRxTimeUs = currentTimeUs; isRXDataNew = true; #ifdef USE_USB_CDC_HID if (!ARMING_FLAG(ARMED)) { sendRcDataToHid(); } #endif // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); updateArmingStatus(); }
void taskUpdateRxMain(void) { processRx(); isRXDataNew = true; #if !defined(BARO) && !defined(SONAR) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); #endif updateLEDs(); #ifdef BARO if (sensors(SENSOR_BARO)) { updateAltHoldState(); } #endif #ifdef SONAR if (sensors(SENSOR_SONAR)) { updateSonarAltHoldState(); } #endif }
static void taskUpdateRxMain(timeUs_t currentTimeUs) { processRx(currentTimeUs); isRXDataNew = true; #if !defined(BARO) && !defined(SONAR) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); #endif updateArmingStatus(); #ifdef BARO if (sensors(SENSOR_BARO)) { updateAltHoldState(); } #endif #ifdef SONAR if (sensors(SENSOR_SONAR)) { updateSonarAltHoldState(); } #endif }
void subTaskMainSubprocesses(void) { const uint32_t startTime = micros(); // 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); } #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #ifdef GTUNE updateGtuneState(); #endif #if defined(BARO) || defined(SONAR) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); 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. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. if (isUsingSticksForArming() && rcData[THROTTLE] <= masterConfig.rxConfig.mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((masterConfig.mixerMode == MIXER_TRI || masterConfig.mixerMode == MIXER_CUSTOM_TRI) && masterConfig.mixerConfig.tri_unarmed_servo) #endif && masterConfig.mixerMode != MIXER_AIRPLANE && masterConfig.mixerMode != MIXER_FLYING_WING #endif ) { rcCommand[YAW] = 0; setpointRate[YAW] = 0; } if (masterConfig.throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(masterConfig.throttle_correction_value); } processRcCommand(); #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif #ifdef USE_SDCARD afatfs_poll(); #endif #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif #ifdef TRANSPONDER updateTransponder(); #endif if (debugMode == DEBUG_PIDLOOP) {debug[1] = micros() - startTime;} }
static void subTaskMainSubprocesses(timeUs_t currentTimeUs) { uint32_t startTime = 0; if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} // Read out gyro temperature if used for telemmetry if (feature(FEATURE_TELEMETRY)) { gyroReadTemperature(); } #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #if defined(BARO) || defined(SONAR) // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState updateRcCommands(); if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(); } } #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. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && servoConfig()->tri_unarmed_servo) #endif && mixerConfig()->mixerMode != MIXER_AIRPLANE && mixerConfig()->mixerMode != MIXER_FLYING_WING #endif ) { resetYawAxis(); } if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value); } processRcCommand(); #ifdef GPS if (sensors(SENSOR_GPS)) { if ((FLIGHT_MODE(GPS_HOME_MODE) || FLIGHT_MODE(GPS_HOLD_MODE)) && STATE(GPS_FIX_HOME)) { updateGpsStateForHomeAndHoldMode(); } } #endif #ifdef USE_SDCARD afatfs_poll(); #endif #ifdef BLACKBOX if (!cliMode && blackboxConfig()->device) { handleBlackbox(currentTimeUs); } #else UNUSED(currentTimeUs); #endif #ifdef TRANSPONDER transponderUpdate(currentTimeUs); #endif DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime); }
void taskMainPidLoop(void) { cycleTime = getTaskDeltaTime(TASK_SELF); dT = (float)cycleTime * 0.000001f; // Calculate average cycle time and average jitter filteredCycleTime = pt1FilterApply4(&filteredCycleTimeState, cycleTime, 1, dT); #ifdef DEBUG_CYCLE_TIME debug[0] = cycleTime; debug[1] = cycleTime - filteredCycleTime; #endif imuUpdateGyroAndAttitude(); updateRcCommands(); // this must be called here since applyAltHold directly manipulates rcCommands[] if (rxConfig()->rcSmoothing) { filterRc(); } #if defined(BARO) || defined(SONAR) haveUpdatedRcCommandsOnce = true; #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); } #ifdef MAG if (sensors(SENSOR_MAG)) { updateMagHold(); } #endif #ifdef GTUNE updateGtuneState(); #endif #if defined(BARO) || defined(SONAR) if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) { if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) { applyAltHold(); } } #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. // Allow yaw control for tricopters if the user wants the servo to move even when unarmed. if (isUsingSticksForArming() && rcData[THROTTLE] <= rxConfig()->mincheck #ifndef USE_QUAD_MIXER_ONLY #ifdef USE_SERVOS && !((mixerConfig()->mixerMode == MIXER_TRI || mixerConfig()->mixerMode == MIXER_CUSTOM_TRI) && mixerConfig()->tri_unarmed_servo) #endif && mixerConfig()->mixerMode != MIXER_AIRPLANE && mixerConfig()->mixerMode != MIXER_FLYING_WING #endif ) { rcCommand[YAW] = 0; } if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->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( pidProfile(), currentControlRateProfile, imuConfig()->max_angle_inclination, &accelerometerConfig()->accelerometerTrims, rxConfig() ); mixTable(); #ifdef USE_SERVOS filterServos(); writeServos(); #endif if (motorControlEnable) { writeMotors(); } #ifdef USE_SDCARD afatfs_poll(); #endif #ifdef BLACKBOX if (!cliMode && feature(FEATURE_BLACKBOX)) { handleBlackbox(); } #endif }