Merge branch 'master' into 20230329_2D-3D_dynamicFilter

emuflight · Oct 15, 2023 · dee53bf · dee53bf
2 parents 00781c8 + c55b0d7
commit dee53bf
Show file tree

Hide file tree

Showing 43 changed files with 1,497 additions and 182 deletions.
diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c
@@ -1138,6 +1138,7 @@ static bool blackboxWriteSysinfo(void) {
         BLACKBOX_PRINT_HEADER_LINE("spa_yaw_p", "%d",                       currentPidProfile->setPointPTransition[YAW]);
         BLACKBOX_PRINT_HEADER_LINE("spa_yaw_i", "%d",                       currentPidProfile->setPointITransition[YAW]);
         BLACKBOX_PRINT_HEADER_LINE("spa_yaw_d", "%d",                       currentPidProfile->setPointDTransition[YAW]);
+        BLACKBOX_PRINT_HEADER_LINE("rates_type", "%d",                      currentControlRateProfile->rates_type);
         BLACKBOX_PRINT_HEADER_LINE("rc_rates", "%d,%d,%d",                  currentControlRateProfile->rcRates[ROLL],
                                    currentControlRateProfile->rcRates[PITCH],
                                    currentControlRateProfile->rcRates[YAW]);
@@ -1266,6 +1267,7 @@ static bool blackboxWriteSysinfo(void) {
         BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d",              motorConfig()->dev.motorPwmProtocol);
         BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d",                  motorConfig()->dev.motorPwmRate);
         BLACKBOX_PRINT_HEADER_LINE("dshot_idle_value", "%d",                motorConfig()->digitalIdleOffsetValue);
+        BLACKBOX_PRINT_HEADER_LINE("motor_poles", "%d",                     motorConfig()->motorPoleCount);
         BLACKBOX_PRINT_HEADER_LINE("debug_mode", "%d",                      systemConfig()->debug_mode);
         BLACKBOX_PRINT_HEADER_LINE("features", "%d",                        featureConfig()->enabledFeatures);
 #ifdef USE_RC_SMOOTHING_FILTER

diff --git a/src/main/common/utils.h b/src/main/common/utils.h
@@ -103,6 +103,8 @@ static inline int32_t cmp32(uint32_t a, uint32_t b) {
     return (int32_t)(a - b);
 }
 
+static inline uint32_t llog2(uint32_t n) { return 31 - __builtin_clz(n | 1); }
+
 // using memcpy_fn will force memcpy function call, instead of inlining it. In most cases function call takes fewer instructions
 //  than inlined version (inlining is cheaper for very small moves < 8 bytes / 2 store instructions)
 #if defined(UNIT_TEST) || defined(SIMULATOR_BUILD)

diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h
@@ -38,12 +38,20 @@
 #endif
 
 #define GYRO_HARDWARE_LPF_NORMAL       0
-#define GYRO_HARDWARE_LPF_EXPERIMENTAL 1
 #define GYRO_HARDWARE_LPF_1KHZ_SAMPLE  2
 
+#if defined(USE_GYRO_SPI_ICM42688P)
+#define GYRO_HARDWARE_LPF_EXPERIMENTAL 3
+#else
+#define GYRO_HARDWARE_LPF_EXPERIMENTAL 1
+#endif
+
 #define GYRO_32KHZ_HARDWARE_LPF_NORMAL       0
 #define GYRO_32KHZ_HARDWARE_LPF_EXPERIMENTAL 1
 
+#define GYRO_HARDWARE_LPF_OPTION_1 1
+#define GYRO_HARDWARE_LPF_OPTION_2 2
+
 #define GYRO_LPF_256HZ      0
 #define GYRO_LPF_188HZ      1
 #define GYRO_LPF_98HZ       2
@@ -91,6 +99,8 @@ typedef struct gyroDev_s {
     ioTag_t mpuIntExtiTag;
     uint8_t gyroHasOverflowProtection;
     gyroSensor_e gyroHardware;
+    uint8_t accDataReg;
+    uint8_t gyroDataReg;
 } gyroDev_t;
 
 typedef struct accDev_s {

diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c
@@ -55,6 +55,7 @@
 #include "drivers/accgyro/accgyro_spi_bmi160.h"
 #include "drivers/accgyro/accgyro_spi_icm20649.h"
 #include "drivers/accgyro/accgyro_spi_icm20689.h"
+#include "drivers/accgyro/accgyro_spi_icm426xx.h"
 #include "drivers/accgyro/accgyro_spi_mpu6000.h"
 #include "drivers/accgyro/accgyro_spi_mpu6500.h"
 #include "drivers/accgyro/accgyro_spi_mpu9250.h"
@@ -365,6 +366,32 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) {
         return true;
     }
 #endif
+#ifdef USE_GYRO_SPI_ICM42605
+#ifdef ICM42605_SPI_INSTANCE
+    spiBusSetInstance(&gyro->bus, ICM42605_SPI_INSTANCE);
+#endif
+#ifdef ICM42605_CS_PIN
+    gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM42605_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
+#endif
+    sensor = icm426xxSpiDetect(&gyro->bus);
+    if (sensor != MPU_NONE) {
+        gyro->mpuDetectionResult.sensor = sensor;
+        return true;
+    }
+#endif
+#ifdef USE_GYRO_SPI_ICM42688P
+#ifdef ICM42688P_SPI_INSTANCE
+    spiBusSetInstance(&gyro->bus, ICM42688P_SPI_INSTANCE);
+#endif
+#ifdef ICM42688P_CS_PIN
+    gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(ICM42688P_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin;
+#endif
+    sensor = icm426xxSpiDetect(&gyro->bus);
+    if (sensor != MPU_NONE) {
+        gyro->mpuDetectionResult.sensor = sensor;
+        return true;
+    }
+#endif
 #ifdef USE_ACCGYRO_BMI160
 #ifndef USE_DUAL_GYRO
     spiBusSetInstance(&gyro->bus, BMI160_SPI_INSTANCE);

diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h
@@ -29,7 +29,7 @@
 //#define DEBUG_MPU_DATA_READY_INTERRUPT
 
 #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) ||  defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20649) \
- || defined(USE_GYRO_SPI_ICM20689)
+ || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P)
 #define GYRO_USES_SPI
 #endif
 
@@ -47,6 +47,8 @@
 #define ICM20608G_WHO_AM_I_CONST            (0xAF)
 #define ICM20649_WHO_AM_I_CONST             (0xE1)
 #define ICM20689_WHO_AM_I_CONST             (0x98)
+#define ICM42605_WHO_AM_I_CONST             (0x42)
+#define ICM42688P_WHO_AM_I_CONST            (0x47)
 
 // RA = Register Address
 
@@ -214,6 +216,8 @@ typedef enum {
     ICM_20608_SPI,
     ICM_20649_SPI,
     ICM_20689_SPI,
+    ICM_42605_SPI,
+    ICM_42688P_SPI,
     BMI_160_SPI,
     IMUF_9001_SPI,
 } mpuSensor_e;