mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
Add 3+ IMU support (Gyro Fusion) (#14383)
* Allow for 3 gyros to be fused at once. * Handle the case where no GYRO_COUNT is defined * Fix accel init bug * Fix bugs found by AI * Fix compile time assert message * Move to picking which IMU you want to enable, allow IMU that have the same scale and looprate to run together even if they aren't identical IMU. * Fully support 8 IMU * Fix suggestions, except for MSP all suggestions * Fix bugs found by AI * Update gyro_init * Fix unit tests (feels wrong though) * Allow MSP to update all gyro alignment * resolve comments * Only auto set up to 4 gyros in a config. * Update MSP implementation * Fix divide by 0 error * Update src/main/target/common_post.h Co-authored-by: Mark Haslinghuis <mark@numloq.nl> * Handle case where gyro 1 does not exist * Fix 426XX driver * fix = logic in if statement * Update src/main/msp/msp.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/drivers/accgyro/accgyro_spi_icm426xx.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Apply ledvinap suggestions * Fix detectedSensors initialization * fix getGyroDetectedFlags * Automatically handle GYRO_COUNT for up to 4 IMU * better handle unit tests * Backwards compatible with older config.h files * Update src/main/sensors/gyro_init.c Co-authored-by: Mark Haslinghuis <mark@numloq.nl> * Update src/main/target/common_pre.h Co-authored-by: nerdCopter <56646290+nerdCopter@users.noreply.github.com> * Update src/main/sensors/gyro_init.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * Update src/main/sensors/gyro.c Co-authored-by: Petr Ledvina <ledvinap@gmail.com> * This needs to be the case or unit tests fail, without this we cannot choose default gyro either. * ledvinap suggestions * whitespace --------- Co-authored-by: Mark Haslinghuis <mark@numloq.nl> Co-authored-by: Petr Ledvina <ledvinap@gmail.com> Co-authored-by: nerdCopter <56646290+nerdCopter@users.noreply.github.com>
This commit is contained in:
Kevin Plaizier
GitHub
parent
3370752442
commit
078ffafec1
19 changed files with 495 additions and 381 deletions
|
|
@ -1687,7 +1687,7 @@ static bool blackboxWriteSysinfo(void)
|
|||
gyroConfig()->gyro_soft_notch_hz_2);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_soft_notch_cutoff_1,
|
||||
gyroConfig()->gyro_soft_notch_cutoff_2);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_TO_USE, "%d", gyroConfig()->gyro_to_use);
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_GYRO_ENABLE_MASK, "%d", gyroConfig()->gyro_enabled_bitmask);
|
||||
BLACKBOX_PRINT_HEADER_LINE("gyro_debug_axis", "%d", gyroConfig()->gyro_filter_debug_axis);
|
||||
#ifdef USE_DYN_NOTCH_FILTER
|
||||
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_MAX_HZ, "%d", dynNotchConfig()->dyn_notch_max_hz);
|
||||
|
|
|
|||
|
|
@ -3675,18 +3675,12 @@ static void cliDumpGyroRegisters(const char *cmdName, char *cmdline)
|
|||
UNUSED(cmdName);
|
||||
UNUSED(cmdline);
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_1) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) {
|
||||
cliPrintLinef("\r\n# Gyro 1");
|
||||
cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1);
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
if (gyroConfig()->gyro_enabled_bitmask & GYRO_MASK(i)) {
|
||||
cliPrintLinef("\r\n# Gyro %d", i + 1);
|
||||
cliPrintGyroRegisters(i); // assuming this takes a 0-based gyro index
|
||||
}
|
||||
if ((gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) || (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_BOTH)) {
|
||||
cliPrintLinef("\r\n# Gyro 2");
|
||||
cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_2);
|
||||
}
|
||||
#else
|
||||
cliPrintGyroRegisters(GYRO_CONFIG_USE_GYRO_1);
|
||||
#endif
|
||||
}
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -244,12 +244,6 @@ static const char * const lookupTableAlignment[] = {
|
|||
"CUSTOM",
|
||||
};
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
static const char * const lookupTableGyro[] = {
|
||||
"FIRST", "SECOND", "BOTH"
|
||||
};
|
||||
#endif
|
||||
|
||||
#ifdef USE_GPS
|
||||
static const char * const lookupTableGpsProvider[] = {
|
||||
"NMEA", "UBLOX", "MSP", "VIRTUAL"
|
||||
|
|
@ -679,9 +673,6 @@ const lookupTableEntry_t lookupTables[] = {
|
|||
#endif
|
||||
#ifdef USE_LED_STRIP
|
||||
LOOKUP_TABLE_ENTRY(lookupLedStripFormatRGB),
|
||||
#endif
|
||||
#ifdef USE_MULTI_GYRO
|
||||
LOOKUP_TABLE_ENTRY(lookupTableGyro),
|
||||
#endif
|
||||
LOOKUP_TABLE_ENTRY(lookupTableThrottleLimitType),
|
||||
#if defined(USE_VIDEO_SYSTEM)
|
||||
|
|
@ -774,9 +765,30 @@ const clivalue_t valueTable[] = {
|
|||
{ "yaw_spin_threshold", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { YAW_SPIN_RECOVERY_THRESHOLD_MIN, YAW_SPIN_RECOVERY_THRESHOLD_MAX }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, yaw_spin_threshold) },
|
||||
#endif
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
{ PARAM_NAME_GYRO_TO_USE, VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_GYRO }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_to_use) },
|
||||
#if GYRO_COUNT > 1
|
||||
{ PARAM_NAME_GYRO_ENABLE_MASK, VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, (1 << GYRO_COUNT) - 1 }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) },
|
||||
{ "gyro_1_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 0, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) },
|
||||
{ "gyro_2_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 1, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) },
|
||||
#endif
|
||||
#if GYRO_COUNT > 2
|
||||
{ "gyro_3_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 2, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) },
|
||||
#endif
|
||||
#if GYRO_COUNT > 3
|
||||
{ "gyro_4_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 3, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) },
|
||||
#endif
|
||||
#if GYRO_COUNT > 4
|
||||
{ "gyro_5_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 4, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) },
|
||||
#endif
|
||||
#if GYRO_COUNT > 5
|
||||
{ "gyro_6_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 5, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) },
|
||||
#endif
|
||||
#if GYRO_COUNT > 6
|
||||
{ "gyro_7_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 6, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) },
|
||||
#endif
|
||||
#if GYRO_COUNT > 7
|
||||
{ "gyro_8_enabled", VAR_UINT8 | HARDWARE_VALUE | MODE_BITSET, .config.bitpos = 7, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_enabled_bitmask) },
|
||||
#endif // GYRO_COUNT
|
||||
|
||||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
{ PARAM_NAME_DYN_NOTCH_COUNT, VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, DYN_NOTCH_COUNT_MAX }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_count) },
|
||||
{ PARAM_NAME_DYN_NOTCH_Q, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 1, 1000 }, PG_DYN_NOTCH_CONFIG, offsetof(dynNotchConfig_t, dyn_notch_q) },
|
||||
|
|
@ -1826,23 +1838,43 @@ const clivalue_t valueTable[] = {
|
|||
#endif
|
||||
|
||||
// PG_GYRO_DEVICE_CONFIG
|
||||
{ "gyro_1_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, busType) },
|
||||
{ "gyro_1_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, spiBus) },
|
||||
{ "gyro_1_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cBus) },
|
||||
{ "gyro_1_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, i2cAddress) },
|
||||
{ "gyro_1_sensor_align", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, alignment) },
|
||||
{ "gyro_1_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.roll) },
|
||||
{ "gyro_1_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.pitch) },
|
||||
{ "gyro_1_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 0, customAlignment.yaw) },
|
||||
#ifdef USE_MULTI_GYRO
|
||||
{ "gyro_2_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, busType) },
|
||||
{ "gyro_2_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, spiBus) },
|
||||
{ "gyro_2_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cBus) },
|
||||
{ "gyro_2_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, i2cAddress) },
|
||||
{ "gyro_2_sensor_align", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, alignment) },
|
||||
{ "gyro_2_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, customAlignment.roll) },
|
||||
{ "gyro_2_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, customAlignment.pitch) },
|
||||
{ "gyro_2_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, 1, customAlignment.yaw) },
|
||||
#define GYRO_DEVICE_RECORDS(N, IDX) \
|
||||
/* bustype */ \
|
||||
{ "gyro_" STR(N) "_bustype", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_BUS_TYPE }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, busType) },\
|
||||
/* spibus */ \
|
||||
{ "gyro_" STR(N) "_spibus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, SPIDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, spiBus) },\
|
||||
/* i2cBus */ \
|
||||
{ "gyro_" STR(N) "_i2cBus", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2CDEV_COUNT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, i2cBus) },\
|
||||
/* i2c addr */ \
|
||||
{ "gyro_" STR(N) "_i2c_address", VAR_UINT8 | HARDWARE_VALUE, .config.minmaxUnsigned = { 0, I2C_ADDR7_MAX }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, i2cAddress) },\
|
||||
/* align lookup */ \
|
||||
{ "gyro_" STR(N) "_sensor_align", VAR_UINT8 | HARDWARE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, alignment) },\
|
||||
/* custom roll/pitch/yaw */ \
|
||||
{ "gyro_" STR(N) "_align_roll", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, customAlignment.roll) },\
|
||||
{ "gyro_" STR(N) "_align_pitch", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, customAlignment.pitch) },\
|
||||
{ "gyro_" STR(N) "_align_yaw", VAR_INT16 | HARDWARE_VALUE, .config.minmax = { -3600, 3600 }, PG_GYRO_DEVICE_CONFIG, PG_ARRAY_ELEMENT_OFFSET(gyroDeviceConfig_t, IDX, customAlignment.yaw) }\
|
||||
|
||||
GYRO_DEVICE_RECORDS(1, 0),
|
||||
#if GYRO_COUNT > 1
|
||||
GYRO_DEVICE_RECORDS(2, 1),
|
||||
#endif
|
||||
#if GYRO_COUNT > 2
|
||||
GYRO_DEVICE_RECORDS(3, 2),
|
||||
#endif
|
||||
#if GYRO_COUNT > 3
|
||||
GYRO_DEVICE_RECORDS(4, 3),
|
||||
#endif
|
||||
#if GYRO_COUNT > 4
|
||||
GYRO_DEVICE_RECORDS(5, 4),
|
||||
#endif
|
||||
#if GYRO_COUNT > 5
|
||||
GYRO_DEVICE_RECORDS(6, 5),
|
||||
#endif
|
||||
#if GYRO_COUNT > 6
|
||||
GYRO_DEVICE_RECORDS(7, 6),
|
||||
#endif
|
||||
#if GYRO_COUNT > 7
|
||||
GYRO_DEVICE_RECORDS(8, 7),
|
||||
#endif
|
||||
#ifdef I2C_FULL_RECONFIGURABILITY
|
||||
#ifdef USE_I2C_DEVICE_0
|
||||
|
|
|
|||
|
|
@ -94,9 +94,6 @@ typedef enum {
|
|||
#endif
|
||||
#ifdef USE_LED_STRIP
|
||||
TABLE_RGB_GRB,
|
||||
#endif
|
||||
#ifdef USE_MULTI_GYRO
|
||||
TABLE_GYRO,
|
||||
#endif
|
||||
TABLE_THROTTLE_LIMIT_TYPE,
|
||||
#if defined(USE_VIDEO_SYSTEM)
|
||||
|
|
|
|||
|
|
@ -76,12 +76,6 @@ static uint8_t rateProfileIndex;
|
|||
static char rateProfileIndexString[MAX_RATE_PROFILE_NAME_LENGTH + PROFILE_INDEX_STRING_ADDITIONAL_SIZE];
|
||||
static controlRateConfig_t rateProfile;
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
static const char * const osdTableGyroToUse[] = {
|
||||
"FIRST", "SECOND", "BOTH"
|
||||
};
|
||||
#endif
|
||||
|
||||
static void setProfileIndexString(char *profileString, int profileIndex, const char *profileName)
|
||||
{
|
||||
int charIndex = 0;
|
||||
|
|
@ -752,7 +746,6 @@ static uint16_t gyroConfig_gyro_soft_notch_hz_1;
|
|||
static uint16_t gyroConfig_gyro_soft_notch_cutoff_1;
|
||||
static uint16_t gyroConfig_gyro_soft_notch_hz_2;
|
||||
static uint16_t gyroConfig_gyro_soft_notch_cutoff_2;
|
||||
static uint8_t gyroConfig_gyro_to_use;
|
||||
|
||||
static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp)
|
||||
{
|
||||
|
|
@ -764,7 +757,6 @@ static const void *cmsx_menuGyro_onEnter(displayPort_t *pDisp)
|
|||
gyroConfig_gyro_soft_notch_cutoff_1 = gyroConfig()->gyro_soft_notch_cutoff_1;
|
||||
gyroConfig_gyro_soft_notch_hz_2 = gyroConfig()->gyro_soft_notch_hz_2;
|
||||
gyroConfig_gyro_soft_notch_cutoff_2 = gyroConfig()->gyro_soft_notch_cutoff_2;
|
||||
gyroConfig_gyro_to_use = gyroConfig()->gyro_to_use;
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
|
@ -780,7 +772,6 @@ static const void *cmsx_menuGyro_onExit(displayPort_t *pDisp, const OSD_Entry *s
|
|||
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = gyroConfig_gyro_soft_notch_cutoff_1;
|
||||
gyroConfigMutable()->gyro_soft_notch_hz_2 = gyroConfig_gyro_soft_notch_hz_2;
|
||||
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = gyroConfig_gyro_soft_notch_cutoff_2;
|
||||
gyroConfigMutable()->gyro_to_use = gyroConfig_gyro_to_use;
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
|
@ -797,9 +788,6 @@ static const OSD_Entry cmsx_menuFilterGlobalEntries[] =
|
|||
{ "GYRO NF1C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_1, 0, 500, 1 } },
|
||||
{ "GYRO NF2", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_hz_2, 0, 500, 1 } },
|
||||
{ "GYRO NF2C", OME_UINT16, NULL, &(OSD_UINT16_t) { &gyroConfig_gyro_soft_notch_cutoff_2, 0, 500, 1 } },
|
||||
#ifdef USE_MULTI_GYRO
|
||||
{ "GYRO TO USE", OME_TAB | REBOOT_REQUIRED, NULL, &(OSD_TAB_t) { &gyroConfig_gyro_to_use, 2, osdTableGyroToUse} },
|
||||
#endif
|
||||
|
||||
{ "BACK", OME_Back, NULL, NULL },
|
||||
{ NULL, OME_END, NULL, NULL}
|
||||
|
|
|
|||
|
|
@ -293,10 +293,10 @@ static void validateAndFixConfig(void)
|
|||
#if defined(USE_MAG)
|
||||
buildAlignmentFromStandardAlignment(&compassConfigMutable()->mag_customAlignment, compassConfig()->mag_alignment);
|
||||
#endif
|
||||
buildAlignmentFromStandardAlignment(&gyroDeviceConfigMutable(0)->customAlignment, gyroDeviceConfig(0)->alignment);
|
||||
#if defined(USE_MULTI_GYRO)
|
||||
buildAlignmentFromStandardAlignment(&gyroDeviceConfigMutable(1)->customAlignment, gyroDeviceConfig(1)->alignment);
|
||||
#endif
|
||||
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
buildAlignmentFromStandardAlignment(&gyroDeviceConfigMutable(i)->customAlignment, gyroDeviceConfig(i)->alignment);
|
||||
}
|
||||
|
||||
#ifdef USE_ACC
|
||||
if (accelerometerConfig()->accZero.values.roll != 0 ||
|
||||
|
|
|
|||
|
|
@ -190,17 +190,25 @@ static void setUserBank(const extDevice_t *dev, const uint8_t user_bank)
|
|||
#if defined(USE_GYRO_CLKIN)
|
||||
static pwmOutputPort_t pwmGyroClk = {0};
|
||||
|
||||
static int findByExtDevice(const extDevice_t *dev) {
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
if (&gyro.gyroSensor[i].gyroDev.dev == dev) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
static bool initExternalClock(const extDevice_t *dev)
|
||||
{
|
||||
int cfg;
|
||||
if (&gyro.gyroSensor1.gyroDev.dev == dev) {
|
||||
cfg = 0;
|
||||
} else if (&gyro.gyroSensor2.gyroDev.dev == dev) {
|
||||
cfg = 1;
|
||||
} else {
|
||||
// only gyroSensor<n> device supported
|
||||
const int cfg = findByExtDevice(dev);
|
||||
|
||||
if (cfg < 0) {
|
||||
// Could not find a valid sensor
|
||||
return false;
|
||||
}
|
||||
|
||||
const ioTag_t tag = gyroDeviceConfig(cfg)->clkIn;
|
||||
const IO_t io = IOGetByTag(tag);
|
||||
if (pwmGyroClk.enabled) {
|
||||
|
|
|
|||
|
|
@ -25,7 +25,7 @@
|
|||
#define PARAM_NAME_GYRO_LPF1_STATIC_HZ "gyro_lpf1_static_hz"
|
||||
#define PARAM_NAME_GYRO_LPF2_TYPE "gyro_lpf2_type"
|
||||
#define PARAM_NAME_GYRO_LPF2_STATIC_HZ "gyro_lpf2_static_hz"
|
||||
#define PARAM_NAME_GYRO_TO_USE "gyro_to_use"
|
||||
#define PARAM_NAME_GYRO_ENABLE_MASK "gyro_enable_bitmask"
|
||||
#define PARAM_NAME_DYN_NOTCH_MAX_HZ "dyn_notch_max_hz"
|
||||
#define PARAM_NAME_DYN_NOTCH_COUNT "dyn_notch_count"
|
||||
#define PARAM_NAME_DYN_NOTCH_Q "dyn_notch_q"
|
||||
|
|
|
|||
|
|
@ -1812,21 +1812,7 @@ case MSP_NAME:
|
|||
break;
|
||||
|
||||
case MSP_SENSOR_ALIGNMENT: {
|
||||
uint8_t gyroAlignment;
|
||||
#ifdef USE_MULTI_GYRO
|
||||
switch (gyroConfig()->gyro_to_use) {
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
gyroAlignment = gyroDeviceConfig(1)->alignment;
|
||||
break;
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH:
|
||||
// for dual-gyro in "BOTH" mode we only read/write gyro 0
|
||||
default:
|
||||
gyroAlignment = gyroDeviceConfig(0)->alignment;
|
||||
break;
|
||||
}
|
||||
#else
|
||||
gyroAlignment = gyroDeviceConfig(0)->alignment;
|
||||
#endif
|
||||
uint8_t gyroAlignment = gyroDeviceConfig(firstEnabledGyro())->alignment;
|
||||
sbufWriteU8(dst, gyroAlignment);
|
||||
sbufWriteU8(dst, gyroAlignment); // Starting with 4.0 gyro and acc alignment are the same
|
||||
#if defined(USE_MAG)
|
||||
|
|
@ -1836,32 +1822,17 @@ case MSP_NAME:
|
|||
#endif
|
||||
|
||||
// API 1.41 - Add multi-gyro indicator, selected gyro, and support for separate gyro 1 & 2 alignment
|
||||
sbufWriteU8(dst, getGyroDetectionFlags());
|
||||
#ifdef USE_MULTI_GYRO
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_to_use);
|
||||
sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
|
||||
sbufWriteU8(dst, gyroDeviceConfig(1)->alignment);
|
||||
#else
|
||||
sbufWriteU8(dst, GYRO_CONFIG_USE_GYRO_1);
|
||||
sbufWriteU8(dst, gyroDeviceConfig(0)->alignment);
|
||||
sbufWriteU8(dst, ALIGN_DEFAULT);
|
||||
#endif
|
||||
// Added in MSP API 1.47
|
||||
switch (gyroConfig()->gyro_to_use) {
|
||||
#ifdef USE_MULTI_GYRO
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.roll);
|
||||
sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.pitch);
|
||||
sbufWriteU16(dst, gyroDeviceConfig(1)->customAlignment.yaw);
|
||||
break;
|
||||
#endif
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH:
|
||||
// for dual-gyro in "BOTH" mode we only read/write gyro 0
|
||||
default:
|
||||
sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.roll);
|
||||
sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.pitch);
|
||||
sbufWriteU16(dst, gyroDeviceConfig(0)->customAlignment.yaw);
|
||||
break;
|
||||
sbufWriteU8(dst, getGyroDetectedFlags());
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_enabled_bitmask); // deprecates gyro_to_use
|
||||
// Added support for more then two IMUs in MSP API 1.47
|
||||
for (int i = 0; i < 8; i++) {
|
||||
sbufWriteU8(dst, i < GYRO_COUNT ? gyroDeviceConfig(i)->alignment : ALIGN_DEFAULT);
|
||||
}
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
for (unsigned j; j < ARRAYLEN(gyroDeviceConfig(i)->customAlignment.raw); j++) {
|
||||
sbufWriteU16(dst, i < GYRO_COUNT ? gyroDeviceConfig(i)->customAlignment.raw[j] : 0);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef USE_MAG
|
||||
|
|
@ -1885,7 +1856,7 @@ case MSP_NAME:
|
|||
sbufWriteU16(dst, motorConfig()->motorIdle);
|
||||
sbufWriteU8(dst, 0); // DEPRECATED: gyro_use_32kHz
|
||||
sbufWriteU8(dst, motorConfig()->dev.motorInversion);
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_to_use);
|
||||
sbufWriteU8(dst, 0); // deprecated gyro_to_use
|
||||
sbufWriteU8(dst, gyroConfig()->gyro_high_fsr);
|
||||
sbufWriteU8(dst, gyroConfig()->gyroMovementCalibrationThreshold);
|
||||
sbufWriteU16(dst, gyroConfig()->gyroCalibrationDuration);
|
||||
|
|
@ -3014,8 +2985,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
break;
|
||||
|
||||
case MSP_SET_SENSOR_ALIGNMENT: {
|
||||
// maintain backwards compatibility for API < 1.41
|
||||
const uint8_t gyroAlignment = sbufReadU8(src);
|
||||
sbufReadU8(src);
|
||||
sbufReadU8(src); // discard deprecated acc_align
|
||||
#if defined(USE_MAG)
|
||||
compassConfigMutable()->mag_alignment = sbufReadU8(src);
|
||||
|
|
@ -3023,53 +2993,33 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
sbufReadU8(src);
|
||||
#endif
|
||||
|
||||
if (sbufBytesRemaining(src) >= 3) {
|
||||
// API >= 1.41 - support the gyro_to_use and alignment for gyros 1 & 2
|
||||
#ifdef USE_MULTI_GYRO
|
||||
gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
|
||||
gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src);
|
||||
gyroDeviceConfigMutable(1)->alignment = sbufReadU8(src);
|
||||
#else
|
||||
sbufReadU8(src); // unused gyro_to_use
|
||||
gyroDeviceConfigMutable(0)->alignment = sbufReadU8(src);
|
||||
sbufReadU8(src); // unused gyro_2_sensor_align
|
||||
#endif
|
||||
} else {
|
||||
// maintain backwards compatibility for API < 1.41
|
||||
#ifdef USE_MULTI_GYRO
|
||||
switch (gyroConfig()->gyro_to_use) {
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
gyroDeviceConfigMutable(1)->alignment = gyroAlignment;
|
||||
break;
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH:
|
||||
// For dual-gyro in "BOTH" mode we'll only update gyro 0
|
||||
default:
|
||||
gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
|
||||
break;
|
||||
}
|
||||
#else
|
||||
gyroDeviceConfigMutable(0)->alignment = gyroAlignment;
|
||||
#endif
|
||||
}
|
||||
gyroConfigMutable()->gyro_enabled_bitmask = sbufReadU8(src);
|
||||
// Added in API 1.47
|
||||
if (sbufBytesRemaining(src) >= 6) {
|
||||
switch (gyroConfig()->gyro_to_use) {
|
||||
#ifdef USE_MULTI_GYRO
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
gyroDeviceConfigMutable(1)->customAlignment.roll = sbufReadU16(src);
|
||||
gyroDeviceConfigMutable(1)->customAlignment.pitch = sbufReadU16(src);
|
||||
gyroDeviceConfigMutable(1)->customAlignment.yaw = sbufReadU16(src);
|
||||
break;
|
||||
#endif
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH:
|
||||
// For dual-gyro in "BOTH" mode we'll only update gyro 0
|
||||
default:
|
||||
gyroDeviceConfigMutable(0)->customAlignment.roll = sbufReadU16(src);
|
||||
gyroDeviceConfigMutable(0)->customAlignment.pitch = sbufReadU16(src);
|
||||
gyroDeviceConfigMutable(0)->customAlignment.yaw = sbufReadU16(src);
|
||||
break;
|
||||
if (sbufBytesRemaining(src) >= 8 * 7) {
|
||||
for (int i = 0; i < 8; i++) {
|
||||
const uint8_t alignment = sbufReadU8(src);
|
||||
if (i < GYRO_COUNT) {
|
||||
gyroDeviceConfigMutable(i)->alignment = alignment;
|
||||
}
|
||||
}
|
||||
|
||||
for (int i = 0; i < 8; i++) {
|
||||
if (i < GYRO_COUNT) {
|
||||
sensorAlignment_t customAlignment;
|
||||
for (unsigned j = 0; j < ARRAYLEN(customAlignment.raw); j++) {
|
||||
customAlignment.raw[j] = (int16_t)sbufReadU16(src);
|
||||
}
|
||||
if (i < GYRO_COUNT) {
|
||||
gyroDeviceConfigMutable(i)->customAlignment = customAlignment;
|
||||
}
|
||||
} else {
|
||||
sbufReadU16(src); // skip unused custom alignment roll
|
||||
sbufReadU16(src); // skip unused custom alignment pitch
|
||||
sbufReadU16(src); // skip unused custom alignment yaw
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (sbufBytesRemaining(src) >= 6) {
|
||||
#ifdef USE_MAG
|
||||
compassConfigMutable()->mag_customAlignment.roll = sbufReadU16(src);
|
||||
|
|
@ -3081,6 +3031,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
sbufReadU16(src);
|
||||
#endif
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
|
|
@ -3100,7 +3051,7 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP,
|
|||
motorConfigMutable()->dev.motorInversion = sbufReadU8(src);
|
||||
}
|
||||
if (sbufBytesRemaining(src) >= 8) {
|
||||
gyroConfigMutable()->gyro_to_use = sbufReadU8(src);
|
||||
sbufReadU8(src); // deprecated gyro_to_use
|
||||
gyroConfigMutable()->gyro_high_fsr = sbufReadU8(src);
|
||||
gyroConfigMutable()->gyroMovementCalibrationThreshold = sbufReadU8(src);
|
||||
gyroConfigMutable()->gyroCalibrationDuration = sbufReadU16(src);
|
||||
|
|
|
|||
|
|
@ -35,6 +35,10 @@
|
|||
#include "drivers/sensor.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
#ifdef SIMULATOR_BUILD
|
||||
#define GYRO_COUNT 1
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_1_CS_PIN
|
||||
#define GYRO_1_CS_PIN NONE
|
||||
#endif
|
||||
|
|
@ -43,6 +47,10 @@
|
|||
#define GYRO_1_EXTI_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_1_CLKIN_PIN
|
||||
#define GYRO_1_CLKIN_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_2_CS_PIN
|
||||
#define GYRO_2_CS_PIN NONE
|
||||
#endif
|
||||
|
|
@ -51,14 +59,34 @@
|
|||
#define GYRO_2_EXTI_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_1_CLKIN_PIN
|
||||
#define GYRO_1_CLKIN_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_2_CLKIN_PIN
|
||||
#define GYRO_2_CLKIN_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_3_CS_PIN
|
||||
#define GYRO_3_CS_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_3_EXTI_PIN
|
||||
#define GYRO_3_EXTI_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_3_CLKIN_PIN
|
||||
#define GYRO_3_CLKIN_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_4_CS_PIN
|
||||
#define GYRO_4_CS_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_4_EXTI_PIN
|
||||
#define GYRO_4_EXTI_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_4_CLKIN_PIN
|
||||
#define GYRO_4_CLKIN_PIN NONE
|
||||
#endif
|
||||
|
||||
#ifdef MPU_ADDRESS
|
||||
#define GYRO_I2C_ADDRESS MPU_ADDRESS
|
||||
#else
|
||||
|
|
@ -102,6 +130,40 @@
|
|||
#endif
|
||||
#endif // GYRO_2_ALIGN_ROLL || GYRO_2_ALIGN_PITCH || GYRO_2_ALIGN_YAW
|
||||
|
||||
#if defined(GYRO_3_ALIGN_ROLL) || defined(GYRO_3_ALIGN_PITCH) || defined(GYRO_3_ALIGN_YAW)
|
||||
#ifndef GYRO_3_ALIGN_ROLL
|
||||
#define GYRO_3_ALIGN_ROLL 0
|
||||
#endif
|
||||
#ifndef GYRO_3_ALIGN_PITCH
|
||||
#define GYRO_3_ALIGN_PITCH 0
|
||||
#endif
|
||||
#ifndef GYRO_3_ALIGN_YAW
|
||||
#define GYRO_3_ALIGN_YAW 0
|
||||
#endif
|
||||
#ifndef GYRO_3_CUSTOM_ALIGN
|
||||
#define GYRO_3_CUSTOM_ALIGN SENSOR_ALIGNMENT( GYRO_3_ALIGN_ROLL / 10, GYRO_3_ALIGN_PITCH / 10, GYRO_3_ALIGN_YAW / 10 )
|
||||
#else
|
||||
#error "GYRO_3_ALIGN_x and GYRO_3_CUSTOM_ALIGN are mutually exclusive"
|
||||
#endif
|
||||
#endif // GYRO_3_ALIGN_ROLL || GYRO_3_ALIGN_PITCH || GYRO_3_ALIGN_YAW
|
||||
|
||||
#if defined(GYRO_4_ALIGN_ROLL) || defined(GYRO_4_ALIGN_PITCH) || defined(GYRO_4_ALIGN_YAW)
|
||||
#ifndef GYRO_4_ALIGN_ROLL
|
||||
#define GYRO_4_ALIGN_ROLL 0
|
||||
#endif
|
||||
#ifndef GYRO_4_ALIGN_PITCH
|
||||
#define GYRO_4_ALIGN_PITCH 0
|
||||
#endif
|
||||
#ifndef GYRO_4_ALIGN_YAW
|
||||
#define GYRO_4_ALIGN_YAW 0
|
||||
#endif
|
||||
#ifndef GYRO_4_CUSTOM_ALIGN
|
||||
#define GYRO_4_CUSTOM_ALIGN SENSOR_ALIGNMENT( GYRO_4_ALIGN_ROLL / 10, GYRO_4_ALIGN_PITCH / 10, GYRO_4_ALIGN_YAW / 10 )
|
||||
#else
|
||||
#error "GYRO_4_ALIGN_x and GYRO_4_CUSTOM_ALIGN are mutually exclusive"
|
||||
#endif
|
||||
#endif // GYRO_4_ALIGN_ROLL || GYRO_4_ALIGN_PITCH || GYRO_4_ALIGN_YAW
|
||||
|
||||
#ifndef GYRO_1_ALIGN
|
||||
#ifdef GYRO_1_CUSTOM_ALIGN
|
||||
#define GYRO_1_ALIGN ALIGN_CUSTOM
|
||||
|
|
@ -130,6 +192,34 @@ STATIC_ASSERT(GYRO_1_ALIGN == ALIGN_CUSTOM, "GYRO_1_ALIGN and GYRO_1_CUSTOM_ALIG
|
|||
STATIC_ASSERT(GYRO_2_ALIGN == ALIGN_CUSTOM, "GYRO_2_ALIGN and GYRO_2_CUSTOM_ALIGN mixed");
|
||||
#endif // GYRO_2_CUSTOM_ALIGN
|
||||
|
||||
#ifndef GYRO_3_ALIGN
|
||||
#ifdef GYRO_3_CUSTOM_ALIGN
|
||||
#define GYRO_3_ALIGN ALIGN_CUSTOM
|
||||
#else
|
||||
#define GYRO_3_ALIGN CW0_DEG
|
||||
#endif
|
||||
#endif // GYRO_3_ALIGN
|
||||
|
||||
#ifndef GYRO_3_CUSTOM_ALIGN
|
||||
#define GYRO_3_CUSTOM_ALIGN SENSOR_ALIGNMENT_FROM_STD(GYRO_3_ALIGN)
|
||||
#else
|
||||
STATIC_ASSERT(GYRO_3_ALIGN == ALIGN_CUSTOM, "GYRO_3_ALIGN and GYRO_3_CUSTOM_ALIGN mixed");
|
||||
#endif // GYRO_3_CUSTOM_ALIGN
|
||||
|
||||
#ifndef GYRO_4_ALIGN
|
||||
#ifdef GYRO_4_CUSTOM_ALIGN
|
||||
#define GYRO_4_ALIGN ALIGN_CUSTOM
|
||||
#else
|
||||
#define GYRO_4_ALIGN CW0_DEG
|
||||
#endif
|
||||
#endif // GYRO_4_ALIGN
|
||||
|
||||
#ifndef GYRO_4_CUSTOM_ALIGN
|
||||
#define GYRO_4_CUSTOM_ALIGN SENSOR_ALIGNMENT_FROM_STD(GYRO_4_ALIGN)
|
||||
#else
|
||||
STATIC_ASSERT(GYRO_4_ALIGN == ALIGN_CUSTOM, "GYRO_4_ALIGN and GYRO_4_CUSTOM_ALIGN mixed");
|
||||
#endif // GYRO_4_CUSTOM_ALIGN
|
||||
|
||||
#if defined(USE_SPI_GYRO) && (defined(GYRO_1_SPI_INSTANCE) || defined(GYRO_2_SPI_INSTANCE))
|
||||
static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *instance, ioTag_t csnTag, ioTag_t extiTag, ioTag_t clkInTag, uint8_t alignment, sensorAlignment_t customAlignment)
|
||||
{
|
||||
|
|
@ -143,7 +233,7 @@ static void gyroResetSpiDeviceConfig(gyroDeviceConfig_t *devconf, SPI_TypeDef *i
|
|||
}
|
||||
#endif
|
||||
|
||||
#if defined(USE_I2C_GYRO) && !defined(USE_MULTI_GYRO)
|
||||
#if defined(USE_I2C_GYRO) && !(GYRO_COUNT > 1)
|
||||
static void gyroResetI2cDeviceConfig(gyroDeviceConfig_t *devconf, I2CDevice i2cbus, ioTag_t extiTag, uint8_t alignment, sensorAlignment_t customAlignment)
|
||||
{
|
||||
devconf->busType = BUS_TYPE_I2C;
|
||||
|
|
@ -162,25 +252,58 @@ void pgResetFn_gyroDeviceConfig(gyroDeviceConfig_t *devconf)
|
|||
devconf[0].index = 0;
|
||||
// All multi-gyro boards use SPI based gyros.
|
||||
#ifdef USE_SPI_GYRO
|
||||
|
||||
#define GYRO_RESET(index, num) \
|
||||
gyroResetSpiDeviceConfig(&devconf[index], GYRO_##num##_SPI_INSTANCE, \
|
||||
IO_TAG(GYRO_##num##_CS_PIN), IO_TAG(GYRO_##num##_EXTI_PIN), \
|
||||
IO_TAG(GYRO_##num##_CLKIN_PIN), GYRO_##num##_ALIGN, GYRO_##num##_CUSTOM_ALIGN)
|
||||
|
||||
#ifdef GYRO_1_SPI_INSTANCE
|
||||
gyroResetSpiDeviceConfig(&devconf[0], GYRO_1_SPI_INSTANCE, IO_TAG(GYRO_1_CS_PIN), IO_TAG(GYRO_1_EXTI_PIN), IO_TAG(GYRO_1_CLKIN_PIN), GYRO_1_ALIGN, GYRO_1_CUSTOM_ALIGN);
|
||||
GYRO_RESET(0, 1);
|
||||
#else
|
||||
devconf[0].busType = BUS_TYPE_NONE;
|
||||
#endif
|
||||
#ifdef USE_MULTI_GYRO
|
||||
|
||||
#if GYRO_COUNT > 1
|
||||
devconf[1].index = 1;
|
||||
#ifdef GYRO_2_SPI_INSTANCE
|
||||
// TODO: CLKIN gyro 2 on separate pin is not supported yet. need to implement it
|
||||
gyroResetSpiDeviceConfig(&devconf[1], GYRO_2_SPI_INSTANCE, IO_TAG(GYRO_2_CS_PIN), IO_TAG(GYRO_2_EXTI_PIN), IO_TAG(GYRO_2_CLKIN_PIN), GYRO_2_ALIGN, GYRO_2_CUSTOM_ALIGN);
|
||||
GYRO_RESET(1, 2);
|
||||
#else
|
||||
devconf[1].busType = BUS_TYPE_NONE;
|
||||
#endif
|
||||
#endif // GYRO_COUNT 2
|
||||
|
||||
#endif // USE_MULTI_GYRO
|
||||
#if GYRO_COUNT > 2
|
||||
devconf[2].index = 2;
|
||||
#ifdef GYRO_3_SPI_INSTANCE
|
||||
// TODO: CLKIN gyro 3 on separate pin is not supported yet. need to implement it
|
||||
GYRO_RESET(2, 3);
|
||||
#else
|
||||
devconf[2].busType = BUS_TYPE_NONE;
|
||||
#endif
|
||||
#endif // GYRO_COUNT 3
|
||||
|
||||
#if GYRO_COUNT > 3
|
||||
devconf[3].index = 3;
|
||||
#ifdef GYRO_4_SPI_INSTANCE
|
||||
// TODO: CLKIN gyro 4 on separate pin is not supported yet. need to implement it
|
||||
GYRO_RESET(3, 4);
|
||||
#else
|
||||
devconf[3].busType = BUS_TYPE_NONE;
|
||||
#endif
|
||||
#endif // GYRO_COUNT 4
|
||||
|
||||
#if GYRO_COUNT > 4
|
||||
// reset all gyro greater than gyro 5
|
||||
for (int i = 4; i < 8; i++) {
|
||||
devconf[i].busType = BUS_TYPE_NONE;
|
||||
}
|
||||
#endif
|
||||
#endif // USE_SPI_GYRO
|
||||
|
||||
// I2C gyros appear as a sole gyro in single gyro boards.
|
||||
#if defined(USE_I2C_GYRO) && !defined(USE_MULTI_GYRO)
|
||||
#if defined(USE_I2C_GYRO) && !(GYRO_COUNT > 1)
|
||||
devconf[0].i2cBus = I2C_DEV_TO_CFG(I2CINVALID); // XXX Not required?
|
||||
gyroResetI2cDeviceConfig(&devconf[0], I2C_DEVICE, IO_TAG(GYRO_1_EXTI_PIN), GYRO_1_ALIGN, customAlignment1);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -26,13 +26,8 @@
|
|||
#include "common/sensor_alignment.h"
|
||||
#include "drivers/io_types.h"
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
#define MAX_GYRODEV_COUNT 2
|
||||
#define MAX_ACCDEV_COUNT 2
|
||||
#else
|
||||
#define MAX_GYRODEV_COUNT 1
|
||||
#define MAX_ACCDEV_COUNT 1
|
||||
#endif
|
||||
#define MAX_GYRODEV_COUNT GYRO_COUNT
|
||||
#define MAX_ACCDEV_COUNT GYRO_COUNT
|
||||
|
||||
typedef struct gyroDeviceConfig_s {
|
||||
int8_t index;
|
||||
|
|
|
|||
|
|
@ -367,15 +367,9 @@ bool accInit(uint16_t accSampleRateHz)
|
|||
// Copy alignment from active gyro, as all production boards use acc-gyro-combi chip.
|
||||
// Exception is STM32F411DISCOVERY, and (may be) handled in future enhancement.
|
||||
|
||||
sensor_align_e alignment = gyroDeviceConfig(0)->alignment;
|
||||
const sensorAlignment_t* customAlignment = &gyroDeviceConfig(0)->customAlignment;
|
||||
sensor_align_e alignment = gyroDeviceConfig(firstEnabledGyro())->alignment;
|
||||
const sensorAlignment_t* customAlignment = &gyroDeviceConfig(firstEnabledGyro())->customAlignment;
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
if (gyroConfig()->gyro_to_use == GYRO_CONFIG_USE_GYRO_2) {
|
||||
alignment = gyroDeviceConfig(1)->alignment;
|
||||
customAlignment = &gyroDeviceConfig(1)->customAlignment;
|
||||
}
|
||||
#endif
|
||||
acc.dev.accAlign = alignment;
|
||||
buildRotationMatrixFromAngles(&acc.dev.rotationMatrix, customAlignment);
|
||||
|
||||
|
|
|
|||
|
|
@ -86,8 +86,8 @@ FAST_DATA uint8_t activePidLoopDenom = 1;
|
|||
static bool firstArmingCalibrationWasStarted = false;
|
||||
|
||||
#ifdef UNIT_TEST
|
||||
STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyro.gyroSensor1;
|
||||
STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor1.gyroDev;
|
||||
STATIC_UNIT_TESTED gyroSensor_t * const gyroSensorPtr = &gyro.gyroSensor[0];
|
||||
STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor[0].gyroDev;
|
||||
#endif
|
||||
|
||||
#define DEBUG_GYRO_CALIBRATION_VALUE 3
|
||||
|
|
@ -97,8 +97,9 @@ STATIC_UNIT_TESTED gyroDev_t * const gyroDevPtr = &gyro.gyroSensor1.gyroDev;
|
|||
|
||||
PG_REGISTER_WITH_RESET_FN(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 9);
|
||||
|
||||
#ifndef DEFAULT_GYRO_TO_USE
|
||||
#define DEFAULT_GYRO_TO_USE GYRO_CONFIG_USE_GYRO_1
|
||||
#ifndef DEFAULT_GYRO_ENABLED
|
||||
// enable the first gyro if none are enabled
|
||||
#define DEFAULT_GYRO_ENABLED GYRO_MASK(0)
|
||||
#endif
|
||||
|
||||
void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
|
||||
|
|
@ -115,7 +116,6 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
|
|||
gyroConfig->gyro_lpf2_type = FILTER_PT1;
|
||||
gyroConfig->gyro_lpf2_static_hz = GYRO_LPF2_HZ_DEFAULT;
|
||||
gyroConfig->gyro_high_fsr = false;
|
||||
gyroConfig->gyro_to_use = DEFAULT_GYRO_TO_USE;
|
||||
gyroConfig->gyro_soft_notch_hz_1 = 0;
|
||||
gyroConfig->gyro_soft_notch_cutoff_1 = 0;
|
||||
gyroConfig->gyro_soft_notch_hz_2 = 0;
|
||||
|
|
@ -130,6 +130,7 @@ void pgResetFn_gyroConfig(gyroConfig_t *gyroConfig)
|
|||
gyroConfig->gyro_lpf1_dyn_expo = 5;
|
||||
gyroConfig->simplified_gyro_filter = true;
|
||||
gyroConfig->simplified_gyro_filter_multiplier = SIMPLIFIED_TUNING_DEFAULT;
|
||||
gyroConfig->gyro_enabled_bitmask = DEFAULT_GYRO_ENABLED;
|
||||
}
|
||||
|
||||
static bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
|
||||
|
|
@ -139,20 +140,15 @@ static bool isGyroSensorCalibrationComplete(const gyroSensor_t *gyroSensor)
|
|||
|
||||
bool gyroIsCalibrationComplete(void)
|
||||
{
|
||||
switch (gyro.gyroToUse) {
|
||||
default:
|
||||
case GYRO_CONFIG_USE_GYRO_1: {
|
||||
return isGyroSensorCalibrationComplete(&gyro.gyroSensor1);
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) {
|
||||
if (!isGyroSensorCalibrationComplete(&gyro.gyroSensor[i])) {
|
||||
return false;
|
||||
}
|
||||
#ifdef USE_MULTI_GYRO
|
||||
case GYRO_CONFIG_USE_GYRO_2: {
|
||||
return isGyroSensorCalibrationComplete(&gyro.gyroSensor2);
|
||||
}
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH: {
|
||||
return isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool isOnFinalGyroCalibrationCycle(const gyroCalibration_t *gyroCalibration)
|
||||
|
|
@ -187,10 +183,11 @@ void gyroStartCalibration(bool isFirstArmingCalibration)
|
|||
return;
|
||||
}
|
||||
|
||||
gyroSetCalibrationCycles(&gyro.gyroSensor1);
|
||||
#ifdef USE_MULTI_GYRO
|
||||
gyroSetCalibrationCycles(&gyro.gyroSensor2);
|
||||
#endif
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) {
|
||||
gyroSetCalibrationCycles(&gyro.gyroSensor[i]);
|
||||
}
|
||||
}
|
||||
|
||||
if (isFirstArmingCalibration) {
|
||||
firstArmingCalibrationWasStarted = true;
|
||||
|
|
@ -410,34 +407,27 @@ static FAST_CODE void gyroUpdateSensor(gyroSensor_t *gyroSensor)
|
|||
|
||||
FAST_CODE void gyroUpdate(void)
|
||||
{
|
||||
switch (gyro.gyroToUse) {
|
||||
case GYRO_CONFIG_USE_GYRO_1:
|
||||
gyroUpdateSensor(&gyro.gyroSensor1);
|
||||
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1)) {
|
||||
gyro.gyroADC[X] = gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale;
|
||||
gyro.gyroADC[Y] = gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale;
|
||||
gyro.gyroADC[Z] = gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale;
|
||||
// ensure that gyroADC don't contain a stale value
|
||||
float adcSum[XYZ_AXIS_COUNT] = {0};
|
||||
|
||||
float active = 0;
|
||||
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) {
|
||||
gyroUpdateSensor(&gyro.gyroSensor[i]);
|
||||
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor[i])) {
|
||||
adcSum[X] += gyro.gyroSensor[i].gyroDev.gyroADC.x * gyro.gyroSensor[i].gyroDev.scale;
|
||||
adcSum[Y] += gyro.gyroSensor[i].gyroDev.gyroADC.y * gyro.gyroSensor[i].gyroDev.scale;
|
||||
adcSum[Z] += gyro.gyroSensor[i].gyroDev.gyroADC.z * gyro.gyroSensor[i].gyroDev.scale;
|
||||
active++;
|
||||
}
|
||||
break;
|
||||
#ifdef USE_MULTI_GYRO
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
gyroUpdateSensor(&gyro.gyroSensor2);
|
||||
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
|
||||
gyro.gyroADC[X] = gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale;
|
||||
gyro.gyroADC[Y] = gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale;
|
||||
gyro.gyroADC[Z] = gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale;
|
||||
}
|
||||
break;
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH:
|
||||
gyroUpdateSensor(&gyro.gyroSensor1);
|
||||
gyroUpdateSensor(&gyro.gyroSensor2);
|
||||
if (isGyroSensorCalibrationComplete(&gyro.gyroSensor1) && isGyroSensorCalibrationComplete(&gyro.gyroSensor2)) {
|
||||
gyro.gyroADC[X] = ((gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
|
||||
gyro.gyroADC[Y] = ((gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
|
||||
gyro.gyroADC[Z] = ((gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale) + (gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale)) / 2.0f;
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
|
||||
if (active != 0) {
|
||||
gyro.gyroADC[X] = adcSum[X] / active;
|
||||
gyro.gyroADC[Y] = adcSum[Y] / active;
|
||||
gyro.gyroADC[Z] = adcSum[Z] / active;
|
||||
}
|
||||
|
||||
if (gyro.downsampleFilterEnabled) {
|
||||
|
|
@ -485,36 +475,24 @@ FAST_CODE void gyroFiltering(timeUs_t currentTimeUs)
|
|||
#endif
|
||||
|
||||
if (gyro.useDualGyroDebugging) {
|
||||
switch (gyro.gyroToUse) {
|
||||
case GYRO_CONFIG_USE_GYRO_1:
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyro.gyroSensor1.gyroDev.gyroADCRaw[X]);
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]);
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale));
|
||||
break;
|
||||
int debugIndex = 0;
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]);
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyro.gyroSensor2.gyroDev.gyroADCRaw[Y]);
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale));
|
||||
break;
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) {
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0 + debugIndex, gyro.gyroSensor[i].gyroDev.gyroADCRaw[X]);
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1 + debugIndex, gyro.gyroSensor[i].gyroDev.gyroADCRaw[Y]);
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0 + debugIndex, lrintf(gyro.gyroSensor[i].gyroDev.gyroADC.x * gyro.gyroSensor[i].gyroDev.scale));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1 + debugIndex, lrintf(gyro.gyroSensor[i].gyroDev.gyroADC.y * gyro.gyroSensor[i].gyroDev.scale));
|
||||
// grab the difference between each gyro and the fused gyro output, gyro.gyroADCf (it hasn't had filters applied yet)
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0 + debugIndex, lrintf((gyro.gyroSensor[i].gyroDev.gyroADC.x * gyro.gyroSensor[i].gyroDev.scale) - gyro.gyroADCf[0]));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1 + debugIndex, lrintf((gyro.gyroSensor[i].gyroDev.gyroADC.y * gyro.gyroSensor[i].gyroDev.scale) - gyro.gyroADCf[1]));
|
||||
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH:
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 0, gyro.gyroSensor1.gyroDev.gyroADCRaw[X]);
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 1, gyro.gyroSensor1.gyroDev.gyroADCRaw[Y]);
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 2, gyro.gyroSensor2.gyroDev.gyroADCRaw[X]);
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_RAW, 3, gyro.gyroSensor2.gyroDev.gyroADCRaw[Y]);
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 0, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 1, lrintf(gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 2, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_SCALED, 3, lrintf(gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 0, lrintf((gyro.gyroSensor1.gyroDev.gyroADC.x * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC.x * gyro.gyroSensor2.gyroDev.scale)));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 1, lrintf((gyro.gyroSensor1.gyroDev.gyroADC.y * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC.y * gyro.gyroSensor2.gyroDev.scale)));
|
||||
DEBUG_SET(DEBUG_DUAL_GYRO_DIFF, 2, lrintf((gyro.gyroSensor1.gyroDev.gyroADC.z * gyro.gyroSensor1.gyroDev.scale) - (gyro.gyroSensor2.gyroDev.gyroADC.z * gyro.gyroSensor2.gyroDev.scale)));
|
||||
debugIndex += 2;
|
||||
if (debugIndex >= 8) {
|
||||
break;
|
||||
#endif
|
||||
// only iterate over the first 4 enabled gyros, debug can't hold more
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -556,23 +534,17 @@ static int16_t gyroReadSensorTemperature(gyroSensor_t gyroSensor)
|
|||
|
||||
void gyroReadTemperature(void)
|
||||
{
|
||||
switch (gyro.gyroToUse) {
|
||||
case GYRO_CONFIG_USE_GYRO_1:
|
||||
gyroSensorTemperature = gyroReadSensorTemperature(gyro.gyroSensor1);
|
||||
break;
|
||||
int16_t max_temp = INT16_MIN;
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
case GYRO_CONFIG_USE_GYRO_2:
|
||||
gyroSensorTemperature = gyroReadSensorTemperature(gyro.gyroSensor2);
|
||||
break;
|
||||
|
||||
case GYRO_CONFIG_USE_GYRO_BOTH:
|
||||
gyroSensorTemperature = MAX(gyroReadSensorTemperature(gyro.gyroSensor1), gyroReadSensorTemperature(gyro.gyroSensor2));
|
||||
break;
|
||||
#endif // USE_MULTI_GYRO
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) {
|
||||
max_temp = MAX(max_temp, gyroReadSensorTemperature(gyro.gyroSensor[i]));
|
||||
}
|
||||
}
|
||||
|
||||
gyroSensorTemperature = max_temp;
|
||||
}
|
||||
|
||||
int16_t gyroGetTemperature(void)
|
||||
{
|
||||
return gyroSensorTemperature;
|
||||
|
|
|
|||
|
|
@ -51,6 +51,8 @@
|
|||
|
||||
#define GYRO_IMU_DOWNSAMPLE_CUTOFF_HZ 200
|
||||
|
||||
#define GYRO_MASK(x) BIT(x)
|
||||
|
||||
typedef union gyroLowpassFilter_u {
|
||||
pt1Filter_t pt1FilterState;
|
||||
biquadFilter_t biquadFilterState;
|
||||
|
|
@ -58,16 +60,6 @@ typedef union gyroLowpassFilter_u {
|
|||
pt3Filter_t pt3FilterState;
|
||||
} gyroLowpassFilter_t;
|
||||
|
||||
typedef enum gyroDetectionFlags_e {
|
||||
GYRO_NONE_MASK = 0,
|
||||
GYRO_1_MASK = BIT(0),
|
||||
#if defined(USE_MULTI_GYRO)
|
||||
GYRO_2_MASK = BIT(1),
|
||||
GYRO_ALL_MASK = (GYRO_1_MASK | GYRO_2_MASK),
|
||||
GYRO_IDENTICAL_MASK = BIT(7), // All gyros are of the same hardware type
|
||||
#endif
|
||||
} gyroDetectionFlags_t;
|
||||
|
||||
typedef struct gyroCalibration_s {
|
||||
float sum[XYZ_AXIS_COUNT];
|
||||
stdev_t var[XYZ_AXIS_COUNT];
|
||||
|
|
@ -90,10 +82,7 @@ typedef struct gyro_s {
|
|||
float sampleSum[XYZ_AXIS_COUNT]; // summed samples used for downsampling
|
||||
bool downsampleFilterEnabled; // if true then downsample using gyro lowpass 2, otherwise use averaging
|
||||
|
||||
gyroSensor_t gyroSensor1;
|
||||
#ifdef USE_MULTI_GYRO
|
||||
gyroSensor_t gyroSensor2;
|
||||
#endif
|
||||
gyroSensor_t gyroSensor[GYRO_COUNT];
|
||||
|
||||
gyroDev_t *rawSensorDev; // pointer to the sensor providing the raw data for DEBUG_GYRO_RAW
|
||||
|
||||
|
|
@ -113,7 +102,7 @@ typedef struct gyro_s {
|
|||
biquadFilter_t notchFilter2[XYZ_AXIS_COUNT];
|
||||
|
||||
uint16_t accSampleRateHz;
|
||||
uint8_t gyroToUse;
|
||||
uint8_t gyroEnabledBitmask;
|
||||
uint8_t gyroDebugMode;
|
||||
bool gyroHasOverflowProtection;
|
||||
bool useDualGyroDebugging;
|
||||
|
|
@ -155,10 +144,6 @@ typedef enum {
|
|||
YAW_SPIN_RECOVERY_AUTO
|
||||
} yawSpinRecoveryMode_e;
|
||||
|
||||
#define GYRO_CONFIG_USE_GYRO_1 0
|
||||
#define GYRO_CONFIG_USE_GYRO_2 1
|
||||
#define GYRO_CONFIG_USE_GYRO_BOTH 2
|
||||
|
||||
enum {
|
||||
FILTER_LPF1 = 0,
|
||||
FILTER_LPF2
|
||||
|
|
@ -168,7 +153,6 @@ typedef struct gyroConfig_s {
|
|||
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.
|
||||
uint8_t gyro_hardware_lpf; // gyro DLPF setting
|
||||
uint8_t gyro_high_fsr;
|
||||
uint8_t gyro_to_use;
|
||||
|
||||
uint16_t gyro_lpf1_static_hz;
|
||||
uint16_t gyro_lpf2_static_hz;
|
||||
|
|
@ -194,10 +178,12 @@ typedef struct gyroConfig_s {
|
|||
|
||||
uint8_t gyro_filter_debug_axis;
|
||||
|
||||
uint8_t gyrosDetected; // What gyros should detection be attempted for on startup. Automatically set on first startup.
|
||||
uint8_t gyrosDetected; // What gyros should be shown as part of the cli status command.
|
||||
uint8_t gyro_lpf1_dyn_expo; // set the curve for dynamic gyro lowpass filter
|
||||
uint8_t simplified_gyro_filter;
|
||||
uint8_t simplified_gyro_filter_multiplier;
|
||||
|
||||
uint8_t gyro_enabled_bitmask;
|
||||
} gyroConfig_t;
|
||||
|
||||
PG_DECLARE(gyroConfig_t, gyroConfig);
|
||||
|
|
|
|||
|
|
@ -73,18 +73,12 @@
|
|||
|
||||
#include "gyro_init.h"
|
||||
|
||||
#ifdef USE_MULTI_GYRO
|
||||
#define ACTIVE_GYRO ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) ? &gyro.gyroSensor2 : &gyro.gyroSensor1)
|
||||
#else
|
||||
#define ACTIVE_GYRO (&gyro.gyroSensor1)
|
||||
#endif
|
||||
|
||||
// The gyro buffer is split 50/50, the first half for the transmit buffer, the second half for the receive buffer
|
||||
// This buffer is large enough for the gyros currently supported in accgyro_mpu.c but should be reviewed id other
|
||||
// gyro types are supported with SPI DMA.
|
||||
#define GYRO_BUF_SIZE 32
|
||||
|
||||
static gyroDetectionFlags_t gyroDetectionFlags = GYRO_NONE_MASK;
|
||||
static uint8_t gyroDetectedFlags = 0;
|
||||
|
||||
static uint16_t calculateNyquistAdjustedNotchHz(uint16_t notchHz, uint16_t notchCutoffHz)
|
||||
{
|
||||
|
|
@ -579,10 +573,9 @@ static void gyroPreInitSensor(const gyroDeviceConfig_t *config)
|
|||
|
||||
void gyroPreInit(void)
|
||||
{
|
||||
gyroPreInitSensor(gyroDeviceConfig(0));
|
||||
#ifdef USE_MULTI_GYRO
|
||||
gyroPreInitSensor(gyroDeviceConfig(1));
|
||||
#endif
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
gyroPreInitSensor(gyroDeviceConfig(i));
|
||||
}
|
||||
}
|
||||
|
||||
bool gyroInit(void)
|
||||
|
|
@ -617,95 +610,104 @@ bool gyroInit(void)
|
|||
break;
|
||||
}
|
||||
|
||||
gyroDetectionFlags = GYRO_NONE_MASK;
|
||||
gyroDetectedFlags = 0;
|
||||
uint8_t gyrosToScan = gyroConfig()->gyrosDetected;
|
||||
// scan all gyros if gyrosDetected is zero
|
||||
if (gyrosToScan == 0) {
|
||||
gyrosToScan = 0xff;
|
||||
}
|
||||
// always scan gyro_enabled_bitmask
|
||||
gyrosToScan |= gyroConfig()->gyro_enabled_bitmask;
|
||||
|
||||
gyro.gyroToUse = gyroConfig()->gyro_to_use;
|
||||
gyro.gyroDebugAxis = gyroConfig()->gyro_filter_debug_axis;
|
||||
|
||||
if ((!gyrosToScan || (gyrosToScan & GYRO_1_MASK)) && gyroDetectSensor(&gyro.gyroSensor1, gyroDeviceConfig(0))) {
|
||||
gyroDetectionFlags |= GYRO_1_MASK;
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
// Only attempt to detect a gyro if it's enabled or we're doing an auto-scan
|
||||
if (gyrosToScan & GYRO_MASK(i)) {
|
||||
if (gyroDetectSensor(&gyro.gyroSensor[i], gyroDeviceConfig(i))) {
|
||||
// If we detected a gyro, make sure it's in the enabled bitmask
|
||||
// This ensures that during first boot, all detected gyros are enabled
|
||||
gyroDetectedFlags |= GYRO_MASK(i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(USE_MULTI_GYRO)
|
||||
if ((!gyrosToScan || (gyrosToScan & GYRO_2_MASK)) && gyroDetectSensor(&gyro.gyroSensor2, gyroDeviceConfig(1))) {
|
||||
gyroDetectionFlags |= GYRO_2_MASK;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (gyroDetectionFlags == GYRO_NONE_MASK) {
|
||||
if (gyroDetectedFlags == 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool eepromWriteRequired = false;
|
||||
if (!gyrosToScan) {
|
||||
gyroConfigMutable()->gyrosDetected = gyroDetectionFlags;
|
||||
if (gyrosToScan != gyroConfigMutable()->gyrosDetected) {
|
||||
gyroConfigMutable()->gyrosDetected = gyroDetectedFlags;
|
||||
eepromWriteRequired = true;
|
||||
}
|
||||
|
||||
#if defined(USE_MULTI_GYRO)
|
||||
if ((gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH && !((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK))
|
||||
|| (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 && !(gyroDetectionFlags & GYRO_1_MASK))
|
||||
|| (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 && !(gyroDetectionFlags & GYRO_2_MASK))) {
|
||||
if (gyroDetectionFlags & GYRO_1_MASK) {
|
||||
gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
|
||||
} else {
|
||||
gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_2;
|
||||
}
|
||||
// check if all enabled sensors are detected
|
||||
gyro.gyroEnabledBitmask = gyroDetectedFlags & gyroConfig()->gyro_enabled_bitmask;
|
||||
|
||||
gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
|
||||
if (gyroConfigMutable()->gyro_enabled_bitmask != gyro.gyroEnabledBitmask) {
|
||||
gyroConfigMutable()->gyro_enabled_bitmask = gyro.gyroEnabledBitmask;
|
||||
eepromWriteRequired = true;
|
||||
}
|
||||
|
||||
// Only allow using both gyros simultaneously if they are the same hardware type.
|
||||
if (((gyroDetectionFlags & GYRO_ALL_MASK) == GYRO_ALL_MASK) && gyro.gyroSensor1.gyroDev.gyroHardware == gyro.gyroSensor2.gyroDev.gyroHardware) {
|
||||
gyroDetectionFlags |= GYRO_IDENTICAL_MASK;
|
||||
} else if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||
// If the user selected "BOTH" and they are not the same type, then reset to using only the first gyro.
|
||||
gyro.gyroToUse = GYRO_CONFIG_USE_GYRO_1;
|
||||
gyroConfigMutable()->gyro_to_use = gyro.gyroToUse;
|
||||
// Only allow using multiple gyros simultaneously if they are the same hardware type.
|
||||
// Or allow using if they have the same sample rate and scale.
|
||||
bool gyro_hardware_compatible = true;
|
||||
uint16_t gyro_sample_rate = 0;
|
||||
float gyro_scale = 0.0f;
|
||||
bool firstFound = false;
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
if (gyro.gyroEnabledBitmask & GYRO_MASK(i)) {
|
||||
if (!firstFound) {
|
||||
firstFound = true;
|
||||
gyro_sample_rate = gyro.gyroSensor[i].gyroDev.gyroSampleRateHz;
|
||||
gyro_scale = gyro.gyroSensor[i].gyroDev.scale;
|
||||
} else if ((gyro_sample_rate != gyro.gyroSensor[i].gyroDev.gyroSampleRateHz) ||
|
||||
(gyro_scale != gyro.gyroSensor[i].gyroDev.scale)) {
|
||||
gyro_hardware_compatible = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!gyro_hardware_compatible) {
|
||||
// If the user enabled multiple IMU and they are not compatible types, then reset to using only the first IMU.
|
||||
gyro.gyroEnabledBitmask = gyro.gyroEnabledBitmask & -gyro.gyroEnabledBitmask;
|
||||
gyroConfigMutable()->gyro_enabled_bitmask = gyro.gyroEnabledBitmask;
|
||||
eepromWriteRequired = true;
|
||||
}
|
||||
|
||||
if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||
static DMA_DATA uint8_t gyroBuf2[GYRO_BUF_SIZE];
|
||||
static DMA_DATA uint8_t gyroBuf[GYRO_COUNT][2][GYRO_BUF_SIZE / 2];
|
||||
|
||||
for (int i = 0; i < GYRO_COUNT; i++) {
|
||||
if (gyroDetectedFlags & GYRO_MASK(i)) { // Only initialize detected gyros
|
||||
// SPI DMA buffer required per device
|
||||
gyro.gyroSensor2.gyroDev.dev.txBuf = gyroBuf2;
|
||||
gyro.gyroSensor2.gyroDev.dev.rxBuf = &gyroBuf2[GYRO_BUF_SIZE / 2];
|
||||
gyro.gyroSensor[i].gyroDev.dev.txBuf = gyroBuf[i][0];
|
||||
gyro.gyroSensor[i].gyroDev.dev.rxBuf = gyroBuf[i][1];
|
||||
|
||||
gyroInitSensor(&gyro.gyroSensor2, gyroDeviceConfig(1));
|
||||
gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor2.gyroDev.gyroHasOverflowProtection;
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor2.gyroDev.gyroHardware;
|
||||
gyroInitSensor(&gyro.gyroSensor[i], gyroDeviceConfig(i));
|
||||
|
||||
gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection
|
||||
&& gyro.gyroSensor[i].gyroDev.gyroHasOverflowProtection;
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
if (eepromWriteRequired) {
|
||||
writeEEPROM();
|
||||
}
|
||||
|
||||
if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_1 || gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_BOTH) {
|
||||
static DMA_DATA uint8_t gyroBuf1[GYRO_BUF_SIZE];
|
||||
// SPI DMA buffer required per device
|
||||
gyro.gyroSensor1.gyroDev.dev.txBuf = gyroBuf1;
|
||||
gyro.gyroSensor1.gyroDev.dev.rxBuf = &gyroBuf1[GYRO_BUF_SIZE / 2];
|
||||
gyroInitSensor(&gyro.gyroSensor1, gyroDeviceConfig(0));
|
||||
gyro.gyroHasOverflowProtection = gyro.gyroHasOverflowProtection && gyro.gyroSensor1.gyroDev.gyroHasOverflowProtection;
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor1.gyroDev.gyroHardware;
|
||||
// Use the first enabled gyro for our scale and raw sensor dev
|
||||
int firstGyro = firstEnabledGyro();
|
||||
if (firstGyro >= 0) {
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = gyro.gyroSensor[firstGyro].gyroDev.gyroHardware;
|
||||
gyro.scale = gyro.gyroSensor[firstGyro].gyroDev.scale;
|
||||
gyro.rawSensorDev = &gyro.gyroSensor[firstGyro].gyroDev;
|
||||
} else {
|
||||
// no gyros enabled
|
||||
detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE;
|
||||
gyro.scale = 1.0f;
|
||||
gyro.rawSensorDev = NULL;
|
||||
}
|
||||
|
||||
// Copy the sensor's scale to the high-level gyro object. If running in "BOTH" mode
|
||||
// then logic above requires both sensors to be the same so we'll use sensor1's scale.
|
||||
// This will need to be revised if we ever allow different sensor types to be used simultaneously.
|
||||
// Likewise determine the appropriate raw data for use in DEBUG_GYRO_RAW
|
||||
gyro.scale = gyro.gyroSensor1.gyroDev.scale;
|
||||
gyro.rawSensorDev = &gyro.gyroSensor1.gyroDev;
|
||||
#if defined(USE_MULTI_GYRO)
|
||||
if (gyro.gyroToUse == GYRO_CONFIG_USE_GYRO_2) {
|
||||
gyro.scale = gyro.gyroSensor2.gyroDev.scale;
|
||||
gyro.rawSensorDev = &gyro.gyroSensor2.gyroDev;
|
||||
}
|
||||
#endif
|
||||
|
||||
if (gyro.rawSensorDev) {
|
||||
gyro.sampleRateHz = gyro.rawSensorDev->gyroSampleRateHz;
|
||||
gyro.accSampleRateHz = gyro.rawSensorDev->accSampleRateHz;
|
||||
|
|
@ -717,9 +719,9 @@ bool gyroInit(void)
|
|||
return true;
|
||||
}
|
||||
|
||||
gyroDetectionFlags_t getGyroDetectionFlags(void)
|
||||
uint8_t getGyroDetectedFlags(void)
|
||||
{
|
||||
return gyroDetectionFlags;
|
||||
return gyroDetectedFlags;
|
||||
}
|
||||
|
||||
void gyroSetTargetLooptime(uint8_t pidDenom)
|
||||
|
|
@ -736,34 +738,41 @@ void gyroSetTargetLooptime(uint8_t pidDenom)
|
|||
|
||||
gyroDev_t *gyroActiveDev(void)
|
||||
{
|
||||
return &ACTIVE_GYRO->gyroDev;
|
||||
return gyro.rawSensorDev;
|
||||
}
|
||||
|
||||
const mpuDetectionResult_t *gyroMpuDetectionResult(void)
|
||||
{
|
||||
return &ACTIVE_GYRO->gyroDev.mpuDetectionResult;
|
||||
return &gyro.rawSensorDev->mpuDetectionResult;
|
||||
}
|
||||
|
||||
int16_t gyroRateDps(int axis)
|
||||
{
|
||||
return lrintf(gyro.gyroADCf[axis] / ACTIVE_GYRO->gyroDev.scale);
|
||||
return lrintf(gyro.gyroADCf[axis] / gyro.rawSensorDev->scale);
|
||||
}
|
||||
|
||||
#ifdef USE_GYRO_REGISTER_DUMP
|
||||
static extDevice_t *gyroSensorDevByInstance(uint8_t whichSensor)
|
||||
static extDevice_t *gyroExtDevice(uint8_t idx)
|
||||
{
|
||||
#ifdef USE_MULTI_GYRO
|
||||
if (whichSensor == GYRO_CONFIG_USE_GYRO_2) {
|
||||
return &gyro.gyroSensor2.gyroDev.dev;
|
||||
}
|
||||
#else
|
||||
UNUSED(whichSensor);
|
||||
#endif
|
||||
return &gyro.gyroSensor1.gyroDev.dev;
|
||||
return &gyro.gyroSensor[idx].gyroDev.dev;
|
||||
}
|
||||
|
||||
uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg)
|
||||
{
|
||||
return mpuGyroReadRegister(gyroSensorDevByInstance(whichSensor), reg);
|
||||
if (whichSensor < GYRO_COUNT) {
|
||||
return mpuGyroReadRegister(gyroExtDevice(whichSensor), reg);
|
||||
} else {
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
#endif // USE_GYRO_REGISTER_DUMP
|
||||
|
||||
int firstEnabledGyro(void)
|
||||
{
|
||||
if (gyro.gyroEnabledBitmask != 0) {
|
||||
return llog2(gyro.gyroEnabledBitmask & -gyro.gyroEnabledBitmask);
|
||||
} else {
|
||||
// no gyro is enabled
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -28,9 +28,10 @@ void gyroPreInit(void);
|
|||
bool gyroInit(void);
|
||||
void gyroInitFilters(void);
|
||||
void gyroInitSensor(gyroSensor_t *gyroSensor, const gyroDeviceConfig_t *config);
|
||||
gyroDetectionFlags_t getGyroDetectionFlags(void);
|
||||
uint8_t getGyroDetectedFlags(void);
|
||||
gyroDev_t *gyroActiveDev(void);
|
||||
struct mpuDetectionResult_s;
|
||||
const struct mpuDetectionResult_s *gyroMpuDetectionResult(void);
|
||||
int16_t gyroRateDps(int axis);
|
||||
uint8_t gyroReadRegister(uint8_t whichSensor, uint8_t reg);
|
||||
int firstEnabledGyro(void);
|
||||
|
|
|
|||
|
|
@ -703,3 +703,4 @@ extern struct linker_symbol __config_end;
|
|||
#define USE_PIN_PULL_UP_DOWN
|
||||
#endif
|
||||
#endif // USE_PINIO
|
||||
|
||||
|
|
|
|||
|
|
@ -372,7 +372,6 @@
|
|||
|
||||
#define USE_AIRMODE_LPF
|
||||
#define USE_GYRO_DLPF_EXPERIMENTAL
|
||||
#define USE_MULTI_GYRO
|
||||
#define USE_SENSOR_NAMES
|
||||
#define USE_UNCOMMON_MIXERS
|
||||
#define USE_SIGNATURE
|
||||
|
|
@ -494,3 +493,66 @@
|
|||
#if defined(USE_POSITION_HOLD) && !defined(USE_GPS)
|
||||
#error "USE_POSITION_HOLD requires USE_GPS to be defined"
|
||||
#endif
|
||||
|
||||
// backwards compatibility for older config.h targets
|
||||
#ifndef GYRO_CONFIG_USE_GYRO_1
|
||||
#define GYRO_CONFIG_USE_GYRO_1 0
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_CONFIG_USE_GYRO_2
|
||||
#define GYRO_CONFIG_USE_GYRO_2 1
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_CONFIG_USE_GYRO_BOTH
|
||||
#define GYRO_CONFIG_USE_GYRO_BOTH 2
|
||||
#endif
|
||||
|
||||
#ifdef DEFAULT_GYRO_TO_USE
|
||||
#ifndef DEFAULT_GYRO_ENABLED
|
||||
#if DEFAULT_GYRO_TO_USE == GYRO_CONFIG_USE_GYRO_1
|
||||
#define DEFAULT_GYRO_ENABLED GYRO_MASK(0)
|
||||
#elif DEFAULT_GYRO_TO_USE == GYRO_CONFIG_USE_GYRO_2
|
||||
#define DEFAULT_GYRO_ENABLED GYRO_MASK(1)
|
||||
#elif DEFAULT_GYRO_TO_USE == GYRO_CONFIG_USE_GYRO_BOTH
|
||||
#define DEFAULT_GYRO_ENABLED (GYRO_MASK(0) | GYRO_MASK(1))
|
||||
#endif
|
||||
#endif
|
||||
#elif !defined(DEFAULT_GYRO_ENABLED)
|
||||
// assume one gyro
|
||||
#define DEFAULT_GYRO_ENABLED GYRO_MASK(0)
|
||||
#endif
|
||||
|
||||
#ifndef GYRO_COUNT
|
||||
#ifdef GYRO_1_CS_PIN
|
||||
#define GYRO_1_DEFINED 1
|
||||
#else
|
||||
#define GYRO_1_DEFINED 0
|
||||
#endif
|
||||
|
||||
#ifdef GYRO_2_CS_PIN
|
||||
#define GYRO_2_DEFINED 1
|
||||
#else
|
||||
#define GYRO_2_DEFINED 0
|
||||
#endif
|
||||
|
||||
#ifdef GYRO_3_CS_PIN
|
||||
#define GYRO_3_DEFINED 1
|
||||
#else
|
||||
#define GYRO_3_DEFINED 0
|
||||
#endif
|
||||
|
||||
#ifdef GYRO_4_CS_PIN
|
||||
#define GYRO_4_DEFINED 1
|
||||
#else
|
||||
#define GYRO_4_DEFINED 0
|
||||
#endif
|
||||
|
||||
#define GYRO_COUNT_RAW (GYRO_1_DEFINED + GYRO_2_DEFINED + GYRO_3_DEFINED + GYRO_4_DEFINED)
|
||||
|
||||
// Ensure GYRO_COUNT is at least 1
|
||||
#if GYRO_COUNT_RAW > 0
|
||||
#define GYRO_COUNT GYRO_COUNT_RAW
|
||||
#else
|
||||
#define GYRO_COUNT 1
|
||||
#endif
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -125,6 +125,7 @@ typedef struct
|
|||
|
||||
#define SPIDEV_COUNT 0
|
||||
#define I2CDEV_COUNT 0
|
||||
#define GYRO_COUNT 1
|
||||
|
||||
#define WS2811_DMA_TC_FLAG (void *)1
|
||||
#define WS2811_DMA_HANDLER_IDENTIFER 0
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue