diff --git a/src/main/drivers/bus_i2c_config.c b/src/main/drivers/bus_i2c_config.c index 65bcebe1be..c633a27b02 100644 --- a/src/main/drivers/bus_i2c_config.c +++ b/src/main/drivers/bus_i2c_config.c @@ -71,14 +71,14 @@ void i2cHardwareConfigure(const i2cConfig_t *i2cConfig) memset(pDev, 0, sizeof(*pDev)); for (int pindex = 0 ; pindex < I2C_PIN_SEL_MAX ; pindex++) { - if (i2cConfig->ioTagScl[device] == hardware->sclPins[pindex].ioTag) { - pDev->scl = IOGetByTag(i2cConfig->ioTagScl[device]); + if (i2cConfig[device].ioTagScl == hardware->sclPins[pindex].ioTag) { + pDev->scl = IOGetByTag(i2cConfig[device].ioTagScl); #if defined(STM32F4) pDev->sclAF = hardware->sclPins[pindex].af; #endif } - if (i2cConfig->ioTagSda[device] == hardware->sdaPins[pindex].ioTag) { - pDev->sda = IOGetByTag(i2cConfig->ioTagSda[device]); + if (i2cConfig[device].ioTagSda == hardware->sdaPins[pindex].ioTag) { + pDev->sda = IOGetByTag(i2cConfig[device].ioTagSda); #if defined(STM32F4) pDev->sdaAF = hardware->sdaPins[pindex].af; #endif @@ -88,8 +88,8 @@ void i2cHardwareConfigure(const i2cConfig_t *i2cConfig) if (pDev->scl && pDev->sda) { pDev->hardware = hardware; pDev->reg = hardware->reg; - pDev->overClock = i2cConfig->overClock[device]; - pDev->pullUp = i2cConfig->pullUp[device]; + pDev->overClock = i2cConfig[device].overClock; + pDev->pullUp = i2cConfig[device].pullUp; } } } diff --git a/src/main/drivers/bus_spi_pinconfig.c b/src/main/drivers/bus_spi_pinconfig.c index 97956a9545..895a221d98 100644 --- a/src/main/drivers/bus_spi_pinconfig.c +++ b/src/main/drivers/bus_spi_pinconfig.c @@ -294,19 +294,19 @@ void spiPinConfigure(const spiPinConfig_t *pConfig) spiDevice_t *pDev = &spiDevice[device]; for (int pindex = 0 ; pindex < MAX_SPI_PIN_SEL ; pindex++) { - if (pConfig->ioTagSck[device] == hw->sckPins[pindex].pin) { + if (pConfig[device].ioTagSck == hw->sckPins[pindex].pin) { pDev->sck = hw->sckPins[pindex].pin; #ifdef STM32F7 pDev->sckAF = hw->sckPins[pindex].af; #endif } - if (pConfig->ioTagMiso[device] == hw->misoPins[pindex].pin) { + if (pConfig[device].ioTagMiso == hw->misoPins[pindex].pin) { pDev->miso = hw->misoPins[pindex].pin; #ifdef STM32F7 pDev->misoAF = hw->misoPins[pindex].af; #endif } - if (pConfig->ioTagMosi[device] == hw->mosiPins[pindex].pin) { + if (pConfig[device].ioTagMosi == hw->mosiPins[pindex].pin) { pDev->mosi = hw->mosiPins[pindex].pin; #ifdef STM32F7 pDev->mosiAF = hw->mosiPins[pindex].af; diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index dcce15e03c..6a4b166d02 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -446,7 +446,7 @@ void init(void) #else #ifdef USE_SPI - spiPinConfigure(spiPinConfig()); + spiPinConfigure(spiPinConfig(0)); // Initialize CS lines and keep them high spiPreInit(); @@ -479,7 +479,7 @@ void init(void) #endif #ifdef USE_I2C - i2cHardwareConfigure(i2cConfig()); + i2cHardwareConfigure(i2cConfig(0)); // Note: Unlike UARTs which are configured when client is present, // I2C buses are initialized unconditionally if they are configured. diff --git a/src/main/interface/cli.c b/src/main/interface/cli.c index f60500fc82..8525b0ee48 100644 --- a/src/main/interface/cli.c +++ b/src/main/interface/cli.c @@ -3360,91 +3360,110 @@ static void cliVersion(char *cmdline) typedef struct { const uint8_t owner; pgn_t pgn; - uint16_t offset; + uint8_t stride; + uint8_t offset; const uint8_t maxIndex; } cliResourceValue_t; +// Handy macros for keeping the table tidy. +// DEFS : Single entry +// DEFA : Array of uint8_t (stride = 1) +// DEFW : Wider stride case; array of structs. + +#define DEFS(owner, pgn, type, member) \ + { owner, pgn, 0, offsetof(type, member), 0 } + +#define DEFA(owner, pgn, type, member, max) \ + { owner, pgn, sizeof(ioTag_t), offsetof(type, member), max } + +#define DEFW(owner, pgn, type, member, max) \ + { owner, pgn, sizeof(type), offsetof(type, member), max } + const cliResourceValue_t resourceTable[] = { #ifdef USE_BEEPER - { OWNER_BEEPER, PG_BEEPER_DEV_CONFIG, offsetof(beeperDevConfig_t, ioTag), 0 }, + DEFS( OWNER_BEEPER, PG_BEEPER_DEV_CONFIG, beeperDevConfig_t, ioTag) , #endif - { OWNER_MOTOR, PG_MOTOR_CONFIG, offsetof(motorConfig_t, dev.ioTags[0]), MAX_SUPPORTED_MOTORS }, + DEFA( OWNER_MOTOR, PG_MOTOR_CONFIG, motorConfig_t, dev.ioTags[0], MAX_SUPPORTED_MOTORS ), #ifdef USE_SERVOS - { OWNER_SERVO, PG_SERVO_CONFIG, offsetof(servoConfig_t, dev.ioTags[0]), MAX_SUPPORTED_SERVOS }, + DEFA( OWNER_SERVO, PG_SERVO_CONFIG, servoConfig_t, dev.ioTags[0], MAX_SUPPORTED_SERVOS ), #endif #if defined(USE_PWM) || defined(USE_PPM) - { OWNER_PPMINPUT, PG_PPM_CONFIG, offsetof(ppmConfig_t, ioTag), 0 }, - { OWNER_PWMINPUT, PG_PWM_CONFIG, offsetof(pwmConfig_t, ioTags[0]), PWM_INPUT_PORT_COUNT }, + DEFS( OWNER_PPMINPUT, PG_PPM_CONFIG, ppmConfig_t, ioTag ), + DEFA( OWNER_PWMINPUT, PG_PWM_CONFIG, pwmConfig_t, ioTags[0], PWM_INPUT_PORT_COUNT ), #endif #ifdef USE_RANGEFINDER_HCSR04 - { OWNER_SONAR_TRIGGER, PG_SONAR_CONFIG, offsetof(sonarConfig_t, triggerTag), 0 }, - { OWNER_SONAR_ECHO, PG_SONAR_CONFIG, offsetof(sonarConfig_t, echoTag), 0 }, + DEFS( OWNER_SONAR_TRIGGER, PG_SONAR_CONFIG, sonarConfig_t, triggerTag ), + DEFS( OWNER_SONAR_ECHO, PG_SONAR_CONFIG, sonarConfig_t, echoTag ), #endif #ifdef USE_LED_STRIP - { OWNER_LED_STRIP, PG_LED_STRIP_CONFIG, offsetof(ledStripConfig_t, ioTag), 0 }, + DEFS( OWNER_LED_STRIP, PG_LED_STRIP_CONFIG, ledStripConfig_t, ioTag ), #endif - { OWNER_SERIAL_TX, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagTx[0]), SERIAL_PORT_MAX_INDEX }, - { OWNER_SERIAL_RX, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagRx[0]), SERIAL_PORT_MAX_INDEX }, + DEFA( OWNER_SERIAL_TX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagTx[0], SERIAL_PORT_MAX_INDEX ), + DEFA( OWNER_SERIAL_RX, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagRx[0], SERIAL_PORT_MAX_INDEX ), #ifdef USE_INVERTER - { OWNER_INVERTER, PG_SERIAL_PIN_CONFIG, offsetof(serialPinConfig_t, ioTagInverter[0]), SERIAL_PORT_MAX_INDEX }, + DEFA( OWNER_INVERTER, PG_SERIAL_PIN_CONFIG, serialPinConfig_t, ioTagInverter[0], SERIAL_PORT_MAX_INDEX ), #endif #ifdef USE_I2C - { OWNER_I2C_SCL, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagScl[0]), I2CDEV_COUNT }, - { OWNER_I2C_SDA, PG_I2C_CONFIG, offsetof(i2cConfig_t, ioTagSda[0]), I2CDEV_COUNT }, + DEFW( OWNER_I2C_SCL, PG_I2C_CONFIG, i2cConfig_t, ioTagScl, I2CDEV_COUNT ), + DEFW( OWNER_I2C_SDA, PG_I2C_CONFIG, i2cConfig_t, ioTagSda, I2CDEV_COUNT ), #endif - { OWNER_LED, PG_STATUS_LED_CONFIG, offsetof(statusLedConfig_t, ioTags[0]), STATUS_LED_NUMBER }, + DEFA( OWNER_LED, PG_STATUS_LED_CONFIG, statusLedConfig_t, ioTags[0], STATUS_LED_NUMBER ), #ifdef USE_SPEKTRUM_BIND - { OWNER_RX_BIND, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_bind_pin_override_ioTag), 0 }, - { OWNER_RX_BIND_PLUG, PG_RX_CONFIG, offsetof(rxConfig_t, spektrum_bind_plug_ioTag), 0 }, + DEFS( OWNER_RX_BIND, PG_RX_CONFIG, rxConfig_t, spektrum_bind_pin_override_ioTag ), + DEFS( OWNER_RX_BIND_PLUG, PG_RX_CONFIG, rxConfig_t, spektrum_bind_plug_ioTag ), #endif #ifdef USE_TRANSPONDER - { OWNER_TRANSPONDER, PG_TRANSPONDER_CONFIG, offsetof(transponderConfig_t, ioTag), 0 }, + DEFS( OWNER_TRANSPONDER, PG_TRANSPONDER_CONFIG, transponderConfig_t, ioTag ), #endif #ifdef USE_SPI - { OWNER_SPI_SCK, PG_SPI_PIN_CONFIG, offsetof(spiPinConfig_t, ioTagSck[0]), SPIDEV_COUNT }, - { OWNER_SPI_MISO, PG_SPI_PIN_CONFIG, offsetof(spiPinConfig_t, ioTagMiso[0]), SPIDEV_COUNT }, - { OWNER_SPI_MOSI, PG_SPI_PIN_CONFIG, offsetof(spiPinConfig_t, ioTagMosi[0]), SPIDEV_COUNT }, + DEFW( OWNER_SPI_SCK, PG_SPI_PIN_CONFIG, spiPinConfig_t, ioTagSck, SPIDEV_COUNT ), + DEFW( OWNER_SPI_MISO, PG_SPI_PIN_CONFIG, spiPinConfig_t, ioTagMiso, SPIDEV_COUNT ), + DEFW( OWNER_SPI_MOSI, PG_SPI_PIN_CONFIG, spiPinConfig_t, ioTagMosi, SPIDEV_COUNT ), #endif #ifdef USE_ESCSERIAL - { OWNER_ESCSERIAL, PG_ESCSERIAL_CONFIG, offsetof(escSerialConfig_t, ioTag), 0 }, + DEFS( OWNER_ESCSERIAL, PG_ESCSERIAL_CONFIG, escSerialConfig_t, ioTag ), #endif #ifdef USE_CAMERA_CONTROL - { OWNER_CAMERA_CONTROL, PG_CAMERA_CONTROL_CONFIG, offsetof(cameraControlConfig_t, ioTag), 0 }, + DEFS( OWNER_CAMERA_CONTROL, PG_CAMERA_CONTROL_CONFIG, cameraControlConfig_t, ioTag ), #endif #ifdef USE_ADC - { OWNER_ADC_BATT, PG_ADC_CONFIG, offsetof(adcConfig_t, vbat.ioTag), 0 }, - { OWNER_ADC_RSSI, PG_ADC_CONFIG, offsetof(adcConfig_t, rssi.ioTag), 0 }, - { OWNER_ADC_CURR, PG_ADC_CONFIG, offsetof(adcConfig_t, current.ioTag), 0 }, - { OWNER_ADC_EXT, PG_ADC_CONFIG, offsetof(adcConfig_t, external1.ioTag), 0 }, + DEFS( OWNER_ADC_BATT, PG_ADC_CONFIG, adcConfig_t, vbat.ioTag ), + DEFS( OWNER_ADC_RSSI, PG_ADC_CONFIG, adcConfig_t, rssi.ioTag ), + DEFS( OWNER_ADC_CURR, PG_ADC_CONFIG, adcConfig_t, current.ioTag ), + DEFS( OWNER_ADC_EXT, PG_ADC_CONFIG, adcConfig_t, external1.ioTag ), #endif #ifdef USE_BARO - { OWNER_BARO_CS, PG_BAROMETER_CONFIG, offsetof(barometerConfig_t, baro_spi_csn), 0 }, + DEFS( OWNER_BARO_CS, PG_BAROMETER_CONFIG, barometerConfig_t, baro_spi_csn ), #endif #ifdef USE_MAG - { OWNER_COMPASS_CS, PG_COMPASS_CONFIG, offsetof(compassConfig_t, mag_spi_csn), 0 }, + DEFS( OWNER_COMPASS_CS, PG_COMPASS_CONFIG, compassConfig_t, mag_spi_csn ), #endif #ifdef USE_SDCARD - { OWNER_SDCARD_CS, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, chipSelectTag), 0 }, - { OWNER_SDCARD_DETECT, PG_SDCARD_CONFIG, offsetof(sdcardConfig_t, cardDetectTag), 0 }, + DEFS( OWNER_SDCARD_CS, PG_SDCARD_CONFIG, sdcardConfig_t, chipSelectTag ), + DEFS( OWNER_SDCARD_DETECT, PG_SDCARD_CONFIG, sdcardConfig_t, cardDetectTag ), #endif #ifdef USE_PINIO - { OWNER_PINIO, PG_PINIO_CONFIG, offsetof(pinioConfig_t, ioTag), PINIO_COUNT }, + DEFA( OWNER_PINIO, PG_PINIO_CONFIG, pinioConfig_t, ioTag, PINIO_COUNT ), #endif #if defined(USE_USB_MSC) - { OWNER_USB_MSC_PIN, PG_USB_CONFIG, offsetof(usbDev_t, mscButtonPin), 0 }, + DEFS( OWNER_USB_MSC_PIN, PG_USB_CONFIG, usbDev_t, mscButtonPin ), #endif #ifdef USE_FLASH - { OWNER_FLASH_CS, PG_FLASH_CONFIG, offsetof(flashConfig_t, csTag), 0 }, + DEFS( OWNER_FLASH_CS, PG_FLASH_CONFIG, flashConfig_t, csTag ), #endif #ifdef USE_MAX7456 - { OWNER_OSD_CS, PG_MAX7456_CONFIG, offsetof(max7456Config_t, csTag), 0 }, + DEFS( OWNER_OSD_CS, PG_MAX7456_CONFIG, max7456Config_t, csTag ), #endif }; +#undef DEFS +#undef DEFA +#undef DEFW + static ioTag_t *getIoTag(const cliResourceValue_t value, uint8_t index) { const pgRegistry_t* rec = pgFind(value.pgn); - return CONST_CAST(ioTag_t *, rec->address + value.offset + index); + return CONST_CAST(ioTag_t *, rec->address + value.stride * index + value.offset); } static void printResource(uint8_t dumpMask) @@ -3463,8 +3482,8 @@ static void printResource(uint8_t dumpMask) } for (int index = 0; index < MAX_RESOURCE_INDEX(resourceTable[i].maxIndex); index++) { - const ioTag_t ioTag = *((const ioTag_t *)currentConfig + resourceTable[i].offset + index); - const ioTag_t ioTagDefault = *((const ioTag_t *)defaultConfig + resourceTable[i].offset + index); + const ioTag_t ioTag = *((const uint8_t *)currentConfig + resourceTable[i].stride * index + resourceTable[i].offset); + const ioTag_t ioTagDefault = *((const uint8_t *)defaultConfig + resourceTable[i].stride * index + resourceTable[i].offset); bool equalsDefault = ioTag == ioTagDefault; const char *format = "resource %s %d %c%02d"; diff --git a/src/main/osd_slave/osd_slave_init.c b/src/main/osd_slave/osd_slave_init.c index a448f4d293..10202c3496 100644 --- a/src/main/osd_slave/osd_slave_init.c +++ b/src/main/osd_slave/osd_slave_init.c @@ -204,7 +204,7 @@ void init(void) #else #ifdef USE_SPI - spiPinConfigure(spiPinConfig()); + spiPinConfigure(spiPinConfig(0)); // Initialize CS lines and keep them high spiPreInit(); @@ -224,7 +224,7 @@ void init(void) #endif /* USE_SPI */ #ifdef USE_I2C - i2cHardwareConfigure(i2cConfig()); + i2cHardwareConfigure(i2cConfig(0)); // Note: Unlike UARTs which are configured when client is present, // I2C buses are initialized unconditionally if they are configured. diff --git a/src/main/pg/bus_i2c.c b/src/main/pg/bus_i2c.c index 65b4c74700..846db887c6 100644 --- a/src/main/pg/bus_i2c.c +++ b/src/main/pg/bus_i2c.c @@ -67,13 +67,14 @@ void pgResetFn_i2cConfig(i2cConfig_t *i2cConfig) for (size_t index = 0 ; index < ARRAYLEN(i2cDefaultConfig) ; index++) { const i2cDefaultConfig_t *defconf = &i2cDefaultConfig[index]; - i2cConfig->ioTagScl[defconf->device] = defconf->ioTagScl; - i2cConfig->ioTagSda[defconf->device] = defconf->ioTagSda; - i2cConfig->overClock[defconf->device] = defconf->overClock; - i2cConfig->pullUp[defconf->device] = defconf->pullUp; + int device = defconf->device; + i2cConfig[device].ioTagScl = defconf->ioTagScl; + i2cConfig[device].ioTagSda = defconf->ioTagSda; + i2cConfig[device].overClock = defconf->overClock; + i2cConfig[device].pullUp = defconf->pullUp; } } -PG_REGISTER_WITH_RESET_FN(i2cConfig_t, i2cConfig, PG_I2C_CONFIG, 0); +PG_REGISTER_ARRAY_WITH_RESET_FN(i2cConfig_t, I2CDEV_COUNT, i2cConfig, PG_I2C_CONFIG, 0); #endif // defined(USE_I2C) && !defined(USE_SOFT_I2C) diff --git a/src/main/pg/bus_i2c.h b/src/main/pg/bus_i2c.h index 9a2e7582a2..7d65a416d0 100644 --- a/src/main/pg/bus_i2c.h +++ b/src/main/pg/bus_i2c.h @@ -29,10 +29,10 @@ #include "pg/pg.h" typedef struct i2cConfig_s { - ioTag_t ioTagScl[I2CDEV_COUNT]; - ioTag_t ioTagSda[I2CDEV_COUNT]; - bool overClock[I2CDEV_COUNT]; - bool pullUp[I2CDEV_COUNT]; + ioTag_t ioTagScl; + ioTag_t ioTagSda; + bool overClock; + bool pullUp; } i2cConfig_t; -PG_DECLARE(i2cConfig_t, i2cConfig); +PG_DECLARE_ARRAY(i2cConfig_t, I2CDEV_COUNT, i2cConfig); diff --git a/src/main/pg/bus_spi.c b/src/main/pg/bus_spi.c index edacd082e4..08c986ea0b 100644 --- a/src/main/pg/bus_spi.c +++ b/src/main/pg/bus_spi.c @@ -51,15 +51,15 @@ const spiDefaultConfig_t spiDefaultConfig[] = { #endif }; -PG_REGISTER_WITH_RESET_FN(spiPinConfig_t, spiPinConfig, PG_SPI_PIN_CONFIG, 0); +PG_REGISTER_ARRAY_WITH_RESET_FN(spiPinConfig_t, SPIDEV_COUNT, spiPinConfig, PG_SPI_PIN_CONFIG, 1); void pgResetFn_spiPinConfig(spiPinConfig_t *spiPinConfig) { for (size_t i = 0 ; i < ARRAYLEN(spiDefaultConfig) ; i++) { const spiDefaultConfig_t *defconf = &spiDefaultConfig[i]; - spiPinConfig->ioTagSck[defconf->device] = defconf->sck; - spiPinConfig->ioTagMiso[defconf->device] = defconf->miso; - spiPinConfig->ioTagMosi[defconf->device] = defconf->mosi; + spiPinConfig[defconf->device].ioTagSck = defconf->sck; + spiPinConfig[defconf->device].ioTagMiso = defconf->miso; + spiPinConfig[defconf->device].ioTagMosi = defconf->mosi; } } #endif diff --git a/src/main/pg/bus_spi.h b/src/main/pg/bus_spi.h index 52e7bfe7ae..26d2e2d3eb 100644 --- a/src/main/pg/bus_spi.h +++ b/src/main/pg/bus_spi.h @@ -26,9 +26,9 @@ #include "pg/pg.h" typedef struct spiPinConfig_s { - ioTag_t ioTagSck[SPIDEV_COUNT]; - ioTag_t ioTagMiso[SPIDEV_COUNT]; - ioTag_t ioTagMosi[SPIDEV_COUNT]; + ioTag_t ioTagSck; + ioTag_t ioTagMiso; + ioTag_t ioTagMosi; } spiPinConfig_t; -PG_DECLARE(spiPinConfig_t, spiPinConfig); +PG_DECLARE_ARRAY(spiPinConfig_t, SPIDEV_COUNT, spiPinConfig); diff --git a/src/main/target/ALIENFLIGHTF3/initialisation.c b/src/main/target/ALIENFLIGHTF3/initialisation.c index 298c5a9523..b67092080c 100644 --- a/src/main/target/ALIENFLIGHTF3/initialisation.c +++ b/src/main/target/ALIENFLIGHTF3/initialisation.c @@ -35,10 +35,10 @@ extern void spiPreInit(void); void targetBusInit(void) { if (hardwareRevision == AFF3_REV_2) { - spiPinConfigure(spiPinConfig()); + spiPinConfigure(spiPinConfig(0)); spiPreInit(); spiInit(SPIDEV_3); } - i2cHardwareConfigure(i2cConfig()); + i2cHardwareConfigure(i2cConfig(0)); i2cInit(I2CDEV_2); } diff --git a/src/main/target/CJMCU/initialisation.c b/src/main/target/CJMCU/initialisation.c index c6fdabc2bf..46b2ce5d13 100644 --- a/src/main/target/CJMCU/initialisation.c +++ b/src/main/target/CJMCU/initialisation.c @@ -35,14 +35,14 @@ extern void spiPreInit(void); // XXX In fc/fc_init.c void targetBusInit(void) { #if defined(USE_SPI) && defined(USE_SPI_DEVICE_1) - spiPinConfigure(spiPinConfig()); + spiPinConfigure(spiPinConfig(0)); spiPreInit(); spiInit(SPIDEV_1); #endif if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { serialRemovePort(SERIAL_PORT_USART3); - i2cHardwareConfigure(i2cConfig()); + i2cHardwareConfigure(i2cConfig(0)); i2cInit(I2C_DEVICE); } } diff --git a/src/main/target/COLIBRI_RACE/target.c b/src/main/target/COLIBRI_RACE/target.c index 2b8adb2eb2..bf3bf05c06 100644 --- a/src/main/target/COLIBRI_RACE/target.c +++ b/src/main/target/COLIBRI_RACE/target.c @@ -59,13 +59,13 @@ const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { void targetBusInit(void) { #ifdef USE_SPI - spiPinConfigure(spiPinConfig()); + spiPinConfigure(spiPinConfig(0)); #ifdef USE_SPI_DEVICE_1 spiInit(SPIDEV_1); #endif #endif - i2cHardwareConfigure(i2cConfig()); + i2cHardwareConfigure(i2cConfig(0)); i2cInit(I2CDEV_2); bstInit(BST_DEVICE); diff --git a/src/main/target/NAZE/initialisation.c b/src/main/target/NAZE/initialisation.c index 26e4166c88..359363dd48 100644 --- a/src/main/target/NAZE/initialisation.c +++ b/src/main/target/NAZE/initialisation.c @@ -36,7 +36,7 @@ extern void spiPreInit(void); // XXX In fc/fc_init.c void targetBusInit(void) { #ifdef USE_SPI - spiPinConfigure(spiPinConfig()); + spiPinConfigure(spiPinConfig(0)); spiPreInit(); #ifdef USE_SPI_DEVICE_2 spiInit(SPIDEV_2); @@ -48,11 +48,11 @@ void targetBusInit(void) if (!doesConfigurationUsePort(SERIAL_PORT_USART3)) { serialRemovePort(SERIAL_PORT_USART3); - i2cHardwareConfigure(i2cConfig()); + i2cHardwareConfigure(i2cConfig(0)); i2cInit(I2C_DEVICE); } } else { - i2cHardwareConfigure(i2cConfig()); + i2cHardwareConfigure(i2cConfig(0)); i2cInit(I2C_DEVICE); } }