mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-13 03:20:00 +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
|
|
@ -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);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue