From 8a8171c7aa941310432224528fe1990c155eed86 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Fri, 25 Jun 2021 14:57:34 -0400
Subject: [PATCH] platforms/nuttx: px4_init initialize and reset all I2C buses

---
 platforms/nuttx/src/px4/common/px4_init.cpp | 35 ++++++++++++++++++++-
 1 file changed, 34 insertions(+), 1 deletion(-)

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 <fcntl.h>
 
+#if defined(CONFIG_I2C)
+# include <px4_platform_common/i2c.h>
+# include <nuttx/i2c/i2c_master.h>
+#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();