From 41cfd39355b2dcc67f4a7883a3dccaf1ca5e044f Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 11 Oct 2023 14:04:14 -0500 Subject: [PATCH 01/16] add rc rates_type to BBL header (#931) --- src/main/blackbox/blackbox.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 8ee2428c8d..b17ed12485 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]); From c55b0d708bcd9d4d127a8ae5523861bddbadf5c3 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 11 Oct 2023 14:05:21 -0500 Subject: [PATCH 02/16] add motor_poles to BBL header (#932) * add motor_poles to BBL header --- src/main/blackbox/blackbox.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b17ed12485..5797c373a8 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1267,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 From 6ba54464405346ac88ab9376e8f603129c973fa0 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 25 Oct 2023 15:17:08 -0500 Subject: [PATCH 03/16] option to apply dynamic-notches to RP vs normal/default RPY (#881) * add dynamicFilter mode. RP or RPY * dynamicFilter mode - OSD * dynamicFilter mode - rename mode to axis * dynamicFilter mode (axis) - #ifdef USE_GYRO_DATA_ANALYSE gating --- src/main/blackbox/blackbox.c | 3 +++ src/main/cms/cms_menu_imu.c | 27 ++++++++++++++++++++++++++- src/main/flight/pid.c | 4 +++- src/main/flight/pid.h | 2 ++ src/main/interface/msp.c | 24 ++++++++++++++++++++++++ src/main/interface/settings.c | 14 +++++++++++++- src/main/interface/settings.h | 3 +++ src/main/sensors/gyro.c | 8 +++++++- src/main/sensors/gyro.h | 10 ++++++++++ 9 files changed, 91 insertions(+), 4 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 5797c373a8..672d39fc3a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1180,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); @@ -1240,6 +1242,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); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index a8f3804735..e479a1fa17 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; @@ -490,10 +496,13 @@ static uint16_t gyroConfig_gyro_lowpass_hz_yaw; static uint16_t gyroConfig_gyro_lowpass2_hz_roll; static uint16_t gyroConfig_gyro_lowpass2_hz_pitch; static uint16_t gyroConfig_gyro_lowpass2_hz_yaw; +#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; @@ -518,14 +527,16 @@ static long cmsx_menuGyro_onEnter(void) { 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]; +#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; @@ -550,10 +561,13 @@ static long cmsx_menuGyro_onExit(const OSD_Entry *self) { 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; +#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; @@ -584,10 +598,13 @@ static OSD_Entry cmsx_menuFilterGlobalEntries[] = { { "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 +726,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 +744,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 +765,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 +786,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/flight/pid.c b/src/main/flight/pid.c index 19b51d18f7..fe0585a8d0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -197,8 +197,10 @@ void resetPidProfile(pidProfile_t *pidProfile) { .dterm_ABG_half_life = 50, .emuGravityGain = 50, .angle_filter = 100, +#ifdef USE_GYRO_DATA_ANALYSE .dtermDynNotch = false, .dterm_dyn_notch_q = 400, +#endif ); } @@ -811,7 +813,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/flight/pid.h b/src/main/flight/pid.h index ea9f16d811..bfe63e31f0 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -155,8 +155,10 @@ typedef struct pidProfile_s { uint16_t dterm_ABG_alpha; uint16_t dterm_ABG_boost; uint8_t dterm_ABG_half_life; +#ifdef USE_GYRO_DATA_ANALYSE uint8_t dtermDynNotch; uint16_t dterm_dyn_notch_q; +#endif } pidProfile_t; #ifndef USE_OSD_SLAVE diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index b3aebb427d..9b3dd142da 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1204,10 +1204,17 @@ bool mspProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst) { 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 +1226,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 @@ -1821,10 +1833,17 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { currentPidProfile->dFilter[YAW].dLpf2 = sbufReadU16(src); //MSP 1.51 removes SmartDTermSmoothing and WitchCraft //MSP 1.51 adds and refactors dynamic_filter +#ifdef USE_GYRO_DATA_ANALYSE gyroConfigMutable()->dyn_notch_count = sbufReadU8(src); //dynamic_gyro_notch_count gyroConfigMutable()->dyn_notch_q = sbufReadU16(src); gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src); gyroConfigMutable()->dyn_notch_max_hz = sbufReadU16(src); //dynamic_gyro_notch_max_hz +#else + sbufReadU8(src); + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); +#endif //end 1.51 add/refactor dynamic_filter //MSP 1.51 gyroConfigMutable()->gyro_ABG_alpha = sbufReadU16(src); @@ -1836,8 +1855,13 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { currentPidProfile->dterm_ABG_half_life = sbufReadU8(src); //end MSP 1.51 //MSP 1.51 dynamic dTerm notch +#ifdef USE_GYRO_DATA_ANALYSE currentPidProfile->dtermDynNotch = sbufReadU8(src); //dterm_dyn_notch_enable currentPidProfile->dterm_dyn_notch_q = sbufReadU16(src); //dterm_dyn_notch_q +#else + sbufReadU8(src); + sbufReadU16(src); +#endif //end MSP 1.51 dynamic dTerm notch } // reinitialize the gyro filters with the new values diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 9634ec73c8..19f857c663 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -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,12 @@ static const char *const lookupTableMixerImplType[] = { "LEGACY", "SMOOTH", "2PASS" }; +#ifdef USE_GYRO_DATA_ANALYSE +static const char *const lookupTableDynNotchAxisType[] = { + "RP", "RPY" +}; +#endif + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -482,6 +489,9 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableOsdLogoOnArming), #endif LOOKUP_TABLE_ENTRY(lookupTableMixerImplType), +#ifdef USE_GYRO_DATA_ANALYSE + LOOKUP_TABLE_ENTRY(lookupTableDynNotchAxisType), +#endif }; #undef LOOKUP_TABLE_ENTRY @@ -553,6 +563,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 +874,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..ab5ea4482e 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -112,6 +112,9 @@ typedef enum { TABLE_OSD_LOGO_ON_ARMING, #endif TABLE_MIXER_IMPL_TYPE, +#ifdef USE_GYRO_DATA_ANALYSE + TABLE_DYN_NOTCH_AXIS_TYPE, +#endif LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b77528e9fd..b957ac7ef3 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -250,10 +250,13 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES, .yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO, .yaw_spin_threshold = 1950, +#ifdef USE_GYRO_DATA_ANALYSE + .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, .dyn_notch_max_hz = 600, +#endif .imuf_mode = GTBCM_GYRO_ACC_FILTER_F, .imuf_rate = IMUF_RATE_16K, .imuf_roll_q = 6000, @@ -305,10 +308,13 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_offset_yaw = 0, .yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO, .yaw_spin_threshold = 1950, +#ifdef USE_GYRO_DATA_ANALYSE + .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, .dyn_notch_max_hz = 600, +#endif .gyro_ABG_alpha = 0, .gyro_ABG_boost = 275, .gyro_ABG_half_life = 50, @@ -843,7 +849,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); } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 0f32c699e1..84bee04419 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -113,6 +113,13 @@ typedef struct smithPredictor_s { } smithPredictor_t; #endif // USE_SMITH_PREDICTOR +#ifdef USE_GYRO_DATA_ANALYSE +typedef enum { + RP = 0, + RPY = 1 +} dynamicGyroAxisType_e; +#endif + 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,10 +153,13 @@ typedef struct gyroConfig_s { int16_t yaw_spin_threshold; uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second +#if defined(USE_GYRO_DATA_ANALYSE) + uint8_t dyn_notch_axis; uint16_t dyn_notch_q; uint8_t dyn_notch_count; uint16_t dyn_notch_min_hz; uint16_t dyn_notch_max_hz; +#endif #if defined(USE_GYRO_IMUF9001) uint16_t imuf_mode; uint16_t imuf_rate; From de71a3a275047d084d3ba7da584c0286ff906248 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 26 Oct 2023 08:23:46 -0500 Subject: [PATCH 04/16] add Gyro LPF2 type to OSD (#934) --- src/main/cms/cms_menu_imu.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index e479a1fa17..64be1292d8 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -493,6 +493,7 @@ 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; +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; @@ -524,6 +525,7 @@ 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]; + 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]; @@ -558,6 +560,7 @@ 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; + 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; @@ -589,11 +592,12 @@ 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 }, From db57386cd614604c83db951e16d36d224641918a Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 26 Oct 2023 09:49:06 -0500 Subject: [PATCH 05/16] fix/add USE_GYRO_LPF2 gating (#935) * fix/add USE_GYRO_LPF2 gating --- src/main/blackbox/blackbox.c | 2 ++ src/main/cms/cms_menu_imu.c | 6 ++++++ src/main/interface/msp.c | 20 ++++++++++++++++++++ src/main/interface/settings.c | 2 ++ src/main/sensors/gyro.c | 10 ++++++++++ src/main/sensors/gyro.h | 4 ++++ src/main/sensors/gyro_filter_impl.h | 2 ++ src/main/target/NBDBBBLV2/config.c | 2 ++ src/main/target/NBDBBBLV3/config.c | 2 ++ src/main/target/NBDHMBF41S/config.c | 2 ++ src/main/target/NBDHMBF4PRO/config.c | 2 ++ src/test/unit/sensor_gyro_unittest.cc | 2 ++ 12 files changed, 56 insertions(+) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 672d39fc3a..436871abb4 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1229,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, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 64be1292d8..50c0bfc589 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -493,10 +493,12 @@ 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; @@ -525,10 +527,12 @@ 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; @@ -560,10 +564,12 @@ 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; diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index 9b3dd142da..a748c4aeb4 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1194,11 +1194,21 @@ 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); @@ -1823,11 +1833,21 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { gyroConfigMutable()->gyro_lowpass_hz[ROLL] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass_hz[PITCH] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass_hz[YAW] = sbufReadU16(src); +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass2_hz[YAW] = sbufReadU16(src); +#else + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); +#endif gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src); +#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src); +#else + sbufReadU8(src); +#endif currentPidProfile->dFilter[ROLL].dLpf2 = sbufReadU16(src); currentPidProfile->dFilter[PITCH].dLpf2 = sbufReadU16(src); currentPidProfile->dFilter[YAW].dLpf2 = sbufReadU16(src); diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 19f857c663..cb2690eb58 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -513,10 +513,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) }, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index b957ac7ef3..cb2c2c6b03 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -158,8 +158,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; @@ -236,10 +238,12 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lowpass_hz[ROLL] = 0, .gyro_lowpass_hz[PITCH] = 0, .gyro_lowpass_hz[YAW] = 0, +#ifdef USE_GYRO_LPF2 .gyro_lowpass2_type = FILTER_PT1, .gyro_lowpass2_hz[ROLL] = 0, .gyro_lowpass2_hz[PITCH] = 0, .gyro_lowpass2_hz[YAW] = 0, +#endif .gyro_high_fsr = false, .gyro_use_32khz = true, .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, @@ -289,10 +293,12 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lowpass_hz[ROLL] = 115, .gyro_lowpass_hz[PITCH] = 115, .gyro_lowpass_hz[YAW] = 105, +#ifdef USE_GYRO_LPF2 .gyro_lowpass2_type = FILTER_PT1, .gyro_lowpass2_hz[ROLL] = 0, .gyro_lowpass2_hz[PITCH] = 0, .gyro_lowpass2_hz[YAW] = 0, +#endif .gyro_high_fsr = false, .gyro_use_32khz = false, .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, @@ -749,6 +755,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; @@ -756,6 +763,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; } @@ -894,11 +902,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 84bee04419..403954835e 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -132,7 +132,9 @@ typedef struct gyroConfig_s { uint8_t gyro_to_use; uint16_t gyro_lowpass_hz[XYZ_AXIS_COUNT]; +#ifdef USE_GYRO_LPF2 uint16_t gyro_lowpass2_hz[XYZ_AXIS_COUNT]; +#endif uint16_t gyro_ABG_alpha; uint16_t gyro_ABG_boost; @@ -147,7 +149,9 @@ typedef struct gyroConfig_s { // Lowpass primary/secondary uint8_t gyro_lowpass_type; +#ifdef USE_GYRO_LPF2 uint8_t gyro_lowpass2_type; +#endif uint8_t yaw_spin_recovery; int16_t yaw_spin_threshold; 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/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/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(); From b2117eb9c6606d804c7361a3b6a08b1c9f6b5b95 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Fri, 27 Oct 2023 10:54:51 -0500 Subject: [PATCH 06/16] revert some USE_GYRO_DATA_ANALYSE and USE_GYRO_LPF2 gating (#936) revert some USE_GYRO_DATA_ANALYSE and USE_GYRO_LPF2 gating because C compiler sucks --- src/main/flight/pid.c | 6 ++++-- src/main/flight/pid.h | 2 -- src/main/interface/msp.c | 22 ---------------------- src/main/interface/settings.c | 4 ---- src/main/interface/settings.h | 2 -- src/main/sensors/gyro.c | 8 -------- src/main/sensors/gyro.h | 8 -------- 7 files changed, 4 insertions(+), 48 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fe0585a8d0..231c6fa489 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -197,10 +197,8 @@ void resetPidProfile(pidProfile_t *pidProfile) { .dterm_ABG_half_life = 50, .emuGravityGain = 50, .angle_filter = 100, -#ifdef USE_GYRO_DATA_ANALYSE .dtermDynNotch = false, .dterm_dyn_notch_q = 400, -#endif ); } @@ -237,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]; @@ -673,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) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index bfe63e31f0..ea9f16d811 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -155,10 +155,8 @@ typedef struct pidProfile_s { uint16_t dterm_ABG_alpha; uint16_t dterm_ABG_boost; uint8_t dterm_ABG_half_life; -#ifdef USE_GYRO_DATA_ANALYSE uint8_t dtermDynNotch; uint16_t dterm_dyn_notch_q; -#endif } pidProfile_t; #ifndef USE_OSD_SLAVE diff --git a/src/main/interface/msp.c b/src/main/interface/msp.c index a748c4aeb4..684b85e213 100644 --- a/src/main/interface/msp.c +++ b/src/main/interface/msp.c @@ -1833,37 +1833,20 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { gyroConfigMutable()->gyro_lowpass_hz[ROLL] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass_hz[PITCH] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass_hz[YAW] = sbufReadU16(src); -#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_hz[ROLL] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass2_hz[PITCH] = sbufReadU16(src); gyroConfigMutable()->gyro_lowpass2_hz[YAW] = sbufReadU16(src); -#else - sbufReadU16(src); - sbufReadU16(src); - sbufReadU16(src); -#endif gyroConfigMutable()->gyro_lowpass_type = sbufReadU8(src); -#ifdef USE_GYRO_LPF2 gyroConfigMutable()->gyro_lowpass2_type = sbufReadU8(src); -#else - sbufReadU8(src); -#endif currentPidProfile->dFilter[ROLL].dLpf2 = sbufReadU16(src); currentPidProfile->dFilter[PITCH].dLpf2 = sbufReadU16(src); currentPidProfile->dFilter[YAW].dLpf2 = sbufReadU16(src); //MSP 1.51 removes SmartDTermSmoothing and WitchCraft //MSP 1.51 adds and refactors dynamic_filter -#ifdef USE_GYRO_DATA_ANALYSE gyroConfigMutable()->dyn_notch_count = sbufReadU8(src); //dynamic_gyro_notch_count gyroConfigMutable()->dyn_notch_q = sbufReadU16(src); gyroConfigMutable()->dyn_notch_min_hz = sbufReadU16(src); gyroConfigMutable()->dyn_notch_max_hz = sbufReadU16(src); //dynamic_gyro_notch_max_hz -#else - sbufReadU8(src); - sbufReadU16(src); - sbufReadU16(src); - sbufReadU16(src); -#endif //end 1.51 add/refactor dynamic_filter //MSP 1.51 gyroConfigMutable()->gyro_ABG_alpha = sbufReadU16(src); @@ -1875,13 +1858,8 @@ mspResult_e mspProcessInCommand(uint8_t cmdMSP, sbuf_t *src) { currentPidProfile->dterm_ABG_half_life = sbufReadU8(src); //end MSP 1.51 //MSP 1.51 dynamic dTerm notch -#ifdef USE_GYRO_DATA_ANALYSE currentPidProfile->dtermDynNotch = sbufReadU8(src); //dterm_dyn_notch_enable currentPidProfile->dterm_dyn_notch_q = sbufReadU16(src); //dterm_dyn_notch_q -#else - sbufReadU8(src); - sbufReadU16(src); -#endif //end MSP 1.51 dynamic dTerm notch } // reinitialize the gyro filters with the new values diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index cb2690eb58..9af64f98e5 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -394,11 +394,9 @@ static const char *const lookupTableMixerImplType[] = { "LEGACY", "SMOOTH", "2PASS" }; -#ifdef USE_GYRO_DATA_ANALYSE static const char *const lookupTableDynNotchAxisType[] = { "RP", "RPY" }; -#endif #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } @@ -489,9 +487,7 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTableOsdLogoOnArming), #endif LOOKUP_TABLE_ENTRY(lookupTableMixerImplType), -#ifdef USE_GYRO_DATA_ANALYSE LOOKUP_TABLE_ENTRY(lookupTableDynNotchAxisType), -#endif }; #undef LOOKUP_TABLE_ENTRY diff --git a/src/main/interface/settings.h b/src/main/interface/settings.h index ab5ea4482e..e8a8269834 100644 --- a/src/main/interface/settings.h +++ b/src/main/interface/settings.h @@ -112,9 +112,7 @@ typedef enum { TABLE_OSD_LOGO_ON_ARMING, #endif TABLE_MIXER_IMPL_TYPE, -#ifdef USE_GYRO_DATA_ANALYSE TABLE_DYN_NOTCH_AXIS_TYPE, -#endif LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index cb2c2c6b03..eefea7e341 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -238,12 +238,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lowpass_hz[ROLL] = 0, .gyro_lowpass_hz[PITCH] = 0, .gyro_lowpass_hz[YAW] = 0, -#ifdef USE_GYRO_LPF2 .gyro_lowpass2_type = FILTER_PT1, .gyro_lowpass2_hz[ROLL] = 0, .gyro_lowpass2_hz[PITCH] = 0, .gyro_lowpass2_hz[YAW] = 0, -#endif .gyro_high_fsr = false, .gyro_use_32khz = true, .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, @@ -254,13 +252,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .checkOverflow = GYRO_OVERFLOW_CHECK_ALL_AXES, .yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO, .yaw_spin_threshold = 1950, -#ifdef USE_GYRO_DATA_ANALYSE .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, .dyn_notch_max_hz = 600, -#endif .imuf_mode = GTBCM_GYRO_ACC_FILTER_F, .imuf_rate = IMUF_RATE_16K, .imuf_roll_q = 6000, @@ -293,12 +289,10 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lowpass_hz[ROLL] = 115, .gyro_lowpass_hz[PITCH] = 115, .gyro_lowpass_hz[YAW] = 105, -#ifdef USE_GYRO_LPF2 .gyro_lowpass2_type = FILTER_PT1, .gyro_lowpass2_hz[ROLL] = 0, .gyro_lowpass2_hz[PITCH] = 0, .gyro_lowpass2_hz[YAW] = 0, -#endif .gyro_high_fsr = false, .gyro_use_32khz = false, .gyro_to_use = GYRO_CONFIG_USE_GYRO_DEFAULT, @@ -314,13 +308,11 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_offset_yaw = 0, .yaw_spin_recovery = YAW_SPIN_RECOVERY_AUTO, .yaw_spin_threshold = 1950, -#ifdef USE_GYRO_DATA_ANALYSE .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, .dyn_notch_max_hz = 600, -#endif .gyro_ABG_alpha = 0, .gyro_ABG_boost = 275, .gyro_ABG_half_life = 50, diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 403954835e..b81475a75c 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -113,12 +113,10 @@ typedef struct smithPredictor_s { } smithPredictor_t; #endif // USE_SMITH_PREDICTOR -#ifdef USE_GYRO_DATA_ANALYSE typedef enum { RP = 0, RPY = 1 } dynamicGyroAxisType_e; -#endif typedef struct gyroConfig_s { uint8_t gyro_align; // gyro alignment @@ -132,9 +130,7 @@ typedef struct gyroConfig_s { uint8_t gyro_to_use; uint16_t gyro_lowpass_hz[XYZ_AXIS_COUNT]; -#ifdef USE_GYRO_LPF2 uint16_t gyro_lowpass2_hz[XYZ_AXIS_COUNT]; -#endif uint16_t gyro_ABG_alpha; uint16_t gyro_ABG_boost; @@ -149,21 +145,17 @@ typedef struct gyroConfig_s { // Lowpass primary/secondary uint8_t gyro_lowpass_type; -#ifdef USE_GYRO_LPF2 uint8_t gyro_lowpass2_type; -#endif uint8_t yaw_spin_recovery; int16_t yaw_spin_threshold; uint16_t gyroCalibrationDuration; // Gyro calibration duration in 1/100 second -#if defined(USE_GYRO_DATA_ANALYSE) uint8_t dyn_notch_axis; uint16_t dyn_notch_q; uint8_t dyn_notch_count; uint16_t dyn_notch_min_hz; uint16_t dyn_notch_max_hz; -#endif #if defined(USE_GYRO_IMUF9001) uint16_t imuf_mode; uint16_t imuf_rate; From 3fd882df176b98fd6c8df51153532dd583ba67dd Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Tue, 31 Oct 2023 15:21:47 -0500 Subject: [PATCH 07/16] ICM426XX - Disable AFSR (#933) * ICM426XX - Disable AFSR Betaflight: Disable ICM426XX AFSR feature to prevent stalls Ardupilot: AP_InertialSensor: fix for ICM42688 stuck gyro issue these undocumented bits in register 0x4d control the "adaptive full scale range" mode of the ICM42688. The feature is enabled by default but has a bug where it gives "stuck" gyro values for short periods (between 1ms and 2ms):, leading to a significant gyro bias at longer time scales, enough to in some cases cause a vehicle to crash if it is unable to switch to an alternative IMU Co-authored-by: Andrew Tridgell Co-authored-by: Steve Evans Thanks to all participants here: https://github.com/betaflight/betaflight/issues/12970 --- src/main/drivers/accgyro/accgyro_spi_icm426xx.c | 14 +++++++++++++- src/main/drivers/bus_spi.c | 3 ++- 2 files changed, 15 insertions(+), 2 deletions(-) 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/bus_spi.c b/src/main/drivers/bus_spi.c index d42f0e6156..6bddbb9c5b 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -272,7 +272,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); From b1cf5d03f94284b1c2bc0ab46380f84a4e0878d6 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 1 Nov 2023 08:00:51 -0500 Subject: [PATCH 08/16] BMI270 Support - FIFO config file 328 bytes (#930) * BMI270 - WIP - failing - incl IFRC_IFLIGHT_F745_AIO_V2 for testing (nerdCopter) (2023-06-20) * bmi270 - add IFLIGHT_F745_AIO_V2 (nerdCopter from BeauBrewski) * bmi270 - fix my obvious mistakes (nerdCopter) * bmi270 - align ifrcf745aiov2 target (nerdCopter) * BMI270 should compile on IFLIGHT_F745_AIO_V2 target (Peck07) * add BMI270 support to BETAFPVF411 target (Peck07) * Delete accgyro_spi_bmi270.c.newww (Peck07) * change to OSD4 by default (Peck07)(discussed with nerdCopter) * put back a working getBmiOsrMode function (Peck07)(discussed with nerdCopter) * LPF_EXPERIMENTAL should give back OSR2 (Peck07) * unneeded files cleanup and file move; target path change (nerdCopter) * reorganize gyroRateKHz with switch/case; add bmi270 3.2 KHz (nerdCopter) * bmi270 - more file cleanup (nerdCopter) * bmi270 - switch/case - samplingTime (nerdCopter) * bmi270 targets - really not sure about this CS_PIN stuff (nerdCopter) * BMI270 - FOXEERF745_AIO (V2) (nerdCopter from BeauBrewski) * BMI270 - NBD_INFINITYAIOV2PRO (nerdCopter) * added SkystarsF7 target updates (BeauBrewski) * fix FLYWOOF411_5IN1_AIO align (nerdCopter) * BMI270 - gyro/pid denom default 1 (nerdCopter) * BMI270 - GYRO_HARDWARE_LPF_EXPERIMENTAL 3 (nerdCopter) * BMI270 - deduplicate spiCalculateDivider in bus_spi.h (nerdCopter) * BMI270 - remove OWNER_GYRO_EXTI from resource.h to fix breakage (nerdCopter) * BMI270 - gyro_hardware_lpf options (USE_GYRO_DLPF_EXPERIMENTAL) (nerdCopter) * BMI270 - replace OWNER_GYRO_EXTI with OWNER_MPU_EXTI (nerdCopter) * BMI270 - fix NBD_INFINITYAIOV2PRO dual-gyro defines (nerdCopter) * fix IFLIGHT_F745_AIO_V2 .mk (nerdCopter) * BMI270 - add bmi gyros to accgyro_mpu.h ifdef (nerdCopter) * BMI270 - fix detect - fixes requirement for mpu6000 defines (nerdCopter) * BMI270 - ensure over (nerdCopter) * BMI270 - move bmi270_maximum_fifo_config_file constant (nerdCopter) * BMI270 - remove extraneous fifo.c file inclusions (nerdCopter) * BMI270 - comment spiCalculateDivider (nerdCopter) * BMI270 - sync targets (nerdCopter) * BMI270 - remove targets and targets modification from this PR (nerdCopter) * BMI270 - emptyline cleanup (nerdCopter) (2023-10-31) --------- Co-authored-by: Peck07 <28628457+Peck07@users.noreply.github.com> Co-authored-by: BeauBrewski <85623381+BeauBrewski@users.noreply.github.com> --- make/source.mk | 1 + src/main/drivers/accgyro/accgyro.h | 5 +- src/main/drivers/accgyro/accgyro_mpu.c | 14 + src/main/drivers/accgyro/accgyro_mpu.h | 3 +- src/main/drivers/accgyro/accgyro_spi_bmi270.c | 499 ++++++++++++++++++ src/main/drivers/accgyro/accgyro_spi_bmi270.h | 28 + src/main/drivers/accgyro/gyro_sync.c | 26 +- src/main/drivers/bus_spi.c | 1 + src/main/fc/config.c | 1 + src/main/flight/pid.c | 2 +- src/main/interface/settings.c | 8 +- src/main/pg/bus_spi.c | 3 + src/main/sensors/acceleration.c | 12 + src/main/sensors/acceleration.h | 1 + src/main/sensors/gyro.c | 17 +- src/main/sensors/gyro.h | 1 + src/main/target/common_fc_pre.h | 2 +- 17 files changed, 605 insertions(+), 19 deletions(-) create mode 100644 src/main/drivers/accgyro/accgyro_spi_bmi270.c create mode 100644 src/main/drivers/accgyro/accgyro_spi_bmi270.h 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/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/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 6bddbb9c5b..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) 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 231c6fa489..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 diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 9af64f98e5..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 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 eefea7e341..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" @@ -211,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 @@ -537,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)) { @@ -566,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 @@ -611,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: diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index b81475a75c..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; 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 From 218da79e563de518cd4fa203ffa20c1283c63345 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Wed, 1 Nov 2023 10:22:34 -0500 Subject: [PATCH 09/16] [target] BETAFPVF411 - add BMI270 (#937) --- src/main/target/BETAFPVF411/target.h | 7 +++++++ src/main/target/BETAFPVF411/target.mk | 1 + 2 files changed, 8 insertions(+) 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 \ From fa26c36cfc8769385dd0e27ebdfaf30b32add4eb Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 2 Nov 2023 11:29:04 -0500 Subject: [PATCH 10/16] [target] FLYWOOF411_5IN1_AIO - add BMI270 (#938) --- src/main/target/FLYWOOF411_5IN1_AIO/target.h | 8 +++++- src/main/target/FLYWOOF411_5IN1_AIO/target.mk | 25 ++++++++++--------- 2 files changed, 20 insertions(+), 13 deletions(-) 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 From 0d1a56a399d3455ca43081b676d00d4504fbd80a Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 2 Nov 2023 11:31:15 -0500 Subject: [PATCH 11/16] [target] FOXEERF745_AIO_V2 (BMI270) (#939) --- .../target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk | 0 src/main/target/FOXEERF745_AIO/target.h | 16 ++++++++++++++++ src/main/target/FOXEERF745_AIO/target.mk | 5 +++-- 3 files changed, 19 insertions(+), 2 deletions(-) create mode 100644 src/main/target/FOXEERF745_AIO/FOXEERF745_AIO_V2.mk 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 From 731894ae28f701601a044effc1cd3bdd314e2da3 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 2 Nov 2023 11:33:07 -0500 Subject: [PATCH 12/16] [target] IFLIGHT_F745_AIO_V2 add BMI270 (#940) --- src/main/target/IFLIGHT_F745_AIO_V2/target.h | 2 +- src/main/target/IFLIGHT_F745_AIO_V2/target.mk | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) 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 \ From c1ef73d4dbbce800952159586d26e5370462a599 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 2 Nov 2023 15:31:12 -0500 Subject: [PATCH 13/16] [target] NBD_INFINITYAIOV2PRO (BMI270) (#941) --- src/main/target/NBD_INFINITYAIOV2PRO/target.c | 37 ++++ src/main/target/NBD_INFINITYAIOV2PRO/target.h | 160 ++++++++++++++++++ .../target/NBD_INFINITYAIOV2PRO/target.mk | 13 ++ 3 files changed, 210 insertions(+) create mode 100644 src/main/target/NBD_INFINITYAIOV2PRO/target.c create mode 100644 src/main/target/NBD_INFINITYAIOV2PRO/target.h create mode 100644 src/main/target/NBD_INFINITYAIOV2PRO/target.mk 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 \ From 2bb2a72c3dce7f6ade3028fcfb74a8cd5ae36398 Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 2 Nov 2023 15:32:15 -0500 Subject: [PATCH 14/16] [target] SKYSTARSF7HD add BMI270 (#942) --- src/main/target/SKYSTARSF7HD/target.h | 7 +++++++ src/main/target/SKYSTARSF7HD/target.mk | 3 ++- 2 files changed, 9 insertions(+), 1 deletion(-) 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 From 01155c0cebd50672c33c7a68d25d6e6c4ec5379b Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Thu, 2 Nov 2023 15:34:56 -0500 Subject: [PATCH 15/16] [target] SKYSTARSF7HDPRO (#943) Co-authored-by: BeauBrewski <85623381+BeauBrewski@users.noreply.github.com> --- src/main/target/SKYSTARSF7HDPRO/config.c | 36 ++++ src/main/target/SKYSTARSF7HDPRO/target.c | 41 +++++ src/main/target/SKYSTARSF7HDPRO/target.h | 207 ++++++++++++++++++++++ src/main/target/SKYSTARSF7HDPRO/target.mk | 16 ++ 4 files changed, 300 insertions(+) create mode 100644 src/main/target/SKYSTARSF7HDPRO/config.c create mode 100644 src/main/target/SKYSTARSF7HDPRO/target.c create mode 100644 src/main/target/SKYSTARSF7HDPRO/target.h create mode 100644 src/main/target/SKYSTARSF7HDPRO/target.mk 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 From aaa46e69e2e89d2c97cc2a1d157fafd2ade3ba9d Mon Sep 17 00:00:00 2001 From: nerdCopter <56646290+nerdCopter@users.noreply.github.com> Date: Sat, 4 Nov 2023 16:14:22 -0500 Subject: [PATCH 16/16] [target] TMOTORF7 update; add ICM42688P and BMI270 (#944) * [target] TMOTORF7 update; add ICM42688P and BMI270 --- src/main/target/TMTR_TMOTORF7/target.c | 21 ++++++----- src/main/target/TMTR_TMOTORF7/target.h | 50 +++++++++++++++++++------ src/main/target/TMTR_TMOTORF7/target.mk | 7 +++- 3 files changed, 55 insertions(+), 23 deletions(-) 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 \