reduce cpu and punish high error more

emuflight · Jul 15, 2022 · f488930 · f488930
1 parent 254ce88
commit f488930
Show file tree

Hide file tree

Showing 6 changed files with 16 additions and 20 deletions.
diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c
@@ -112,7 +112,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
     gyroConfig->gyroMovementCalibrationThreshold = 48;
     gyroConfig->gyro_hardware_lpf = GYRO_HARDWARE_LPF_NORMAL;
     gyroConfig->gyro_lpf1_type = FILTER_PT1;
-    gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;  
+    gyroConfig->gyro_lpf1_static_hz = GYRO_LPF1_DYN_MIN_HZ_DEFAULT;
         // NOTE: dynamic lpf is enabled by default so this setting is actually
         // overridden and the static lowpass 1 is disabled. We can't set this
         // value to 0 otherwise Configurator versions 10.4 and earlier will also
@@ -432,9 +432,7 @@ FAST_CODE float gyroVariance(gyroVariance_t *variance, float gyroRate) {
     variance->axisMean = variance->axisSumMean * variance->inverseN;
     variance->axisVar = variance->axisSumVar * variance->inverseN;
 
-    float squirt = 0;
-    arm_sqrt_f32(variance->axisVar, &squirt);
-    return squirt * 100.0f + 1.0f;
+    return variance->axisVar * 100.0f + 0.5f;
 }
 
 FAST_CODE void gyroUpdate(void)

diff --git a/src/main/target/CLRACINGF7/target.h b/src/main/target/CLRACINGF7/target.h
@@ -57,7 +57,7 @@
 #define GYRO_2_ALIGN            CW90_DEG
 //#define ACC_2_ALIGN             CW90_DEG
 
-#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH
+#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE
 
 #define USE_MAG
 #define USE_MAG_HMC5883
@@ -161,4 +161,3 @@
 
 #define USABLE_TIMER_CHANNEL_COUNT      9
 #define USED_TIMERS             ( TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5)  | TIM_N(8)   )
-
diff --git a/src/main/target/EXF722DUAL/target.h b/src/main/target/EXF722DUAL/target.h
@@ -56,7 +56,7 @@
 
 #define GYRO_2_ALIGN        CW90_DEG
 
-#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH
+#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE
 
 #define USE_VCP
 #define USE_UART1
@@ -125,7 +125,7 @@
 
 #define USE_ADC
 #define ADC_INSTANCE            ADC3
-#define ADC3_DMA_OPT            0  // DMA 2 Stream 0 Channel 2 
+#define ADC3_DMA_OPT            0  // DMA 2 Stream 0 Channel 2
 
 #define USE_I2C
 #define USE_I2C_DEVICE_2

diff --git a/src/main/target/IFLIGHT_H743_AIO/target.h b/src/main/target/IFLIGHT_H743_AIO/target.h
@@ -27,7 +27,7 @@
 
 #define USE_BEEPER
 #define BEEPER_PIN              PD15
-#define BEEPER_INVERTED 
+#define BEEPER_INVERTED
 
 #define USE_UART
 
@@ -129,13 +129,13 @@
 
 #define GYRO_1_CS_PIN           SPI1_NSS_PIN
 #define GYRO_1_SPI_INSTANCE     SPI1
-#define GYRO_1_ALIGN            CW0_DEG 
+#define GYRO_1_ALIGN            CW0_DEG
 
 #define GYRO_2_CS_PIN           SPI2_NSS_PIN
 #define GYRO_2_SPI_INSTANCE     SPI2
 #define GYRO_2_ALIGN            CW0_DEG
 
-#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH
+#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE
 
 #define USE_FLASHFS
 #define USE_FLASH_TOOLS
@@ -148,7 +148,7 @@
 #define FLASH_CS_PIN            SPI3_NSS_PIN
 #define FLASH_SPI_INSTANCE      SPI3
 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
- 
+
 #define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI4
 #define MAX7456_SPI_CS_PIN      SPI4_NSS_PIN

diff --git a/src/main/target/IFLIGHT_H743_AIO_V2/target.h b/src/main/target/IFLIGHT_H743_AIO_V2/target.h
@@ -27,7 +27,7 @@
 
 #define USE_BEEPER
 #define BEEPER_PIN              PD2
-#define BEEPER_INVERTED 
+#define BEEPER_INVERTED
 
 #define USE_UART
 
@@ -129,13 +129,13 @@
 
 #define GYRO_1_CS_PIN           SPI1_NSS_PIN
 #define GYRO_1_SPI_INSTANCE     SPI1
-#define GYRO_1_ALIGN            CW0_DEG 
+#define GYRO_1_ALIGN            CW0_DEG
 
 #define GYRO_2_CS_PIN           SPI2_NSS_PIN
 #define GYRO_2_SPI_INSTANCE     SPI2
 #define GYRO_2_ALIGN            CW0_DEG
 
-#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH
+#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE
 
 #define USE_FLASHFS
 #define USE_FLASH_TOOLS
@@ -148,7 +148,7 @@
 #define FLASH_CS_PIN            SPI3_NSS_PIN
 #define FLASH_SPI_INSTANCE      SPI3
 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT
- 
+
 #define USE_MAX7456
 #define MAX7456_SPI_INSTANCE    SPI4
 #define MAX7456_SPI_CS_PIN      SPI4_NSS_PIN

diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h
@@ -65,7 +65,7 @@
 #define GYRO_2_ALIGN        CW270_DEG
 #endif
 
-#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH
+#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_BOTH_SIMPLE
 
 #define USE_BARO
 #define USE_BARO_BMP280
@@ -159,12 +159,12 @@
 #define USE_ADC
 // It's possible to use ADC1 or ADC3 on this target, same pins.
 //#define ADC_INSTANCE                        ADC1
-//#define ADC1_DMA_OPT                        0  // DMA 2 Stream 0 Channel 0 
+//#define ADC1_DMA_OPT                        0  // DMA 2 Stream 0 Channel 0
 
 
 // Using ADC3 frees up DMA2_Stream0 for SPI1_RX (not necessarily, SPI1_RX has DMA2_Stream2 as well)
 #define ADC_INSTANCE                        ADC3
-#define ADC3_DMA_OPT                        0  // DMA 2 Stream 0 Channel 2 
+#define ADC3_DMA_OPT                        0  // DMA 2 Stream 0 Channel 2
 
 
 #define VBAT_ADC_PIN                        PC1
@@ -206,4 +206,3 @@
 //#define USABLE_TIMER_CHANNEL_COUNT 17 // 4xPWM, 8xESC, 2xESC via UART3 RX/TX, 1xLED Strip, 1xIR, CAM_C
 #define USABLE_TIMER_CHANNEL_COUNT 17
 #define USED_TIMERS  (TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(5) | TIM_N(8) | TIM_N(9))
-