static void activateConfig(void)
{
activateControlRateConfig();
resetAdjustmentStates();
useRcControlsConfig(modeActivationProfile()->modeActivationConditions);
pidInitFilters(pidProfile());
#ifdef GPS
gpsUsePIDs(pidProfile());
#endif
useFailsafeConfig();
setAccelerationTrims(&sensorTrims()->accZero);
accelerationFilterInit(accelerometerConfig()->acc_cut_hz);
#ifdef USE_SERVOS
mixerUseConfigs(servoProfile()->servoConf);
#endif
#ifdef GPS
recalculateMagneticDeclination();
#endif
#ifdef VTX
initVTXState();
#endif
static imuRuntimeConfig_t imuRuntimeConfig;
imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f;
imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f;
imuRuntimeConfig.acc_cut_hz = accelerometerConfig()->acc_cut_hz;
imuRuntimeConfig.acc_unarmedcal = accelerometerConfig()->acc_unarmedcal;
imuRuntimeConfig.small_angle = imuConfig()->small_angle;
imuConfigure(
&imuRuntimeConfig,
&accelerometerConfig()->accDeadband,
accelerometerConfig()->accz_lpf_cutoff,
throttleCorrectionConfig()->throttle_correction_angle
);
}
void activateConfig(void)
{
#ifndef USE_OSD_SLAVE
initRcProcessing();
resetAdjustmentStates();
pidInit(currentPidProfile);
useRcControlsConfig(currentPidProfile);
useAdjustmentConfig(currentPidProfile);
#ifdef USE_NAV
gpsUsePIDs(currentPidProfile);
#endif
failsafeReset();
setAccelerationTrims(&accelerometerConfigMutable()->accZero);
accInitFilters();
imuConfigure(throttleCorrectionConfig()->throttle_correction_angle);
#endif // USE_OSD_SLAVE
}
void taskMainPidLoop(void)
{
cycleTime = getTaskDeltaTime(TASK_SELF);
dT = (float)cycleTime * 0.000001f;
// Calculate average cycle time and average jitter
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
debug[0] = cycleTime;
debug[1] = cycleTime - filteredCycleTime;
imuUpdateGyroAndAttitude();
annexCode();
if (rxConfig()->rcSmoothing) {
filterRc();
}
#if defined(BARO) || defined(SONAR)
haveProcessedAnnexCodeOnce = true;
#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();
}
}
#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
}
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 GTUNE
updateGtuneState();
#endif
#if defined(BARO) || defined(SONAR)
// FIXME outdated comments?
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
// this must be called here since applyAltHold directly manipulates rcCommands[]
updateRcCommands();
if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
applyAltHold();
}
}
#endif
#ifdef MAG
if (sensors(SENSOR_MAG)) {
updateMagHold();
}
#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);
}
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
if (debugMode == DEBUG_PIDLOOP) {debug[1] = micros() - startTime;}
}
