diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 7c88bf57d..7578b2ecb 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -31,12 +31,12 @@ #include "timer.h" #include "drivers/pwm_output.h" -static FAST_RAM pwmWriteFn *pwmWrite; -static FAST_RAM pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; -static FAST_RAM pwmCompleteWriteFn *pwmCompleteWrite = NULL; +static FAST_RAM_ZERO_INIT pwmWriteFn *pwmWrite; +static FAST_RAM_ZERO_INIT pwmOutputPort_t motors[MAX_SUPPORTED_MOTORS]; +static FAST_RAM_ZERO_INIT pwmCompleteWriteFn *pwmCompleteWrite = NULL; #ifdef USE_DSHOT -FAST_RAM loadDmaBufferFn *loadDmaBuffer; +FAST_RAM_ZERO_INIT loadDmaBufferFn *loadDmaBuffer; #endif #ifdef USE_SERVOS @@ -51,7 +51,7 @@ static uint16_t freqBeep = 0; static bool pwmMotorsEnabled = false; static bool isDshot = false; #ifdef USE_DSHOT_DMAR -FAST_RAM bool useBurstDshot = false; +FAST_RAM_ZERO_INIT bool useBurstDshot = false; #endif static void pwmOCConfig(TIM_TypeDef *tim, uint8_t channel, uint16_t value, uint8_t output) diff --git a/src/main/drivers/pwm_output_dshot_hal.c b/src/main/drivers/pwm_output_dshot_hal.c index 74e6306e9..8e36a8ebd 100644 --- a/src/main/drivers/pwm_output_dshot_hal.c +++ b/src/main/drivers/pwm_output_dshot_hal.c @@ -33,9 +33,9 @@ #include "dma.h" #include "rcc.h" -static FAST_RAM uint8_t dmaMotorTimerCount = 0; -static FAST_RAM motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; -static FAST_RAM motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; +static FAST_RAM_ZERO_INIT uint8_t dmaMotorTimerCount = 0; +static FAST_RAM_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS]; +static FAST_RAM_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS]; motorDmaOutput_t *getMotorDmaOutput(uint8_t index) { diff --git a/src/main/drivers/serial_uart_pinconfig.c b/src/main/drivers/serial_uart_pinconfig.c index 5b4c22f83..d9d6f9ad9 100644 --- a/src/main/drivers/serial_uart_pinconfig.c +++ b/src/main/drivers/serial_uart_pinconfig.c @@ -39,8 +39,8 @@ #include "drivers/serial_uart.h" #include "drivers/serial_uart_impl.h" -FAST_RAM uartDevice_t uartDevice[UARTDEV_COUNT]; // Only those configured in target.h -FAST_RAM uartDevice_t *uartDevmap[UARTDEV_COUNT_MAX]; // Full array +FAST_RAM_ZERO_INIT uartDevice_t uartDevice[UARTDEV_COUNT]; // Only those configured in target.h +FAST_RAM_ZERO_INIT uartDevice_t *uartDevmap[UARTDEV_COUNT_MAX]; // Full array void uartPinConfigure(const serialPinConfig_t *pSerialPinConfig) { diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 4ddaf906d..6eea85939 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -275,7 +275,7 @@ void init(void) #endif #ifdef USE_FAST_RAM - /* Load FAST_RAM_INITIALIZED variable intializers into FAST RAM */ + /* Load FAST_RAM variable intializers into DTCM RAM */ extern uint8_t _sfastram_data; extern uint8_t _efastram_data; extern uint8_t _sfastram_idata; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index af857fe77..bc4d94764 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -115,10 +115,10 @@ PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR #define PWM_RANGE_MID 1500 -static FAST_RAM uint8_t motorCount; -static FAST_RAM float motorMixRange; +static FAST_RAM_ZERO_INIT uint8_t motorCount; +static FAST_RAM_ZERO_INIT float motorMixRange; -float FAST_RAM motor[MAX_SUPPORTED_MOTORS]; +float FAST_RAM_ZERO_INIT motor[MAX_SUPPORTED_MOTORS]; float motor_disarmed[MAX_SUPPORTED_MOTORS]; mixerMode_e currentMixerMode; @@ -313,12 +313,12 @@ const mixer_t mixers[] = { }; #endif // !USE_QUAD_MIXER_ONLY -FAST_RAM float motorOutputHigh, motorOutputLow; +FAST_RAM_ZERO_INIT float motorOutputHigh, motorOutputLow; -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; +static FAST_RAM_ZERO_INIT float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; +static FAST_RAM_ZERO_INIT uint16_t rcCommand3dDeadBandLow; +static FAST_RAM_ZERO_INIT uint16_t rcCommand3dDeadBandHigh; +static FAST_RAM_ZERO_INIT float rcCommandThrottleRange, rcCommandThrottleRange3dLow, rcCommandThrottleRange3dHigh; uint8_t getMotorCount(void) { @@ -519,12 +519,12 @@ void stopPwmAllMotors(void) delayMicroseconds(1500); } -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_RAM_ZERO_INIT float throttle = 0; +static FAST_RAM_ZERO_INIT float motorOutputMin; +static FAST_RAM_ZERO_INIT float motorRangeMin; +static FAST_RAM_ZERO_INIT float motorRangeMax; +static FAST_RAM_ZERO_INIT float motorOutputRange; +static FAST_RAM_ZERO_INIT int8_t motorOutputMixSign; static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 10859b5dc..0292d2c28 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -56,14 +56,14 @@ #include "sensors/acceleration.h" -FAST_RAM uint32_t targetPidLooptime; -FAST_RAM pidAxisData_t pidData[XYZ_AXIS_COUNT]; +FAST_RAM_ZERO_INIT uint32_t targetPidLooptime; +FAST_RAM_ZERO_INIT pidAxisData_t pidData[XYZ_AXIS_COUNT]; -static FAST_RAM bool pidStabilisationEnabled; +static FAST_RAM_ZERO_INIT bool pidStabilisationEnabled; -static FAST_RAM bool inCrashRecoveryMode = false; +static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false; -static FAST_RAM float dT; +static FAST_RAM_ZERO_INIT float dT; PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); @@ -162,7 +162,7 @@ void pidResetITerm(void) } } -static FAST_RAM_INITIALIZED float itermAccelerator = 1.0f; +static FAST_RAM float itermAccelerator = 1.0f; void pidSetItermAccelerator(float newItermAccelerator) { @@ -189,14 +189,14 @@ typedef union dtermLowpass_u { #endif } dtermLowpass_t; -static FAST_RAM filterApplyFnPtr dtermNotchApplyFn; -static FAST_RAM biquadFilter_t dtermNotch[2]; -static FAST_RAM filterApplyFnPtr dtermLowpassApplyFn; -static FAST_RAM dtermLowpass_t dtermLowpass[2]; -static FAST_RAM filterApplyFnPtr dtermLowpass2ApplyFn; -static FAST_RAM pt1Filter_t dtermLowpass2[2]; -static FAST_RAM filterApplyFnPtr ptermYawLowpassApplyFn; -static FAST_RAM pt1Filter_t ptermYawLowpass; +static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn; +static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[2]; +static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn; +static FAST_RAM_ZERO_INIT dtermLowpass_t dtermLowpass[2]; +static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpass2ApplyFn; +static FAST_RAM_ZERO_INIT pt1Filter_t dtermLowpass2[2]; +static FAST_RAM_ZERO_INIT filterApplyFnPtr ptermYawLowpassApplyFn; +static FAST_RAM_ZERO_INIT pt1Filter_t ptermYawLowpass; void pidInitFilters(const pidProfile_t *pidProfile) { @@ -289,25 +289,25 @@ typedef struct pidCoefficient_s { float Kd; } pidCoefficient_t; -static FAST_RAM pidCoefficient_t pidCoefficient[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; -FAST_RAM float throttleBoost; +static FAST_RAM_ZERO_INIT pidCoefficient_t pidCoefficient[3]; +static FAST_RAM_ZERO_INIT float maxVelocity[3]; +static FAST_RAM_ZERO_INIT float relaxFactor; +static FAST_RAM_ZERO_INIT float dtermSetpointWeight; +static FAST_RAM_ZERO_INIT float levelGain, horizonGain, horizonTransition, horizonCutoffDegrees, horizonFactorRatio; +static FAST_RAM_ZERO_INIT float ITermWindupPointInv; +static FAST_RAM_ZERO_INIT uint8_t horizonTiltExpertMode; +static FAST_RAM_ZERO_INIT timeDelta_t crashTimeLimitUs; +static FAST_RAM_ZERO_INIT timeDelta_t crashTimeDelayUs; +static FAST_RAM_ZERO_INIT int32_t crashRecoveryAngleDeciDegrees; +static FAST_RAM_ZERO_INIT float crashRecoveryRate; +static FAST_RAM_ZERO_INIT float crashDtermThreshold; +static FAST_RAM_ZERO_INIT float crashGyroThreshold; +static FAST_RAM_ZERO_INIT float crashSetpointThreshold; +static FAST_RAM_ZERO_INIT float crashLimitYaw; +static FAST_RAM_ZERO_INIT float itermLimit; +FAST_RAM_ZERO_INIT float throttleBoost; pt1Filter_t throttleLpf; -static FAST_RAM bool itermRotation; +static FAST_RAM_ZERO_INIT bool itermRotation; void pidInitConfig(const pidProfile_t *pidProfile) { diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 3e2071322..d5ea17b74 100644 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -45,21 +45,21 @@ // 2 - time spent in scheduler // 3 - time spent executing check function -static FAST_RAM cfTask_t *currentTask = NULL; +static FAST_RAM_ZERO_INIT cfTask_t *currentTask = NULL; -static FAST_RAM uint32_t totalWaitingTasks; -static FAST_RAM uint32_t totalWaitingTasksSamples; +static FAST_RAM_ZERO_INIT uint32_t totalWaitingTasks; +static FAST_RAM_ZERO_INIT uint32_t totalWaitingTasksSamples; -static FAST_RAM bool calculateTaskStatistics; -FAST_RAM uint16_t averageSystemLoadPercent = 0; +static FAST_RAM_ZERO_INIT bool calculateTaskStatistics; +FAST_RAM_ZERO_INIT uint16_t averageSystemLoadPercent = 0; -static FAST_RAM int taskQueuePos = 0; -STATIC_UNIT_TESTED FAST_RAM int taskQueueSize = 0; +static FAST_RAM_ZERO_INIT int taskQueuePos = 0; +STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT int taskQueueSize = 0; // No need for a linked list for the queue, since items are only inserted at startup -STATIC_UNIT_TESTED FAST_RAM cfTask_t* taskQueueArray[TASK_COUNT + 1]; // extra item for NULL pointer at end of queue +STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT 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 ea2e589bc..c1616fca0 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -82,7 +82,7 @@ #endif -FAST_RAM acc_t acc; // acc access functions +FAST_RAM_ZERO_INIT 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 501afad69..9af0a71ef 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -83,18 +83,18 @@ #define USE_GYRO_SLEW_LIMITER #endif -FAST_RAM gyro_t gyro; -static FAST_RAM uint8_t gyroDebugMode; +FAST_RAM_ZERO_INIT gyro_t gyro; +static FAST_RAM_ZERO_INIT uint8_t gyroDebugMode; static uint8_t gyroToUse = 0; #ifdef USE_GYRO_OVERFLOW_CHECK -static FAST_RAM uint8_t overflowAxisMask; +static FAST_RAM_ZERO_INIT uint8_t overflowAxisMask; #endif -static FAST_RAM float accumulatedMeasurements[XYZ_AXIS_COUNT]; -static FAST_RAM float gyroPrevious[XYZ_AXIS_COUNT]; -static FAST_RAM timeUs_t accumulatedMeasurementTimeUs; -static FAST_RAM timeUs_t accumulationLastTimeSampledUs; +static FAST_RAM_ZERO_INIT float accumulatedMeasurements[XYZ_AXIS_COUNT]; +static FAST_RAM_ZERO_INIT float gyroPrevious[XYZ_AXIS_COUNT]; +static FAST_RAM_ZERO_INIT timeUs_t accumulatedMeasurementTimeUs; +static FAST_RAM_ZERO_INIT timeUs_t accumulationLastTimeSampledUs; static bool gyroHasOverflowProtection = true; @@ -152,9 +152,9 @@ typedef struct gyroSensor_s { } gyroSensor_t; -STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor1; +STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor1; #ifdef USE_DUAL_GYRO -STATIC_UNIT_TESTED FAST_RAM gyroSensor_t gyroSensor2; +STATIC_UNIT_TESTED FAST_RAM_ZERO_INIT gyroSensor_t gyroSensor2; #endif #ifdef UNIT_TEST diff --git a/src/main/sensors/gyroanalyse.c b/src/main/sensors/gyroanalyse.c index 8059c6fa8..a5958894b 100644 --- a/src/main/sensors/gyroanalyse.c +++ b/src/main/sensors/gyroanalyse.c @@ -57,27 +57,27 @@ #define BIQUAD_Q 1.0f / sqrtf(2.0f) // quality factor - butterworth -static FAST_RAM uint16_t fftSamplingScale; +static FAST_RAM_ZERO_INIT uint16_t fftSamplingScale; // gyro data used for frequency analysis -static float FAST_RAM gyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; +static float FAST_RAM_ZERO_INIT gyroData[XYZ_AXIS_COUNT][FFT_WINDOW_SIZE]; -static FAST_RAM arm_rfft_fast_instance_f32 fftInstance; -static FAST_RAM float fftData[FFT_WINDOW_SIZE]; -static FAST_RAM float rfftData[FFT_WINDOW_SIZE]; -static FAST_RAM gyroFftData_t fftResult[XYZ_AXIS_COUNT]; +static FAST_RAM_ZERO_INIT arm_rfft_fast_instance_f32 fftInstance; +static FAST_RAM_ZERO_INIT float fftData[FFT_WINDOW_SIZE]; +static FAST_RAM_ZERO_INIT float rfftData[FFT_WINDOW_SIZE]; +static FAST_RAM_ZERO_INIT gyroFftData_t fftResult[XYZ_AXIS_COUNT]; // use a circular buffer for the last FFT_WINDOW_SIZE samples -static FAST_RAM uint16_t fftIdx; +static FAST_RAM_ZERO_INIT uint16_t fftIdx; // bandpass filter gyro data -static FAST_RAM biquadFilter_t fftGyroFilter[XYZ_AXIS_COUNT]; +static FAST_RAM_ZERO_INIT biquadFilter_t fftGyroFilter[XYZ_AXIS_COUNT]; // filter for smoothing frequency estimation -static FAST_RAM biquadFilter_t fftFreqFilter[XYZ_AXIS_COUNT]; +static FAST_RAM_ZERO_INIT biquadFilter_t fftFreqFilter[XYZ_AXIS_COUNT]; // Hanning window, see https://en.wikipedia.org/wiki/Window_function#Hann_.28Hanning.29_window -static FAST_RAM float hanningWindow[FFT_WINDOW_SIZE]; +static FAST_RAM_ZERO_INIT float hanningWindow[FFT_WINDOW_SIZE]; void initHanning(void) { @@ -128,10 +128,10 @@ const gyroFftData_t *gyroFftData(int axis) void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn) { // accumulator for oversampled data => no aliasing and less noise - static FAST_RAM float fftAcc[XYZ_AXIS_COUNT]; - static FAST_RAM uint32_t fftAccCount; + static FAST_RAM_ZERO_INIT float fftAcc[XYZ_AXIS_COUNT]; + static FAST_RAM_ZERO_INIT uint32_t fftAccCount; - static FAST_RAM uint32_t gyroDataAnalyseUpdateTicks; + static FAST_RAM_ZERO_INIT uint32_t gyroDataAnalyseUpdateTicks; // if gyro sampling is > 1kHz, accumulate multiple samples for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 3e714192f..bbcf6c6dc 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -101,11 +101,11 @@ #endif // USE_ITCM_RAM #ifdef USE_FAST_RAM -#define FAST_RAM __attribute__ ((section(".fastram_bss"), aligned(4))) -#define FAST_RAM_INITIALIZED __attribute__ ((section(".fastram_data"), aligned(4))) +#define FAST_RAM_ZERO_INIT __attribute__ ((section(".fastram_bss"), aligned(4))) +#define FAST_RAM __attribute__ ((section(".fastram_data"), aligned(4))) #else +#define FAST_RAM_ZERO_INIT #define FAST_RAM -#define FAST_RAM_INITIALIZED #endif // USE_FAST_RAM #ifdef STM32F4 diff --git a/src/main/target/common_osd_slave.h b/src/main/target/common_osd_slave.h index 99988fda4..1f155d940 100644 --- a/src/main/target/common_osd_slave.h +++ b/src/main/target/common_osd_slave.h @@ -60,6 +60,7 @@ #endif #define FAST_CODE +#define FAST_RAM_ZERO_INIT #define FAST_RAM //CLI needs FC dependencies removed before we can compile it, disabling for now diff --git a/src/test/unit/platform.h b/src/test/unit/platform.h index 633cf2dfc..940d771b6 100644 --- a/src/test/unit/platform.h +++ b/src/test/unit/platform.h @@ -27,8 +27,8 @@ #define NOINLINE #define FAST_CODE +#define FAST_RAM_ZERO_INIT #define FAST_RAM -#define FAST_RAM_INITIALIZED #define MAX_PROFILE_COUNT 3 #define USE_MAG