diff --git a/platforms/common/include/px4_platform_common/init.h b/platforms/common/include/px4_platform_common/init.h index e8a135fbf0..5ce6851afd 100644 --- a/platforms/common/include/px4_platform_common/init.h +++ b/platforms/common/include/px4_platform_common/init.h @@ -34,6 +34,7 @@ __BEGIN_DECLS int px4_platform_init(void); +void px4_platform_i2c_init(void); int px4_platform_console_init(void); int px4_platform_configure(void); diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index d22905cd38..ceeb79bd72 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -93,6 +93,37 @@ static void cxx_initialize(void) } #endif +#if defined(CONFIG_I2C) +void px4_platform_i2c_init() +{ + + I2CBusIterator i2c_bus_iterator {I2CBusIterator::FilterType::All}; + + while (i2c_bus_iterator.next()) { + i2c_master_s *i2c_dev = px4_i2cbus_initialize(i2c_bus_iterator.bus().bus); + +#if defined(CONFIG_I2C_RESET) + I2C_RESET(i2c_dev); +#endif // CONFIG_I2C_RESET + + // send software reset to all + uint8_t buf[1] {}; + buf[0] = 0x06; // software reset + + i2c_msg_s msg{}; + msg.frequency = I2C_SPEED_STANDARD; + msg.addr = 0x00; // general call address + msg.buffer = &buf[0]; + msg.length = 1; + + I2C_TRANSFER(i2c_dev, &msg, 1); + + px4_i2cbus_uninitialize(i2c_dev); + } +} + +#endif // CONFIG_I2C + int px4_platform_init() { @@ -135,32 +166,9 @@ int px4_platform_init() #endif -#if defined(CONFIG_I2C) - I2CBusIterator i2c_bus_iterator {I2CBusIterator::FilterType::All}; - - while (i2c_bus_iterator.next()) { - i2c_master_s *i2c_dev = px4_i2cbus_initialize(i2c_bus_iterator.bus().bus); - -#if defined(CONFIG_I2C_RESET) - I2C_RESET(i2c_dev); -#endif // CONFIG_I2C_RESET - - // send software reset to all - uint8_t buf[1] {}; - buf[0] = 0x06; // software reset - - i2c_msg_s msg{}; - msg.frequency = I2C_SPEED_STANDARD; - msg.addr = 0x00; // general call address - msg.buffer = &buf[0]; - msg.length = 1; - - I2C_TRANSFER(i2c_dev, &msg, 1); - - px4_i2cbus_uninitialize(i2c_dev); - } - -#endif // CONFIG_I2C +#if defined(CONFIG_I2C) && !defined(BOARD_I2C_LATEINIT) + px4_platform_i2c_init(); +#endif #if defined(CONFIG_FS_PROCFS) int ret_mount_procfs = mount(nullptr, "/proc", "procfs", 0, nullptr);