diff --git a/make/source.mk b/make/source.mk index ad1a5d0f7a..dd602fab3c 100644 --- a/make/source.mk +++ b/make/source.mk @@ -198,6 +198,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ drivers/accgyro/accgyro_mpu6050.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_bmi160.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_icm20689.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6500.c \ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 8ee2428c8d..436871abb4 100644 --- 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]); @@ -1179,8 +1180,10 @@ static bool blackboxWriteSysinfo(void) { BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz_roll", "%d", currentPidProfile->dFilter[ROLL].dLpf2); BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz_pitch", "%d", currentPidProfile->dFilter[PITCH].dLpf2); BLACKBOX_PRINT_HEADER_LINE("dterm_lowpass2_hz_yaw", "%d", currentPidProfile->dFilter[YAW].dLpf2); +#ifdef USE_GYRO_DATA_ANALYSE BLACKBOX_PRINT_HEADER_LINE("dterm_dyn_notch_enable", "%d", currentPidProfile->dtermDynNotch); BLACKBOX_PRINT_HEADER_LINE("dterm_dyn_notch_q", "%d", currentPidProfile->dterm_dyn_notch_q); +#endif BLACKBOX_PRINT_HEADER_LINE("dterm_ABG_alpha", "%d", currentPidProfile->dterm_ABG_alpha); BLACKBOX_PRINT_HEADER_LINE("dterm_ABG_boost", "%d", currentPidProfile->dterm_ABG_boost); BLACKBOX_PRINT_HEADER_LINE("dterm_ABG_half_life", "%d", currentPidProfile->dterm_ABG_half_life); @@ -1226,10 +1229,12 @@ static bool blackboxWriteSysinfo(void) { BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz_roll", "%d", gyroConfig()->gyro_lowpass_hz[ROLL]); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz_pitch", "%d", gyroConfig()->gyro_lowpass_hz[PITCH]); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass_hz_yaw", "%d", gyroConfig()->gyro_lowpass_hz[YAW]); +#ifdef USE_GYRO_LPF2 BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_type", "%d", gyroConfig()->gyro_lowpass2_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz_roll", "%d", gyroConfig()->gyro_lowpass2_hz[ROLL]); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz_pitch", "%d", gyroConfig()->gyro_lowpass2_hz[PITCH]); BLACKBOX_PRINT_HEADER_LINE("gyro_lowpass2_hz_yaw", "%d", gyroConfig()->gyro_lowpass2_hz[YAW]); +#endif BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_hz_2); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1, @@ -1239,6 +1244,7 @@ static bool blackboxWriteSysinfo(void) { BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_count", "%d", gyroConfig()->dyn_notch_count); BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_min_hz", "%d", gyroConfig()->dyn_notch_min_hz); BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_max_hz", "%d", gyroConfig()->dyn_notch_max_hz); + BLACKBOX_PRINT_HEADER_LINE("dynamic_gyro_notch_axis", "%d", gyroConfig()->dyn_notch_axis); #endif BLACKBOX_PRINT_HEADER_LINE("gyro_ABG_alpha", "%d", gyroConfig()->gyro_ABG_alpha); BLACKBOX_PRINT_HEADER_LINE("gyro_ABG_boost", "%d", gyroConfig()->gyro_ABG_boost); @@ -1266,6 +1272,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/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index a8f3804735..50c0bfc589 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -104,6 +104,12 @@ static const char * const cms_FilterType[] = { "PT1", "BIQUAD", "PT2", "PT3", "PT4", }; +#ifdef USE_GYRO_DATA_ANALYSE +static const char * const cms_dynNotchAxisType[] = { + "RP", "RPY" +}; +#endif + static long cmsx_menuImu_onEnter(void) { pidProfileIndex = getCurrentPidProfileIndex(); tmpPidProfileIndex = pidProfileIndex + 1; @@ -487,13 +493,19 @@ static uint8_t gyroConfig_gyro_lowpass1_type; static uint16_t gyroConfig_gyro_lowpass_hz_roll; static uint16_t gyroConfig_gyro_lowpass_hz_pitch; static uint16_t gyroConfig_gyro_lowpass_hz_yaw; +#ifdef USE_GYRO_LPF2 +static uint8_t gyroConfig_gyro_lowpass2_type; static uint16_t gyroConfig_gyro_lowpass2_hz_roll; static uint16_t gyroConfig_gyro_lowpass2_hz_pitch; static uint16_t gyroConfig_gyro_lowpass2_hz_yaw; +#endif +#ifdef USE_GYRO_DATA_ANALYSE static uint16_t gyroConfig_gyro_q; static uint8_t gyroConfig_gyro_notch_count; static uint16_t gyroConfig_gyro_notch_min_hz; static uint16_t gyroConfig_gyro_notch_max_hz; +static uint8_t gyroConfig_gyro_notch_axis; +#endif static uint16_t gyroConfig_gyro_abg_alpha; static uint16_t gyroConfig_gyro_abg_boost; static uint8_t gyroConfig_gyro_abg_half_life; @@ -515,17 +527,22 @@ static long cmsx_menuGyro_onEnter(void) { gyroConfig_gyro_lowpass_hz_roll = gyroConfig()->gyro_lowpass_hz[ROLL]; gyroConfig_gyro_lowpass_hz_pitch = gyroConfig()->gyro_lowpass_hz[PITCH]; gyroConfig_gyro_lowpass_hz_yaw = gyroConfig()->gyro_lowpass_hz[YAW]; +#ifdef USE_GYRO_LPF2 + gyroConfig_gyro_lowpass2_type = gyroConfig()->gyro_lowpass2_type; gyroConfig_gyro_lowpass2_hz_roll = gyroConfig()->gyro_lowpass2_hz[ROLL]; gyroConfig_gyro_lowpass2_hz_pitch = gyroConfig()->gyro_lowpass2_hz[PITCH]; gyroConfig_gyro_lowpass2_hz_yaw = gyroConfig()->gyro_lowpass2_hz[YAW]; +#endif +#ifdef USE_GYRO_DATA_ANALYSE gyroConfig_gyro_q = gyroConfig()->dyn_notch_q; gyroConfig_gyro_notch_count = gyroConfig()->dyn_notch_count; gyroConfig_gyro_notch_min_hz = gyroConfig()->dyn_notch_min_hz; gyroConfig_gyro_notch_max_hz = gyroConfig()->dyn_notch_max_hz; + gyroConfig_gyro_notch_axis = gyroConfig()->dyn_notch_axis; +#endif gyroConfig_gyro_abg_alpha = gyroConfig()->gyro_ABG_alpha; gyroConfig_gyro_abg_boost = gyroConfig()->gyro_ABG_boost; gyroConfig_gyro_abg_half_life = gyroConfig()->gyro_ABG_half_life; - #ifndef USE_GYRO_IMUF9001 gyroConfig_imuf_roll_q = gyroConfig()->imuf_roll_q; gyroConfig_imuf_pitch_q = gyroConfig()->imuf_pitch_q; @@ -547,13 +564,19 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) { gyroConfigMutable()->gyro_lowpass_hz[ROLL] = gyroConfig_gyro_lowpass_hz_roll; gyroConfigMutable()->gyro_lowpass_hz[PITCH] = gyroConfig_gyro_lowpass_hz_pitch; gyroConfigMutable()->gyro_lowpass_hz[YAW] = gyroConfig_gyro_lowpass_hz_yaw; +#ifdef USE_GYRO_LPF2 + gyroConfigMutable()->gyro_lowpass2_type = gyroConfig_gyro_lowpass2_type; gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = gyroConfig_gyro_lowpass2_hz_roll; gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = gyroConfig_gyro_lowpass2_hz_pitch; gyroConfigMutable()->gyro_lowpass2_hz[YAW] = gyroConfig_gyro_lowpass2_hz_yaw; +#endif +#ifdef USE_GYRO_DATA_ANALYSE gyroConfigMutable()->dyn_notch_q = gyroConfig_gyro_q; gyroConfigMutable()->dyn_notch_count = gyroConfig_gyro_notch_count; gyroConfigMutable()->dyn_notch_min_hz = gyroConfig_gyro_notch_min_hz; gyroConfigMutable()->dyn_notch_max_hz = gyroConfig_gyro_notch_max_hz; + gyroConfigMutable()->dyn_notch_axis = gyroConfig_gyro_notch_axis; +#endif gyroConfigMutable()->gyro_ABG_alpha = gyroConfig_gyro_abg_alpha; gyroConfigMutable()->gyro_ABG_boost = gyroConfig_gyro_abg_boost; gyroConfigMutable()->gyro_ABG_half_life = gyroConfig_gyro_abg_half_life; @@ -575,19 +598,23 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) { static OSD_Entry cmsx_menuFilterGlobalEntries[] = { { "-- FILTER GLB --", OME_Label, NULL, NULL, 0 }, + { "GYRO LPF TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_lowpass1_type, 4, cms_FilterType }, 0 }, { "GYRO LPF ROLL", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz_roll, 0, 16000, 1 }, 0 }, { "GYRO LPF PITCH", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz_pitch, 0, 16000, 1 }, 0 }, { "GYRO LPF YAW", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass_hz_yaw, 0, 16000, 1 }, 0 }, - { "GYRO LPF TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_lowpass1_type, 4, cms_FilterType }, 0 }, #ifdef USE_GYRO_LPF2 + { "GYRO LPF2 TYPE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_lowpass2_type, 4, cms_FilterType }, 0 }, { "GYRO LPF2 ROLL", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_roll, 0, 16000, 1 }, 0 }, { "GYRO LPF2 PITCH", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_pitch, 0, 16000, 1 }, 0 }, { "GYRO LPF2 YAW", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_lowpass2_hz_yaw, 0, 16000, 1 }, 0 }, #endif +#ifdef USE_GYRO_DATA_ANALYSE { "DYN NOTCH Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_q, 0, 1000, 1 }, 0 }, { "DYN NOTCH COUNT", OME_UINT8, NULL, &(OSD_UINT8_t) { &gyroConfig_gyro_notch_count, 1, 5, 1 }, 0 }, { "DYN NOTCH MIN HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_min_hz, 30, 1000, 1 }, 0 }, { "DYN NOTCH MAX HZ", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_notch_max_hz, 200, 1000, 1 }, 0 }, + { "DYN NOTCH AXIS", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &gyroConfig_gyro_notch_axis, 1, cms_dynNotchAxisType }, 0 }, +#endif #ifndef USE_GYRO_IMUF9001 { "IMUF W", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_w, 0, 512, 1 }, 0 }, { "ROLL Q", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_imuf_roll_q, 100, 16000, 100 }, 0 }, @@ -709,8 +736,10 @@ static uint16_t cmsx_dterm_lowpass2_type; static uint16_t cmsx_dterm_lowpass2_hz_roll; static uint16_t cmsx_dterm_lowpass2_hz_pitch; static uint16_t cmsx_dterm_lowpass2_hz_yaw; +#ifdef USE_GYRO_DATA_ANALYSE static uint8_t cmsx_dterm_dyn_notch_enable ; static uint16_t cmsx_dterm_dyn_notch_q; +#endif static uint16_t cmsx_dterm_abg_alpha; static uint16_t cmsx_dterm_abg_boost; static uint8_t cmsx_dterm_abg_half_life; @@ -725,8 +754,10 @@ static long cmsx_FilterPerProfileRead(void) { cmsx_dterm_lowpass2_hz_roll = pidProfile->dFilter[ROLL].dLpf2; cmsx_dterm_lowpass2_hz_pitch = pidProfile->dFilter[PITCH].dLpf2; cmsx_dterm_lowpass2_hz_yaw = pidProfile->dFilter[YAW].dLpf2; +#ifdef USE_GYRO_DATA_ANALYSE cmsx_dterm_dyn_notch_enable = pidProfile->dtermDynNotch; cmsx_dterm_dyn_notch_q = pidProfile->dterm_dyn_notch_q; +#endif cmsx_dterm_abg_alpha = pidProfile->dterm_ABG_alpha; cmsx_dterm_abg_boost = pidProfile->dterm_ABG_boost; cmsx_dterm_abg_half_life = pidProfile->dterm_ABG_half_life; @@ -744,8 +775,10 @@ static long cmsx_FilterPerProfileWriteback(const OSD_Entry *self) { pidProfile->dFilter[ROLL].dLpf2 = cmsx_dterm_lowpass2_hz_roll; pidProfile->dFilter[PITCH].dLpf2 = cmsx_dterm_lowpass2_hz_pitch; pidProfile->dFilter[YAW].dLpf2 = cmsx_dterm_lowpass2_hz_yaw; +#ifdef USE_GYRO_DATA_ANALYSE pidProfile->dtermDynNotch = cmsx_dterm_dyn_notch_enable; pidProfile->dterm_dyn_notch_q = cmsx_dterm_dyn_notch_q; +#endif pidProfile->dterm_ABG_alpha = cmsx_dterm_abg_alpha; pidProfile->dterm_ABG_boost = cmsx_dterm_abg_boost; pidProfile->dterm_ABG_half_life = cmsx_dterm_abg_half_life; @@ -763,8 +796,10 @@ static OSD_Entry cmsx_menuFilterPerProfileEntries[] = { { "DTERM LPF2 ROLL", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_roll, 0, 500, 1 }, 0 }, { "DTERM LPF2 PITCH", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_pitch, 0, 500, 1 }, 0 }, { "DTERM LPF2 YAW", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_lowpass2_hz_yaw, 0, 500, 1 }, 0 }, +#ifdef USE_GYRO_DATA_ANALYSE { "DTERM DYN ENABLE", OME_TAB, NULL, &(OSD_TAB_t) { (uint8_t *) &cmsx_dterm_dyn_notch_enable, 1, cms_offOnLabels }, 0 }, { "DTERM DYN NOT Q", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_dyn_notch_q, 0, 2000, 1 }, 0 }, +#endif { "DTERM ABG ALPHA", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_abg_alpha, 0, 1000, 1 }, 0 }, { "DTERM ABG BOOST", OME_UINT16, NULL, &(OSD_UINT16_t){ &cmsx_dterm_abg_boost, 0, 2000, 1 }, 0 }, { "DTERM ABG HL", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_dterm_abg_half_life, 0, 250, 1 }, 0 }, diff --git a/src/main/drivers/accgyro/accgyro.h b/src/main/drivers/accgyro/accgyro.h index 2c17f4b2f5..0bcf72738c 100644 --- a/src/main/drivers/accgyro/accgyro.h +++ b/src/main/drivers/accgyro/accgyro.h @@ -33,6 +33,9 @@ #include #endif +#define GYRO_SCALE_2000DPS (2000.0f / (1 << 15)) // 16.384 dps/lsb scalefactor for 2000dps sensors +#define GYRO_SCALE_4000DPS (4000.0f / (1 << 15)) // 8.192 dps/lsb scalefactor for 4000dps sensor + #ifndef MPU_I2C_INSTANCE #define MPU_I2C_INSTANCE I2C_DEVICE #endif @@ -40,7 +43,7 @@ #define GYRO_HARDWARE_LPF_NORMAL 0 #define GYRO_HARDWARE_LPF_1KHZ_SAMPLE 2 -#if defined(USE_GYRO_SPI_ICM42688P) +#if defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI270) #define GYRO_HARDWARE_LPF_EXPERIMENTAL 3 #else #define GYRO_HARDWARE_LPF_EXPERIMENTAL 1 diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index 5265e27342..f4b913736e 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -53,6 +53,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" @@ -404,6 +405,19 @@ static bool detectSPISensorsAndUpdateDetectionResult(gyroDev_t *gyro) { gyro->mpuDetectionResult.sensor = sensor; return true; } +#endif +#ifdef USE_ACCGYRO_BMI270 +#ifndef USE_DUAL_GYRO + spiBusSetInstance(&gyro->bus, BMI270_SPI_INSTANCE); +#endif +#ifdef BMI270_CS_PIN + gyro->bus.busdev_u.spi.csnPin = gyro->bus.busdev_u.spi.csnPin == IO_NONE ? IOGetByTag(IO_TAG(BMI270_CS_PIN)) : gyro->bus.busdev_u.spi.csnPin; +#endif + sensor = bmi270Detect(&gyro->bus); + if (sensor != MPU_NONE) { + gyro->mpuDetectionResult.sensor = sensor; + return true; + } #endif return false; } diff --git a/src/main/drivers/accgyro/accgyro_mpu.h b/src/main/drivers/accgyro/accgyro_mpu.h index aa0f770e29..b85e6d4ca4 100644 --- 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_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) + || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) #define GYRO_USES_SPI #endif @@ -219,6 +219,7 @@ typedef enum { ICM_42605_SPI, ICM_42688P_SPI, BMI_160_SPI, + BMI_270_SPI, IMUF_9001_SPI, } mpuSensor_e; diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.c b/src/main/drivers/accgyro/accgyro_spi_bmi270.c new file mode 100644 index 0000000000..b7e4749770 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.c @@ -0,0 +1,499 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include + +#include "platform.h" + +#ifdef USE_ACCGYRO_BMI270 + +#include "drivers/accgyro/accgyro.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" +#include "drivers/bus_spi.h" +#include "drivers/exti.h" +#include "drivers/io.h" +#include "drivers/io_impl.h" +#include "drivers/nvic.h" +#include "drivers/sensor.h" +#include "drivers/time.h" + +// 10 MHz max SPI frequency +#define BMI270_MAX_SPI_CLK_HZ 10000000 + +#define BMI270_FIFO_FRAME_SIZE 6 + +#define BMI270_CONFIG_SIZE 328 + +// Declaration for the device config (microcode) that must be uploaded to the sensor +const uint8_t bmi270_maximum_fifo_config_file[] = { + 0xc8, 0x2e, 0x00, 0x2e, 0x80, 0x2e, 0x1a, 0x00, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, + 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0xc8, 0x2e, 0x00, 0x2e, 0x90, 0x32, 0x21, 0x2e, 0x59, 0xf5, + 0x10, 0x30, 0x21, 0x2e, 0x6a, 0xf5, 0x1a, 0x24, 0x22, 0x00, 0x80, 0x2e, 0x3b, 0x00, 0xc8, 0x2e, 0x44, 0x47, 0x22, + 0x00, 0x37, 0x00, 0xa4, 0x00, 0xff, 0x0f, 0xd1, 0x00, 0x07, 0xad, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, + 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, + 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x80, 0x2e, 0x00, 0xc1, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x11, 0x24, 0xfc, 0xf5, 0x80, 0x30, 0x40, 0x42, 0x50, 0x50, 0x00, 0x30, 0x12, 0x24, 0xeb, + 0x00, 0x03, 0x30, 0x00, 0x2e, 0xc1, 0x86, 0x5a, 0x0e, 0xfb, 0x2f, 0x21, 0x2e, 0xfc, 0xf5, 0x13, 0x24, 0x63, 0xf5, + 0xe0, 0x3c, 0x48, 0x00, 0x22, 0x30, 0xf7, 0x80, 0xc2, 0x42, 0xe1, 0x7f, 0x3a, 0x25, 0xfc, 0x86, 0xf0, 0x7f, 0x41, + 0x33, 0x98, 0x2e, 0xc2, 0xc4, 0xd6, 0x6f, 0xf1, 0x30, 0xf1, 0x08, 0xc4, 0x6f, 0x11, 0x24, 0xff, 0x03, 0x12, 0x24, + 0x00, 0xfc, 0x61, 0x09, 0xa2, 0x08, 0x36, 0xbe, 0x2a, 0xb9, 0x13, 0x24, 0x38, 0x00, 0x64, 0xbb, 0xd1, 0xbe, 0x94, + 0x0a, 0x71, 0x08, 0xd5, 0x42, 0x21, 0xbd, 0x91, 0xbc, 0xd2, 0x42, 0xc1, 0x42, 0x00, 0xb2, 0xfe, 0x82, 0x05, 0x2f, + 0x50, 0x30, 0x21, 0x2e, 0x21, 0xf2, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0xf0, 0x6f, 0x02, 0x30, 0x02, 0x42, 0x20, + 0x26, 0xe0, 0x6f, 0x02, 0x31, 0x03, 0x40, 0x9a, 0x0a, 0x02, 0x42, 0xf0, 0x37, 0x05, 0x2e, 0x5e, 0xf7, 0x10, 0x08, + 0x12, 0x24, 0x1e, 0xf2, 0x80, 0x42, 0x83, 0x84, 0xf1, 0x7f, 0x0a, 0x25, 0x13, 0x30, 0x83, 0x42, 0x3b, 0x82, 0xf0, + 0x6f, 0x00, 0x2e, 0x00, 0x2e, 0xd0, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x00, 0x2e, 0x12, 0x40, 0x52, 0x42, 0x3e, 0x84, + 0x00, 0x40, 0x40, 0x42, 0x7e, 0x82, 0xe1, 0x7f, 0xf2, 0x7f, 0x98, 0x2e, 0x6a, 0xd6, 0x21, 0x30, 0x23, 0x2e, 0x61, + 0xf5, 0xeb, 0x2c, 0xe1, 0x6f +}; + +#define BMI270_CHIP_ID 0x24 + +// BMI270 registers (not the complete list) +typedef enum { + BMI270_REG_CHIP_ID = 0x00, + BMI270_REG_ERR_REG = 0x02, + BMI270_REG_STATUS = 0x03, + BMI270_REG_ACC_DATA_X_LSB = 0x0C, + BMI270_REG_GYR_DATA_X_LSB = 0x12, + BMI270_REG_SENSORTIME_0 = 0x18, + BMI270_REG_SENSORTIME_1 = 0x19, + BMI270_REG_SENSORTIME_2 = 0x1A, + BMI270_REG_EVENT = 0x1B, + BMI270_REG_INT_STATUS_0 = 0x1C, + BMI270_REG_INT_STATUS_1 = 0x1D, + BMI270_REG_INTERNAL_STATUS = 0x21, + BMI270_REG_TEMPERATURE_LSB = 0x22, + BMI270_REG_TEMPERATURE_MSB = 0x23, + BMI270_REG_FIFO_LENGTH_LSB = 0x24, + BMI270_REG_FIFO_LENGTH_MSB = 0x25, + BMI270_REG_FIFO_DATA = 0x26, + BMI270_REG_ACC_CONF = 0x40, + BMI270_REG_ACC_RANGE = 0x41, + BMI270_REG_GYRO_CONF = 0x42, + BMI270_REG_GYRO_RANGE = 0x43, + BMI270_REG_AUX_CONF = 0x44, + BMI270_REG_FIFO_DOWNS = 0x45, + BMI270_REG_FIFO_WTM_0 = 0x46, + BMI270_REG_FIFO_WTM_1 = 0x47, + BMI270_REG_FIFO_CONFIG_0 = 0x48, + BMI270_REG_FIFO_CONFIG_1 = 0x49, + BMI270_REG_SATURATION = 0x4A, + BMI270_REG_INT1_IO_CTRL = 0x53, + BMI270_REG_INT2_IO_CTRL = 0x54, + BMI270_REG_INT_LATCH = 0x55, + BMI270_REG_INT1_MAP_FEAT = 0x56, + BMI270_REG_INT2_MAP_FEAT = 0x57, + BMI270_REG_INT_MAP_DATA = 0x58, + BMI270_REG_INIT_CTRL = 0x59, + BMI270_REG_INIT_DATA = 0x5E, + BMI270_REG_ACC_SELF_TEST = 0x6D, + BMI270_REG_GYR_SELF_TEST_AXES = 0x6E, + BMI270_REG_PWR_CONF = 0x7C, + BMI270_REG_PWR_CTRL = 0x7D, + BMI270_REG_CMD = 0x7E, +} bmi270Register_e; + +// BMI270 register configuration values +typedef enum { + BMI270_VAL_CMD_SOFTRESET = 0xB6, + BMI270_VAL_CMD_FIFOFLUSH = 0xB0, + BMI270_VAL_PWR_CTRL = 0x0E, // enable gyro, acc and temp sensors + BMI270_VAL_PWR_CONF = 0x02, // disable advanced power save, enable FIFO self-wake + BMI270_VAL_ACC_CONF_ODR800 = 0x0B, // set acc sample rate to 800hz + BMI270_VAL_ACC_CONF_ODR1600 = 0x0C, // set acc sample rate to 1600hz + BMI270_VAL_ACC_CONF_BWP = 0x02, // set acc filter in normal mode + BMI270_VAL_ACC_CONF_HP = 0x01, // set acc in high performance mode + BMI270_VAL_ACC_RANGE_8G = 0x02, // set acc to 8G full scale + BMI270_VAL_ACC_RANGE_16G = 0x03, // set acc to 16G full scale + BMI270_VAL_GYRO_CONF_ODR3200 = 0x0D, // set gyro sample rate to 3200hz + BMI270_VAL_GYRO_CONF_BWP_OSR4 = 0x00, // set gyro filter in OSR4 mode + BMI270_VAL_GYRO_CONF_BWP_OSR2 = 0x01, // set gyro filter in OSR2 mode + BMI270_VAL_GYRO_CONF_BWP_NORM = 0x02, // set gyro filter in normal mode + BMI270_VAL_GYRO_CONF_NOISE_PERF = 0x01, // set gyro in high performance noise mode + BMI270_VAL_GYRO_CONF_FILTER_PERF = 0x01, // set gyro in high performance filter mode + + BMI270_VAL_GYRO_RANGE_2000DPS = 0x08, // set gyro to 2000dps full scale + // for some reason you have to enable the ois_range bit (bit 3) for 2000dps as well + // or else the gyro scale will be 250dps when in prefiltered FIFO mode (not documented in datasheet!) + + BMI270_VAL_INT_MAP_DATA_DRDY_INT1 = 0x04,// enable the data ready interrupt pin 1 + BMI270_VAL_INT_MAP_FIFO_WM_INT1 = 0x02, // enable the FIFO watermark interrupt pin 1 + BMI270_VAL_INT1_IO_CTRL_PINMODE = 0x0A, // active high, push-pull, output enabled, input disabled + BMI270_VAL_FIFO_CONFIG_0 = 0x00, // don't stop when full, disable sensortime frame + BMI270_VAL_FIFO_CONFIG_1 = 0x80, // only gyro data in FIFO, use headerless mode + BMI270_VAL_FIFO_DOWNS = 0x00, // select unfiltered gyro data with no downsampling (6.4KHz samples) + BMI270_VAL_FIFO_WTM_0 = 0x06, // set the FIFO watermark level to 1 gyro sample (6 bytes) + BMI270_VAL_FIFO_WTM_1 = 0x00, // FIFO watermark MSB +} bmi270ConfigValues_e; + +// BMI270 register reads are 16bits with the first byte a "dummy" value 0 +// that must be ignored. The result is in the second byte. +static uint8_t bmi270RegisterRead(const busDevice_t *bus, bmi270Register_e registerId) +{ + uint8_t data[2] = { 0, 0 }; + + if (spiBusReadRegisterBuffer(bus, registerId, data, 2)) { + return data[1]; + } else { + return 0; + } +} + +static void bmi270RegisterWrite(const busDevice_t *bus, bmi270Register_e registerId, uint8_t value, unsigned delayMs) +{ + spiBusWriteRegister(bus, registerId, value); + if (delayMs) { + delay(delayMs); + } +} + +// Toggle the CS to switch the device into SPI mode. +// Device switches initializes as I2C and switches to SPI on a low to high CS transition +static void bmi270EnableSPI(const busDevice_t *bus) +{ + IOLo(bus->busdev_u.spi.csnPin); + delay(1); + IOHi(bus->busdev_u.spi.csnPin); + delay(10); +} + +uint8_t bmi270Detect(const busDevice_t *bus) +{ +#ifndef USE_DUAL_GYRO + IOInit(bus->busdev_u.spi.csnPin, OWNER_MPU_CS, 0); + IOConfigGPIO(bus->busdev_u.spi.csnPin, SPI_IO_CS_CFG); + IOHi(bus->busdev_u.spi.csnPin); +#endif + + spiSetDivisor(bus->busdev_u.spi.instance, spiCalculateDivider(BMI270_MAX_SPI_CLK_HZ)); + bmi270EnableSPI(bus); + + if (bmi270RegisterRead(bus, BMI270_REG_CHIP_ID) == BMI270_CHIP_ID) { + return BMI_270_SPI; + } + + return MPU_NONE; +} + +static void bmi270UploadConfig(const busDevice_t *bus) +{ + bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, 0, 1); + bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 0, 1); + + // Transfer the config file + IOLo(bus->busdev_u.spi.csnPin); + spiTransferByte(bus->busdev_u.spi.instance, BMI270_REG_INIT_DATA); + spiTransfer(bus->busdev_u.spi.instance, bmi270_maximum_fifo_config_file, NULL, sizeof(bmi270_maximum_fifo_config_file)); + IOHi(bus->busdev_u.spi.csnPin); + + delay(10); + bmi270RegisterWrite(bus, BMI270_REG_INIT_CTRL, 1, 1); +} + +static uint8_t getBmiOsrMode() +{ + switch(gyroConfig()->gyro_hardware_lpf) { + case GYRO_HARDWARE_LPF_NORMAL: + return BMI270_VAL_GYRO_CONF_BWP_OSR4; + case GYRO_HARDWARE_LPF_EXPERIMENTAL: + return BMI270_VAL_GYRO_CONF_BWP_OSR2; + } + return 0; +} + +static void bmi270Config(const gyroDev_t *gyro) +{ + const busDevice_t *bus = &gyro->bus; + + // If running in hardware_lpf experimental mode then switch to FIFO-based, + // 6.4KHz sampling, unfiltered data vs. the default 3.2KHz with hardware filtering +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + const bool fifoMode = (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL); +#else + const bool fifoMode = false; +#endif + + // Perform a soft reset to set all configuration to default + // Delay 100ms before continuing configuration + bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_SOFTRESET, 100); + + // Toggle the chip into SPI mode + bmi270EnableSPI(bus); + + bmi270UploadConfig(bus); + + // Configure the FIFO + if (fifoMode) { + bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_0, BMI270_VAL_FIFO_CONFIG_0, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_CONFIG_1, BMI270_VAL_FIFO_CONFIG_1, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_DOWNS, BMI270_VAL_FIFO_DOWNS, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_0, BMI270_VAL_FIFO_WTM_0, 1); + bmi270RegisterWrite(bus, BMI270_REG_FIFO_WTM_1, BMI270_VAL_FIFO_WTM_1, 1); + } + + // Configure the accelerometer + bmi270RegisterWrite(bus, BMI270_REG_ACC_CONF, (BMI270_VAL_ACC_CONF_HP << 7) | (BMI270_VAL_ACC_CONF_BWP << 4) | BMI270_VAL_ACC_CONF_ODR800, 1); + + // Configure the accelerometer full-scale range + bmi270RegisterWrite(bus, BMI270_REG_ACC_RANGE, BMI270_VAL_ACC_RANGE_16G, 1); + + // Configure the gyro + bmi270RegisterWrite(bus, BMI270_REG_GYRO_CONF, (BMI270_VAL_GYRO_CONF_FILTER_PERF << 7) | (BMI270_VAL_GYRO_CONF_NOISE_PERF << 6) | (getBmiOsrMode() << 4) | BMI270_VAL_GYRO_CONF_ODR3200, 1); + + // Configure the gyro full-range scale + bmi270RegisterWrite(bus, BMI270_REG_GYRO_RANGE, BMI270_VAL_GYRO_RANGE_2000DPS, 1); + + // Configure the gyro data ready interrupt + if (fifoMode) { + // Interrupt driven by FIFO watermark level + bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_FIFO_WM_INT1, 1); + } else { + // Interrupt driven by data ready + bmi270RegisterWrite(bus, BMI270_REG_INT_MAP_DATA, BMI270_VAL_INT_MAP_DATA_DRDY_INT1, 1); + } + + // Configure the behavior of the INT1 pin + bmi270RegisterWrite(bus, BMI270_REG_INT1_IO_CTRL, BMI270_VAL_INT1_IO_CTRL_PINMODE, 1); + + // Configure the device for performance mode + bmi270RegisterWrite(bus, BMI270_REG_PWR_CONF, BMI270_VAL_PWR_CONF, 1); + + // Enable the gyro, accelerometer and temperature sensor - disable aux interface + bmi270RegisterWrite(bus, BMI270_REG_PWR_CTRL, BMI270_VAL_PWR_CTRL, 1); + + // Flush the FIFO + if (fifoMode) { + bmi270RegisterWrite(bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 1); + } +} + +extiCallbackRec_t bmi270IntCallbackRec; + +#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) +void bmi270ExtiHandler(extiCallbackRec_t *cb) +{ + gyroDev_t *gyro = container_of(cb, gyroDev_t, exti); + gyro->dataReady = true; +} + +static void bmi270IntExtiInit(gyroDev_t *gyro) +{ + if (gyro->mpuIntExtiTag == IO_TAG_NONE) { + return; + } + + IO_t mpuIntIO = IOGetByTag(gyro->mpuIntExtiTag); + + IOInit(mpuIntIO, OWNER_MPU_EXTI, 0); + EXTIHandlerInit(&gyro->exti, bmi270ExtiHandler); + EXTIConfig(mpuIntIO, &gyro->exti, NVIC_PRIO_MPU_INT_EXTI, IOCFG_IN_FLOATING ); + EXTIEnable(mpuIntIO, true); +} +#endif + +static bool bmi270AccRead(accDev_t *acc) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_ACCEL_XOUT_L, + IDX_ACCEL_XOUT_H, + IDX_ACCEL_YOUT_L, + IDX_ACCEL_YOUT_H, + IDX_ACCEL_ZOUT_L, + IDX_ACCEL_ZOUT_H, + BUFFER_SIZE, + }; + + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_ACC_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; + + IOLo(acc->bus.busdev_u.spi.csnPin); + spiTransfer(acc->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(acc->bus.busdev_u.spi.csnPin); + + acc->ADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_XOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_XOUT_L]); + acc->ADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_YOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_YOUT_L]); + acc->ADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_ACCEL_ZOUT_H] << 8) | bmi270_rx_buf[IDX_ACCEL_ZOUT_L]); + + return true; +} + +static bool bmi270GyroReadRegister(gyroDev_t *gyro) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; + + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_GYR_DATA_X_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0}; + + IOLo(gyro->bus.busdev_u.spi.csnPin); + spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(gyro->bus.busdev_u.spi.csnPin); + + gyro->gyroADCRaw[X] = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); + gyro->gyroADCRaw[Y] = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); + gyro->gyroADCRaw[Z] = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); + + return true; +} + +#ifdef USE_GYRO_DLPF_EXPERIMENTAL +static bool bmi270GyroReadFifo(gyroDev_t *gyro) +{ + enum { + IDX_REG = 0, + IDX_SKIP, + IDX_FIFO_LENGTH_L, + IDX_FIFO_LENGTH_H, + IDX_GYRO_XOUT_L, + IDX_GYRO_XOUT_H, + IDX_GYRO_YOUT_L, + IDX_GYRO_YOUT_H, + IDX_GYRO_ZOUT_L, + IDX_GYRO_ZOUT_H, + BUFFER_SIZE, + }; + + bool dataRead = false; + static const uint8_t bmi270_tx_buf[BUFFER_SIZE] = {BMI270_REG_FIFO_LENGTH_LSB | 0x80, 0, 0, 0, 0, 0, 0, 0, 0, 0}; + uint8_t bmi270_rx_buf[BUFFER_SIZE]; + + // Burst read the FIFO length followed by the next 6 bytes containing the gyro axis data for + // the first sample in the queue. It's possible for the FIFO to be empty so we need to check the + // length before using the sample. + IOLo(gyro->bus.busdev_u.spi.csnPin); + spiTransfer(gyro->bus.busdev_u.spi.instance, bmi270_tx_buf, bmi270_rx_buf, BUFFER_SIZE); // receive response + IOHi(gyro->bus.busdev_u.spi.csnPin); + + int fifoLength = (uint16_t)((bmi270_rx_buf[IDX_FIFO_LENGTH_H] << 8) | bmi270_rx_buf[IDX_FIFO_LENGTH_L]); + + if (fifoLength >= BMI270_FIFO_FRAME_SIZE) { + + const int16_t gyroX = (int16_t)((bmi270_rx_buf[IDX_GYRO_XOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_XOUT_L]); + const int16_t gyroY = (int16_t)((bmi270_rx_buf[IDX_GYRO_YOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_YOUT_L]); + const int16_t gyroZ = (int16_t)((bmi270_rx_buf[IDX_GYRO_ZOUT_H] << 8) | bmi270_rx_buf[IDX_GYRO_ZOUT_L]); + + // If the FIFO data is invalid then the returned values will be 0x8000 (-32768) (pg. 43 of datasheet). + // This shouldn't happen since we're only using the data if the FIFO length indicates + // that data is available, but this safeguard is needed to prevent bad things in + // case it does happen. + if ((gyroX != INT16_MIN) || (gyroY != INT16_MIN) || (gyroZ != INT16_MIN)) { + gyro->gyroADCRaw[X] = gyroX; + gyro->gyroADCRaw[Y] = gyroY; + gyro->gyroADCRaw[Z] = gyroZ; + dataRead = true; + } + fifoLength -= BMI270_FIFO_FRAME_SIZE; + } + + // If there are additional samples in the FIFO then we don't use those for now and simply + // flush the FIFO. Under normal circumstances we only expect one sample in the FIFO since + // the gyro loop is running at the native sample rate of 6.4KHz. + // However the way the FIFO works in the sensor is that if a frame is partially read then + // it remains in the queue instead of bein removed. So if we ever got into a state where there + // was a partial frame or other unexpected data in the FIFO is may never get cleared and we + // would end up in a lock state of always re-reading the same partial or invalid sample. + if (fifoLength > 0) { + // Partial or additional frames left - flush the FIFO + bmi270RegisterWrite(&gyro->bus, BMI270_REG_CMD, BMI270_VAL_CMD_FIFOFLUSH, 0); + } + + return dataRead; +} +#endif + +static bool bmi270GyroRead(gyroDev_t *gyro) +{ +#ifdef USE_GYRO_DLPF_EXPERIMENTAL + if (gyro->hardware_lpf == GYRO_HARDWARE_LPF_EXPERIMENTAL) { + // running in 6.4KHz FIFO mode + return bmi270GyroReadFifo(gyro); + } else +#endif + { + // running in 3.2KHz register mode + return bmi270GyroReadRegister(gyro); + } +} + +static void bmi270SpiGyroInit(gyroDev_t *gyro) +{ + bmi270Config(gyro); + +#if defined(USE_GYRO_EXTI) && defined(USE_MPU_DATA_READY_SIGNAL) + bmi270IntExtiInit(gyro); +#endif +} + +static void bmi270SpiAccInit(accDev_t *acc) +{ + // sensor is configured during gyro init + acc->acc_1G = 512 * 4; // 16G sensor scale +} + +bool bmi270SpiAccDetect(accDev_t *acc) +{ + if (acc->mpuDetectionResult.sensor != BMI_270_SPI) { + return false; + } + + acc->initFn = bmi270SpiAccInit; + acc->readFn = bmi270AccRead; + + return true; +} + + +bool bmi270SpiGyroDetect(gyroDev_t *gyro) +{ + if (gyro->mpuDetectionResult.sensor != BMI_270_SPI) { + return false; + } + + gyro->initFn = bmi270SpiGyroInit; + gyro->readFn = bmi270GyroRead; + gyro->scale = GYRO_SCALE_2000DPS; + + return true; +} + +// Used to query the status register to determine what event caused the EXTI to fire. +// When in 3.2KHz mode the interrupt is mapped to the data ready state. However the data ready +// trigger will fire for both gyro and accelerometer. So it's necessary to check this register +// to determine which event caused the interrupt. +// When in 6.4KHz mode the interrupt is configured to be the FIFO watermark size of 6 bytes. +// Since in this mode we only put gyro data in the FIFO it's sufficient to check for the FIFO +// watermark reason as an idication of gyro data ready. +uint8_t bmi270InterruptStatus(gyroDev_t *gyro) +{ + return bmi270RegisterRead(&gyro->bus, BMI270_REG_INT_STATUS_1); +} +#endif // USE_ACCGYRO_BMI270 diff --git a/src/main/drivers/accgyro/accgyro_spi_bmi270.h b/src/main/drivers/accgyro/accgyro_spi_bmi270.h new file mode 100644 index 0000000000..c0ea0495fa --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_spi_bmi270.h @@ -0,0 +1,28 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "drivers/bus.h" + +uint8_t bmi270Detect(const busDevice_t *bus); +bool bmi270SpiAccDetect(accDev_t *acc); +bool bmi270SpiGyroDetect(gyroDev_t *gyro); +uint8_t bmi270InterruptStatus(gyroDev_t *gyro); diff --git a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c index 24003f9017..0b284fcee7 100644 --- a/src/main/drivers/accgyro/accgyro_spi_icm426xx.c +++ b/src/main/drivers/accgyro/accgyro_spi_icm426xx.c @@ -48,6 +48,11 @@ #define ICM426XX_BANK_SELECT3 0x03 #define ICM426XX_BANK_SELECT4 0x04 +// Fix for stalls in gyro output. See https://github.com/ArduPilot/ardupilot/pull/25332 ; https://github.com/betaflight/betaflight/pull/13132 +#define ICM426XX_INTF_CONFIG1 0x4D +#define ICM426XX_INTF_CONFIG1_AFSR_MASK 0xC0 +#define ICM426XX_INTF_CONFIG1_AFSR_DISABLE 0x40 + #define ICM426XX_RA_PWR_MGMT0 0x4E // User Bank 0 #define ICM426XX_PWR_MGMT0_ACCEL_MODE_LN (3 << 0) #define ICM426XX_PWR_MGMT0_GYRO_MODE_LN (3 << 2) @@ -284,13 +289,20 @@ void icm426xxGyroInit(gyroDev_t *gyro) spiBusWriteRegister(&gyro->bus, ICM426XX_RA_INT_SOURCE0, ICM426XX_UI_DRDY_INT1_EN_ENABLED); - uint8_t intConfig1Value = spiReadRegMsk(&gyro->bus, ICM426XX_RA_INT_CONFIG1); + uint8_t intConfig1Value = spiBusReadRegister(&gyro->bus, ICM426XX_RA_INT_CONFIG1); // Datasheet says: "User should change setting to 0 from default setting of 1, for proper INT1 and INT2 pin operation" intConfig1Value &= ~(1 << ICM426XX_INT_ASYNC_RESET_BIT); intConfig1Value |= (ICM426XX_INT_TPULSE_DURATION_8 | ICM426XX_INT_TDEASSERT_DISABLED); spiBusWriteRegister(&gyro->bus, ICM426XX_RA_INT_CONFIG1, intConfig1Value); + // Disable AFSR to prevent stalls in gyro output + // ICM426XX_INTF_CONFIG1 location in user bank 0 + uint8_t intfConfig1Value = spiBusReadRegister(&gyro->bus, ICM426XX_INTF_CONFIG1); + intfConfig1Value &= ~ICM426XX_INTF_CONFIG1_AFSR_MASK; + intfConfig1Value |= ICM426XX_INTF_CONFIG1_AFSR_DISABLE; + spiBusWriteRegister(&gyro->bus, ICM426XX_INTF_CONFIG1, intfConfig1Value); + // Turn on gyro and acc on again so ODR and FSR can be configured turnGyroAccOn(gyro); diff --git a/src/main/drivers/accgyro/gyro_sync.c b/src/main/drivers/accgyro/gyro_sync.c index bc9ad37f27..3653c24d43 100644 --- a/src/main/drivers/accgyro/gyro_sync.c +++ b/src/main/drivers/accgyro/gyro_sync.c @@ -53,16 +53,24 @@ uint32_t gyroSetSampleRate(gyroDev_t *gyro, uint8_t lpf, uint8_t gyroSyncDenomin } gyro->mpuDividerDrops = gyroSyncDenominator - 1; gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_8_kHz : GYRO_RATE_1_kHz; - //20649 is a weird gyro - if (gyro->mpuDetectionResult.sensor == ICM_20649_SPI) { - gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_9_kHz : GYRO_RATE_1100_Hz; - } else if (gyro->mpuDetectionResult.sensor == BMI_160_SPI && lpfNoneOr256) { - //brainFPV is also a weird gyro - gyro->gyroRateKHz = GYRO_RATE_3200_Hz; - } else if (gyro_use_32khz) { - //use full 32k - gyro->gyroRateKHz = GYRO_RATE_32_kHz; + + switch (gyro->mpuDetectionResult.sensor) { + case ICM_20649_SPI: //20649 is a weird gyro + gyro->gyroRateKHz = lpfNoneOr256 ? GYRO_RATE_9_kHz : GYRO_RATE_1100_Hz; + break; + case BMI_160_SPI: //brainFPV is also a weird gyro + if (lpfNoneOr256) { gyro->gyroRateKHz = GYRO_RATE_3200_Hz; } + break; + case BMI_270_SPI: //bmi270 + gyro->gyroRateKHz = GYRO_RATE_3200_Hz; + break; + default: + if (gyro_use_32khz) { + //use full 32k + gyro->gyroRateKHz = GYRO_RATE_32_kHz; + } } + // return the targetLooptime (expected cycleTime) return (uint32_t)(gyroSyncDenominator * gyro->gyroRateKHz); } diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index d42f0e6156..71a62b210b 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -252,6 +252,7 @@ void spiBusSetInstance(busDevice_t *bus, SPI_TypeDef *instance) { bus->busdev_u.spi.instance = instance; } +// icm42688p and bmi270 porting uint16_t spiCalculateDivider(uint32_t freq) { #if defined(STM32F4) || defined(STM32G4) || defined(STM32F7) @@ -272,7 +273,8 @@ uint16_t spiCalculateDivider(uint32_t freq) return divisor; } -// Wait for bus to become free, then read a byte of data where the register is ORed with 0x80 +// Wait for bus to become free, then read a byte of data where the register is bitwise OR'ed with 0x80 +// EmuFlight codebase is old. Bitwise or 0x80 is redundant here as spiBusReadRegister already contains such. uint8_t spiReadRegMsk(const busDevice_t *bus, uint8_t reg) { return spiBusReadRegister(bus, reg | 0x80); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 5ccab8f8b5..37ca42323d 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -434,6 +434,7 @@ void validateAndFixGyroConfig(void) { samplingTime = 1.0f / 9000.0f; break; case BMI_160_SPI: + case BMI_270_SPI: samplingTime = 0.0003125f; break; default: diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 19b51d18f7..be511b02f7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -86,7 +86,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2); #ifdef STM32F10X #define PID_PROCESS_DENOM_DEFAULT 1 -#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) +#elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) #define PID_PROCESS_DENOM_DEFAULT 1 #else #define PID_PROCESS_DENOM_DEFAULT 2 @@ -235,7 +235,9 @@ static FAST_RAM filterApplyFnPtr angleSetpointFilterApplyFn = nullFilterApply; static FAST_RAM_ZERO_INIT pt1Filter_t angleSetpointFilter[2]; static FAST_RAM filterApplyFnPtr dtermABGapplyFn = nullFilterApply; static FAST_RAM_ZERO_INIT alphaBetaGammaFilter_t dtermABG[XYZ_AXIS_COUNT]; +#ifdef USE_GYRO_DATA_ANALYSE static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT][5]; +#endif #if defined(USE_ITERM_RELAX) static FAST_RAM_ZERO_INIT pt1Filter_t windupLpf[XYZ_AXIS_COUNT]; @@ -671,7 +673,9 @@ static FAST_RAM_ZERO_INIT float stickMovement[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float lastRcDeflectionAbs[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float previousError[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT float previousMeasurement[XYZ_AXIS_COUNT]; +#ifdef USE_GYRO_DATA_ANALYSE static FAST_RAM_ZERO_INIT float previousNotchCenterFreq[XYZ_AXIS_COUNT][5]; +#endif static FAST_RAM_ZERO_INIT timeUs_t crashDetectedAtUs; void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) { @@ -811,7 +815,7 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an float dDelta = ((feathered_pids * pureMeasurement) + ((1 - feathered_pids) * pureError)) * pidFrequency; //calculating the dterm determine how much is calculated using measurement vs error //filter the dterm #ifdef USE_GYRO_DATA_ANALYSE - if (isDynamicFilterActive() && pidProfile->dtermDynNotch) { + if (isDynamicFilterActive() && pidProfile->dtermDynNotch && axis <= gyroConfig()->dyn_notch_axis+1) { for (int p = 0; p < gyroConfig()->dyn_notch_count; p++) { if (getCenterFreq(axis, p) != previousNotchCenterFreq[axis][p]) { previousNotchCenterFreq[axis][p] = getCenterFreq(axis, p); diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index b3aebb427d..684b85e213 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1194,20 +1194,37 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) { sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz[ROLL]); sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz[PITCH]); sbufWriteU16(dst, gyroConfig()->gyro_lowpass_hz[YAW]); +#ifdef USE_GYRO_LPF2 sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz[ROLL]); sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz[PITCH]); sbufWriteU16(dst, gyroConfig()->gyro_lowpass2_hz[YAW]); +#else + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); +#endif sbufWriteU8(dst, gyroConfig()->gyro_lowpass_type); +#ifdef USE_GYRO_LPF2 sbufWriteU8(dst, gyroConfig()->gyro_lowpass2_type); +#else + sbufWriteU8(dst, 0); +#endif sbufWriteU16(dst, currentPidProfile->dFilter[ROLL].dLpf2); sbufWriteU16(dst, currentPidProfile->dFilter[PITCH].dLpf2); sbufWriteU16(dst, currentPidProfile->dFilter[YAW].dLpf2); //MSP 1.51 removes SmartDTermSmoothing and WitchCraft //MSP 1.51 adds and refactors dynamic_filter +#ifdef USE_GYRO_DATA_ANALYSE sbufWriteU8(dst, gyroConfig()->dyn_notch_count); //dynamic_gyro_notch_count sbufWriteU16(dst, gyroConfig()->dyn_notch_q); sbufWriteU16(dst, gyroConfig()->dyn_notch_min_hz); sbufWriteU16(dst, gyroConfig()->dyn_notch_max_hz); //dynamic_gyro_notch_max_hz +#else + sbufWriteU8(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); +#endif //end MSP 1.51 add/refactor dynamic filter //MSP 1.51 sbufWriteU16(dst, gyroConfig()->gyro_ABG_alpha); @@ -1219,8 +1236,13 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) { sbufWriteU8(dst, currentPidProfile->dterm_ABG_half_life); //end MSP 1.51 //MSP 1.51 dynamic dTerm notch +#ifdef USE_GYRO_DATA_ANALYSE sbufWriteU8(dst, currentPidProfile->dtermDynNotch); //dterm_dyn_notch_enable sbufWriteU16(dst, currentPidProfile->dterm_dyn_notch_q); //dterm_dyn_notch_q +#else + sbufWriteU8(dst, 0); + sbufWriteU16(dst, 0); +#endif //end MSP 1.51 dynamic dTerm notch break; /*#ifndef USE_GYRO_IMUF9001 diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 9634ec73c8..9cedcd3a45 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -109,13 +109,13 @@ const char * const lookupTableAccHardware[] = { "AUTO", "NONE", "ADXL345", "MPU6050", "MMA8452", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", - "ICM42605", "ICM42688P", "BMI160", "ACC_IMUF9001", "FAKE" + "ICM42605", "ICM42688P", "BMI160", "BMI270", "ACC_IMUF9001", "FAKE" }; // sync with gyroSensor_e const char * const lookupTableGyroHardware[] = { "AUTO", "NONE", "MPU6050", "L3G4200D", "MPU3050", "L3GD20", "MPU6000", "MPU6500", "MPU9250", "ICM20601", "ICM20602", "ICM20608G", "ICM20649", "ICM20689", - "ICM42605", "ICM42688P", "BMI160", "GYRO_IMUF9001", "FAKE" + "ICM42605", "ICM42688P", "BMI160", "BMI270", "GYRO_IMUF9001", "FAKE" }; #if defined(USE_SENSOR_NAMES) || defined(USE_BARO) @@ -252,10 +252,10 @@ static const char * const lookupTableRxSpi[] = { static const char * const lookupTableGyroHardwareLpf[] = { "NORMAL", -#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) +#if defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI270) "OPTION1", "OPTION2", -#if defined(USE_GYRO_SPI_ICM42688P) && defined(USE_GYRO_DLPF_EXPERIMENTAL) // will need bmi270 logic as well +#if ( defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI270) ) && defined(USE_GYRO_DLPF_EXPERIMENTAL) // icm42688p & bmi270 experimental "EXPERIMENTAL", #endif #else @@ -293,6 +293,7 @@ static const char * const lookupTableRcInterpolation[] = { static const char * const lookupTableRcInterpolationChannels[] = { "RP", "RPY", "RPYT", "T", "RPT", }; + static const char * const lookupTableFilterType[] = { "PT1", "BIQUAD", "PT2", "PT3", "PT4", }; @@ -393,6 +394,10 @@ static const char *const lookupTableMixerImplType[] = { "LEGACY", "SMOOTH", "2PASS" }; +static const char *const lookupTableDynNotchAxisType[] = { + "RP", "RPY" +}; + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -482,6 +487,7 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableOsdLogoOnArming), #endif LOOKUP_TABLE_ENTRY(lookupTableMixerImplType), + LOOKUP_TABLE_ENTRY(lookupTableDynNotchAxisType), }; #undef LOOKUP_TABLE_ENTRY @@ -503,10 +509,12 @@ const clivalue_t valueTable[] = { { "gyro_lowpass_hz_pitch", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz[PITCH]) }, { "gyro_lowpass_hz_yaw", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass_hz[YAW]) }, +#ifdef USE_GYRO_LPF2 { "gyro_lowpass2_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_FILTER_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_type) }, { "gyro_lowpass2_hz_roll", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz[ROLL]) }, { "gyro_lowpass2_hz_pitch", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz[PITCH]) }, { "gyro_lowpass2_hz_yaw", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 16000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_lowpass2_hz[YAW]) }, +#endif { "gyro_abg_alpha", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_ABG_alpha) }, { "gyro_abg_boost", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, 2000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_ABG_boost) }, @@ -553,6 +561,7 @@ const clivalue_t valueTable[] = { { "gyro_to_use", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) }, #endif #if defined(USE_GYRO_DATA_ANALYSE) + { "dynamic_gyro_notch_axis", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_DYN_NOTCH_AXIS_TYPE }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_axis) }, { "dynamic_gyro_notch_q", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 1, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_q) }, { "dynamic_gyro_notch_count", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 5 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_count) }, { "dynamic_gyro_notch_min_hz", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 30, 1000 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, dyn_notch_min_hz) }, @@ -863,9 +872,10 @@ const clivalue_t valueTable[] = { { "runaway_takeoff_deactivate_throttle_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_throttle) }, // minimum throttle percentage during deactivation phase #endif // PG_PID_PROFILE +#ifdef USE_GYRO_DATA_ANALYSE { "dterm_dyn_notch_enable", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermDynNotch) }, { "dterm_dyn_notch_q", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_dyn_notch_q) }, - +#endif { "dterm_abg_alpha", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_alpha) }, { "dterm_abg_boost", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_boost) }, { "dterm_abg_half_life", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_ABG_half_life) }, diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index f351a2aa12..e8a8269834 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -112,6 +112,7 @@ typedef enum { TABLE_OSD_LOGO_ON_ARMING, #endif TABLE_MIXER_IMPL_TYPE, + TABLE_DYN_NOTCH_AXIS_TYPE, LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/pg/bus_spi.c b/src/main/pg/bus_spi.c index 3b1fcfa7a8..e8e50fc84f 100644 --- a/src/main/pg/bus_spi.c +++ b/src/main/pg/bus_spi.c @@ -100,6 +100,9 @@ ioTag_t preinitIPUList[SPI_PREINIT_IPU_COUNT] = { #ifdef BMI160_CS_PIN IO_TAG(BMI160_CS_PIN), #endif +#ifdef BMI270_CS_PIN + IO_TAG(BMI270_CS_PIN), +#endif #ifdef L3GD20_CS_PIN IO_TAG(L3GD20_CS_PIN), #endif diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 62460a182c..6a45d357b4 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -43,6 +43,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" @@ -299,6 +300,17 @@ bool accDetect(accDev_t *dev, accelerationSensor_e accHardwareToUse) { } FALLTHROUGH; #endif +#ifdef USE_ACCGYRO_BMI270 + case ACC_BMI270: + if (bmi270SpiAccDetect(dev)) { + accHardware = ACC_BMI270; +#ifdef ACC_BMI270_ALIGN + dev->accAlign = ACC_BMI270_ALIGN; +#endif + break; + } + FALLTHROUGH; +#endif #if defined(USE_ACC_SPI_ICM42605) || defined(USE_ACC_SPI_ICM42688P) case ACC_ICM42605: case ACC_ICM42688P: diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index b448d2419b..5b132c53f6 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -50,6 +50,7 @@ typedef enum { ACC_ICM42605, ACC_ICM42688P, ACC_BMI160, + ACC_BMI270, ACC_IMUF9001, ACC_FAKE } accelerationSensor_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b77528e9fd..9ec6a642fc 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -44,6 +44,7 @@ #include "drivers/accgyro/accgyro_mpu6050.h" #include "drivers/accgyro/accgyro_mpu6500.h" #include "drivers/accgyro/accgyro_spi_bmi160.h" +#include "drivers/accgyro/accgyro_spi_bmi270.h" #include "drivers/accgyro/accgyro_spi_icm20649.h" #include "drivers/accgyro/accgyro_spi_icm20689.h" #include "drivers/accgyro/accgyro_spi_icm426xx.h" @@ -158,8 +159,10 @@ typedef struct gyroSensor_s { gyroLowpassFilter_t lowpassFilter[XYZ_AXIS_COUNT]; // lowpass2 gyro soft filter +#ifdef USE_GYRO_LPF2 filterApplyFnPtr lowpass2FilterApplyFn; gyroLowpassFilter_t lowpass2Filter[XYZ_AXIS_COUNT]; +#endif // ABG filter filterApplyFnPtr gyroABGFilterApplyFn; @@ -209,7 +212,7 @@ static void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int typ #ifdef STM32F10X #define GYRO_SYNC_DENOM_DEFAULT 8 #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) \ - || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) + || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) #define GYRO_SYNC_DENOM_DEFAULT 1 #else #define GYRO_SYNC_DENOM_DEFAULT 3 @@ -250,6 +253,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES, .yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO, .yaw_spin_threshold = 1950, + .dyn_notch_axis = RPY, .dyn_notch_q = 400, .dyn_notch_count = 3, // default of 3 is similar to the matrix filter. .dyn_notch_min_hz = 150, @@ -305,6 +309,7 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_offset_yaw = 0, .yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO, .yaw_spin_threshold = 1950, + .dyn_notch_axis = RPY, .dyn_notch_q = 350, .dyn_notch_count = 3, // default of 3 is similar to the matrix filter. .dyn_notch_min_hz = 150, @@ -533,6 +538,17 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) { } FALLTHROUGH; #endif +#ifdef USE_ACCGYRO_BMI270 + case GYRO_BMI270: + if (bmi270SpiGyroDetect(dev)) { + gyroHardware = GYRO_BMI270; +#ifdef GYRO_BMI270_ALIGN + dev->gyroAlign = GYRO_BMI270_ALIGN; +#endif + break; + } + FALLTHROUGH; +#endif #ifdef USE_GYRO_IMUF9001 case GYRO_IMUF9001: if (imufSpiGyroDetect(dev)) { @@ -562,7 +578,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev) { static bool gyroInitSensor(gyroSensor_t *gyroSensor) { gyroSensor->gyroDev.gyro_high_fsr = gyroConfig()->gyro_high_fsr; #if defined(USE_GYRO_MPU6050) || defined(USE_GYRO_MPU3050) || defined(USE_GYRO_MPU6500) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU6000) \ - || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_GYRO_IMUF9001) || defined(USE_ACCGYRO_BMI160) + || defined(USE_ACC_MPU6050) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20649) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_ICM42605) || defined(USE_GYRO_SPI_ICM42688P) || defined(USE_GYRO_IMUF9001) || defined(USE_ACCGYRO_BMI160) || defined(USE_ACCGYRO_BMI270) mpuDetect(&gyroSensor->gyroDev); mpuResetFn = gyroSensor->gyroDev.mpuConfiguration.resetFn; // must be set after mpuDetect #endif @@ -607,6 +623,7 @@ static bool gyroInitSensor(gyroSensor_t *gyroSensor) { case GYRO_MPU3050: case GYRO_L3GD20: case GYRO_BMI160: + case GYRO_BMI270: case GYRO_MPU6000: case GYRO_MPU6500: case GYRO_MPU9250: @@ -743,6 +760,7 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type) { lpfHz[PITCH] = gyroConfig()->gyro_lowpass_hz[PITCH]; lpfHz[YAW] = gyroConfig()->gyro_lowpass_hz[YAW]; break; +#ifdef USE_GYRO_LPF2 case FILTER_LOWPASS2: lowpassFilterApplyFn = &gyroSensor->lowpass2FilterApplyFn; lowpassFilter = gyroSensor->lowpass2Filter; @@ -750,6 +768,7 @@ void gyroInitLowpassFilterLpf(gyroSensor_t *gyroSensor, int slot, int type) { lpfHz[PITCH] = gyroConfig()->gyro_lowpass2_hz[PITCH]; lpfHz[YAW] = gyroConfig()->gyro_lowpass2_hz[YAW]; break; +#endif default: return; } @@ -843,7 +862,7 @@ static void gyroInitFilterDynamicNotch(gyroSensor_t *gyroSensor) { gyroSensor->notchFilterDynApplyFn = nullFilterApply; if (isDynamicFilterActive()) { gyroSensor->notchFilterDynApplyFn = (filterApplyFnPtr)biquadFilterApplyDF1; // must be this function, not DF2 - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + for (int axis = 0; axis < gyroConfig()->dyn_notch_axis+1; axis++) { for (int axis2 = 0; axis2 < gyroConfig()->dyn_notch_count; axis2++) { biquadFilterInit(&gyroSensor->gyroAnalyseState.notchFilterDyn[axis][axis2], 400, gyro.targetLooptime, gyroConfig()->dyn_notch_q / 100.0f, FILTER_NOTCH); } @@ -888,11 +907,13 @@ static void gyroInitSensorFilters(gyroSensor_t *gyroSensor) { FILTER_LOWPASS, gyroConfig()->gyro_lowpass_type ); +#ifdef USE_GYRO_LPF2 gyroInitLowpassFilterLpf( gyroSensor, FILTER_LOWPASS2, gyroConfig()->gyro_lowpass2_type ); +#endif gyroInitFilterNotch1(gyroSensor, gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1); gyroInitFilterNotch2(gyroSensor, gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2); #ifdef USE_GYRO_DATA_ANALYSE diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 0f32c699e1..671d7b7cbd 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -56,6 +56,7 @@ typedef enum { GYRO_ICM42605, GYRO_ICM42688P, GYRO_BMI160, + GYRO_BMI270, GYRO_IMUF9001, GYRO_FAKE } gyroSensor_e; @@ -113,6 +114,11 @@ typedef struct smithPredictor_s { } smithPredictor_t; #endif // USE_SMITH_PREDICTOR +typedef enum { + RP = 0, + RPY = 1 +} dynamicGyroAxisType_e; + typedef struct gyroConfig_s { uint8_t gyro_align; // gyro alignment uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. @@ -146,6 +152,7 @@ typedef struct gyroConfig_s { int16_t yaw_spin_threshold; uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second + uint8_t dyn_notch_axis; uint16_t dyn_notch_q; uint8_t dyn_notch_count; uint16_t dyn_notch_min_hz; diff --git a/src/main/sensors/gyro_filter_impl.h b/src/main/sensors/gyro_filter_impl.h index 2008434496..4bf1060e19 100644 --- a/src/main/sensors/gyro_filter_impl.h +++ b/src/main/sensors/gyro_filter_impl.h @@ -36,7 +36,9 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor) { #endif // apply static notch filters and software lowpass filters +#ifdef USE_GYRO_LPF2 gyroADCf = gyroSensor->lowpass2FilterApplyFn((filter_t *)&gyroSensor->lowpass2Filter[axis], gyroADCf); +#endif gyroADCf = gyroSensor->lowpassFilterApplyFn((filter_t *)&gyroSensor->lowpassFilter[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf); gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf); diff --git a/src/main/target/BETAFPVF411/target.h b/src/main/target/BETAFPVF411/target.h index 68172fdc56..ddd244caef 100644 --- a/src/main/target/BETAFPVF411/target.h +++ b/src/main/target/BETAFPVF411/target.h @@ -34,6 +34,13 @@ #define USE_SPI #define USE_SPI_DEVICE_1 +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 +#define BMI270_CS_PIN PA4 +#define BMI270_SPI_INSTANCE SPI1 +#define ACC_BMI270_ALIGN CW90_DEG +#define GYRO_BMI270_ALIGN CW90_DEG + #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 diff --git a/src/main/target/BETAFPVF411/target.mk b/src/main/target/BETAFPVF411/target.mk index 3971d0ca39..ab77872afb 100644 --- a/src/main/target/BETAFPVF411/target.mk +++ b/src/main/target/BETAFPVF411/target.mk @@ -4,6 +4,7 @@ FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/barometer/barometer_bmp085.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_ms5611.c \ diff --git a/src/main/target/FLYWOOF411_5IN1_AIO/target.h b/src/main/target/FLYWOOF411_5IN1_AIO/target.h index 5e93cd6460..33c06dde11 100644 --- a/src/main/target/FLYWOOF411_5IN1_AIO/target.h +++ b/src/main/target/FLYWOOF411_5IN1_AIO/target.h @@ -65,10 +65,16 @@ #define USE_ACC_SPI_ICM42688P #define ICM42688P_SPI_INSTANCE SPI1 -#define ICM42688P_CS_PIN PB2 +#define ICM42688P_CS_PIN PA4 #define ACC_ICM42688P_ALIGN CW0_DEG_FLIP #define GYRO_ICM42688P_ALIGN CW0_DEG_FLIP +#define USE_SPI_GYRO +#define BMI270_SPI_INSTANCE SPI1 +#define BMI270_CS_PIN PA4 +#define ACC_BMI270_ALIGN CW0_DEG_FLIP +#define GYRO_BMI270_ALIGN CW0_DEG_FLIP + // *************** Baro ************************** #define USE_I2C diff --git a/src/main/target/FLYWOOF411_5IN1_AIO/target.mk b/src/main/target/FLYWOOF411_5IN1_AIO/target.mk index 19d3a639b7..a0f0b8aff2 100644 --- a/src/main/target/FLYWOOF411_5IN1_AIO/target.mk +++ b/src/main/target/FLYWOOF411_5IN1_AIO/target.mk @@ -3,22 +3,23 @@ F411_TARGETS += $(TARGET) FEATURES += VCP TARGET_SRC = \ - drivers/accgyro/accgyro_mpu.c \ - drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/accgyro/accgyro_spi_icm20689.c \ - drivers/accgyro/accgyro_spi_icm426xx.c \ - drivers/barometer/barometer_bmp085.c \ - drivers/barometer/barometer_bmp280.c \ - drivers/barometer/barometer_ms5611.c \ - drivers/compass/compass_hmc5883l.c \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/accgyro/accgyro_spi_icm20689.c \ + drivers/accgyro/accgyro_spi_icm426xx.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/max7456.c \ drivers/rx/rx_cc2500.c \ - rx/cc2500_common.c \ + rx/cc2500_common.c \ rx/cc2500_frsky_shared.c \ rx/cc2500_frsky_d.c \ - rx/cc2500_frsky_x.c \ - rx/cc2500_redpine.c \ + rx/cc2500_frsky_x.c \ + rx/cc2500_redpine.c \ rx/cc2500_sfhss.c\ drivers/rx/rx_a7105.c \ - drivers/light_ws2811strip.c + drivers/light_ws2811strip.c diff --git a/src/main/target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk b/src/main/target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/main/target/FOXEERF745_AIO/target.h b/src/main/target/FOXEERF745_AIO/target.h index bd56acc8ef..80c7fb0d14 100644 --- a/src/main/target/FOXEERF745_AIO/target.h +++ b/src/main/target/FOXEERF745_AIO/target.h @@ -21,7 +21,13 @@ #pragma once #define TARGET_BOARD_IDENTIFIER "FOXE" + #define TARGET_MANUFACTURER_IDENTIFIER "FOXE" + + #if defined(FOXEERF745_AIO_V2) + #define USBD_PRODUCT_STRING "FOXEERF745_AIO_V2" + #else #define USBD_PRODUCT_STRING "FOXEERF745_AIO" + #endif #define LED0_PIN PC13 @@ -55,6 +61,16 @@ //#define MAG_HMC5883_ALIGN CW270_DEG_FLIP //#define MAG_ALIGN CW180_DEG //not sure if this command will work or if should be more specific to mag + #if defined(FOXEERF745_AIO_V2) + //BMI270 + #define USE_SPI_GYRO + #define USE_ACCGYRO_BMI270 + #define BMI270_CS_PIN PA15 + #define BMI270_SPI_INSTANCE SPI3 + #define ACC_BMI270_ALIGN CW180_DEG + #define GYRO_BMI270_ALIGN CW180_DEG + #endif + #define USE_BARO #define USE_BARO_BMP280 #define BARO_I2C_INSTANCE (I2CDEV_1) diff --git a/src/main/target/FOXEERF745_AIO/target.mk b/src/main/target/FOXEERF745_AIO/target.mk index e4faff73d9..01b1543813 100644 --- a/src/main/target/FOXEERF745_AIO/target.mk +++ b/src/main/target/FOXEERF745_AIO/target.mk @@ -2,11 +2,12 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ - drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu.c \ drivers/barometer/barometer_bmp280.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ - drivers/compass/compass_lis3mdl.c \ + drivers/compass/compass_lis3mdl.c \ drivers/light_ws2811strip.c \ drivers/max7456.c diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.h b/src/main/target/IFLIGHT_F745_AIO_V2/target.h index 6eec787ed9..e771a75672 100644 --- a/src/main/target/IFLIGHT_F745_AIO_V2/target.h +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.h @@ -39,7 +39,7 @@ #define USE_DUAL_GYRO #define USE_SPI_GYRO -//#define USE_ACCGYRO_BMI270 +#define USE_ACCGYRO_BMI270 #define USE_GYRO_SPI_MPU6000 #define USE_ACC_SPI_MPU6000 diff --git a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk index 525f68127c..7cfd60c081 100644 --- a/src/main/target/IFLIGHT_F745_AIO_V2/target.mk +++ b/src/main/target/IFLIGHT_F745_AIO_V2/target.mk @@ -2,12 +2,12 @@ F7X5XG_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_mpu.c \ drivers/barometer/barometer_ms5611.c \ drivers/barometer/barometer_bmp280.c \ drivers/barometer/barometer_bmp085.c \ - drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ drivers/light_ws2811strip.c \ diff --git a/src/main/target/NBDBBBLV2/config.c b/src/main/target/NBDBBBLV2/config.c index 2c735aec79..407f0f6f97 100644 --- a/src/main/target/NBDBBBLV2/config.c +++ b/src/main/target/NBDBBBLV2/config.c @@ -168,9 +168,11 @@ void targetConfiguration(void) gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 200; gyroConfigMutable()->gyro_lowpass_hz[PITCH] = 200; gyroConfigMutable()->gyro_lowpass_hz[YAW] = 200; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 250; gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = 250; gyroConfigMutable()->gyro_lowpass2_hz[YAW] = 250; +#endif gyroConfigMutable()->yaw_spin_threshold = 1950; rxConfigMutable()->mincheck = 1075; rxConfigMutable()->maxcheck = 1900; diff --git a/src/main/target/NBDBBBLV3/config.c b/src/main/target/NBDBBBLV3/config.c index 27f7b894ad..0774446586 100644 --- a/src/main/target/NBDBBBLV3/config.c +++ b/src/main/target/NBDBBBLV3/config.c @@ -168,9 +168,11 @@ void targetConfiguration(void) gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 200; gyroConfigMutable()->gyro_lowpass_hz[PITCH] = 200; gyroConfigMutable()->gyro_lowpass_hz[YAW] = 200; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 250; gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = 250; gyroConfigMutable()->gyro_lowpass2_hz[YAW] = 250; +#endif gyroConfigMutable()->yaw_spin_threshold = 1950; rxConfigMutable()->mincheck = 1075; rxConfigMutable()->maxcheck = 1900; diff --git a/src/main/target/NBDHMBF41S/config.c b/src/main/target/NBDHMBF41S/config.c index 493da570e9..92fe2d8b70 100644 --- a/src/main/target/NBDHMBF41S/config.c +++ b/src/main/target/NBDHMBF41S/config.c @@ -114,7 +114,9 @@ void targetConfiguration(void) gyroConfigMutable()->gyro_lowpass_type = FILTER_PT1; gyroConfigMutable()->gyro_lowpass_hz = 200; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz = 200; +#endif gyroConfigMutable()->yaw_spin_threshold = 1950; gyroConfigMutable()->dyn_lpf_gyro_min_hz = 160; gyroConfigMutable()->dyn_lpf_gyro_max_hz = 400; diff --git a/src/main/target/NBDHMBF4PRO/config.c b/src/main/target/NBDHMBF4PRO/config.c index 42862bed35..fd309465bc 100644 --- a/src/main/target/NBDHMBF4PRO/config.c +++ b/src/main/target/NBDHMBF4PRO/config.c @@ -149,9 +149,11 @@ void targetConfiguration(void) { gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 200; gyroConfigMutable()->gyro_lowpass_hz[PITCH] = 200; gyroConfigMutable()->gyro_lowpass_hz[YAW] = 200; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 250; gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = 250; gyroConfigMutable()->gyro_lowpass2_hz[YAW] = 250; +#endif gyroConfigMutable()->yaw_spin_threshold = 1950; // gyroConfigMutable()->dyn_lpf_gyro_min_hz = 200; // gyroConfigMutable()->dyn_lpf_gyro_max_hz = 500; diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.c b/src/main/target/NBD_INFINITYAIOV2PRO/target.c new file mode 100644 index 0000000000..b0d2fbf100 --- /dev/null +++ b/src/main/target/NBD_INFINITYAIOV2PRO/target.c @@ -0,0 +1,37 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM1, CH2, PA9, TIM_USE_LED, 0, 0 ), // LED_STRIP, + + DEF_TIM(TIM3, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), // M1 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MOTOR, 0, 0 ), // M2 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0 ), // M3 + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 0, 0 ), // M4 +}; diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.h b/src/main/target/NBD_INFINITYAIOV2PRO/target.h new file mode 100644 index 0000000000..6b871d43ef --- /dev/null +++ b/src/main/target/NBD_INFINITYAIOV2PRO/target.h @@ -0,0 +1,160 @@ +/* +* This file is part of Cleanflight and Betaflight. +* +* Cleanflight and Betaflight are free software. You can redistribute +* this software and/or modify this software under the terms of the +* GNU General Public License as published by the Free Software +* Foundation, either version 3 of the License, or (at your option) +* any later version. +* +* Cleanflight and Betaflight are distributed in the hope that they +* will be useful, but WITHOUT ANY WARRANTY; without even the implied +* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. +* See the GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with this software. +* +* If not, see . +*/ + +#pragma once + +#define TARGET_MANUFACTURER_IDENTIFIER "NEBD" +#define USBD_PRODUCT_STRING "NBD_INFINITYAIOV2PRO" + +#define FC_TARGET_MCU STM32F745 // not used in EmuF +#define TARGET_BOARD_IDENTIFIER "S745" // generic ID + +#define LED0_PIN PC0 + +#define USE_BEEPER +#define BEEPER_PIN PD13 +#define BEEPER_INVERTED + +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 +#define BMI270_CS_PIN PE11 +#define BMI270_SPI_INSTANCE SPI4 +#define ACC_BMI270_ALIGN CW270_DEG +#define GYRO_BMI270_ALIGN CW270_DEG + +// dual gyro +#define USE_DUAL_GYRO + +#define USE_EXTI //REQUIRED when USE_GYRO_EXTI +#define USE_GYRO_EXTI +#define USE_MPU_DATA_READY_SIGNAL + +// gyro 1 +#define GYRO_1_CS_PIN PE11 +#define GYRO_1_SPI_INSTANCE SPI4 +#define GYRO_1_EXTI_PIN PB1 +#define GYRO_1_ALIGN CW270_DEG +#define ACC_1_ALIGN CW270_DEG + +// gyro 2 +#define GYRO_2_CS_PIN PB12 +#define GYRO_2_SPI_INSTANCE SPI2 +#define GYRO_2_EXTI_PIN PD0 +#define GYRO_2_ALIGN CW270_DEG +#define ACC_2_ALIGN CW270_DEG + +#define USE_VCP +#define USE_USB_DETECT + +#define USE_UART1 +#define UART1_RX_PIN PB7 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART5 +#define UART5_RX_PIN PD2 + +#define USE_UART7 +#define UART7_TX_PIN PE8 + +#define USE_UART8 +#define UART8_RX_PIN PE0 +#define UART8_TX_PIN PE1 + +#define SERIAL_PORT_COUNT 7 //VCP, USART1, USART2, USART3, UART5, USART7, UART8 + +#define USE_ESC_SENSOR + +#define USE_SPI +#define USE_SPI_DEVICE_1 +#define USE_SPI_DEVICE_2 +#define USE_SPI_DEVICE_3 +#define USE_SPI_DEVICE_4 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define SPI3_SCK_PIN PB3 +#define SPI3_MISO_PIN PB4 +#define SPI3_MOSI_PIN PD6 + +#define SPI4_SCK_PIN PE12 +#define SPI4_MISO_PIN PE13 +#define SPI4_MOSI_PIN PE14 + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI3 +#define MAX7456_SPI_CS_PIN PA15 +#define MAX7456_SPI_CLK (SPI_CLOCK_STANDARD) // 10MHz +#define MAX7456_RESTORE_CLK (SPI_CLOCK_FAST) + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT +#define USE_FLASH +#define USE_FLASHFS +#define USE_FLASH_W25Q128FV //official +#define FLASH_CS_PIN PB0 +#define FLASH_SPI_INSTANCE SPI1 + +#define USE_FLASH_M25P16 // testing // 16MB Micron M25P16 and others (ref: https://github.com/betaflight/betaflight/blob/master/src/main/drivers/flash_m25p16.c) +#define USE_FLASH_W25M // testing // 1Gb NAND flash support +#define USE_FLASH_W25M512 // testing // 16, 32, 64 or 128MB Winbond stacked die support +#define USE_FLASH_W25Q // testing // 512Kb (256Kb x 2 stacked) NOR flash support + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C_DEVICE_1 (I2CDEV_1) +#define MAG_I2C_INSTANCE (I2CDEV_1) +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +#define USE_ADC +#define VBAT_ADC_PIN PC1 +#define CURRENT_METER_ADC_PIN PC2 +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC +#define CURRENT_METER_SCALE_DEFAULT 100 + +#define BEEPER_PWM_HZ 5400 + +#define USE_LED_STRIP +#define LED_STRIP_PIN PA9 + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff +#define TARGET_IO_PORTE 0xffff + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_AIRMODE | FEATURE_RX_SERIAL) +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL + +#define USABLE_TIMER_CHANNEL_COUNT 7 +#define USED_TIMERS ( TIM_N(1) | TIM_N(3) | TIM_N(4) ) diff --git a/src/main/target/NBD_INFINITYAIOV2PRO/target.mk b/src/main/target/NBD_INFINITYAIOV2PRO/target.mk new file mode 100644 index 0000000000..ff6b7c6818 --- /dev/null +++ b/src/main/target/NBD_INFINITYAIOV2PRO/target.mk @@ -0,0 +1,13 @@ +F7X5XG_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c \ + drivers/compass/compass_fake.c \ + drivers/compass/compass_ak8963.c \ + drivers/compass/compass_ak8975.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_lis3mdl.c \ + drivers/compass/compass_qmc5883l.c \ diff --git a/src/main/target/SKYSTARSF7HD/target.h b/src/main/target/SKYSTARSF7HD/target.h index bf68dc5fc4..31d1d6f3fa 100644 --- a/src/main/target/SKYSTARSF7HD/target.h +++ b/src/main/target/SKYSTARSF7HD/target.h @@ -53,6 +53,13 @@ #define USE_ACC +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 +#define BMI270_CS_PIN PA4 +#define BMI270_SPI_INSTANCE SPI1 +#define ACC_BMI270_ALIGN CW180_DEG +#define GYRO_BMI270_ALIGN CW180_DEG + #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 diff --git a/src/main/target/SKYSTARSF7HD/target.mk b/src/main/target/SKYSTARSF7HD/target.mk index 1b54266132..1f9b3af4ff 100644 --- a/src/main/target/SKYSTARSF7HD/target.mk +++ b/src/main/target/SKYSTARSF7HD/target.mk @@ -2,6 +2,7 @@ F7X2RE_TARGETS += $(TARGET) FEATURES += VCP ONBOARDFLASH TARGET_SRC = \ + drivers/accgyro/accgyro_spi_bmi270.c \ drivers/accgyro/accgyro_mpu.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/barometer/barometer_bmp085.c \ @@ -9,5 +10,5 @@ TARGET_SRC = \ drivers/barometer/barometer_ms5611.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ - drivers/light_ws2811strip.c \ + drivers/light_ws2811strip.c \ drivers/max7456.c diff --git a/src/main/target/SKYSTARSF7HDPRO/config.c b/src/main/target/SKYSTARSF7HDPRO/config.c new file mode 100644 index 0000000000..30086932e3 --- /dev/null +++ b/src/main/target/SKYSTARSF7HDPRO/config.c @@ -0,0 +1,36 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" + +#define USE_TARGET_CONFIG + +#include "io/serial.h" +#include "pg/pinio.h" +#include "pg/piniobox.h" +#include "target.h" + + +void targetConfiguration(void) { + pinioBoxConfigMutable()->permanentId[0] = 40; + pinioConfigMutable()->config[0] = 129; +} diff --git a/src/main/target/SKYSTARSF7HDPRO/target.c b/src/main/target/SKYSTARSF7HDPRO/target.c new file mode 100644 index 0000000000..d993355657 --- /dev/null +++ b/src/main/target/SKYSTARSF7HDPRO/target.c @@ -0,0 +1,41 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include + +#include "platform.h" +#include "drivers/io.h" + +#include "drivers/dma.h" +#include "drivers/timer.h" +#include "drivers/timer_def.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM3, CH1, PB4, TIM_USE_PPM, 0, 0 ), // PPM IN + + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MOTOR, 0, 0 ), // S1_OUT – UP2-1 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0 ), // S2_OUT – UP2-1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0 ), // S3_OUT – UP2-5 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0 ), // S4_OUT – UP2-5 + + DEF_TIM(TIM1, CH1, PA8, TIM_USE_ANY, 0, 0 ), // FC CAM – DMA1_ST7 + + DEF_TIM(TIM2, CH2, PB3, TIM_USE_LED, 0, 0 ), // LED_STRIP – DMA1_ST6 +}; diff --git a/src/main/target/SKYSTARSF7HDPRO/target.h b/src/main/target/SKYSTARSF7HDPRO/target.h new file mode 100644 index 0000000000..19c0c2e753 --- /dev/null +++ b/src/main/target/SKYSTARSF7HDPRO/target.h @@ -0,0 +1,207 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#define USE_TARGET_CONFIG + +#define TARGET_MANUFACTURER_IDENTIFIER "SKST" +#define USBD_PRODUCT_STRING "SKST7HDPRO" + +#define FC_TARGET_MCU STM32F7X2 // not used in EmuF +#define TARGET_BOARD_IDENTIFIER "S7X2" + +// ******* LEDs and BEEPER ******** + +#define LED0_PIN PC15 +#define LED1_PIN PC14 + +#define USE_BEEPER +#define BEEPER_PIN PB2 +#define BEEPER_INVERTED + +//#define ENABLE_DSHOT_DMAR true + +#define USE_PINIO +#define PINIO1_PIN PA14 // Bluetooth mode control, PB0 is connected to the 36 pin (P2.0) of the Bluetooth chip. Replace PB0 with the pin for your flight control and 36-pin connection + +#define USE_CAMERA_CONTROL +#define CAMERA_CONTROL_PIN PA8 // define dedicated camera osd pin + + +// ******* GYRO and ACC ******** + +#define USE_DUAL_GYRO +#define USE_EXTI +#define GYRO_1_EXTI_PIN PC4 +#define GYRO_2_EXTI_PIN PC0 +#define MPU_INT_EXTI + +#define GYRO_1_CS_PIN PA4 +#define GYRO_1_SPI_INSTANCE SPI1 +#define GYRO_2_CS_PIN PC13 +#define GYRO_2_SPI_INSTANCE SPI1 + +#define USE_GYRO +#define USE_GYRO_SPI_MPU6000 +#define USE_GYRO_SPI_MPU6500 +#define USE_SPI_GYRO +#define USE_ACCGYRO_BMI270 + +#define USE_ACC +#define USE_ACC_SPI_MPU6000 +#define USE_ACC_SPI_MPU6500 + +#define GYRO_1_ALIGN CW90_DEG_FLIP +#define ACC_1_ALIGN CW90_DEG_FLIP + +#define GYRO_2_ALIGN CW0_DEG +#define ACC_2_ALIGN CW0_DEG + +#define USE_MPU_DATA_READY_SIGNAL +#define ENSURE_MPU_DATA_READY_IS_LOW +#define GYRO_CONFIG_USE_GYRO_DEFAULT GYRO_CONFIG_USE_GYRO_1 + +// *************** Baro ************************ + +#define USE_SPI + +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PB14 +#define SPI2_MOSI_PIN PB15 + +#define USE_BARO +#define USE_BARO_SPI_BMP280 +#define DEFAULT_BARO_SPI_BMP280 +#define BMP280_SPI_INSTANCE SPI2 +#define BMP280_CS_PIN PB1 + +#define USE_I2C +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + + +//*********** Magnetometer / Compass ************* +#define USE_MAG +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define MAG_I2C_INSTANCE (I2CDEV_1) + +// ******* SERIAL ******** + +#define USE_VCP + +#define USE_UART1 +#define USE_UART2 +#define USE_UART3 +#define USE_UART4 +#define USE_UART5 +#define USE_UART6 + +#define UART1_TX_PIN PA9 +#define UART1_RX_PIN PA10 + +#define UART2_TX_PIN PA2 +#define UART2_RX_PIN PA3 + +#define UART3_TX_PIN PB10 +#define UART3_RX_PIN PB11 + +#define UART4_TX_PIN PA0 +#define UART4_RX_PIN PA1 + +#define UART5_TX_PIN PC12 +#define UART5_RX_PIN PD2 + +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define USE_SOFTSERIAL1 +#define USE_SOFTSERIAL2 + +#define SERIAL_PORT_COUNT 9 //VCP, UART1-UART6 , 2 x Soft Serial + +// ******* SPI ******** + +#define USE_SPI + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +// ******* ADC ******** + +#define USE_ADC +#define ADC_INSTANCE ADC2 +#define ADC2_DMA_STREAM DMA2_Stream3 + +#define VBAT_ADC_PIN PC1 +#define RSSI_ADC_PIN PC2 +#define CURRENT_METER_ADC_PIN PC3 + +// ******* OSD ******** + +#define USE_MAX7456 +#define MAX7456_SPI_INSTANCE SPI2 +#define MAX7456_SPI_CS_PIN PB12 +#define MAX7456_SPI_CLK ( SPI_CLOCK_STANDARD ) +#define MAX7456_RESTORE_CLK ( SPI_CLOCK_FAST ) + +//******* FLASH ******** + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define FLASH_CS_PIN PA15 +#define FLASH_SPI_INSTANCE SPI3 + +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// ******* FEATURES ******** + +#define USE_OSD + +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART1 +#define SERIALRX_PROVIDER SERIALRX_SBUS + +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL) +#define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC +#define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC +#define CURRENT_METER_SCALE_DEFAULT 290 + + +#define USE_ESCSERIAL + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD 0xffff + +#define USABLE_TIMER_CHANNEL_COUNT 7 +#define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3)| TIM_N(4) | TIM_N(8) ) diff --git a/src/main/target/SKYSTARSF7HDPRO/target.mk b/src/main/target/SKYSTARSF7HDPRO/target.mk new file mode 100644 index 0000000000..6d549c15d0 --- /dev/null +++ b/src/main/target/SKYSTARSF7HDPRO/target.mk @@ -0,0 +1,16 @@ +F7X2RE_TARGETS += $(TARGET) +FEATURES += VCP ONBOARDFLASH + +TARGET_SRC = \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/accgyro/accgyro_spi_mpu6000.c \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/accgyro/accgyro_spi_mpu6500.c \ + drivers/barometer/barometer_bmp280.c \ + drivers/barometer/barometer_ms5611.c \ + drivers/barometer/barometer_bmp085.c \ + drivers/compass/compass_hmc5883l.c \ + drivers/compass/compass_qmc5883l.c \ + drivers/light_ws2811strip.c \ + drivers/max7456.c diff --git a/src/main/target/TMTR_TMOTORF7/target.c b/src/main/target/TMTR_TMOTORF7/target.c index 847810cf45..ecaa7704b1 100644 --- a/src/main/target/TMTR_TMOTORF7/target.c +++ b/src/main/target/TMTR_TMOTORF7/target.c @@ -25,15 +25,16 @@ #include "drivers/timer_def.h" const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MOTOR, 0, 0), // motor 1 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MOTOR, 0, 0), // motor 2 + DEF_TIM(TIM8, CH2N, PB0, TIM_USE_MOTOR, 0, 0), // motor 3 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), // motor 4 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0), // motor 5 + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), // motor 6 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MOTOR, 0, 0), // motor 7 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MOTOR, 0, 0), // motor 8 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MOTOR, 0, 0), - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MOTOR, 0, 0), - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MOTOR, 0, 0), - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MOTOR, 0, 0), - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MOTOR, 0, 0), - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MOTOR, 0, 0), - - DEF_TIM(TIM2, CH1, PA15, TIM_USE_PPM, 0, 0), - DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0) + DEF_TIM(TIM2, CH1, PA15, TIM_USE_PPM, 0, 0), // ppm + DEF_TIM(TIM11, CH1, PB9, TIM_USE_ANY, 0, 0), // cam ctrl + DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 0), // led }; diff --git a/src/main/target/TMTR_TMOTORF7/target.h b/src/main/target/TMTR_TMOTORF7/target.h index 1353cbead9..20ce09a1c2 100644 --- a/src/main/target/TMTR_TMOTORF7/target.h +++ b/src/main/target/TMTR_TMOTORF7/target.h @@ -20,9 +20,11 @@ #pragma once -#define TARGET_BOARD_IDENTIFIER "TMR7" -#define USBD_PRODUCT_STRING "TMOTORF7" #define TARGET_MANUFACTURER_IDENTIFIER "TMTR" +#define USBD_PRODUCT_STRING "TMOTORF7" + +#define FC_TARGET_MCU STM32F7X2 // not used in EmuF +#define TARGET_BOARD_IDENTIFIER "S7X2" // generic ID #define LED0_PIN PB2 @@ -35,7 +37,7 @@ #define USE_PINIO #define PINIO1_PIN PC14 #define USE_PINIOBOX - +#define PINIO1_BOX 40 #define ENABLE_DSHOT_DMAR true @@ -44,21 +46,44 @@ #define USE_MPU_DATA_READY_SIGNAL #define USE_GYRO +#define USE_ACC + #define USE_GYRO_SPI_MPU6000 +#define USE_ACC_SPI_MPU6000 +#define USE_ACCGYRO_BMI270 +#define USE_GYRO_SPI_ICM42688P +#define USE_ACC_SPI_ICM42688P +#define USE_GYRO_SPI_MPU6500 +#define USE_ACC_SPI_MPU6500 + #define MPU6000_CS_PIN PA4 #define MPU6000_SPI_INSTANCE SPI1 -#define ACC_MPU6000_ALIGN CW0_DEG -#define GYRO_MPU6000_ALIGN CW0_DEG +#define ACC_MPU6000_ALIGN CW0_DEG +#define GYRO_MPU6000_ALIGN CW0_DEG -#define USE_ACC -#define USE_ACC_SPI_MPU6000 -#define USE_ACC_SPI_MPU6500 +#define MPU6500_CS_PIN PA4 +#define MPU6500_SPI_INSTANCE SPI1 +#define ACC_MPU6500_ALIGN CW0_DEG +#define GYRO_MPU6500_ALIGN CW0_DEG + +#define USE_SPI_GYRO +#define BMI270_CS_PIN PA4 +#define BMI270_SPI_INSTANCE SPI1 +#define ACC_BMI270_ALIGN CW0_DEG +#define GYRO_BMI270_ALIGN CW0_DEG + +#define ICM42688P_CS_PIN PA4 +#define ICM42688P_SPI_INSTANCE SPI1 +#define ACC_ICM42688P_ALIGN CW0_DEG +#define GYRO_ICM42688P_ALIGN CW0_DEG #define USE_BARO #define USE_BARO_SPI_BMP280 #define DEFAULT_BARO_SPI_BMP280 #define BMP280_SPI_INSTANCE SPI3 #define BMP280_CS_PIN PC15 +#define BARO_SPI_INSTANCE SPI3 +#define BARO_CS_PIN PC15 #define USE_MAG #define USE_MAG_HMC5883 @@ -133,20 +158,21 @@ #define VBAT_ADC_PIN PC1 #define CURRENT_METER_ADC_PIN PC2 #define RSSI_ADC_PIN PC3 +#define ADC1_DMA_STREAM DMA2_Stream4 //# ADC 1: DMA2 Stream 4 Channel 0 #define DEFAULT_VOLTAGE_METER_SOURCE VOLTAGE_METER_ADC #define DEFAULT_CURRENT_METER_SOURCE CURRENT_METER_ADC -#define SERIALRX_PROVIDER SERIALRX_SBUS +//#define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_UART5 -//#define SBUS_TELEMETRY_UART SERIAL_PORT_USART1 +#define SBUS_TELEMETRY_UART SERIAL_PORT_USART1 // soft UART?? #define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL -#define DEFAULT_FEATURES ( FEATURE_OSD ) +#define DEFAULT_FEATURES (FEATURE_OSD | FEATURE_TELEMETRY | FEATURE_AIRMODE | FEATURE_RX_SERIAL) #define TARGET_IO_PORTA ( 0xffff ) #define TARGET_IO_PORTB ( 0xffff ) #define TARGET_IO_PORTC ( 0xffff ) #define TARGET_IO_PORTD ( 0x0004 ) -#define USABLE_TIMER_CHANNEL_COUNT 9 +#define USABLE_TIMER_CHANNEL_COUNT 11 #define USED_TIMERS ( TIM_N(1) | TIM_N(2) | TIM_N(3) | TIM_N(4) | TIM_N(8) | TIM_N(11) ) diff --git a/src/main/target/TMTR_TMOTORF7/target.mk b/src/main/target/TMTR_TMOTORF7/target.mk index 15077cfbd3..c023c93328 100644 --- a/src/main/target/TMTR_TMOTORF7/target.mk +++ b/src/main/target/TMTR_TMOTORF7/target.mk @@ -1,10 +1,15 @@ F7X2RE_TARGETS += $(TARGET) FEATURES = VCP ONBOARDFLASH TARGET_SRC = \ + drivers/accgyro/accgyro_spi_bmi270.c \ + drivers/accgyro/accgyro_spi_icm426xx.c \ drivers/accgyro/accgyro_mpu6500.c \ drivers/accgyro/accgyro_spi_mpu6000.c \ drivers/accgyro/accgyro_spi_mpu6500.c \ drivers/barometer/barometer_bmp280.c \ drivers/compass/compass_hmc5883l.c \ drivers/compass/compass_qmc5883l.c \ - drivers/max7456.c + drivers/light_led.h \ + drivers/light_ws2811strip.c \ + drivers/pinio.c \ + drivers/max7456.c \ diff --git a/src/main/target/common_fc_pre.h b/src/main/target/common_fc_pre.h index 426ec9b767..4d378c10ac 100644 --- a/src/main/target/common_fc_pre.h +++ b/src/main/target/common_fc_pre.h @@ -229,5 +229,5 @@ #define USE_CMS_GPS_RESCUE_MENU #endif -// ICM42688P define +// ICM42688P & BMI270 experimental define #define USE_GYRO_DLPF_EXPERIMENTAL diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index fc2dbc20e4..0ee0877b84 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -116,7 +116,9 @@ TEST(SensorGyro, Update) pgResetAll(); // turn off filters gyroConfigMutable()->gyro_lowpass_hz[ROLL] = 0; +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = 0; +#endif gyroConfigMutable()->gyro_soft_notch_hz_1 = 0; gyroConfigMutable()->gyro_soft_notch_hz_2 = 0; gyroInit();