diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 48eeb52530..0c5e7f3b5b 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -600,7 +600,7 @@ void processRx(timeUs_t currentTimeUs) #endif } -static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs) +static void subTaskPidController(timeUs_t currentTimeUs) { uint32_t startTime = 0; if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} @@ -609,7 +609,7 @@ static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime); } -static FAST_CODE void subTaskMainSubprocesses(timeUs_t currentTimeUs) +static void subTaskMainSubprocesses(timeUs_t currentTimeUs) { uint32_t startTime = 0; if (debugMode == DEBUG_PIDLOOP) {startTime = micros();} @@ -683,7 +683,7 @@ static FAST_CODE void subTaskMainSubprocesses(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime); } -static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs) +static void subTaskMotorUpdate(timeUs_t currentTimeUs) { uint32_t startTime = 0; if (debugMode == DEBUG_CYCLETIME) { @@ -711,7 +711,7 @@ static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime); } -FAST_CODE uint8_t setPidUpdateCountDown(void) +uint8_t setPidUpdateCountDown(void) { if (gyroConfig()->gyro_soft_lpf_hz) { return pidConfig()->pid_process_denom - 1; @@ -721,7 +721,7 @@ FAST_CODE uint8_t setPidUpdateCountDown(void) } // Function for loop trigger -FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs) +void taskMainPidLoop(timeUs_t currentTimeUs) { static uint8_t pidUpdateCountdown = 0; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d6686970bd..abc300999a 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -482,7 +482,7 @@ void mixerResetDisarmedMotors(void) } } -FAST_CODE void writeMotors(void) +void writeMotors(void) { if (pwmAreMotorsEnabled()) { for (int i = 0; i < motorCount; i++) { @@ -520,7 +520,7 @@ static FAST_RAM float motorRangeMax; static FAST_RAM float motorOutputRange; static FAST_RAM int8_t motorOutputMixSign; -static FAST_CODE void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) +static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { static uint16_t rcThrottlePrevious = 0; // Store the last throttle direction for deadband transitions static timeUs_t reversalTimeUs = 0; // time when motors last reversed in 3D mode @@ -637,7 +637,7 @@ static void applyFlipOverAfterCrashModeToMotors(void) } } -static FAST_CODE void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) +static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) { // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. @@ -668,7 +668,7 @@ static FAST_CODE void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS]) } } -FAST_CODE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) +void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation) { if (isFlipOverAfterCrashMode()) { applyFlipOverAfterCrashModeToMotors(); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ba32b5f703..46db64289e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -405,7 +405,7 @@ static float accelerationLimit(int axis, float currentPidSetpoint) // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) -FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) +void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) { static float previousRateError[2]; const float tpaFactor = getThrottlePIDAttenuation(); diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 5b4f1a0615..1c3a7c0e30 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -136,7 +136,7 @@ const gyroFftData_t *gyroFftData(int axis) return &fftResult[axis]; } -FAST_CODE bool isDynamicFilterActive(void) +bool isDynamicFilterActive(void) { return feature(FEATURE_DYNAMIC_FILTER); } @@ -144,7 +144,7 @@ FAST_CODE bool isDynamicFilterActive(void) /* * Collect gyro data, to be analysed in gyroDataAnalyseUpdate function */ -FAST_CODE void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) +void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) { if (!isDynamicFilterActive()) { return; @@ -197,7 +197,7 @@ typedef enum { /* * Analyse last gyro data from the last FFT_WINDOW_SIZE milliseconds */ -FAST_CODE void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) +void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn) { static int axis = 0; static int step = 0; diff --git a/src/main/target/KISSFCV2F7/stm32_flash_f722_no_split.ld b/src/main/target/KISSFCV2F7/stm32_flash_f722_no_split.ld index 20c215538a..ad06053459 100644 --- a/src/main/target/KISSFCV2F7/stm32_flash_f722_no_split.ld +++ b/src/main/target/KISSFCV2F7/stm32_flash_f722_no_split.ld @@ -16,6 +16,9 @@ ENTRY(Reset_Handler) /* Specify the memory areas */ MEMORY { + ITCM_ISR (rx) : ORIGIN = 0x00000000, LENGTH = 1K + ITCM_RAM (rx) : ORIGIN = 0x00000400, LENGTH = 15K + /* Bootloader stays in the first sector (16K) */ FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K /*second (16K) sector for eeprom emulation*/ FLASH (rx) : ORIGIN = 0x08008000, LENGTH = 480K /*main fw*/ @@ -27,6 +30,7 @@ MEMORY /* note TCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) +REGION_ALIAS("FASTRAM", TCM) /* Entry Point */ @@ -55,6 +59,19 @@ SECTIONS . = ALIGN(4); } >FLASH + /* Critical program code goes into ITCM RAM */ + /* Copy specific fast-executing code to ITCM RAM */ + tcm_code = LOADADDR(.tcm_code); + .tcm_code : + { + . = ALIGN(4); + tcm_code_start = .; + *(.tcm_code) + *(.tcm_code*) + . = ALIGN(4); + tcm_code_end = .; + } >ITCM_RAM AT >FLASH + /* The program code and other data goes into FLASH */ .text : { diff --git a/src/main/target/KISSFCV2F7/target.mk b/src/main/target/KISSFCV2F7/target.mk index 86ea87a949..25e86b723b 100644 --- a/src/main/target/KISSFCV2F7/target.mk +++ b/src/main/target/KISSFCV2F7/target.mk @@ -1,7 +1,7 @@ F7X2RE_TARGETS += $(TARGET) -LD_SCRIPT = $(ROOT)/src/main/target/$(TARGET)/stm32_flash_f722_no_split.ld -CFLAGS += -D'CUSTOM_VECT_TAB_OFFSET=0x8000' \ - -DCLOCK_SOURCE_USE_HSI +LD_SCRIPT = $(ROOT)/src/main/target/$(TARGET)/stm32_flash_f722_no_split.ld +CFLAGS += -D'CUSTOM_VECT_TAB_OFFSET=0x8000' \ + -DCLOCK_SOURCE_USE_HSI TARGET_SRC = \ drivers/accgyro/accgyro_spi_mpu6000.c diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 4e2c655052..5098f59714 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -53,6 +53,7 @@ #endif #ifdef STM32F7 +#define USE_ITCM_RAM #define USE_DSHOT #define USE_DSHOT_DMAR #define USE_ESC_SENSOR @@ -80,7 +81,7 @@ #define FAST_CODE __attribute__((section(".tcm_code"))) #else #define FAST_CODE -#endif +#endif // USE_ITCM_RAM #ifdef USE_FAST_RAM #ifdef __APPLE__ diff --git a/src/main/target/link/stm32_flash_f722.ld b/src/main/target/link/stm32_flash_f722.ld index b4f390fa21..598e936d82 100644 --- a/src/main/target/link/stm32_flash_f722.ld +++ b/src/main/target/link/stm32_flash_f722.ld @@ -41,4 +41,4 @@ MEMORY REGION_ALIAS("STACKRAM", TCM) REGION_ALIAS("FASTRAM", TCM) -INCLUDE "stm32_flash_split.ld" +INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld index f5d8f81f3b..078f0fb8bb 100644 --- a/src/main/target/link/stm32_flash_f745.ld +++ b/src/main/target/link/stm32_flash_f745.ld @@ -34,4 +34,4 @@ MEMORY REGION_ALIAS("STACKRAM", TCM) REGION_ALIAS("FASTRAM", TCM) -INCLUDE "stm32_flash_split.ld" +INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_f746.ld b/src/main/target/link/stm32_flash_f746.ld index 2ca0791f79..d50fd3fd94 100644 --- a/src/main/target/link/stm32_flash_f746.ld +++ b/src/main/target/link/stm32_flash_f746.ld @@ -34,4 +34,4 @@ MEMORY REGION_ALIAS("STACKRAM", TCM) REGION_ALIAS("FASTRAM", TCM) -INCLUDE "stm32_flash_split.ld" +INCLUDE "stm32_flash_f7_split.ld" diff --git a/src/main/target/link/stm32_flash_f7_split.ld b/src/main/target/link/stm32_flash_f7_split.ld new file mode 100644 index 0000000000..f56d6dcab9 --- /dev/null +++ b/src/main/target/link/stm32_flash_f7_split.ld @@ -0,0 +1,182 @@ +/* +***************************************************************************** +** +** File : stm32_flash_f7_split.ld +** +** Abstract : Common linker script for STM32 devices. +** +***************************************************************************** +*/ + +/* Entry Point */ +ENTRY(Reset_Handler) + +/* Highest address of the user mode stack */ +_estack = ORIGIN(STACKRAM) + LENGTH(STACKRAM); /* end of RAM */ + +/* Base address where the config is stored. */ +__config_start = ORIGIN(FLASH_CONFIG); +__config_end = ORIGIN(FLASH_CONFIG) + LENGTH(FLASH_CONFIG); + +/* Generate a link error if heap and stack don't fit into RAM */ +_Min_Heap_Size = 0; /* required amount of heap */ +_Min_Stack_Size = 0x800; /* required amount of stack */ + +/* Define output sections */ +SECTIONS +{ + /* The startup code goes first into FLASH */ + .isr_vector : + { + . = ALIGN(4); + PROVIDE (isr_vector_table_base = .); + KEEP(*(.isr_vector)) /* Startup code */ + . = ALIGN(4); + } >FLASH + + /* Critical program code goes into ITCM RAM */ + /* Copy specific fast-executing code to ITCM RAM */ + tcm_code = LOADADDR(.tcm_code); + .tcm_code : + { + . = ALIGN(4); + tcm_code_start = .; + *(.tcm_code) + *(.tcm_code*) + . = ALIGN(4); + tcm_code_end = .; + } >ITCM_RAM AT >FLASH1 + + /* The program code and other data goes into FLASH */ + .text : + { + . = ALIGN(4); + *(.text) /* .text sections (code) */ + *(.text*) /* .text* sections (code) */ + *(.rodata) /* .rodata sections (constants, strings, etc.) */ + *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ + *(.glue_7) /* glue arm to thumb code */ + *(.glue_7t) /* glue thumb to arm code */ + *(.eh_frame) + + KEEP (*(.init)) + KEEP (*(.fini)) + + . = ALIGN(4); + _etext = .; /* define a global symbols at end of code */ + } >FLASH1 + + .ARM.extab : + { + *(.ARM.extab* .gnu.linkonce.armextab.*) + } >FLASH + + .ARM : + { + __exidx_start = .; + *(.ARM.exidx*) __exidx_end = .; + } >FLASH + + .preinit_array : + { + PROVIDE_HIDDEN (__preinit_array_start = .); + KEEP (*(.preinit_array*)) + PROVIDE_HIDDEN (__preinit_array_end = .); + } >FLASH + + .init_array : + { + PROVIDE_HIDDEN (__init_array_start = .); + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array*)) + PROVIDE_HIDDEN (__init_array_end = .); + } >FLASH + + .fini_array : + { + PROVIDE_HIDDEN (__fini_array_start = .); + KEEP (*(.fini_array*)) + KEEP (*(SORT(.fini_array.*))) + PROVIDE_HIDDEN (__fini_array_end = .); + } >FLASH + + .pg_registry : + { + PROVIDE_HIDDEN (__pg_registry_start = .); + KEEP (*(.pg_registry)) + KEEP (*(SORT(.pg_registry.*))) + PROVIDE_HIDDEN (__pg_registry_end = .); + } >FLASH + + .pg_resetdata : + { + PROVIDE_HIDDEN (__pg_resetdata_start = .); + KEEP (*(.pg_resetdata)) + PROVIDE_HIDDEN (__pg_resetdata_end = .); + } >FLASH + + /* used by the startup to initialize data */ + _sidata = .; + + /* Initialized data sections goes into RAM, load LMA copy after code */ + .data : + { + . = ALIGN(4); + _sdata = .; /* create a global symbol at data start */ + *(.data) /* .data sections */ + *(.data*) /* .data* sections */ + + . = ALIGN(4); + _edata = .; /* define a global symbol at data end */ + } >RAM AT >FLASH + + /* Uninitialized data section */ + . = ALIGN(4); + .bss : + { + /* This is used by the startup in order to initialize the .bss secion */ + _sbss = .; /* define a global symbol at bss start */ + __bss_start__ = _sbss; + *(.bss) + *(SORT_BY_ALIGNMENT(.bss*)) + *(COMMON) + + . = ALIGN(4); + _ebss = .; /* define a global symbol at bss end */ + __bss_end__ = _ebss; + } >RAM + + /* User_heap_stack section, used to check that there is enough RAM left */ + _heap_stack_end = ORIGIN(STACKRAM)+LENGTH(STACKRAM) - 8; /* 8 bytes to allow for alignment */ + _heap_stack_begin = _heap_stack_end - _Min_Stack_Size - _Min_Heap_Size; + . = _heap_stack_begin; + ._user_heap_stack : + { + . = ALIGN(4); + PROVIDE ( end = . ); + PROVIDE ( _end = . ); + . = . + _Min_Heap_Size; + . = . + _Min_Stack_Size; + . = ALIGN(4); + } >STACKRAM = 0xa5 + + /* MEMORY_bank1 section, code must be located here explicitly */ + /* Example: extern int foo(void) __attribute__ ((section (".mb1text"))); */ + .memory_b1_text : + { + *(.mb1text) /* .mb1text sections (code) */ + *(.mb1text*) /* .mb1text* sections (code) */ + *(.mb1rodata) /* read-only data (constants) */ + *(.mb1rodata*) + } >MEMORY_B1 + + /* Remove information from the standard libraries */ + /DISCARD/ : + { + libc.a ( * ) + libm.a ( * ) + libgcc.a ( * ) + } + + .ARM.attributes 0 : { *(.ARM.attributes) } +} diff --git a/src/main/target/stm32f7xx_hal_conf.h b/src/main/target/stm32f7xx_hal_conf.h index e8114a5a07..98a158d128 100644 --- a/src/main/target/stm32f7xx_hal_conf.h +++ b/src/main/target/stm32f7xx_hal_conf.h @@ -158,8 +158,8 @@ #define VDD_VALUE ((uint32_t)3300U) /*!< Value of VDD in mv */ #define TICK_INT_PRIORITY ((uint32_t)0U) /*!< tick interrupt priority */ #define USE_RTOS 0U -#define PREFETCH_ENABLE 0U -#define ART_ACCLERATOR_ENABLE 0U /* To enable instruction cache and prefetch */ +#define PREFETCH_ENABLE 1U +#define ART_ACCLERATOR_ENABLE 1U /* To enable instruction cache and prefetch */ #define INSTRUCTION_CACHE_ENABLE 1U #define DATA_CACHE_ENABLE 0U