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 }
/** * this function controls motors by PID output * * @param * void * * @return * void * */ void motorControler() { float rollRateOutput = 0.f; float rollCcw1 = 0.f; float rollCcw2 = 0.f; float rollCw1 = 0.f; float rollCw2 = 0.f; float pitchRateOutput = 0.f; float pitchCcw1 = 0.f; float pitchCcw2 = 0.f; float pitchCw1 = 0.f; float pitchCw2 = 0.f; float yawRateOutput = 0.f; float yawCcw1 = 0.f; float yawCcw2 = 0.f; float yawCw1 = 0.f; float yawCw2 = 0.f; float outCcw1 = 0.f; float outCcw2 = 0.f; float outCw1 = 0.f; float outCw2 = 0.f; float maxLimit = 0.f; float minLimit = 0.f; float accelThrottleOffset = 0.f; float altholdThrottleOffset = 0.f; float throttleOffset = 0.f; float centerThrottle = 0.f; altholdThrottleOffset = (getEnableAltHold() && getAltHoldIsReady()) ? getThrottleOffsetByAltHold(updateAltHold()) : 0.f; accelThrottleOffset = getThrottleOffsetByAcceleration(); throttleOffset = altholdThrottleOffset + accelThrottleOffset; centerThrottle = (float) getThrottlePowerLevel() + throttleOffset; maxLimit = (float) min(centerThrottle + getAdjustPowerLeveRange(), getMaxPowerLeve()); minLimit = (float) max(centerThrottle - getAdjustPowerLeveRange(), getMinPowerLevel()); getAttitudePidOutput(); getRatePidOutput(&rollRateOutput, &pitchRateOutput, &yawRateOutput); /* * rollCa>0 * - CCW2 CW2 + * X * - CW1 CCW1 + * F * * rollCa<0 * + CCW2 CW2 - * X * + CW1 CCW1 - * F */ rollCcw1 = rollRateOutput; rollCcw2 = -rollRateOutput; rollCw1 = -rollRateOutput; rollCw2 = rollRateOutput; /* * pitchCa>0 * + CCW2 CW2 + * X * - CW1 CCW1 - * F * * pitchCa<0 * - CCW2 CW2 - * X * + CW1 CCW1 + * F */ pitchCcw1 = -pitchRateOutput; pitchCcw2 = pitchRateOutput; pitchCw1 = -pitchRateOutput; pitchCw2 = pitchRateOutput; /* * yawCa>0 * + CCW2 CW2 - * X * - CW1 CCW1 + * F * * yawCa<0 * - CCW2 CW2 + * X * + CW1 CCW1 - * F */ yawCcw1 = yawRateOutput; yawCcw2 = yawRateOutput; yawCw1 = -yawRateOutput; yawCw2 = -yawRateOutput; outCcw1 = centerThrottle + LIMIT_MIN_MAX_VALUE(rollCcw1 + pitchCcw1 + yawCcw1, -getPidOutputLimitation(), getPidOutputLimitation()); outCcw2 = centerThrottle + LIMIT_MIN_MAX_VALUE(rollCcw2 + pitchCcw2 + yawCcw2, -getPidOutputLimitation(), getPidOutputLimitation()); outCw1 = centerThrottle + LIMIT_MIN_MAX_VALUE(rollCw1 + pitchCw1 + yawCw1, -getPidOutputLimitation(), getPidOutputLimitation()); outCw2 = centerThrottle + LIMIT_MIN_MAX_VALUE(rollCw2 + pitchCw2 + yawCw2, -getPidOutputLimitation(), getPidOutputLimitation()); outCcw1 = getMotorGain( SOFT_PWM_CCW1) * LIMIT_MIN_MAX_VALUE(outCcw1, minLimit, maxLimit); outCcw2 = getMotorGain( SOFT_PWM_CCW2) * LIMIT_MIN_MAX_VALUE(outCcw2, minLimit, maxLimit); outCw1 = getMotorGain( SOFT_PWM_CW1) * LIMIT_MIN_MAX_VALUE(outCw1, minLimit, maxLimit); outCw2 = getMotorGain( SOFT_PWM_CW2) * LIMIT_MIN_MAX_VALUE(outCw2, minLimit, maxLimit); setupCcw1MotorPoewrLevel((unsigned short) outCcw1); setupCcw2MotorPoewrLevel((unsigned short) outCcw2); setupCw1MotorPoewrLevel((unsigned short) outCw1); setupCw2MotorPoewrLevel((unsigned short) outCw2); #if 0 _DEBUG(DEBUG_NORMAL,"outCcw1=%d,outCcw2=%d,outCw1=%d,outCw2=%d\n" , (unsigned short)outCcw1, (unsigned short)outCcw2, (unsigned short)outCw1, (unsigned short)outCw2); #endif }