Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
54 changes: 31 additions & 23 deletions src/main/sensors/gyro.c
Original file line number Diff line number Diff line change
Expand Up @@ -373,10 +373,41 @@ bool gyroInit(void)
gyroDev[0].imuSensorToUse = 0;
#endif

#ifdef USE_DUAL_GYRO
// Optional secondary IMU, logged as gyroRaw2 and read by nothing else. It goes before the
// primary: a driver probes at the slowest SPI clock and leaves it there when it finds no
// chip, so on a bus the two share the primary has to set its clock after the last probe.
// Each gyro still starts right after its own detection, as a driver keeps the variant it
// detected in one static that its init reads
if (gyroConfig()->gyro_secondary_enabled) {
// Targets disagree on how they tag the second IMU, so probe rather than assume tag 1
for (uint8_t tag = 0; tag <= MAX_GYRO_SENSOR_TAG; tag++) {
if (tag == gyroConfig()->gyro_to_use) {
continue;
}

gyroDev[GYRO_SECONDARY].imuSensorToUse = tag;
if (gyroDetect(&gyroDev[GYRO_SECONDARY], GYRO_AUTODETECT) != GYRO_NONE) {
#ifdef USE_SPI_DATA_READY
// Its interrupt stays off while no log is written
gyroDev[GYRO_SECONDARY].readOnDataReady = gyroReadOnDataReady();
#endif
gyroDevStart(&gyroDev[GYRO_SECONDARY]);
gyro.secondaryInitialized = true;
break;
}
}
}
#endif

// Detecting gyro0
gyroSensor_e gyroHardware = gyroDetect(&gyroDev[0], GYRO_AUTODETECT);
if (gyroHardware == GYRO_NONE) {
gyro.initialized = false;
#ifdef USE_DUAL_GYRO
// Without a primary nothing reads the secondary
gyro.secondaryInitialized = false;
#endif
detectedSensors[SENSOR_INDEX_GYRO] = GYRO_NONE;
return true;
}
Expand All @@ -398,29 +429,6 @@ bool gyroInit(void)

gyroInitFilters();

#ifdef USE_DUAL_GYRO
// Optional secondary IMU, logged as gyroRaw2 and read by nothing else
if (gyroConfig()->gyro_secondary_enabled) {
// Targets disagree on how they tag the second IMU, so probe rather than assume tag 1
for (uint8_t tag = 0; tag <= MAX_GYRO_SENSOR_TAG; tag++) {
if (tag == gyroConfig()->gyro_to_use) {
continue;
}

gyroDev[GYRO_SECONDARY].imuSensorToUse = tag;
if (gyroDetect(&gyroDev[GYRO_SECONDARY], GYRO_AUTODETECT) != GYRO_NONE) {
#ifdef USE_SPI_DATA_READY
// Its interrupt stays off while no log is written
gyroDev[GYRO_SECONDARY].readOnDataReady = gyroReadOnDataReady();
#endif
gyroDevStart(&gyroDev[GYRO_SECONDARY]);
gyro.secondaryInitialized = true;
break;
}
}
}
#endif

#ifdef USE_DYNAMIC_FILTERS
// Dynamic notch running at PID frequency
dynamicGyroNotchFiltersInit(&dynamicGyroNotchState);
Expand Down
6 changes: 1 addition & 5 deletions src/main/target/DAKEFPVH743/target.h
Original file line number Diff line number Diff line change
Expand Up @@ -67,10 +67,6 @@
#define USE_IMU_BMI270
#define USE_IMU_LSM6DXX


// Enable dual gyro mode
#define USE_DUAL_GYRO

// Primary IMU options
#define IMU_1_CS_PIN PA4
#define IMU_1_IRQ_PIN PC4
Expand All @@ -79,7 +75,7 @@

// Secondary IMU options
#define IMU_2_CS_PIN PB1
#define IMU_2_SPI_BUS BUS_SPI1
#define IMU_2_SPI_BUS BUS_SPI4
#define IMU_2_ALIGN CW180_DEG


Expand Down
Loading