diff --git a/boards/px4/fmu-v5x/default.px4board b/boards/px4/fmu-v5x/default.px4board index f0ff95bb6d..a8cf33b393 100644 --- a/boards/px4/fmu-v5x/default.px4board +++ b/boards/px4/fmu-v5x/default.px4board @@ -87,6 +87,7 @@ CONFIG_MODULES_TEMPERATURE_COMPENSATION=y CONFIG_MODULES_VTOL_ATT_CONTROL=y CONFIG_SYSTEMCMDS_ACTUATOR_TEST=y CONFIG_SYSTEMCMDS_BL_UPDATE=y +CONFIG_SYSTEMCMDS_BOARDCTL=y CONFIG_SYSTEMCMDS_DMESG=y CONFIG_SYSTEMCMDS_DUMPFILE=y CONFIG_SYSTEMCMDS_GPIO=y diff --git a/platforms/common/i2c_spi_buses.cpp b/platforms/common/i2c_spi_buses.cpp index 91f4c6844f..5c32d02da1 100644 --- a/platforms/common/i2c_spi_buses.cpp +++ b/platforms/common/i2c_spi_buses.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2020, 2021 PX4 Development Team. All rights reserved. + * Copyright (C) 2020-2022 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 @@ -49,6 +49,185 @@ static List i2c_spi_module_instances; ///< list of currently r static pthread_mutex_t i2c_spi_module_instances_mutex = PTHREAD_MUTEX_INITIALIZER; +void px4_print_all_instances() +{ + pthread_mutex_lock(&i2c_spi_module_instances_mutex); + +#if defined(CONFIG_I2C) + // I2C + { + PX4_INFO_RAW("\nI2C Type Address\n"); + int num_i2c_buses = 0; + + for (auto &i2c_bus : px4_i2c_buses) { + if (i2c_bus.bus != -1) { + num_i2c_buses++; + + PX4_INFO_RAW("|__ I2C:%d \n", i2c_bus.bus); // TODO: internal/external + // TODO: print differently if last + + for (I2CSPIInstance *instance : i2c_spi_module_instances) { + if (instance && (instance->bus() == i2c_bus.bus) + && (instance->bus_option() == I2CSPIBusOption::I2CInternal || instance->bus_option() == I2CSPIBusOption::I2CExternal) + ) { + //PX4_INFO_RAW("|__%2d) ", i); + //PX4_INFO_RAW("|__ I2C %d %s\n", px4_i2c_buses[i].bus, px4_i2c_buses[i].is_external ? "external" : "internal"); + // Type: 0x%02X, I2C:%d 0x%02X + PX4_INFO_RAW("| |__ %s 0x%02X 0x%02X\n", instance->module_name(), instance->devid_driver_index(), + instance->get_i2c_address()); + + // TODO: if running? + // TOOD: if exiting? + // TODO: general status otherwise? + } + } + } + } + + // int num_i2c_instances = 0; + // for (const I2CSPIInstance *instance : i2c_spi_module_instances) { + // if (instance) { + // if (instance->bus_option() == I2CSPIBusOption::I2CInternal || instance->bus_option() == I2CSPIBusOption::I2CExternal) { + // num_i2c_instances++; + // } + // } + // } + } +#endif // CONFIG_I2C + +#if defined(CONFIG_SPI) + // SPI + { + PX4_INFO_RAW("\nSPI Type \n"); + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + const auto &spi_bus = px4_spi_buses[i]; + + if (spi_bus.bus != -1) { + //num_i2c_buses++; + + PX4_INFO_RAW("|__ SPI:%d (%s) %s %s \n", + spi_bus.bus, + px4_spi_buses[i].is_external ? "external" : "internal", + (px4_spi_buses[i].power_enable_gpio != 0) ? " Power Control " : "", + px4_spi_buses[i].requires_locking ? " requires locking " : "" + ); + + + + + + // TODO: print differently if last + + // power_enable_gpio + // is_external + // requires_locking + + // PX4_SPIDEVID_TYPE(bus_data.devices[j].devid + // PX4_SPIDEVID_TYPE(devid) == PX4_SPIDEVID_TYPE(bus_data.devices[j].devid) + // PX4_SPI_DEV_ID(devid) == bus_data.devices[j].devtype_driver + + for (const auto &dev : spi_bus.devices) { + if (dev.cs_gpio || dev.devid || dev.devtype_driver) { + // cs_gpio + // drdy_gpio + // devid + // devtype_driver + PX4_INFO_RAW("| |__ %" PRIu32 ", devtype_driver: 0x%02X \n", dev.devid, dev.devtype_driver); + + // match bus type, bus, devtype_driver + + // _bus_device_index = -1; + + + // devtype_driver devid_driver_index() + //PX4_INFO_RAW("| |__ %s 0x%02X \n", instance->module_name(), instance->devid_driver_index()); + } + + // match bus + // match devtype_driver devid_driver_index() + } + + for (I2CSPIInstance *instance : i2c_spi_module_instances) { + if (instance && (instance->bus() == spi_bus.bus) + && (instance->bus_option() == I2CSPIBusOption::SPIInternal || instance->bus_option() == I2CSPIBusOption::SPIExternal) + ) { + //PX4_INFO_RAW("|__%2d) ", i); + //PX4_INFO_RAW("|__ I2C %d %s\n", px4_i2c_buses[i].bus, px4_i2c_buses[i].is_external ? "external" : "internal"); + // Type: 0x%02X, I2C:%d 0x%02X + PX4_INFO_RAW("| |__ %s 0x%02X \n", instance->module_name(), instance->devid_driver_index()); + + //instance-> + + // TODO: if running? + // TOOD: if exiting? + // TODO: general status otherwise? + } + } + } + } + } +#endif // CONFIG_SPI + + +#if defined(CONFIG_I2C) && 0 + + for (int i = 0; i < I2C_BUS_MAX_BUS_ITEMS; ++i) { + if ((px4_i2c_buses[i].bus != -1)) { + PX4_INFO_RAW("I2C %d %s\n", px4_i2c_buses[i].bus, px4_i2c_buses[i].is_external ? "external" : "internal"); + } + } + +#endif // CONFIG_I2C + +#if defined(CONFIG_SPI) && 0 + + for (int i = 0; i < SPI_BUS_MAX_BUS_ITEMS; ++i) { + if ((px4_spi_buses[i].bus != -1)) { + PX4_INFO_RAW("SPI %d %s\n", px4_spi_buses[i].bus, px4_spi_buses[i].is_external ? "external" : "internal"); + + for (const auto &dev : px4_spi_buses[i].devices) { + if (dev.devid || dev.devtype_driver) { + PX4_INFO_RAW("-->devid: %" PRIu32 ", devtype_driver: %" PRIu16 "\n", dev.devid, dev.devtype_driver); + } + } + + // SPI_BUS_MAX_DEVICES + // devid + // devtype_driver + // for (px4_spi_buses[i].devices) { + + // } + } + } + +#endif // CONFIG_SPI + + + // TODO: decode device id + // // Device ID + // uint32_t device_id = *(uint32_t *)(data_ptr + previous_data_offset); + // char device_id_buffer[80]; + // device::Device::device_id_print_buffer(device_id_buffer, sizeof(device_id_buffer), device_id); + // PX4_INFO_RAW(" (%s)", device_id_buffer); + + + // for (const I2CSPIInstance *instance : i2c_spi_module_instances) { + // if (instance) { + + + // // TODO: I2CSPIBusOption + // // I2C address + + // PX4_INFO_RAW("name: %s, bus: %d, devid_driver_index: %d, bus_device_index: %d\n", instance->module_name(), + // instance->bus(), instance->devid_driver_index(), instance->bus_device_index()); + // } + // } + + pthread_mutex_unlock(&i2c_spi_module_instances_mutex); +} + + I2CSPIDriverConfig::I2CSPIDriverConfig(const BusCLIArguments &cli, const BusInstanceIterator &iterator, const px4::wq_config_t &wq_config_) : module_name(iterator.moduleName()), @@ -237,7 +416,6 @@ int BusCLIArguments::getOpt(int argc, char *argv[], const char *options) if (bus_option == I2CSPIBusOption::I2CExternal || bus_option == I2CSPIBusOption::I2CInternal) { bus_frequency = default_i2c_frequency; - } #endif // CONFIG_I2C @@ -332,7 +510,7 @@ bool BusInstanceIterator::next() } else if (busType() == BOARD_SPI_BUS) { if (_spi_bus_iterator.next()) { - bus = _spi_bus_iterator.bus().bus; + bus = _spi_bus_iterator.bus(); } #endif // CONFIG_SPI @@ -340,7 +518,7 @@ bool BusInstanceIterator::next() } else if (busType() == BOARD_I2C_BUS) { if (_i2c_bus_iterator.next()) { - bus = _i2c_bus_iterator.bus().bus; + bus = _i2c_bus_iterator.bus(); } #endif // CONFIG_I2C @@ -448,14 +626,14 @@ int BusInstanceIterator::bus() const #if defined(CONFIG_SPI) if (busType() == BOARD_SPI_BUS) { - return _spi_bus_iterator.bus().bus; + return _spi_bus_iterator.bus(); } #endif // CONFIG_SPI #if defined(CONFIG_I2C) if (busType() == BOARD_I2C_BUS) { - return _i2c_bus_iterator.bus().bus; + return _i2c_bus_iterator.bus(); } #endif // CONFIG_I2C @@ -813,6 +991,31 @@ void I2CSPIDriverBase::print_status() #endif // CONFIG_SPI } +void I2CSPIDriverBase::print_run_status() +{ + // specialized ScheduledWorkItem::print_run_status() with additional I2C or SPI info + char name_description[34] {}; +#if defined(CONFIG_SPI) + + if (_bus_option == I2CSPIBusOption::SPIInternal || _bus_option == I2CSPIBusOption::SPIExternal) { + snprintf(name_description, sizeof(name_description), "%s (Type: 0x%02X)", _item_name, _devid_driver_index); + } + +#endif // CONFIG_SPI + +#if defined(CONFIG_I2C) + + if (_bus_option == I2CSPIBusOption::I2CExternal || _bus_option == I2CSPIBusOption::I2CInternal) { + snprintf(name_description, sizeof(name_description), "%s (Type: 0x%02X, I2C:%d 0x%02X)", _item_name, + _devid_driver_index, _bus, _i2c_address); + } + +#endif // CONFIG_I2C + + PX4_INFO_RAW("%-34s %8.1f Hz %12.0f us (%" PRId64 " us)\n", name_description, (double)average_rate(), + (double)average_interval(), 0ULL); +} + void I2CSPIDriverBase::request_stop_and_wait() { _task_should_exit.store(true); diff --git a/platforms/common/include/px4_platform_common/i2c.h b/platforms/common/include/px4_platform_common/i2c.h index 3ad80103d0..25427e0352 100644 --- a/platforms/common/include/px4_platform_common/i2c.h +++ b/platforms/common/include/px4_platform_common/i2c.h @@ -86,7 +86,7 @@ public: bool next(); - const px4_i2c_bus_t &bus() const { return px4_i2c_buses[_index]; } + int bus() const { return px4_i2c_buses[_index].bus; } int externalBusIndex() const { return _external_bus_counter; } diff --git a/platforms/common/include/px4_platform_common/i2c_spi_buses.h b/platforms/common/include/px4_platform_common/i2c_spi_buses.h index aa6dc90703..cc291700a8 100644 --- a/platforms/common/include/px4_platform_common/i2c_spi_buses.h +++ b/platforms/common/include/px4_platform_common/i2c_spi_buses.h @@ -56,6 +56,9 @@ # include #endif // CONFIG_SPI + +__EXPORT void px4_print_all_instances(); + enum class I2CSPIBusOption : uint8_t { All = 0, ///< select all runnning instances #if defined(CONFIG_I2C) @@ -114,6 +117,15 @@ public: #if defined(CONFIG_I2C) virtual int8_t get_i2c_address() {return _i2c_address;} #endif // CONFIG_I2C + + const char *module_name() const { return _module_name; } + const int &bus() const { return _bus; } + const I2CSPIBusOption &bus_option() const { return _bus_option; } + const uint16_t &devid_driver_index() const { return _devid_driver_index; } + const int8_t &bus_device_index() const { return _bus_device_index; } + + // I2C address? + private: I2CSPIInstance(const I2CSPIDriverConfig &config) : _module_name(config.module_name), _bus_option(config.bus_option), _bus(config.bus), @@ -295,6 +307,7 @@ protected: virtual ~I2CSPIDriverBase() = default; virtual void print_status(); + virtual void print_run_status(); virtual void custom_method(const BusCLIArguments &cli) {} @@ -356,6 +369,7 @@ private: { template static constexpr I2CSPIDriverBase::instantiate_method get(decltype(&C::instantiate)) { return &C::instantiate; } + template static constexpr I2CSPIDriverBase::instantiate_method get(...) { return &C::instantiate_default; } public: diff --git a/platforms/common/include/px4_platform_common/spi.h b/platforms/common/include/px4_platform_common/spi.h index 6664409822..7ab84c1326 100644 --- a/platforms/common/include/px4_platform_common/spi.h +++ b/platforms/common/include/px4_platform_common/spi.h @@ -153,14 +153,15 @@ public: bool next(); - const px4_spi_bus_t &bus() const { return px4_spi_buses[_index]; } + //const px4_spi_bus_t &bus() const { return px4_spi_buses[_index]; } + int bus() const { return px4_spi_buses[_index].bus; } spi_drdy_gpio_t DRDYGPIO() const { return px4_spi_buses[_index].devices[_bus_device_index].drdy_gpio; } uint32_t devid() const { return px4_spi_buses[_index].devices[_bus_device_index].devid; } int externalBusIndex() const { return _external_bus_counter; } - bool external() const { return px4_spi_bus_external(bus()); } + bool external() const { return px4_spi_bus_external(px4_spi_buses[_index]); } int busDeviceIndex() const { return _bus_device_index; } diff --git a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp index 794e390b37..8ab5bfa795 100644 --- a/platforms/common/px4_work_queue/ScheduledWorkItem.cpp +++ b/platforms/common/px4_work_queue/ScheduledWorkItem.cpp @@ -75,7 +75,7 @@ void ScheduledWorkItem::ScheduleClear() void ScheduledWorkItem::print_run_status() { if (_call.period > 0) { - PX4_INFO_RAW("%-29s %8.1f Hz %12.0f us (%" PRId64 " us)\n", _item_name, (double)average_rate(), + PX4_INFO_RAW("%-34s %8.1f Hz %12.0f us (%" PRId64 " us)\n", _item_name, (double)average_rate(), (double)average_interval(), _call.period); } else { diff --git a/platforms/common/px4_work_queue/WorkItem.cpp b/platforms/common/px4_work_queue/WorkItem.cpp index ffc0dca276..ebc876abfa 100644 --- a/platforms/common/px4_work_queue/WorkItem.cpp +++ b/platforms/common/px4_work_queue/WorkItem.cpp @@ -134,7 +134,7 @@ float WorkItem::average_interval() const void WorkItem::print_run_status() { - PX4_INFO_RAW("%-29s %8.1f Hz %12.0f us\n", _item_name, (double)average_rate(), (double)average_interval()); + PX4_INFO_RAW("%-34s %8.1f Hz %12.0f us\n", _item_name, (double)average_rate(), (double)average_interval()); // reset statistics _run_count = 0; diff --git a/platforms/common/px4_work_queue/WorkQueueManager.cpp b/platforms/common/px4_work_queue/WorkQueueManager.cpp index 3852215b29..25c61c41bf 100644 --- a/platforms/common/px4_work_queue/WorkQueueManager.cpp +++ b/platforms/common/px4_work_queue/WorkQueueManager.cpp @@ -455,7 +455,7 @@ WorkQueueManagerStatus() if (!_wq_manager_should_exit.load() && (_wq_manager_wqs_list != nullptr)) { const size_t num_wqs = _wq_manager_wqs_list->size(); - PX4_INFO_RAW("\nWork Queue: %-2zu threads RATE INTERVAL\n", num_wqs); + PX4_INFO_RAW("\nWork Queue: %-2zu threads RATE INTERVAL\n", num_wqs); LockGuard lg{_wq_manager_wqs_list->mutex()}; size_t i = 0; diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index fb371b9093..5d16fdf3b7 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -139,7 +139,7 @@ int px4_platform_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); + i2c_master_s *i2c_dev = px4_i2cbus_initialize(i2c_bus_iterator.bus()); #if defined(CONFIG_I2C_RESET) I2C_RESET(i2c_dev); diff --git a/src/lib/drivers/device/Device.hpp b/src/lib/drivers/device/Device.hpp index 8e3ffa1cba..bc1cc9ad9b 100644 --- a/src/lib/drivers/device/Device.hpp +++ b/src/lib/drivers/device/Device.hpp @@ -169,7 +169,7 @@ public: union DeviceId { struct DeviceStructure devid_s; - uint32_t devid; + uint32_t devid{0}; }; uint32_t get_device_id() const { return _device_id.devid; } @@ -268,8 +268,8 @@ protected: Device(uint8_t devtype, const char *name, DeviceBusType bus_type, uint8_t bus, uint8_t address) : _name(name) { set_device_type(devtype); - _device_id.devid_s.bus_type = bus_type; - _device_id.devid_s.bus = bus; + set_device_bus_type(bus_type); + set_device_bus(bus); set_device_address(address); } diff --git a/src/systemcmds/boardctl/CMakeLists.txt b/src/systemcmds/boardctl/CMakeLists.txt new file mode 100644 index 0000000000..92cd2d6c2f --- /dev/null +++ b/src/systemcmds/boardctl/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# Copyright (c) 2022 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 +# 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. +# +############################################################################ +px4_add_module( + MODULE systemcmds__boardctl + MAIN boardctl + COMPILE_FLAGS + SRCS + boardctl.cpp + DEPENDS + ) diff --git a/src/systemcmds/boardctl/Kconfig b/src/systemcmds/boardctl/Kconfig new file mode 100644 index 0000000000..b1e1050480 --- /dev/null +++ b/src/systemcmds/boardctl/Kconfig @@ -0,0 +1,5 @@ +menuconfig SYSTEMCMDS_BOARDCTL + bool "boardctl" + default n + ---help--- + Enable support for boardctl diff --git a/src/systemcmds/boardctl/boardctl.cpp b/src/systemcmds/boardctl/boardctl.cpp new file mode 100644 index 0000000000..320f6d2008 --- /dev/null +++ b/src/systemcmds/boardctl/boardctl.cpp @@ -0,0 +1,82 @@ +/**************************************************************************** + * + * Copyright (c) 2022 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 + * 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. + * + ****************************************************************************/ + +#include +#include +#include + +#include + + +// per bus power control? + +// boardctl stop-all + +// boardctl status + +// boardctl i2c status +// boardctl spi status + +// boardctl spi 1 poweroff + + + +// #define VDD_5V_PERIPH_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_PERIPH_nEN, !(on_true)) +// #define VDD_5V_HIPOWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_5V_HIPOWER_nEN, !(on_true)) +// #define VDD_3V3_SENSORS4_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SENSORS4_EN, (on_true)) +// #define VDD_3V3_SPEKTRUM_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SPEKTRUM_POWER_EN, (on_true)) +// #define VDD_3V3_SD_CARD_EN(on_true) px4_arch_gpiowrite(GPIO_VDD_3V3_SD_CARD_EN, (on_true)) +// #define VDD_3V3_ETH_POWER_EN(on_true) px4_arch_gpiowrite(GPIO_ETH_POWER_EN, (on_true)) + + +// board specific power status? +// GPIO_nPOWER_IN_A +// GPIO_nPOWER_IN_B + + + +// heater control +// HEATER_OUTPUT_EN + +// version + +// px4_hw_mft_item_t + +// timer test app + +extern "C" __EXPORT int boardctl_main(int argc, char *argv[]) +{ + px4_print_all_instances(); + + return 0; +}