Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 21:35:44 +03:00
Code Issues Wiki Activity

Preparation for using instruction cache on F7

Browse source
This commit is contained in:
Martin Budden 2017-12-15 10:07:19 +00:00
parent a0f6d10b72
commit 12640972a9
13 changed files with 59 additions and 31 deletions
Download patch file Download diff file Expand all files Collapse all files

20
src/main/common/filter.c
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)
FAST_CODE float nullFilterApply(void *filter, float input)
{
UNUSED(filter);
return input;
@ -47,13 +49,13 @@ void pt1FilterInit(pt1Filter_t *filter, uint8_t f_cut, float dT)
filter->k = filter->dT / (filter->RC + filter->dT);
}
float pt1FilterApply(pt1Filter_t *filter, float input)
FAST_CODE float 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)
FAST_CODE float pt1FilterApply4(pt1Filter_t *filter, float input, uint8_t f_cut, float dT)
{
// Pre calculate and store RC
if (!filter->RC) {
@ -76,7 +78,7 @@ void slewFilterInit(slewFilter_t *filter, float slewLimit, float threshold)
filter->threshold = threshold;
}
float slewFilterApply(slewFilter_t *filter, float input)
FAST_CODE float slewFilterApply(slewFilter_t *filter, float input)
{
if (filter->state >= filter->threshold) {
if (input >= filter->state - filter->slewLimit) {
@ -153,7 +155,7 @@ void biquadFilterInit(biquadFilter_t *filter, float filterFreq, uint32_t refresh
filter->y1 = filter->y2 = 0;
}
void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
FAST_CODE void biquadFilterUpdate(biquadFilter_t *filter, float filterFreq, uint32_t refreshRate, float Q, biquadFilterType_e filterType)
{
// backup state
float x1 = filter->x1;
@ -171,7 +173,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)
FAST_CODE float 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;
@ -188,7 +190,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)
FAST_CODE float biquadFilterApply(biquadFilter_t *filter, float input)
{
const float result = filter->b0 * input + filter->x1;
filter->x1 = filter->b1 * input - filter->a1 * result + filter->x2;
@ -244,7 +246,7 @@ void firFilterUpdateAverage(firFilter_t *filter, float input)
}
}
float firFilterApply(const firFilter_t *filter)
FAST_CODE float firFilterApply(const firFilter_t *filter)
{
float ret = 0.0f;
int ii = 0;
@ -258,7 +260,7 @@ float firFilterApply(const firFilter_t *filter)
return ret;
}
float firFilterUpdateAndApply(firFilter_t *filter, float input)
FAST_CODE float firFilterUpdateAndApply(firFilter_t *filter, float input)
{
firFilterUpdate(filter, input);
return firFilterApply(filter);

10
src/main/fc/fc_core.c
View file

@ -600,7 +600,7 @@ void processRx(timeUs_t currentTimeUs)
#endif
}
static void subTaskPidController(timeUs_t currentTimeUs)
static FAST_CODE void subTaskPidController(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
@ -609,7 +609,7 @@ static void subTaskPidController(timeUs_t currentTimeUs)
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
}
static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
static FAST_CODE void subTaskMainSubprocesses(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_PIDLOOP) {startTime = micros();}
@ -683,7 +683,7 @@ static void subTaskMainSubprocesses(timeUs_t currentTimeUs)
DEBUG_SET(DEBUG_PIDLOOP, 3, micros() - startTime);
}
static void subTaskMotorUpdate(timeUs_t currentTimeUs)
static FAST_CODE void subTaskMotorUpdate(timeUs_t currentTimeUs)
{
uint32_t startTime = 0;
if (debugMode == DEBUG_CYCLETIME) {
@ -711,7 +711,7 @@ static void subTaskMotorUpdate(timeUs_t currentTimeUs)
DEBUG_SET(DEBUG_PIDLOOP, 2, micros() - startTime);
}
uint8_t setPidUpdateCountDown(void)
FAST_CODE uint8_t setPidUpdateCountDown(void)
{
if (gyroConfig()->gyro_soft_lpf_hz) {
return pidConfig()->pid_process_denom - 1;
@ -721,7 +721,7 @@ uint8_t setPidUpdateCountDown(void)
}
// Function for loop trigger
void taskMainPidLoop(timeUs_t currentTimeUs)
FAST_CODE void taskMainPidLoop(timeUs_t currentTimeUs)
{
static uint8_t pidUpdateCountdown = 0;

8
src/main/fc/fc_init.c
View file

@ -236,6 +236,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

8
src/main/flight/mixer.c
View file

@ -482,7 +482,7 @@ void mixerResetDisarmedMotors(void)
}
}
void writeMotors(void)
FAST_CODE void writeMotors(void)
{
if (pwmAreMotorsEnabled()) {
for (int i = 0; i < motorCount; i++) {
@ -520,7 +520,7 @@ static float motorRangeMax;
static float motorOutputRange;
static int8_t motorOutputMixSign;
static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs)
static FAST_CODE 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 void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
static FAST_CODE 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 void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS])
}
}
void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
FAST_CODE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensation)
{
if (isFlipOverAfterCrashMode()) {
applyFlipOverAfterCrashModeToMotors();

2
src/main/flight/pid.c
View file

@ -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)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
FAST_CODE void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
{
static float previousRateError[2];
const float tpaFactor = getThrottlePIDAttenuation();

6
src/main/scheduler/scheduler.c
View file

@ -106,7 +106,7 @@ bool queueRemove(cfTask_t *task)
/*
* Returns first item queue or NULL if queue empty
*/
cfTask_t *queueFirst(void)
FAST_CODE cfTask_t *queueFirst(void)
{
taskQueuePos = 0;
return taskQueueArray[0]; // guaranteed to be NULL if queue is empty
@ -115,7 +115,7 @@ cfTask_t *queueFirst(void)
/*
* Returns next item in queue or NULL if at end of queue
*/
cfTask_t *queueNext(void)
FAST_CODE cfTask_t *queueNext(void)
{
return taskQueueArray[++taskQueuePos]; // guaranteed to be NULL at end of queue
}
@ -225,7 +225,7 @@ void schedulerInit(void)
queueAdd(&cfTasks[TASK_SYSTEM]);
}
void scheduler(void)
FAST_CODE void scheduler(void)
{
// Cache currentTime
const timeUs_t currentTimeUs = micros();

4
src/main/sensors/boardalignment.c
View file

@ -20,6 +20,8 @@
#include <math.h>
#include <string.h>
#include "platform.h"
#include "common/maths.h"
#include "common/axis.h"
@ -68,7 +70,7 @@ static void alignBoard(int32_t *vec)
vec[Z] = lrintf(boardRotation[0][Z] * x + boardRotation[1][Z] * y + boardRotation[2][Z] * z);
}
void alignSensors(int32_t *dest, uint8_t rotation)
FAST_CODE void alignSensors(int32_t *dest, uint8_t rotation)
{
const int32_t x = dest[X];
const int32_t y = dest[Y];

10
src/main/sensors/gyro.c
View file

@ -518,12 +518,12 @@ void gyroInitFilters(void)
gyroInitSensorFilters(&gyroSensor1);
}
bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
FAST_CODE bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
{
return gyroSensor->calibration.calibratingG == 0;
}
bool isGyroCalibrationComplete(void)
FAST_CODE bool isGyroCalibrationComplete(void)
{
return isGyroSensorCalibrationComplete(&gyroSensor1);
}
@ -604,7 +604,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(gyroSensor_t *gyroSensor, uint8_t
}
#if defined(USE_GYRO_SLEW_LIMITER)
int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
FAST_CODE int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
{
int32_t newRawGyro = (int32_t)gyroSensor->gyroDev.gyroADCRaw[axis];
if (abs(newRawGyro - gyroSensor->gyroDev.gyroADCRawPrevious[axis]) > (1<<14)) {
@ -616,7 +616,7 @@ int32_t gyroSlewLimiter(gyroSensor_t *gyroSensor, int axis)
}
#endif
static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
{
if (!gyroSensor->gyroDev.readFn(&gyroSensor->gyroDev)) {
return;
@ -701,7 +701,7 @@ static void gyroUpdateSensor(gyroSensor_t *gyroSensor, timeUs_t currentTimeUs)
}
}
void gyroUpdate(timeUs_t currentTimeUs)
FAST_CODE void gyroUpdate(timeUs_t currentTimeUs)
{
gyroUpdateSensor(&gyroSensor1, currentTimeUs);
}

6
src/main/sensors/gyroanalyse.c
View file

@ -136,7 +136,7 @@ const gyroFftData_t *gyroFftData(int axis)
return &fftResult[axis];
}
bool isDynamicFilterActive(void)
FAST_CODE bool isDynamicFilterActive(void)
{
return feature(FEATURE_DYNAMIC_FILTER);
}
@ -144,7 +144,7 @@ bool isDynamicFilterActive(void)
/*
* Collect gyro data, to be analysed in gyroDataAnalyseUpdate function
*/
void gyroDataAnalyse(const gyroDev_t *gyroDev, biquadFilter_t *notchFilterDyn)
FAST_CODE 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
*/
void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
FAST_CODE void gyroDataAnalyseUpdate(biquadFilter_t *notchFilterDyn)
{
static int axis = 0;
static int step = 0;

6
src/main/target/common_fc_pre.h
View file

@ -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

2
src/main/target/common_osd_slave.h
View file

@ -56,5 +56,7 @@
#define SCHEDULER_DELAY_LIMIT 100
#endif
#define FAST_CODE
//CLI needs FC dependencies removed before we can compile it, disabling for now
//#define USE_CLI

6
src/main/target/link/stm32_flash_f722.ld
View file

@ -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

2
src/test/unit/platform.h
View file

@ -25,6 +25,8 @@
#define U_ID_1 1
#define U_ID_2 2
#define FAST_CODE
#define MAX_PROFILE_COUNT 3
#define USE_MAG
#define USE_BARO