diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 34de6bd473..d6686970bd 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -108,10 +108,10 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR #define TRICOPTER_ERROR_RATE_YAW_SATURATED 75 // rate at which tricopter yaw axis becomes saturated, determined experimentally by TriFlight -static uint8_t motorCount; -static float motorMixRange; +static FAST_RAM uint8_t motorCount; +static FAST_RAM float motorMixRange; -float motor[MAX_SUPPORTED_MOTORS]; +float FAST_RAM motor[MAX_SUPPORTED_MOTORS]; float motor_disarmed[MAX_SUPPORTED_MOTORS]; mixerMode_e currentMixerMode; @@ -306,12 +306,12 @@ const mixer_t mixers[] = { }; #endif // !USE_QUAD_MIXER_ONLY -float motorOutputHigh, motorOutputLow; +FAST_RAM float motorOutputHigh, motorOutputLow; -static float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; -static uint16_t rcCommand3dDeadBandLow; -static uint16_t rcCommand3dDeadBandHigh; -static float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; +static FAST_RAM float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; +static FAST_RAM uint16_t rcCommand3dDeadBandLow; +static FAST_RAM uint16_t rcCommand3dDeadBandHigh; +static FAST_RAM float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; uint8_t getMotorCount(void) { @@ -513,12 +513,12 @@ void stopPwmAllMotors(void) delayMicroseconds(1500); } -static float throttle = 0; -static float motorOutputMin; -static float motorRangeMin; -static float motorRangeMax; -static float motorOutputRange; -static int8_t motorOutputMixSign; +static FAST_RAM float throttle = 0; +static FAST_RAM float motorOutputMin; +static FAST_RAM float motorRangeMin; +static FAST_RAM float motorRangeMax; +static FAST_RAM float motorOutputRange; +static FAST_RAM int8_t motorOutputMixSign; static FAST_CODE void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a1150730e3..ba32b5f703 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -51,14 +51,14 @@ #include "sensors/gyro.h" #include "sensors/acceleration.h" -uint32_t targetPidLooptime; -static bool pidStabilisationEnabled; +FAST_RAM uint32_t targetPidLooptime; +static FAST_RAM bool pidStabilisationEnabled; -static bool inCrashRecoveryMode = false; +static FAST_RAM bool inCrashRecoveryMode = false; -float axisPID_P[3], axisPID_I[3], axisPID_D[3]; +FAST_RAM float axisPID_P[3], axisPID_I[3], axisPID_D[3]; -static float dT; +static FAST_RAM float dT; PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 1); @@ -143,7 +143,7 @@ void pidResetITerm(void) } } -static float itermAccelerator = 1.0f; +static FAST_RAM float itermAccelerator = 1.0f; void pidSetItermAccelerator(float newItermAccelerator) { @@ -157,12 +157,12 @@ void pidStabilisationState(pidStabilisationState_e pidControllerState) const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH }; -static filterApplyFnPtr dtermNotchFilterApplyFn; -static void *dtermFilterNotch[2]; -static filterApplyFnPtr dtermLpfApplyFn; -static void *dtermFilterLpf[2]; -static filterApplyFnPtr ptermYawFilterApplyFn; -static void *ptermYawFilter; +static FAST_RAM filterApplyFnPtr dtermNotchFilterApplyFn; +static FAST_RAM void *dtermFilterNotch[2]; +static FAST_RAM filterApplyFnPtr dtermLpfApplyFn; +static FAST_RAM void *dtermFilterLpf[2]; +static FAST_RAM filterApplyFnPtr ptermYawFilterApplyFn; +static FAST_RAM void *ptermYawFilter; typedef union dtermFilterLpf_u { pt1Filter_t pt1Filter[2]; @@ -249,22 +249,22 @@ void pidInitFilters(const pidProfile_t *pidProfile) } } -static float Kp[3], Ki[3], Kd[3]; -static float maxVelocity[3]; -static float relaxFactor; -static float dtermSetpointWeight; -static float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; -static float ITermWindupPointInv; -static uint8_t horizonTiltExpertMode; -static timeDelta_t crashTimeLimitUs; -static timeDelta_t crashTimeDelayUs; -static int32_t crashRecoveryAngleDeciDegrees; -static float crashRecoveryRate; -static float crashDtermThreshold; -static float crashGyroThreshold; -static float crashSetpointThreshold; -static float crashLimitYaw; -static float itermLimit; +static FAST_RAM float Kp[3], Ki[3], Kd[3]; +static FAST_RAM float maxVelocity[3]; +static FAST_RAM float relaxFactor; +static FAST_RAM float dtermSetpointWeight; +static FAST_RAM float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; +static FAST_RAM float ITermWindupPointInv; +static FAST_RAM uint8_t horizonTiltExpertMode; +static FAST_RAM timeDelta_t crashTimeLimitUs; +static FAST_RAM timeDelta_t crashTimeDelayUs; +static FAST_RAM int32_t crashRecoveryAngleDeciDegrees; +static FAST_RAM float crashRecoveryRate; +static FAST_RAM float crashDtermThreshold; +static FAST_RAM float crashGyroThreshold; +static FAST_RAM float crashSetpointThreshold; +static FAST_RAM float crashLimitYaw; +static FAST_RAM float itermLimit; void pidInitConfig(const pidProfile_t *pidProfile) { diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index c3d0a54ae0..ef1d688d99 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -42,21 +42,21 @@ // 2 - time spent in scheduler // 3 - time spent executing check function -static cfTask_t *currentTask = NULL; +static FAST_RAM cfTask_t *currentTask = NULL; -static uint32_t totalWaitingTasks; -static uint32_t totalWaitingTasksSamples; +static FAST_RAM uint32_t totalWaitingTasks; +static FAST_RAM uint32_t totalWaitingTasksSamples; -static bool calculateTaskStatistics; -uint16_t averageSystemLoadPercent = 0; +static FAST_RAM bool calculateTaskStatistics; +FAST_RAM uint16_t averageSystemLoadPercent = 0; -static int taskQueuePos = 0; -STATIC_UNIT_TESTED int taskQueueSize = 0; +static FAST_RAM int taskQueuePos = 0; +STATIC_UNIT_TESTED FAST_RAM int taskQueueSize = 0; // No need for a linked list for the queue, since items are only inserted at startup -STATIC_UNIT_TESTED cfTask_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue +STATIC_UNIT_TESTED FAST_RAM cfTask_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue void queueClear(void) { diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 38db3c8179..0ec36037bd 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -67,7 +67,7 @@ #endif -acc_t acc; // acc access functions +FAST_RAM acc_t acc; // acc access functions static float accumulatedMeasurements[XYZ_AXIS_COUNT]; static int accumulatedMeasurementCount; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index c759adb558..59d31a7508 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -74,12 +74,12 @@ #define USE_GYRO_SLEW_LIMITER #endif -gyro_t gyro; -static uint8_t gyroDebugMode; +FAST_RAM gyro_t gyro; +static FAST_RAM uint8_t gyroDebugMode; -static float accumulatedMeasurements[XYZ_AXIS_COUNT]; -static timeUs_t accumulatedMeasurementTimeUs; -static timeUs_t accumulationLastTimeSampledUs; +static FAST_RAM float accumulatedMeasurements[XYZ_AXIS_COUNT]; +static FAST_RAM timeUs_t accumulatedMeasurementTimeUs; +static FAST_RAM timeUs_t accumulationLastTimeSampledUs; typedef struct gyroCalibration_s { int32_t sum[XYZ_AXIS_COUNT]; @@ -111,7 +111,7 @@ typedef struct gyroSensor_s { biquadFilter_t notchFilterDyn[XYZ_AXIS_COUNT]; } gyroSensor_t; -static gyroSensor_t gyroSensor1; +static FAST_RAM gyroSensor_t gyroSensor1; static void gyroInitSensorFilters(gyroSensor_t *gyroSensor); diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 078acc3db5..4e2c655052 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -82,6 +82,17 @@ #define FAST_CODE #endif +#ifdef USE_FAST_RAM +#ifdef __APPLE__ +#define FAST_RAM __attribute__ ((section("__DATA,__.fastram_bss"), aligned(4))) +#else +#define FAST_RAM __attribute__ ((section(".fastram_bss"), aligned(4))) +#endif +#else +#define FAST_RAM +#endif // USE_FAST_RAM + + #define USE_CLI #define USE_PPM #define USE_PWM diff --git a/src/main/target/common_osd_slave.h b/src/main/target/common_osd_slave.h index 732b13fe6b..53b5dcb3a5 100644 --- a/src/main/target/common_osd_slave.h +++ b/src/main/target/common_osd_slave.h @@ -57,6 +57,7 @@ #endif #define FAST_CODE +#define FAST_RAM //CLI needs FC dependencies removed before we can compile it, disabling for now //#define USE_CLI diff --git a/src/main/target/link/stm32_flash.ld b/src/main/target/link/stm32_flash.ld index eaf65e7b9a..89d3e97f52 100644 --- a/src/main/target/link/stm32_flash.ld +++ b/src/main/target/link/stm32_flash.ld @@ -125,6 +125,15 @@ SECTIONS _ebss = .; /* define a global symbol at bss end */ __bss_end__ = _ebss; } >RAM + + .fastram_bss (NOLOAD) : + { + __fastram_bss_start__ = .; + *(.fastram_bss) + *(SORT_BY_ALIGNMENT(.fastram_bss*)) + . = ALIGN(4); + __fastram_bss_end__ = .; + } >FASTRAM /* 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 */ diff --git a/src/main/target/link/stm32_flash_f103_128k.ld b/src/main/target/link/stm32_flash_f103_128k.ld index 03b5c6d63d..0a83416296 100644 --- a/src/main/target/link/stm32_flash_f103_128k.ld +++ b/src/main/target/link/stm32_flash_f103_128k.ld @@ -20,5 +20,6 @@ MEMORY } REGION_ALIAS("STACKRAM", RAM) +REGION_ALIAS("FASTRAM", RAM) INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f103_128k_opbl.ld b/src/main/target/link/stm32_flash_f103_128k_opbl.ld index da058e343e..e2fd15f6be 100644 --- a/src/main/target/link/stm32_flash_f103_128k_opbl.ld +++ b/src/main/target/link/stm32_flash_f103_128k_opbl.ld @@ -21,5 +21,6 @@ MEMORY } REGION_ALIAS("STACKRAM", RAM) +REGION_ALIAS("FASTRAM", RAM) INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f103_256k.ld b/src/main/target/link/stm32_flash_f103_256k.ld index ab2dee6a14..c2e985e658 100644 --- a/src/main/target/link/stm32_flash_f103_256k.ld +++ b/src/main/target/link/stm32_flash_f103_256k.ld @@ -20,5 +20,6 @@ MEMORY } REGION_ALIAS("STACKRAM", RAM) +REGION_ALIAS("FASTRAM", RAM) INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f103_64k.ld b/src/main/target/link/stm32_flash_f103_64k.ld index 9e79bf0f67..6354a0af24 100644 --- a/src/main/target/link/stm32_flash_f103_64k.ld +++ b/src/main/target/link/stm32_flash_f103_64k.ld @@ -20,5 +20,6 @@ MEMORY } REGION_ALIAS("STACKRAM", RAM) +REGION_ALIAS("FASTRAM", RAM) INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f303_128k.ld b/src/main/target/link/stm32_flash_f303_128k.ld index ff92201814..68c462e599 100644 --- a/src/main/target/link/stm32_flash_f303_128k.ld +++ b/src/main/target/link/stm32_flash_f303_128k.ld @@ -21,5 +21,6 @@ MEMORY } REGION_ALIAS("STACKRAM", CCM) +REGION_ALIAS("FASTRAM", CCM) INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f303_256k.ld b/src/main/target/link/stm32_flash_f303_256k.ld index 156bffd788..d4b76cc650 100644 --- a/src/main/target/link/stm32_flash_f303_256k.ld +++ b/src/main/target/link/stm32_flash_f303_256k.ld @@ -21,5 +21,6 @@ MEMORY } REGION_ALIAS("STACKRAM", CCM) +REGION_ALIAS("FASTRAM", CCM) INCLUDE "stm32_flash.ld" diff --git a/src/main/target/link/stm32_flash_f405.ld b/src/main/target/link/stm32_flash_f405.ld index d7ba9ca86d..25dcc1a3df 100644 --- a/src/main/target/link/stm32_flash_f405.ld +++ b/src/main/target/link/stm32_flash_f405.ld @@ -33,5 +33,6 @@ MEMORY } REGION_ALIAS("STACKRAM", CCM) +REGION_ALIAS("FASTRAM", CCM) -INCLUDE "stm32_flash_split.ld" \ No newline at end of file +INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f405_opbl.ld b/src/main/target/link/stm32_flash_f405_opbl.ld index a78cf601f9..b511167cae 100644 --- a/src/main/target/link/stm32_flash_f405_opbl.ld +++ b/src/main/target/link/stm32_flash_f405_opbl.ld @@ -33,5 +33,6 @@ MEMORY } REGION_ALIAS("STACKRAM", CCM) +REGION_ALIAS("FASTRAM", CCM) INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f411.ld b/src/main/target/link/stm32_flash_f411.ld index 18684c2593..ec9fd4fab4 100644 --- a/src/main/target/link/stm32_flash_f411.ld +++ b/src/main/target/link/stm32_flash_f411.ld @@ -27,9 +27,11 @@ MEMORY FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K RAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K + CCM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K MEMORY_B1 (rx) : ORIGIN = 0x60000000, LENGTH = 0K } -REGION_ALIAS("STACKRAM", RAM) +REGION_ALIAS("STACKRAM", CCM) +REGION_ALIAS("FASTRAM", CCM) INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f411_opbl.ld b/src/main/target/link/stm32_flash_f411_opbl.ld index 4a57959110..7293475c91 100644 --- a/src/main/target/link/stm32_flash_f411_opbl.ld +++ b/src/main/target/link/stm32_flash_f411_opbl.ld @@ -33,5 +33,6 @@ MEMORY } REGION_ALIAS("STACKRAM", CCM) +REGION_ALIAS("FASTRAM", CCM) INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f446.ld b/src/main/target/link/stm32_flash_f446.ld index a258aa59dc..f329d29d92 100644 --- a/src/main/target/link/stm32_flash_f446.ld +++ b/src/main/target/link/stm32_flash_f446.ld @@ -31,5 +31,6 @@ MEMORY } REGION_ALIAS("STACKRAM", RAM) +REGION_ALIAS("FASTRAM", RAM) INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f722.ld b/src/main/target/link/stm32_flash_f722.ld index ce22546f52..b4f390fa21 100644 --- a/src/main/target/link/stm32_flash_f722.ld +++ b/src/main/target/link/stm32_flash_f722.ld @@ -39,5 +39,6 @@ MEMORY /* note TCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) +REGION_ALIAS("FASTRAM", TCM) INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f745.ld b/src/main/target/link/stm32_flash_f745.ld index 306d669b44..f5d8f81f3b 100644 --- a/src/main/target/link/stm32_flash_f745.ld +++ b/src/main/target/link/stm32_flash_f745.ld @@ -32,5 +32,6 @@ MEMORY } /* note CCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) +REGION_ALIAS("FASTRAM", TCM) INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_f746.ld b/src/main/target/link/stm32_flash_f746.ld index 16be8311d8..2ca0791f79 100644 --- a/src/main/target/link/stm32_flash_f746.ld +++ b/src/main/target/link/stm32_flash_f746.ld @@ -32,5 +32,6 @@ MEMORY } /* note CCM could be used for stack */ REGION_ALIAS("STACKRAM", TCM) +REGION_ALIAS("FASTRAM", TCM) INCLUDE "stm32_flash_split.ld" diff --git a/src/main/target/link/stm32_flash_split.ld b/src/main/target/link/stm32_flash_split.ld index cc6d264fab..6b8f7c27a4 100644 --- a/src/main/target/link/stm32_flash_split.ld +++ b/src/main/target/link/stm32_flash_split.ld @@ -126,6 +126,15 @@ SECTIONS __bss_end__ = _ebss; } >RAM + .fastram_bss (NOLOAD) : + { + __fastram_bss_start__ = .; + *(.fastram_bss) + *(SORT_BY_ALIGNMENT(.fastram_bss*)) + . = ALIGN(4); + __fastram_bss_end__ = .; + } >FASTRAM + /* 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; diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 243c4d185c..fb042eb001 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -26,6 +26,7 @@ #define U_ID_2 2 #define FAST_CODE +#define FAST_RAM #define MAX_PROFILE_COUNT 3 #define USE_MAG