rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Discussion: Move time-critical code from fc_core.c to fc_tasks.c 

Change fc_core.c to be size-optimized to reduce overall firmware size. Saves 3288 bytes on OMNIBUS as an example.
This commit is contained in:
Bruce Luckcuck 2018-01-29 12:33:15 -05:00
parent dbf0883470
commit 4712b0aca1
5 changed files with 166 additions and 157 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
make/source.mk
View File

@ -236,7 +236,6 @@ 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 \
@ -292,6 +291,7 @@ 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 \

155
src/main/fc/fc_core.c
View File

@ -50,7 +50,6 @@
#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"
@ -59,7 +58,6 @@
#include "interface/cli.h"
#include "io/asyncfatfs/asyncfatfs.h"
#include "io/beeper.h"
#include "io/gps.h"
#include "io/motors.h"
@ -82,7 +80,6 @@
#include "flight/mixer.h"
#include "flight/navigation.h"
#include "flight/pid.h"
#include "flight/servos.h"
// June 2013 V2.2-dev
@ -604,158 +601,6 @@ 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)
{

5
src/main/fc/fc_core.h
View File

@ -46,5 +46,8 @@ 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

160
src/main/fc/fc_tasks.c
View File

@ -23,6 +23,8 @@
#include "build/debug.h"
#include "blackbox/blackbox.h"
#include "cms/cms.h"
#include "common/color.h"
@ -41,6 +43,7 @@
#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"
@ -49,11 +52,14 @@
#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"
@ -329,6 +335,160 @@ 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",

1
src/main/fc/fc_tasks.h
View File

@ -24,3 +24,4 @@
#define TASK_PERIOD_US(us) (us)
void fcTasksInit(void);
void taskMainPidLoop(timeUs_t currentTimeUs);