feat(voxl2): Added i2cdetect system command to voxl2-slpi build. Needed to implement the required i2c API for it.

Also, changed the printf into PX4_INFO so the output can be seen for Qurt platforms.

boards/modalai/voxl2/slpi.px4board

@@ -13,6 +13,7 @@ CONFIG_MODULES_NAVIGATOR=n
 CONFIG_MODULES_UXRCE_DDS_CLIENT=n
 CONFIG_SYSTEMCMDS_ACTUATOR_TEST=n
 CONFIG_SYSTEMCMDS_BSONDUMP=n
+CONFIG_SYSTEMCMDS_I2CDETECT=y
 CONFIG_SYSTEMCMDS_PERF=n
 CONFIG_SYSTEMCMDS_TOPIC_LISTENER=n
 CONFIG_SYSTEMCMDS_VER=n

platforms/qurt/include/px4_arch/micro_hal.h

@@ -31,4 +31,33 @@
  *
  ****************************************************************************/
 
-// Placeholder
+#pragma once
+
+#include <stdint.h>
+#include <board_config.h>
+
+#if defined(CONFIG_I2C)
+
+__BEGIN_DECLS
+
+struct i2c_master_s;
+
+struct i2c_msg_s {
+	uint32_t frequency;
+	uint16_t addr;
+	uint16_t flags;
+	uint8_t *buffer;
+	int      length;
+};
+
+#define I2C_M_READ   0x0001
+
+#define I2C_TRANSFER(dev, msgs, count) px4_qurt_i2c_transfer(dev, msgs, count)
+
+struct i2c_master_s *px4_i2cbus_initialize(int bus);
+int                  px4_i2cbus_uninitialize(struct i2c_master_s *dev);
+int                  px4_qurt_i2c_transfer(struct i2c_master_s *dev, struct i2c_msg_s *msgs, unsigned count);
+
+__END_DECLS
+
+#endif /* CONFIG_I2C */

platforms/qurt/src/px4/CMakeLists.txt

@@ -40,6 +40,7 @@ set(QURT_LAYER_SRCS
 	main.cpp
 	qurt_log.cpp
 	SerialImpl.cpp
+	px4_i2c.cpp
 	)
 
 add_library(px4_layer
@@ -48,3 +49,4 @@ add_library(px4_layer
 
 target_link_libraries(px4_layer PRIVATE work_queue)
 target_link_libraries(px4_layer PRIVATE drivers_board)
+target_link_libraries(px4_layer PRIVATE drivers__device)

platforms/qurt/src/px4/px4_i2c.cpp

@@ -0,0 +1,164 @@
+/****************************************************************************
+ *
+ * Copyright (C) 2022 ModalAI, Inc. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file px4_i2c.cpp
+ *
+ * NuttX-compatible I2C API shim for QuRT/SLPI.
+ * Implements px4_i2cbus_initialize/uninitialize and I2C_TRANSFER
+ * on top of the QuRT device::I2C callback infrastructure by deriving
+ * from device::I2C to access its protected transfer methods.
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <px4_arch/micro_hal.h>
+
+#if defined(CONFIG_I2C)
+
+#include <lib/drivers/device/qurt/I2C.hpp>
+
+/**
+ * Minimal derived class that exposes the protected I2C::transfer()
+ * and I2C::set_device_address() for use by the compatibility shim.
+ */
+class I2CShim : public device::I2C
+{
+public:
+	I2CShim(int bus) :
+		I2C(0, "i2c_shim", bus, 0x01, 100000)
+	{}
+
+	int do_transfer(uint8_t addr, const uint8_t *send, unsigned send_len,
+			uint8_t *recv, unsigned recv_len)
+	{
+		set_device_address(addr, false);
+		return transfer(send, send_len, recv, recv_len);
+	}
+};
+
+struct i2c_master_s {
+	I2CShim *shim;
+};
+
+struct i2c_master_s *px4_i2cbus_initialize(int bus)
+{
+	auto *shim = new I2CShim(bus);
+
+	if (shim == nullptr) {
+		return nullptr;
+	}
+
+	if (shim->init() != PX4_OK) {
+		delete shim;
+		return nullptr;
+	}
+
+	auto *dev = new i2c_master_s;
+
+	if (dev == nullptr) {
+		delete shim;
+		return nullptr;
+	}
+
+	dev->shim = shim;
+	return dev;
+}
+
+int px4_i2cbus_uninitialize(struct i2c_master_s *dev)
+{
+	if (dev != nullptr) {
+		delete dev->shim;
+		delete dev;
+	}
+
+	return PX4_OK;
+}
+
+int px4_qurt_i2c_transfer(struct i2c_master_s *dev, struct i2c_msg_s *msgs, unsigned count)
+{
+	if (dev == nullptr || dev->shim == nullptr || msgs == nullptr || count == 0) {
+		return PX4_ERROR;
+	}
+
+	unsigned i = 0;
+
+	while (i < count) {
+		struct i2c_msg_s *msg = &msgs[i];
+
+		// Check for write+read pair to the same address (common i2cdetect pattern)
+		if ((i + 1 < count) &&
+		    !(msg->flags & I2C_M_READ) &&
+		    (msgs[i + 1].flags & I2C_M_READ) &&
+		    (msg->addr == msgs[i + 1].addr)) {
+
+			int ret = dev->shim->do_transfer(msg->addr,
+							 msg->buffer, msg->length,
+							 msgs[i + 1].buffer, msgs[i + 1].length);
+
+			if (ret != PX4_OK) {
+				return ret;
+			}
+
+			i += 2;
+
+		} else if (msg->flags & I2C_M_READ) {
+			// Single read
+			int ret = dev->shim->do_transfer(msg->addr,
+							 nullptr, 0,
+							 msg->buffer, msg->length);
+
+			if (ret != PX4_OK) {
+				return ret;
+			}
+
+			i++;
+
+		} else {
+			// Single write
+			int ret = dev->shim->do_transfer(msg->addr,
+							 msg->buffer, msg->length,
+							 nullptr, 0);
+
+			if (ret != PX4_OK) {
+				return ret;
+			}
+
+			i++;
+		}
+	}
+
+	return PX4_OK;
+}
+
+#endif /* CONFIG_I2C */

src/lib/drivers/device/qurt/I2C.cpp

@@ -158,10 +158,12 @@ out:
 }
 
 void
-I2C::set_device_address(int address)
+I2C::set_device_address(int address, bool log)
 {
 	if ((_i2c_fd != PX4_ERROR) && (_set_i2c_address != NULL)) {
+		if (log) {
 			PX4_INFO("Set i2c address 0x%x, fd %d", address, _i2c_fd);
+		}
 
 		pthread_mutex_lock(_mutex);
 		_set_i2c_address(_i2c_fd, address);

src/lib/drivers/device/qurt/I2C.hpp

@@ -103,7 +103,7 @@ protected:
 	 */
 	virtual int	probe() { return PX4_OK; }
 
-	virtual void set_device_address(int address);
+	virtual void set_device_address(int address, bool log = true);
 
 	/**
 	 * Perform an I2C transaction to the device.

src/systemcmds/i2cdetect/i2cdetect.cpp

@@ -50,7 +50,7 @@ namespace i2cdetect
 
 int detect(int bus)
 {
-	printf("Scanning I2C bus: %d\n", bus);
+	PX4_INFO("Scanning I2C bus: %d", bus);
 
 	int ret = PX4_ERROR;
 
@@ -62,15 +62,17 @@ int detect(int bus)
 		return PX4_ERROR;
 	}
 
-	printf("     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f\n");
+	// Line buffer for building up output
+	char line[80];
+	int pos = 0;
+
+	PX4_INFO("     0  1  2  3  4  5  6  7  8  9  a  b  c  d  e  f");
 
 	for (int i = 0; i < 128; i += 16) {
-		printf("%02x: ", i);
+		pos = snprintf(line, sizeof(line), "%02x: ", i);
 
 		for (int j = 0; j < 16; j++) {
 
-			fflush(stdout);
-
 			uint8_t addr = i + j;
 
 			unsigned retry_count = 0;
@@ -115,14 +117,14 @@ int detect(int bus)
 			} while (retry_count++ < retries);
 
 			if (found) {
-				printf("%02x ", addr);
+				pos += snprintf(line + pos, sizeof(line) - pos, "%02x ", addr);
 
 			} else {
-				printf("-- ");
+				pos += snprintf(line + pos, sizeof(line) - pos, "-- ");
 			}
 		}
 
-		printf("\n");
+		PX4_INFO("%s", line);
 	}
 
 	px4_i2cbus_uninitialize(i2c_dev);