Merge pull request iNavFlight#4263 from iNavFlight/dzikuvx-f7-speedup

F7 optimizations

src/main/common/filter.c

@@ -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"
@@ -58,7 +60,7 @@ float pt1FilterGetLastOutput(pt1Filter_t *filter) {
     return filter->state;
 }
 
-float pt1FilterApply(pt1Filter_t *filter, float input)
+float FAST_CODE NOINLINE pt1FilterApply(pt1Filter_t *filter, float input)
 {
     filter->state = filter->state + filter->dT / (filter->RC + filter->dT) * (input - filter->state);
     return filter->state;
@@ -198,7 +200,7 @@ void biquadFilterInit(biquadFilter_t *filter, uint16_t filterFreq, uint32_t samp
 }
 
 // Computes a biquad_t filter on a sample
-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;

src/main/drivers/pwm_output.c

@@ -297,7 +297,7 @@ void pwmCompleteDshotUpdate(uint8_t motorCount)
     }
 }
 
-bool isMotorProtocolDshot(void)
+bool FAST_CODE NOINLINE isMotorProtocolDshot(void)
 {
     return isProtocolDshot;
 }

src/main/fc/rc_controls.c

@@ -67,7 +67,7 @@
 
 stickPositions_e rcStickPositions;
 
-int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
+FASTRAM int16_t rcCommand[4];           // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
 
 PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
 

src/main/flight/imu.c

@@ -581,7 +581,7 @@ void imuCheckVibrationLevels(void)
     DEBUG_SET(DEBUG_VIBE, 3, accClipCount);
 }
 
-void imuUpdateAttitude(timeUs_t currentTimeUs)
+void FAST_CODE NOINLINE imuUpdateAttitude(timeUs_t currentTimeUs)
 {
     /* Calculate dT */
     static timeUs_t previousIMUUpdateTimeUs;

src/main/flight/mixer.c

@@ -58,8 +58,8 @@
 
 static uint8_t motorCount;
 
-int16_t motor[MAX_SUPPORTED_MOTORS];
-int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
+FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
+FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
 static float motorMixRange;
 
 PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
@@ -176,7 +176,7 @@ void mixerResetDisarmedMotors(void)
     }
 }
 
-void writeMotors(void)
+void FAST_CODE NOINLINE writeMotors(void)
 {
     for (int i = 0; i < motorCount; i++) {
         uint16_t motorValue;
@@ -280,7 +280,7 @@ static void applyMotorRateLimiting(const float dT)
     }
 }
 
-void mixTable(const float dT)
+void FAST_CODE NOINLINE mixTable(const float dT)
 {
     int16_t input[3];   // RPY, range [-500:+500]
     // Allow direct stick input to motors in passthrough mode on airplanes

src/main/flight/pid.c

@@ -337,7 +337,7 @@ void schedulePidGainsUpdate(void)
     pidGainsUpdateRequired = true;
 }
 
-void updatePIDCoefficients(void)
+void FAST_CODE NOINLINE updatePIDCoefficients(void)
 {
     STATIC_FASTRAM uint16_t prevThrottle = 0;
 
@@ -459,7 +459,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h
 }
 
 /* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
-static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis)
+static void FAST_CODE pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis)
 {
     const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch;
 
@@ -481,7 +481,7 @@ bool isFixedWingItermLimitActive(float stickPosition)
     return fabsf(stickPosition) > pidProfile()->fixedWingItermLimitOnStickPosition;
 }
 
-static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis)
+static void FAST_CODE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis)
 {
     const float rateError = pidState->rateTarget - pidState->gyroRate;
 
@@ -774,7 +774,7 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
     pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
 }
 
-void pidController(void)
+void FAST_CODE pidController(void)
 {
     bool canUseFpvCameraMix = true;
     uint8_t headingHoldState = getHeadingHoldState();

src/main/flight/servos.c

@@ -405,12 +405,12 @@ void processServoAutotrim(void)
     }
 }
 
-bool isServoOutputEnabled(void)
+bool FAST_CODE NOINLINE isServoOutputEnabled(void)
 {
     return servoOutputEnabled;
 }
 
-bool isMixerUsingServos(void)
+bool FAST_CODE NOINLINE isMixerUsingServos(void)
 {
     return mixerUsesServos;
 }

src/main/sensors/boardalignment.c

@@ -20,6 +20,8 @@
 #include <math.h>
 #include <string.h>
 
+#include "platform.h"
+
 #include "common/maths.h"
 #include "common/vector.h"
 #include "common/axis.h"
@@ -70,7 +72,7 @@ void updateBoardAlignment(int16_t roll, int16_t pitch)
     initBoardAlignment();
 }
 
-void applyBoardAlignment(int32_t *vec)
+void FAST_CODE applyBoardAlignment(int32_t *vec)
 {
     if (standardBoardAlignment) {
         return;
@@ -84,7 +86,7 @@ void applyBoardAlignment(int32_t *vec)
     vec[Z] = lrintf(fpVec.z);
 }
 
-void applySensorAlignment(int32_t * dest, int32_t * src, uint8_t rotation)
+void FAST_CODE applySensorAlignment(int32_t * dest, int32_t * src, uint8_t rotation)
 {
     // Create a copy so we could use the same buffer for src & dest
     const int32_t x = src[X];

src/main/sensors/gyro.c

@@ -344,7 +344,7 @@ void gyroStartCalibration(void)
     zeroCalibrationStartV(&gyroCalibration, CALIBRATING_GYRO_TIME_MS, gyroConfig()->gyroMovementCalibrationThreshold, false);
 }
 
-bool gyroIsCalibrationComplete(void)
+bool FAST_CODE NOINLINE gyroIsCalibrationComplete(void)
 {
     return zeroCalibrationIsCompleteV(&gyroCalibration) && zeroCalibrationIsSuccessfulV(&gyroCalibration);
 }
@@ -387,7 +387,7 @@ void gyroGetMeasuredRotationRate(fpVector3_t *measuredRotationRate)
     }
 }
 
-void gyroUpdate()
+void FAST_CODE NOINLINE gyroUpdate()
 {
     // range: +/- 8192; +/- 2000 deg/sec
     if (gyroDev0.readFn(&gyroDev0)) {

src/main/startup/startup_stm32f722xx.s

@@ -134,6 +134,9 @@ LoopMarkHeapStack:
 	cmp	r2, r3
 	bcc	MarkHeapStack
 
+  //If there was code addressed into ITCM, copy from flash to ITCM_RAM for execution
+  bl CopyFastCode
+
 /* Call the clock system intitialization function.*/
   bl  SystemInit   
 /* Call static constructors */

src/main/startup/startup_stm32f745xx.s

@@ -138,6 +138,9 @@ LoopMarkHeapStack:
   orr  r1,r1,#(0xF << 20)
   str  r1,[r0]
 
+  //If there was code addressed into ITCM, copy from flash to ITCM_RAM for execution
+  bl CopyFastCode
+
 /* Call the clock system initialization function.*/
   bl  SystemInit   
 /* Call the application's entry point.*/

src/main/startup/startup_stm32f746xx.s

@@ -135,6 +135,9 @@ LoopMarkHeapStack:
 	cmp	r2, r3
 	bcc	MarkHeapStack
 
+  //If there was code addressed into ITCM, copy from flash to ITCM_RAM for execution
+  bl CopyFastCode
+
 /* Call the clock system initialization function.*/
   bl  SystemInit   
 /* Call static constructors */

src/main/target/MATEKF722/target.h

@@ -161,4 +161,4 @@
 #define TARGET_IO_PORTD         (BIT(2))
 
 #define MAX_PWM_OUTPUT_PORTS        7
-#define USE_DSHOT
+#define USE_DSHOT

src/main/target/MATEKF722SE/target.h

@@ -196,4 +196,4 @@
 #define TARGET_IO_PORTD 0xffff
 
 #define MAX_PWM_OUTPUT_PORTS        8
-#define USE_DSHOT
+#define USE_DSHOT

src/main/target/OMNIBUSF7/target.h

@@ -211,4 +211,4 @@
 #define TARGET_IO_PORTD 0xffff
 #define TARGET_IO_PORTE 0xffff
 
-#define PCA9685_I2C_BUS         BUS_I2C2
+#define PCA9685_I2C_BUS         BUS_I2C2

src/main/target/common.h

@@ -155,3 +155,7 @@
 #define SKIP_TASK_STATISTICS
 
 #endif
+
+#ifdef STM32F7
+#define USE_ITCM_RAM
+#endif

src/main/target/common_post.h

@@ -35,3 +35,11 @@
 #if !defined(USE_MSP_DISPLAYPORT) && (FLASH_SIZE > 128) && !defined(USE_OSD)
 #define USE_MSP_DISPLAYPORT
 #endif
+
+#ifdef USE_ITCM_RAM
+#define FAST_CODE                   __attribute__((section(".tcm_code")))
+#define NOINLINE                    __NOINLINE
+#else
+#define FAST_CODE
+#define NOINLINE
+#endif

src/main/target/link/stm32_flash_f722.ld

@@ -26,17 +26,23 @@ ENTRY(Reset_Handler)
 /* Specify the memory areas */
 MEMORY
 {
-    FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 16K
-    FLASH_CONFIG (r)  : ORIGIN = 0x08004000, LENGTH = 16K
-    FLASH1 (rx)       : ORIGIN = 0x08008000, LENGTH = 480K
+    ITCM_RAM (rwx)           : ORIGIN = 0x00000000, LENGTH = 16K
 
-    TCM (rwx)         : ORIGIN = 0x20000000, LENGTH = 64K
-    RAM (rwx)         : ORIGIN = 0x20010000, LENGTH = 192K
-    MEMORY_B1 (rx)    : ORIGIN = 0x60000000, LENGTH = 0K
+    ITCM_FLASH (rx)         : ORIGIN = 0x00200000, LENGTH = 16K
+    /* config occupies the entire flash sector 1 for the ease of erasure, 16K on F72x */
+    ITCM_FLASH_CONFIG (r)   : ORIGIN = 0x00204000, LENGTH = 16K
+    ITCM_FLASH1 (rx)        : ORIGIN = 0x00208000, LENGTH = 480K
+
+    FLASH (rx)         : ORIGIN = 0x08000000, LENGTH = 16K
+    FLASH_CONFIG (r)   : ORIGIN = 0x08004000, LENGTH = 16K
+    FLASH1 (rx)        : ORIGIN = 0x08008000, LENGTH = 480K
+
+    TCM (rwx)          : ORIGIN = 0x20000000, LENGTH = 64K
+    RAM (rwx)          : ORIGIN = 0x20010000, LENGTH = 192K
+    MEMORY_B1 (rx)     : ORIGIN = 0x60000000, LENGTH = 0K
 }
 
-/* note TCM could be used for stack */
 REGION_ALIAS("STACKRAM", TCM)
 REGION_ALIAS("FASTRAM", TCM)
 
-INCLUDE "stm32_flash_split.ld"
+INCLUDE "stm32_flash_f7_split.ld"

src/main/target/link/stm32_flash_f745.ld

@@ -17,6 +17,7 @@ _Min_Stack_Size = 0x1800;
 ENTRY(Reset_Handler)
 
 /*
+0x00000000 to 0x00003FFF   16K ITCM RAM,
 0x08000000 to 0x080FFFFF 1024K full flash,
 0x08000000 to 0x08007FFF   32K isr vector, startup code,
 0x08008000 to 0x0800FFFF   32K config,                  // FLASH_Sector_1
@@ -26,6 +27,12 @@ ENTRY(Reset_Handler)
 /* Specify the memory areas */
 MEMORY
 {
+    ITCM_RAM (rx)			: ORIGIN = 0x00000000, LENGTH = 16K
+    ITCM_FLASH (rx)        	: ORIGIN = 0x00200000, LENGTH = 32K
+    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
+    ITCM_FLASH_CONFIG (r)  	: ORIGIN = 0x00208000, LENGTH = 32K
+    ITCM_FLASH1 (rx)       	: ORIGIN = 0x00210000, LENGTH = 960K
+
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 32K
     FLASH_CONFIG (r)  : ORIGIN = 0x08008000, LENGTH = 32K
     FLASH1 (rx)       : ORIGIN = 0x08010000, LENGTH = 960K
@@ -38,4 +45,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", TCM)
 REGION_ALIAS("FASTRAM", TCM)
 
-INCLUDE "stm32_flash_split.ld"
+INCLUDE "stm32_flash_f7_split.ld"

src/main/target/link/stm32_flash_f746.ld

@@ -26,6 +26,12 @@ ENTRY(Reset_Handler)
 /* Specify the memory areas */
 MEMORY
 {
+    ITCM_RAM (rx)			: ORIGIN = 0x00000000, LENGTH = 16K
+    ITCM_FLASH (rx)        	: ORIGIN = 0x00200000, LENGTH = 32K
+    /* config occupies the entire flash sector 1 for the ease of erasure, 32K on F74x */
+    ITCM_FLASH_CONFIG (r)  	: ORIGIN = 0x00208000, LENGTH = 32K
+    ITCM_FLASH1 (rx)       	: ORIGIN = 0x00210000, LENGTH = 960K
+
     FLASH (rx)        : ORIGIN = 0x08000000, LENGTH = 32K
     FLASH_CONFIG (r)  : ORIGIN = 0x08008000, LENGTH = 32K
     FLASH1 (rx)       : ORIGIN = 0x08010000, LENGTH = 960K
@@ -38,4 +44,4 @@ MEMORY
 REGION_ALIAS("STACKRAM", TCM)
 REGION_ALIAS("FASTRAM", TCM)
 
-INCLUDE "stm32_flash_split.ld"
+INCLUDE "stm32_flash_f7_split.ld"