boardctl hacks

boards/bitcraze/crazyflie/src/board_config.h

@@ -85,17 +85,8 @@
 #define GPIO_NRF_TXEN			(GPIO_INPUT|GPIO_PULLUP|GPIO_EXTI|GPIO_PORTA|GPIO_PIN4)
 
 
-/*
- * I2C busses
- */
-#define PX4_I2C_BUS_ONBOARD_HZ      400000
-#define PX4_I2C_BUS_EXPANSION_HZ      400000
-
 #define PX4_I2C_BUS_MTD	1
 
-#define BOARD_NUMBER_I2C_BUSES  3
-#define BOARD_I2C_BUS_CLOCK_INIT {PX4_I2C_BUS_ONBOARD_HZ, 100000, PX4_I2C_BUS_EXPANSION_HZ}
-
 
 /* Devices on the onboard bus.
  *

boards/bitcraze/crazyflie21/src/board_config.h

@@ -86,17 +86,8 @@
 
 #define GPIO_I2C4_DRDY1_BMP388        	(GPIO_INPUT|GPIO_FLOAT|GPIO_EXTI|GPIO_PORTG|GPIO_PIN5)
 
-/*
- * I2C busses
- */
-#define PX4_I2C_BUS_ONBOARD_HZ      400000
-#define PX4_I2C_BUS_EXPANSION_HZ      400000
-
 #define PX4_I2C_BUS_MTD	1
 
-#define BOARD_NUMBER_I2C_BUSES  3
-#define BOARD_I2C_BUS_CLOCK_INIT {PX4_I2C_BUS_ONBOARD_HZ, 400000, PX4_I2C_BUS_EXPANSION_HZ}
-
 
 /* Devices on the onboard bus.
  *

boards/bitcraze/crazyflie21/src/i2c.cpp

@@ -34,6 +34,6 @@
 #include <px4_arch/i2c_hw_description.h>
 
 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
-	initI2CBusExternal(1),
+	initI2CBusExternal(1, 400'000),
-	initI2CBusInternal(3),
+	initI2CBusInternal(3, 400'000),
 };

boards/px4/fmu-v2/src/i2c.cpp

@@ -43,7 +43,7 @@
 
 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
 	initI2CBusExternal(1),
-	initI2CBusInternal(2),
+	initI2CBusInternal(2, 100'000),
 };
 
 

boards/px4/fmu-v3/src/i2c.cpp

@@ -43,7 +43,7 @@
 
 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
 	initI2CBusExternal(1),
-	initI2CBusInternal(2),
+	initI2CBusInternal(2, 100'000),
 };
 
 

boards/px4/fmu-v4pro/src/i2c.cpp

@@ -34,6 +34,6 @@
 #include <px4_arch/i2c_hw_description.h>
 
 constexpr px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS] = {
-	initI2CBusInternal(1),
+	initI2CBusInternal(1, 100'000),
 	initI2CBusExternal(2),
 };

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

platforms/common/i2c.cpp

@@ -41,7 +41,7 @@
 bool px4_i2c_bus_external(int bus)
 {
 	for (int i = 0; i < I2C_BUS_MAX_BUS_ITEMS; ++i) {
-		if (px4_i2c_buses[i].bus == bus) {
+		if ((px4_i2c_buses[i].bus != -1) && (px4_i2c_buses[i].bus == bus)) {
 			return px4_i2c_buses[i].is_external;
 		}
 	}
@@ -60,6 +60,17 @@ bool px4_i2c_device_external(const uint32_t device_id)
 }
 #endif // BOARD_OVERRIDE_I2C_DEVICE_EXTERNAL
 
+int px4_i2c_bus_max_speed(int bus)
+{
+	for (int i = 0; i < I2C_BUS_MAX_BUS_ITEMS; ++i) {
+		if ((px4_i2c_buses[i].bus != -1) && (px4_i2c_buses[i].bus == bus)) {
+			return px4_i2c_buses[i].max_speed_hz;
+		}
+	}
+
+	return 0;
+}
+
 bool I2CBusIterator::next()
 {
 	while (++_index < I2C_BUS_MAX_BUS_ITEMS && px4_i2c_buses[_index].bus != -1) {

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<I2CSPIInstance *> 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);

platforms/common/include/px4_platform_common/board_common.h

@@ -51,27 +51,6 @@
  * Definitions
  ************************************************************************************/
 
-/* I2C PX4 clock configuration
- *
- * A board may override BOARD_I2C_BUS_CLOCK_INIT simply by defining the #defines.
- */
-
-#if defined(BOARD_I2C_BUS_CLOCK_INIT)
-#  define PX4_I2C_BUS_CLOCK_INIT BOARD_I2C_BUS_CLOCK_INIT
-#else
-#  if (PX4_NUMBER_I2C_BUSES) == 1
-#    define PX4_I2C_BUS_CLOCK_INIT {100000}
-#  elif (PX4_NUMBER_I2C_BUSES) == 2
-#    define PX4_I2C_BUS_CLOCK_INIT {100000, 100000}
-#  elif (PX4_NUMBER_I2C_BUSES) == 3
-#    define PX4_I2C_BUS_CLOCK_INIT {100000, 100000, 100000}
-#  elif (PX4_NUMBER_I2C_BUSES) == 4
-#    define PX4_I2C_BUS_CLOCK_INIT {100000, 100000, 100000, 100000}
-#  else
-#    error PX4_NUMBER_I2C_BUSES not supported
-#  endif
-#endif
-
 #ifdef BOARD_SPI_BUS_MAX_BUS_ITEMS
 #define SPI_BUS_MAX_BUS_ITEMS BOARD_SPI_BUS_MAX_BUS_ITEMS
 #else

platforms/common/include/px4_platform_common/i2c.h

@@ -40,8 +40,9 @@
 #define I2C_BUS_MAX_BUS_ITEMS PX4_NUMBER_I2C_BUSES
 
 struct px4_i2c_bus_t {
-	int bus{-1}; ///< physical bus number (1, ...) (-1 means this is unused)
+	uint32_t max_speed_hz{100'000}; ///< max speed in Hz
-	bool is_external; ///< static external configuration. Use px4_i2c_bus_external() to check if a bus is really external
+	int8_t bus{-1}; ///< physical bus number (1, ...) (-1 means this is unused)
+	bool is_external{true}; ///< static external configuration. Use px4_i2c_bus_external() to check if a bus is really external
 };
 
 __EXPORT extern const px4_i2c_bus_t px4_i2c_buses[I2C_BUS_MAX_BUS_ITEMS]; ///< board-specific I2C bus configuration
@@ -58,6 +59,11 @@ __EXPORT bool px4_i2c_bus_external(int bus);
  */
 __EXPORT bool px4_i2c_device_external(const uint32_t device_id);
 
+/**
+ * Return board defined max speed per bus in Hz. 0 if unknown or invalid bus.
+ */
+__EXPORT int px4_i2c_bus_max_speed(int bus);
+
 /**
  * @class I2CBusIterator
  * Iterate over configured I2C buses by the board
@@ -80,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; }
 

platforms/common/include/px4_platform_common/i2c_spi_buses.h

@@ -56,6 +56,9 @@
 # include <drivers/device/spi.h>
 #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 <typename C>
 		static constexpr I2CSPIDriverBase::instantiate_method get(decltype(&C::instantiate)) { return &C::instantiate; }
+
 		template <typename C>
 		static constexpr I2CSPIDriverBase::instantiate_method get(...) { return &C::instantiate_default; }
 	public:

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; }
 

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 {

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;

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;

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);

platforms/nuttx/src/px4/nxp/imxrt/include/px4_arch/i2c_hw_description.h

@@ -38,17 +38,19 @@
 
 #if defined(CONFIG_I2C)
 
-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }
 
-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;

platforms/nuttx/src/px4/nxp/kinetis/include/px4_arch/i2c_hw_description.h

@@ -38,17 +38,19 @@
 
 #if defined(CONFIG_I2C)
 
-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }
 
-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;

platforms/nuttx/src/px4/nxp/s32k14x/include/px4_arch/i2c_hw_description.h

@@ -37,17 +37,19 @@
 
 #if defined(CONFIG_I2C)
 
-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }
 
-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;

platforms/nuttx/src/px4/nxp/s32k1xx/include/px4_arch/i2c_hw_description.h

@@ -37,17 +37,19 @@
 
 #if defined(CONFIG_I2C)
 
-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }
 
-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;

platforms/nuttx/src/px4/rpi/rpi_common/include/px4_arch/i2c_hw_description.h

@@ -36,19 +36,23 @@
 #include <px4_arch/hw_description.h>
 #include <px4_platform_common/i2c.h>
 
+#if defined(CONFIG_I2C)
 
-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }
 
-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;
 }
+#endif // CONFIG_I2C

platforms/nuttx/src/px4/stm/stm32_common/include/px4_arch/i2c_hw_description.h

@@ -38,17 +38,19 @@
 
 #if defined(CONFIG_I2C)
 
-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }
 
-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;

platforms/posix/src/px4/generic/generic/include/px4_arch/i2c_hw_description.h

@@ -37,17 +37,19 @@
 
 #if defined(CONFIG_I2C)
 
-static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusInternal(int bus, uint32_t max_speed_hz = 400'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = false;
 	return ret;
 }
 
-static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus)
+static inline constexpr px4_i2c_bus_t initI2CBusExternal(int bus, uint32_t max_speed_hz = 100'000)
 {
 	px4_i2c_bus_t ret{};
+	ret.max_speed_hz = max_speed_hz;
 	ret.bus = bus;
 	ret.is_external = true;
 	return ret;

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;
+		set_device_bus_type(bus_type);
-		_device_id.devid_s.bus = bus;
+		set_device_bus(bus);
 		set_device_address(address);
 	}
 

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

@@ -49,12 +49,6 @@
 
 namespace device
 {
-/*
- *  N.B. By defaulting the value of _bus_clocks to non Zero
- *  All calls to init() will NOT set the buss frequency
- */
-
-unsigned int I2C::_bus_clocks[PX4_NUMBER_I2C_BUSES] = PX4_I2C_BUS_CLOCK_INIT;
 
 I2C::I2C(uint8_t device_type, const char *name, const int bus, const uint16_t address, const uint32_t frequency) :
 	CDev(name, nullptr),
@@ -80,98 +74,50 @@ I2C::~I2C()
 	}
 }
 
-int
-I2C::set_bus_clock(unsigned bus, unsigned clock_hz)
-{
-	int index = bus - 1;
-
-	if (index < 0 || index >= static_cast<int>(sizeof(_bus_clocks) / sizeof(_bus_clocks[0]))) {
-		return -EINVAL;
-	}
-
-	if (_bus_clocks[index] > 0) {
-		// DEVICE_DEBUG("overriding clock of %u with %u Hz\n", _bus_clocks[index], clock_hz);
-	}
-
-	_bus_clocks[index] = clock_hz;
-
-	return OK;
-}
-
 int
 I2C::init()
 {
-	int ret = PX4_ERROR;
-	unsigned bus_index;
-
 	// attach to the i2c bus
 	_dev = px4_i2cbus_initialize(get_device_bus());
 
 	if (_dev == nullptr) {
 		DEVICE_DEBUG("failed to init I2C");
-		ret = -ENOENT;
+		return -ENOENT;
-		goto out;
 	}
 
-	// the above call fails for a non-existing bus index,
+	const uint32_t max_speed_hz = px4_i2c_bus_max_speed(get_device_bus());
-	// so the index math here is safe.
-	bus_index = get_device_bus() - 1;
 
-	// abort if the max frequency we allow (the frequency we ask)
+	if (_frequency > max_speed_hz) {
-	// is smaller than the bus frequency
+		DEVICE_DEBUG("frequency %" PRIu32 " Hz exceeds bus %d maximum, limited to max %" PRIu32 " Hz", _frequency,
-	if (_bus_clocks[bus_index] > _frequency) {
+			     get_device_bus(), max_speed_hz);
-		(void)px4_i2cbus_uninitialize(_dev);
+		_frequency = max_speed_hz;
-		_dev = nullptr;
-		DEVICE_LOG("FAIL: too slow for bus #%u: %u KHz, device max: %" PRIu32 " KHz)",
-			   get_device_bus(), _bus_clocks[bus_index] / 1000, _frequency / 1000);
-		ret = -EINVAL;
-		goto out;
-	}
-
-	// set frequency for this instance once to the bus speed
-	// the bus speed is the maximum supported by all devices on the bus,
-	// as we have to prioritize performance over compatibility.
-	// If a new device requires a lower clock speed, this has to be
-	// manually set via "fmu i2c <bus> <clock>" before starting any
-	// drivers.
-	// This is necessary as automatically lowering the bus speed
-	// for maximum compatibility could induce timing issues on
-	// critical sensors the adopter might be unaware of.
-
-	// set the bus frequency on the first access if it has
-	// not been set yet
-	if (_bus_clocks[bus_index] == 0) {
-		_bus_clocks[bus_index] = _frequency;
 	}
 
 	// call the probe function to check whether the device is present
-	ret = probe();
+	int probe_ret = probe();
 
-	if (ret != OK) {
+	if (probe_ret != OK) {
 		DEVICE_DEBUG("probe failed");
-		goto out;
+		px4_i2cbus_uninitialize(_dev);
+		_dev = nullptr;
+		return probe_ret;
 	}
 
 	// do base class init, which will create device node, etc
-	ret = CDev::init();
+	int cdev_init_ret = CDev::init();
 
-	if (ret != OK) {
+	if (cdev_init_ret != OK) {
 		DEVICE_DEBUG("cdev init failed");
-		goto out;
+		px4_i2cbus_uninitialize(_dev);
+		_dev = nullptr;
+		return cdev_init_ret;
 	}
 
 	// tell the world where we are
-	DEVICE_DEBUG("on I2C bus %d at 0x%02x (bus: %u KHz, max: %" PRIu32 " KHz)",
+	DEVICE_DEBUG("on I2C bus %d at 0x%02x (bus: %" PRIu32 " KHz)",
-		     get_device_bus(), get_device_address(), _bus_clocks[bus_index] / 1000, _frequency / 1000);
+		     get_device_bus(), get_device_address(), _frequency / 1000);
 
-out:
+	return PX4_OK;
-
-	if ((ret != OK) && (_dev != nullptr)) {
-		px4_i2cbus_uninitialize(_dev);
-		_dev = nullptr;
-	}
-
-	return ret;
 }
 
 int
@@ -192,7 +138,7 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const
 		unsigned msgs = 0;
 
 		if (send_len > 0) {
-			msgv[msgs].frequency = _bus_clocks[get_device_bus() - 1];
+			msgv[msgs].frequency = _frequency;
 			msgv[msgs].addr = get_device_address();
 			msgv[msgs].flags = 0;
 			msgv[msgs].buffer = const_cast<uint8_t *>(send);
@@ -201,7 +147,7 @@ I2C::transfer(const uint8_t *send, const unsigned send_len, uint8_t *recv, const
 		}
 
 		if (recv_len > 0) {
-			msgv[msgs].frequency = _bus_clocks[get_device_bus() - 1];
+			msgv[msgs].frequency = _frequency;
 			msgv[msgs].addr = get_device_address();
 			msgv[msgs].flags = I2C_M_READ;
 			msgv[msgs].buffer = recv;

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

@@ -67,8 +67,6 @@ public:
 
 	virtual int	init() override;
 
-	static int	set_bus_clock(unsigned bus, unsigned clock_hz);
-
 protected:
 	/**
 	 * The number of times a read or write operation will be retried on
@@ -111,9 +109,7 @@ protected:
 	bool	external() const override { return px4_i2c_device_external(_device_id.devid); }
 
 private:
-	static unsigned	int	_bus_clocks[PX4_NUMBER_I2C_BUSES];
+	uint32_t		_frequency{100'000};
-
-	const uint32_t		_frequency;
 	i2c_master_s		*_dev{nullptr};
 
 };

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
+	)

src/systemcmds/boardctl/Kconfig

@@ -0,0 +1,5 @@
+menuconfig SYSTEMCMDS_BOARDCTL
+	bool "boardctl"
+	default n
+	---help---
+		Enable support for boardctl

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 <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/px4_config.h>
+
+#include <px4_platform_common/i2c_spi_buses.h>
+
+
+// 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;
+}