diff --git a/boards/modalai/voxl2/slpi.px4board b/boards/modalai/voxl2/slpi.px4board index dc88af3cde..2ca84442a6 100644 --- a/boards/modalai/voxl2/slpi.px4board +++ b/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 diff --git a/platforms/qurt/include/px4_arch/micro_hal.h b/platforms/qurt/include/px4_arch/micro_hal.h index d88e2ee29e..236cc48ce7 100644 --- a/platforms/qurt/include/px4_arch/micro_hal.h +++ b/platforms/qurt/include/px4_arch/micro_hal.h @@ -31,4 +31,33 @@ * ****************************************************************************/ -// Placeholder +#pragma once + +#include +#include + +#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 */ diff --git a/platforms/qurt/src/px4/CMakeLists.txt b/platforms/qurt/src/px4/CMakeLists.txt index 097ec098f2..4c10b97aa1 100644 --- a/platforms/qurt/src/px4/CMakeLists.txt +++ b/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) diff --git a/platforms/qurt/src/px4/px4_i2c.cpp b/platforms/qurt/src/px4/px4_i2c.cpp new file mode 100644 index 0000000000..5b6c9cc647 --- /dev/null +++ b/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 +#include +#include + +#if defined(CONFIG_I2C) + +#include + +/** + * 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 */ diff --git a/src/lib/drivers/device/qurt/I2C.cpp b/src/lib/drivers/device/qurt/I2C.cpp index aa48429c94..0d1eb0757e 100644 --- a/src/lib/drivers/device/qurt/I2C.cpp +++ b/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)) { - PX4_INFO("Set i2c address 0x%x, fd %d", address, _i2c_fd); + if (log) { + PX4_INFO("Set i2c address 0x%x, fd %d", address, _i2c_fd); + } pthread_mutex_lock(_mutex); _set_i2c_address(_i2c_fd, address); diff --git a/src/lib/drivers/device/qurt/I2C.hpp b/src/lib/drivers/device/qurt/I2C.hpp index ccf5e1f5df..2e4e214bab 100644 --- a/src/lib/drivers/device/qurt/I2C.hpp +++ b/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. diff --git a/src/systemcmds/i2cdetect/i2cdetect.cpp b/src/systemcmds/i2cdetect/i2cdetect.cpp index cf2ce1290a..d383cb1cff 100644 --- a/src/systemcmds/i2cdetect/i2cdetect.cpp +++ b/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);