From 14b3d574f7e22950e50801a704f2de67537feca7 Mon Sep 17 00:00:00 2001 From: Michael Keller Date: Wed, 31 Jan 2018 08:41:46 +1300 Subject: [PATCH] Revert "Size Optimization: Move time-critical code from fc_core.c to fc_tasks.c" --- make/source.mk | 2 +- src/main/fc/fc_core.c | 155 +++++++++++++++++++++++++++++++++++++++ src/main/fc/fc_core.h | 5 +- src/main/fc/fc_tasks.c | 160 ----------------------------------------- src/main/fc/fc_tasks.h | 1 - 5 files changed, 157 insertions(+), 166 deletions(-) diff --git a/make/source.mk b/make/source.mk index fe04503c0..addba3b85 100644 --- a/make/source.mk +++ b/make/source.mk @@ -236,6 +236,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/serial_uart.c \ drivers/system.c \ drivers/timer.c \ + fc/fc_core.c \ fc/fc_tasks.c \ fc/fc_rc.c \ fc/rc_controls.c \ @@ -291,7 +292,6 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ drivers/vtx_rtc6705.c \ drivers/vtx_common.c \ fc/fc_init.c \ - fc/fc_core.c \ config/config_eeprom.c \ config/feature.c \ config/config_streamer.c \ diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 086aea180..e3a769a83 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -50,6 +50,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/fc_core.h" +#include "fc/fc_rc.h" #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" @@ -58,6 +59,7 @@ #include "interface/cli.h" +#include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" #include "io/gps.h" #include "io/motors.h" @@ -80,6 +82,7 @@ #include "flight/mixer.h" #include "flight/navigation.h" #include "flight/pid.h" +#include "flight/servos.h" // June 2013 V2.2-dev @@ -601,6 +604,158 @@ bool processRx(timeUs_t currentTimeUs) return true; } +static void subTaskPidController(timeUs_t currentTimeUs) +{ + uint32_t startTime = 0; + if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} + // PID - note this is function pointer set by setPIDController() + pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs); + DEBUG_SET(DEBUG_PIDLOOP, 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 USE_MAG + if (sensors(SENSOR_MAG)) { + updateMagHold(); + } +#endif + +#if defined(USE_ALT_HOLD) + // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState + updateRcCommands(); + if (sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER)) { + if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_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 USE_NAV + 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 USE_BLACKBOX + if (!cliMode && blackboxConfig()->device) { + blackboxUpdate(currentTimeUs); + } +#else + UNUSED(currentTimeUs); +#endif + +#ifdef USE_TRANSPONDER + transponderUpdate(currentTimeUs); +#endif + DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime); +} + +static void subTaskMotorUpdate(timeUs_t currentTimeUs) +{ + uint32_t startTime = 0; + if (debugMode == DEBUG_CYCLETIME) { + startTime = micros(); + static uint32_t previousMotorUpdateTime; + const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime; + debug[2] = currentDeltaTime; + debug[3] = currentDeltaTime - targetPidLooptime; + previousMotorUpdateTime = startTime; + } else if (debugMode == DEBUG_PIDLOOP) { + startTime = micros(); + } + + mixTable(currentTimeUs, currentPidProfile->vbatPidCompensation); + +#ifdef USE_SERVOS + // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos. + if (isMixerUsingServos()) { + writeServos(); + } +#endif + + writeMotors(); + + DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime); +} + +uint8_t setPidUpdateCountDown(void) +{ + if (gyroConfig()->gyro_soft_lpf_hz) { + return pidConfig()->pid_process_denom - 1; + } else { + return 1; + } +} + +// Function for loop trigger +void taskMainPidLoop(timeUs_t currentTimeUs) +{ + static uint8_t pidUpdateCountdown = 0; + +#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC) + if (lockMainPID() != 0) return; +#endif + + // DEBUG_PIDLOOP, timings for: + // 0 - gyroUpdate() + // 1 - pidController() + // 2 - subTaskMotorUpdate() + // 3 - subTaskMainSubprocesses() + gyroUpdate(currentTimeUs); + DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs); + + if (pidUpdateCountdown) { + pidUpdateCountdown--; + } else { + pidUpdateCountdown = setPidUpdateCountDown(); + subTaskPidController(currentTimeUs); + subTaskMotorUpdate(currentTimeUs); + subTaskMainSubprocesses(currentTimeUs); + } + + if (debugMode == DEBUG_CYCLETIME) { + debug[0] = getTaskDeltaTime(TASK_SELF); + debug[1] = averageSystemLoadPercent; + } +} + bool isFlipOverAfterCrashMode(void) { diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 1e8383715..6d215beab 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -46,8 +46,5 @@ bool processRx(timeUs_t currentTimeUs); void updateArmingStatus(void); void updateRcCommands(void); +void taskMainPidLoop(timeUs_t currentTimeUs); bool isFlipOverAfterCrashMode(void); - -#if defined(USE_GPS) || defined(USE_MAG) -void updateMagHold(void); -#endif diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index fa2f4143e..a7cfc9496 100644 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -23,8 +23,6 @@ #include "build/debug.h" -#include "blackbox/blackbox.h" - #include "cms/cms.h" #include "common/color.h" @@ -43,7 +41,6 @@ #include "fc/config.h" #include "fc/fc_core.h" -#include "fc/fc_rc.h" #include "fc/fc_dispatch.h" #include "fc/fc_tasks.h" #include "fc/rc_controls.h" @@ -52,14 +49,11 @@ #include "flight/altitude.h" #include "flight/imu.h" #include "flight/mixer.h" -#include "flight/navigation.h" #include "flight/pid.h" -#include "flight/servos.h" #include "interface/cli.h" #include "interface/msp.h" -#include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" #include "io/dashboard.h" #include "io/gps.h" @@ -335,160 +329,6 @@ void fcTasksInit(void) #endif } - -static void subTaskPidController(timeUs_t currentTimeUs) -{ - uint32_t startTime = 0; - if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} - // PID - note this is function pointer set by setPIDController() - pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs); - DEBUG_SET(DEBUG_PIDLOOP, 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 USE_MAG - if (sensors(SENSOR_MAG)) { - updateMagHold(); - } -#endif - -#if defined(USE_ALT_HOLD) - // updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState - updateRcCommands(); - if (sensors(SENSOR_BARO) || sensors(SENSOR_RANGEFINDER)) { - if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(RANGEFINDER_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 USE_NAV - 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 USE_BLACKBOX - if (!cliMode && blackboxConfig()->device) { - blackboxUpdate(currentTimeUs); - } -#else - UNUSED(currentTimeUs); -#endif - -#ifdef USE_TRANSPONDER - transponderUpdate(currentTimeUs); -#endif - DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime); -} - -static void subTaskMotorUpdate(timeUs_t currentTimeUs) -{ - uint32_t startTime = 0; - if (debugMode == DEBUG_CYCLETIME) { - startTime = micros(); - static uint32_t previousMotorUpdateTime; - const uint32_t currentDeltaTime = startTime - previousMotorUpdateTime; - debug[2] = currentDeltaTime; - debug[3] = currentDeltaTime - targetPidLooptime; - previousMotorUpdateTime = startTime; - } else if (debugMode == DEBUG_PIDLOOP) { - startTime = micros(); - } - - mixTable(currentTimeUs, currentPidProfile->vbatPidCompensation); - -#ifdef USE_SERVOS - // motor outputs are used as sources for servo mixing, so motors must be calculated using mixTable() before servos. - if (isMixerUsingServos()) { - writeServos(); - } -#endif - - writeMotors(); - - DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime); -} - -uint8_t setPidUpdateCountDown(void) -{ - if (gyroConfig()->gyro_soft_lpf_hz) { - return pidConfig()->pid_process_denom - 1; - } else { - return 1; - } -} - -// Function for loop trigger -void taskMainPidLoop(timeUs_t currentTimeUs) -{ - static uint8_t pidUpdateCountdown = 0; - -#if defined(SIMULATOR_BUILD) && defined(SIMULATOR_GYROPID_SYNC) - if (lockMainPID() != 0) return; -#endif - - // DEBUG_PIDLOOP, timings for: - // 0 - gyroUpdate() - // 1 - pidController() - // 2 - subTaskMotorUpdate() - // 3 - subTaskMainSubprocesses() - gyroUpdate(currentTimeUs); - DEBUG_SET(DEBUG_PIDLOOP, 0, micros() - currentTimeUs); - - if (pidUpdateCountdown) { - pidUpdateCountdown--; - } else { - pidUpdateCountdown = setPidUpdateCountDown(); - subTaskPidController(currentTimeUs); - subTaskMotorUpdate(currentTimeUs); - subTaskMainSubprocesses(currentTimeUs); - } - - if (debugMode == DEBUG_CYCLETIME) { - debug[0] = getTaskDeltaTime(TASK_SELF); - debug[1] = averageSystemLoadPercent; - } -} - - cfTask_t cfTasks[TASK_COUNT] = { [TASK_SYSTEM] = { .taskName = "SYSTEM", diff --git a/src/main/fc/fc_tasks.h b/src/main/fc/fc_tasks.h index cc712b1e4..1033f2f9d 100644 --- a/src/main/fc/fc_tasks.h +++ b/src/main/fc/fc_tasks.h @@ -24,4 +24,3 @@ #define TASK_PERIOD_US(us) (us) void fcTasksInit(void); -void taskMainPidLoop(timeUs_t currentTimeUs);