Skip to content

Commit

Permalink
Set FAST_RAM to go into .fastram_data by default. Added FAST_RAM_NOINIT
Browse files Browse the repository at this point in the history
  • Loading branch information
DieHertz committed May 23, 2018
1 parent 35d6067 commit d8dd6f2
Show file tree
Hide file tree
Showing 13 changed files with 93 additions and 92 deletions.
10 changes: 5 additions & 5 deletions src/main/drivers/pwm_output.c
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand Down
6 changes: 3 additions & 3 deletions src/main/drivers/pwm_output_dshot_hal.c
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
4 changes: 2 additions & 2 deletions src/main/drivers/serial_uart_pinconfig.c
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
2 changes: 1 addition & 1 deletion src/main/fc/fc_init.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
28 changes: 14 additions & 14 deletions src/main/flight/mixer.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand Down
64 changes: 32 additions & 32 deletions src/main/flight/pid.c
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -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)
{
Expand All @@ -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)
{
Expand Down Expand Up @@ -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)
{
Expand Down
16 changes: 8 additions & 8 deletions src/main/scheduler/scheduler.c
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down
2 changes: 1 addition & 1 deletion src/main/sensors/acceleration.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
18 changes: 9 additions & 9 deletions src/main/sensors/gyro.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -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
Expand Down
26 changes: 13 additions & 13 deletions src/main/sensors/gyroanalyse.c
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -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++) {
Expand Down
6 changes: 3 additions & 3 deletions src/main/target/common_fc_pre.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions src/main/target/common_osd_slave.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion src/test/unit/platform.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit d8dd6f2

Please sign in to comment.