New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

STM32F7: Enabled cache #3422

Closed
wants to merge 3 commits into
base: master
from
File filter...
Filter file types
Jump to file or symbol
Failed to load files and symbols.
+252 −38
Diff settings

Always

Just for now

Copy path View file
@@ -20,6 +20,8 @@
#include <string.h>
#include <math.h>

#include "platform.h"

#include "common/filter.h"
#include "common/maths.h"
#include "common/utils.h"
@@ -31,7 +33,7 @@

// NULL filter

float nullFilterApply(void *filter, float input)
float FAST_CODE nullFilterApply(void *filter, float input)
{
UNUSED(filter);
return input;
@@ -40,20 +42,20 @@ float nullFilterApply(void *filter, float input)

// PT1 Low Pass filter

void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
void FAST_CODE pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
{
filter->RC = 1.0f / ( 2.0f * M_PI_FLOAT * f_cut );
filter->dT = dT;
filter->k = filter->dT / (filter->RC + filter->dT);
}

float pt1FilterApply(pt1Filter_t *filter, float input)
float FAST_CODE pt1FilterApply(pt1Filter_t *filter, float input)
{
filter->state = filter->state + filter->k * (input - filter->state);
return filter->state;
}

float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
float FAST_CODE pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
{
// Pre calculate and store RC
if (!filter->RC) {
@@ -67,18 +69,18 @@ float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
return filter->state;
}

float filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff) {
float FAST_CODE filterGetNotchQ(uint16_t centerFreq, uint16_t cutoff) {
float octaves = log2f((float) centerFreq / (float) cutoff) * 2;
return sqrtf(powf(2, octaves)) / (powf(2, octaves) - 1);
}

/* sets up a biquad Filter */
void biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
void FAST_CODE biquadFilterInitLPF(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate)
{
biquadFilterInit(filter, filterFreq, refreshRate, BIQUAD_Q, FILTER_LPF);
}

void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
void FAST_CODE biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{
// setup variables
const float omega = 2.0f * M_PI_FLOAT * filterFreq * refreshRate * 0.000001f;
@@ -128,7 +130,7 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
filter->d1 = filter->d2 = 0;
}

void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
void FAST_CODE biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{
// backup state
float x1 = filter->x1;
@@ -150,7 +152,7 @@ void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refre
}

/* Computes a biquadFilter_t filter on a sample (slightly less precise than df2 but works in dynamic mode) */
float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
float FAST_CODE biquadFilterApplyDF1(biquadFilter_t *filter, float input)
{
/* compute result */
const float result = filter->b0 * input + filter->b1 * filter->x1 + filter->b2 * filter->x2 - filter->a1 * filter->y1 - filter->a2 * filter->y2;
@@ -167,7 +169,7 @@ float biquadFilterApplyDF1(biquadFilter_t *filter, float input)
}

/* Computes a biquadFilter_t filter in direct form 2 on a sample (higher precision but can't handle changes in coefficients */
float biquadFilterApply(biquadFilter_t *filter, float input)
float FAST_CODE biquadFilterApply(biquadFilter_t *filter, float input)
{
const float result = filter->b0 * input + filter->d1;
filter->d1 = filter->b1 * input - filter->a1 * result + filter->d2;
@@ -178,7 +180,7 @@ float biquadFilterApply(biquadFilter_t *filter, float input)
/*
* FIR filter
*/
void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
void FAST_CODE firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs, uint8_t coeffsLength)
{
filter->buf = buf;
filter->bufLength = bufLength;
@@ -194,12 +196,12 @@ void firFilterInit2(firFilter_t *filter, float *buf, uint8_t bufLength, const fl
* FIR filter initialisation
* If the FIR filter is just to be used for averaging, then coeffs can be set to NULL
*/
void firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
void FAST_CODE firFilterInit(firFilter_t *filter, float *buf, uint8_t bufLength, const float *coeffs)
{
firFilterInit2(filter, buf, bufLength, coeffs, bufLength);
}

void firFilterUpdate(firFilter_t *filter, float input)
void FAST_CODE firFilterUpdate(firFilter_t *filter, float input)
{
filter->buf[filter->index++] = input; // index is at the first empty buffer positon
if (filter->index >= filter->bufLength) {
@@ -210,7 +212,7 @@ void firFilterUpdate(firFilter_t *filter, float input)
/*
* Update FIR filter maintaining a moving sum for quick moving average computation
*/
void firFilterUpdateAverage(firFilter_t *filter, float input)
void FAST_CODE firFilterUpdateAverage(firFilter_t *filter, float input)
{
filter->movingSum += input; // sum of the last <count> items, to allow quick moving average computation
filter->movingSum -= filter->buf[filter->index]; // subtract the value that "drops off" the end of the moving sum
@@ -223,7 +225,7 @@ void firFilterUpdateAverage(firFilter_t *filter, float input)
}
}

float firFilterApply(const firFilter_t *filter)
float FAST_CODE firFilterApply(const firFilter_t *filter)
{
float ret = 0.0f;
int ii = 0;
@@ -237,7 +239,7 @@ float firFilterApply(const firFilter_t *filter)
return ret;
}

float firFilterUpdateAndApply(firFilter_t *filter, float input)
float FAST_CODE firFilterUpdateAndApply(firFilter_t *filter, float input)
{
firFilterUpdate(filter, input);
return firFilterApply(filter);
@@ -246,7 +248,7 @@ float firFilterUpdateAndApply(firFilter_t *filter, float input)
/*
* Returns average of the last <count> items.
*/
float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count)
float FAST_CODE firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count)
{
float ret = 0.0f;
int index = filter->index;
@@ -260,25 +262,25 @@ float firFilterCalcPartialAverage(const firFilter_t *filter, uint8_t count)
return ret / count;
}

float firFilterCalcMovingAverage(const firFilter_t *filter)
float FAST_CODE firFilterCalcMovingAverage(const firFilter_t *filter)
{
return filter->movingSum / filter->count;
}

float firFilterLastInput(const firFilter_t *filter)
float FAST_CODE firFilterLastInput(const firFilter_t *filter)
{
// filter->index points to next empty item in buffer
const int index = filter->index == 0 ? filter->bufLength - 1 : filter->index - 1;
return filter->buf[index];
}

void firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime)
void FAST_CODE firFilterDenoiseInit(firFilterDenoise_t *filter, uint8_t gyroSoftLpfHz, uint16_t targetLooptime)
{
filter->targetCount = constrain(lrintf((1.0f / (0.000001f * (float)targetLooptime)) / gyroSoftLpfHz), 1, MAX_FIR_DENOISE_WINDOW_SIZE);
}

// prototype function for denoising of signal by dynamic moving average. Mainly for test purposes
float firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
float FAST_CODE firFilterDenoiseUpdate(firFilterDenoise_t *filter, float input)
{
filter->state[filter->index] = input;
filter->movingSum += filter->state[filter->index++];
Copy path View file
@@ -240,6 +240,14 @@ void spiPreInit(void)

void init(void)
{
#ifdef USE_ITCM_RAM
/* Load functions into ITCM RAM */
extern unsigned char tcm_code_start;
extern unsigned char tcm_code_end;
extern unsigned char tcm_code;
memcpy(&tcm_code_start, &tcm_code, (int)(&tcm_code_end - &tcm_code_start));
#endif

#ifdef USE_HAL_DRIVER
HAL_Init();
#endif
Copy path View file
@@ -52,19 +52,23 @@
static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation;

float getSetpointRate(int axis) {
float FAST_CODE getSetpointRate(int axis)
{
return setpointRate[axis];
}

float getRcDeflection(int axis) {
float FAST_CODE getRcDeflection(int axis)
{
return rcDeflection[axis];
}

float getRcDeflectionAbs(int axis) {
float FAST_CODE getRcDeflectionAbs(int axis)
{
return rcDeflectionAbs[axis];
}

float getThrottlePIDAttenuation(void) {
float FAST_CODE getThrottlePIDAttenuation(void)
{
return throttlePIDAttenuation;
}

Copy path View file
@@ -321,7 +321,7 @@ uint8_t getMotorCount()
return motorCount;
}

float getMotorMixRange()
float FAST_CODE getMotorMixRange()
{
return motorMixRange;
}
Copy path View file
@@ -138,7 +138,8 @@ void pidResetErrorGyroState(void)

static float itermAccelerator = 1.0f;

void pidSetItermAccelerator(float newItermAccelerator) {
void pidSetItermAccelerator(float newItermAccelerator)
{
itermAccelerator = newItermAccelerator;
}

@@ -242,7 +243,8 @@ static float crashRecoveryRate;
static float crashDtermThreshold;
static float crashGyroThreshold;

void pidInitConfig(const pidProfile_t *pidProfile) {
void pidInitConfig(const pidProfile_t *pidProfile)
{
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
Kp[axis] = PTERM_SCALE * pidProfile->pid[axis].P;
Ki[axis] = ITERM_SCALE * pidProfile->pid[axis].I;
@@ -276,7 +278,8 @@ void pidInit(const pidProfile_t *pidProfile)
}

// calculates strength of horizon leveling; 0 = none, 1.0 = most leveling
static float calcHorizonLevelStrength() {
static float FAST_CODE calcHorizonLevelStrength()
{
// start with 1.0 at center stick, 0.0 at max stick deflection:
float horizonLevelStrength = 1.0f -
MAX(getRcDeflectionAbs(FD_ROLL), getRcDeflectionAbs(FD_PITCH));
@@ -335,7 +338,8 @@ static float calcHorizonLevelStrength() {
return constrainf(horizonLevelStrength, 0, 1);
}

static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint) {
static float FAST_CODE pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, float currentPidSetpoint)
{
// calculate error angle and limit the angle to the max inclination
float angle = pidProfile->levelSensitivity * getRcDeflection(axis);
#ifdef GPS
@@ -355,7 +359,8 @@ static float pidLevel(int axis, const pidProfile_t *pidProfile, const rollAndPit
return currentPidSetpoint;
}

static float accelerationLimit(int axis, float currentPidSetpoint) {
static float FAST_CODE accelerationLimit(int axis, float currentPidSetpoint)
{
static float previousSetpoint[3];
const float currentVelocity = currentPidSetpoint- previousSetpoint[axis];

@@ -368,7 +373,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)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
{
static float previousRateError[2];
const float tpaFactor = getThrottlePIDAttenuation();
@@ -76,6 +76,12 @@
#define DEFAULT_AUX_CHANNEL_COUNT 6
#endif

#ifdef USE_ITCM_RAM
#define FAST_CODE __attribute__((section(".tcm_code")))
#else
#define FAST_CODE
#endif

#define USE_CLI
#define USE_PPM
#define USE_PWM
@@ -33,6 +33,8 @@

#define MAX_PROFILE_COUNT 0

#define FAST_CODE

#ifdef STM32F1
#define MINIMAL_CLI
#endif
@@ -22,6 +22,12 @@ ENTRY(Reset_Handler)
/* Specify the memory areas */
MEMORY
{
ITCM_ISR (rx) : ORIGIN = 0x00000000, LENGTH = 1K
ITCM_RAM (rx) : ORIGIN = 0x00000400, LENGTH = 15K

ITCMFL (rx) : ORIGIN = 0x00200000, LENGTH = 16K
ITCMFL1 (rx) : ORIGIN = 0x00208000, LENGTH = 480K

FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K
FLASH_CONFIG (r) : ORIGIN = 0x08004000, LENGTH = 16K
FLASH1 (rx) : ORIGIN = 0x08008000, LENGTH = 480K
@@ -34,4 +40,4 @@ MEMORY
/* note TCM could be used for stack */
REGION_ALIAS("STACKRAM", TCM)

INCLUDE "stm32_flash_split.ld"
INCLUDE "stm32_flash_f7_split.ld"
Oops, something went wrong.
ProTip! Use n and p to navigate between commits in a pull request.