diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index 041eccafb9..a8fdc83c06 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2019, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2019-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,6 +44,10 @@ #include +#if defined(CONFIG_I2C) +# include +# include +#endif // CONFIG_I2C int px4_platform_init(void) { @@ -72,6 +76,35 @@ int px4_platform_init(void) cpuload_initialize_once(); #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 + + px4::WorkQueueManagerStart(); uorb_start();