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),
-	initI2CBusInternal(3),
+	initI2CBusExternal(1, 400'000),
+	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

msg/CMakeLists.txt

@@ -56,7 +56,6 @@ set(msg_files
 	camera_capture.msg
 	camera_status.msg
 	camera_trigger.msg
-	can_frame.msg
 	cellular_status.msg
 	collision_constraints.msg
 	collision_report.msg

msg/can_frame.msg

@@ -1,15 +0,0 @@
-uint64 timestamp	# time since system start (microseconds)
-
-uint32 id
-
-bool is_rtr
-bool is_extended
-bool is_error
-
-uint8 dlc
-
-uint8[8] data
-
-uint8 ORB_QUEUE_LENGTH = 8
-
-# TOPICS can_frame can_frame_out can_frame_in

msg/vehicle_command.msg

@@ -96,7 +96,6 @@ uint16 VEHICLE_CMD_DO_VTOL_TRANSITION = 3000    	# Command VTOL transition
 uint16 VEHICLE_CMD_ARM_AUTHORIZATION_REQUEST = 3001	# Request arm authorization
 uint16 VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY = 30001	# Prepare a payload deployment in the flight plan
 uint16 VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY = 30002	# Control a pre-programmed payload deployment
-uint16 VEHICLE_CMD_CAN_FORWARD = 32000
 uint16 VEHICLE_CMD_FIXED_MAG_CAL_YAW = 42006            # Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location.
 uint16 VEHICLE_CMD_DO_WINCH = 42600			# Command to operate winch.
 

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)
-	bool is_external; ///< static external configuration. Use px4_i2c_bus_external() to check if a bus is really external
+	uint32_t max_speed_hz{100'000}; ///< max speed in Hz
+	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/drivers/uavcan/uavcan_main.cpp

@@ -130,10 +130,6 @@ UavcanNode::~UavcanNode()
 		} while (_instance);
 	}
 
-	_node.removeRxFrameListener();
-	delete _rx_frame_listener_uorb;
-	_rx_frame_listener_uorb = nullptr;
-
 	// Removing the sensor bridges
 	_sensor_bridges.clear();
 
@@ -502,17 +498,6 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 
 	fill_node_info();
 
-	// install frame listener
-	int32_t uavcan_frame_dbg = 0;
-	param_get(param_find("UAVCAN_FRAME_DBG"), &uavcan_frame_dbg);
-
-	if (uavcan_frame_dbg == 1) {
-		PX4_INFO("UAVCAN FRAME DBG enabled");
-		_rx_frame_listener_uorb = new RxFrameUorbPublisher();
-		_node.getDispatcher().installRxFrameListener(_rx_frame_listener_uorb);
-		_mirror_to_uorb = true;
-	}
-
 	int ret = _beep_controller.init();
 
 	if (ret < 0) {
@@ -704,22 +689,6 @@ UavcanNode::Run()
 		update_params();
 	}
 
-	if (_mirror_to_uorb && _can_frame_in_sub.updated()) {
-		can_frame_s can_frame;
-
-		if (_can_frame_in_sub.copy(&can_frame)) {
-			uavcan::CanFrame frame{};
-			frame.id = can_frame.id;
-			memcpy(frame.data, can_frame.data, sizeof(can_frame.data));
-			frame.dlc = can_frame.dlc;
-
-			uavcan::MonotonicTime tx_deadline = _node.getMonotonicTime() + uavcan::MonotonicDuration::fromUSec(1000);
-			uint8_t iface_mask = 3;
-
-			_node.injectTxFrame(frame, tx_deadline, iface_mask, uavcan::CanTxQueue::Volatile);
-		}
-	}
-
 	// Check for parameter requests (get/set/list)
 	if (_param_request_sub.updated() && !_param_list_in_progress && !_param_in_progress && !_count_in_progress) {
 		uavcan_parameter_request_s request{};
@@ -908,13 +877,6 @@ UavcanNode::Run()
 					break;
 				}
 			}
-
-		} else if (cmd.command == vehicle_command_s::VEHICLE_CMD_CAN_FORWARD) {
-			if (_rx_frame_listener_uorb == nullptr) {
-				_rx_frame_listener_uorb = new RxFrameUorbPublisher();
-				_node.getDispatcher().installRxFrameListener(_rx_frame_listener_uorb);
-				_mirror_to_uorb = true;
-			}
 		}
 
 		if (acknowledge) {

src/drivers/uavcan/uavcan_main.hpp

@@ -74,7 +74,6 @@
 #include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
-#include <uORB/topics/can_frame.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/uavcan_parameter_request.h>
 #include <uORB/topics/uavcan_parameter_value.h>
@@ -316,26 +315,4 @@ private:
 	bool _check_fw{false};
 
 	UavcanServers   *_servers{nullptr};
-
-
-	struct RxFrameUorbPublisher : public uavcan::IRxFrameListener {
-		void handleRxFrame(const uavcan::CanRxFrame &frame, uavcan::CanIOFlags flags) override
-		{
-			can_frame_s can_frame{};
-			can_frame.id = frame.id;
-			//can_frame.iface_index = frame.iface_index;
-			can_frame.dlc = frame.dlc;
-			memcpy(&can_frame.data, &frame.data, sizeof(frame.data));
-			can_frame.timestamp = hrt_absolute_time();
-			_can_frame_pub.publish(can_frame);
-		}
-	private:
-		uORB::Publication<can_frame_s> _can_frame_pub{ORB_ID(can_frame_out)};
-	};
-
-	RxFrameUorbPublisher *_rx_frame_listener_uorb{nullptr};
-
-	uORB::Subscription _can_frame_in_sub{ORB_ID(can_frame_in)};
-
-	bool _mirror_to_uorb{false};
 };

src/drivers/uavcan/uavcan_params.c

@@ -77,18 +77,6 @@ PARAM_DEFINE_INT32(UAVCAN_NODE_ID, 1);
  */
 PARAM_DEFINE_INT32(UAVCAN_BITRATE, 1000000);
 
-/**
- * UAVCAN CAN frame debug
- *
- * Publish UAVCAN CAN frames to ORB_ID(can_frame_out)
- * CAN frames published to ORB_ID(can_frame_in) are injected into UAVCAN
- *
- * @boolean
- * @reboot_required true
- * @group UAVCAN
- */
-PARAM_DEFINE_INT32(UAVCAN_FRAME_DBG, 0);
-
 /**
  * UAVCAN rangefinder minimum range
  *

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

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;
-		goto out;
+		return -ENOENT;
 	}
 
-	// the above call fails for a non-existing bus index,
-	// so the index math here is safe.
-	bus_index = get_device_bus() - 1;
+	const uint32_t max_speed_hz = px4_i2c_bus_max_speed(get_device_bus());
 
-	// abort if the max frequency we allow (the frequency we ask)
-	// is smaller than the bus frequency
-	if (_bus_clocks[bus_index] > _frequency) {
-		(void)px4_i2cbus_uninitialize(_dev);
-		_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;
+	if (_frequency > max_speed_hz) {
+		DEVICE_DEBUG("frequency %" PRIu32 " Hz exceeds bus %d maximum, limited to max %" PRIu32 " Hz", _frequency,
+			     get_device_bus(), max_speed_hz);
+		_frequency = max_speed_hz;
 	}
 
 	// 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)",
-		     get_device_bus(), get_device_address(), _bus_clocks[bus_index] / 1000, _frequency / 1000);
+	DEVICE_DEBUG("on I2C bus %d at 0x%02x (bus: %" PRIu32 " KHz)",
+		     get_device_bus(), get_device_address(), _frequency / 1000);
 
-out:
-
-	if ((ret != OK) && (_dev != nullptr)) {
-		px4_i2cbus_uninitialize(_dev);
-		_dev = nullptr;
-	}
-
-	return ret;
+	return PX4_OK;
 }
 
 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];
-
-	const uint32_t		_frequency;
+	uint32_t		_frequency{100'000};
 	i2c_master_s		*_dev{nullptr};
 
 };

src/modules/commander/commander_params.c

@@ -609,7 +609,7 @@ PARAM_DEFINE_FLOAT(COM_ARM_IMU_GYR, 0.25f);
  * @min 3
  * @max 180
  */
-PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 60);
+PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 45);
 
 /**
  * Enable mag strength preflight check

src/modules/mavlink/mavlink_main.cpp

@@ -1711,7 +1711,6 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
 		configure_stream_local("ATTITUDE_TARGET", 8.0f);
 		configure_stream_local("BATTERY_STATUS", 0.5f);
 		configure_stream_local("CAMERA_IMAGE_CAPTURED", unlimited_rate);
-		configure_stream_local("CAN_FRAME", unlimited_rate);
 		configure_stream_local("COLLISION", unlimited_rate);
 		configure_stream_local("EFI_STATUS", 10.0f);
 		configure_stream_local("ESC_INFO", 10.0f);

src/modules/mavlink/mavlink_messages.cpp

@@ -66,7 +66,6 @@
 #include "streams/BATTERY_STATUS.hpp"
 #include "streams/CAMERA_IMAGE_CAPTURED.hpp"
 #include "streams/CAMERA_TRIGGER.hpp"
-#include "streams/CAN_FRAME.hpp"
 #include "streams/COLLISION.hpp"
 #include "streams/COMMAND_LONG.hpp"
 #include "streams/COMPONENT_INFORMATION.hpp"
@@ -475,9 +474,6 @@ static const StreamListItem streams_list[] = {
 #if defined(NAV_CONTROLLER_OUTPUT_HPP)
 	create_stream_list_item<MavlinkStreamNavControllerOutput>(),
 #endif // NAV_CONTROLLER_OUTPUT_HPP
-#if defined(CAN_FRAME_HPP)
-	create_stream_list_item<MavlinkStreamCanFrame>(),
-#endif // CAN_FRAME_HPP
 #if defined(CAMERA_TRIGGER_HPP)
 	create_stream_list_item<MavlinkStreamCameraTrigger>(),
 #endif // CAMERA_TRIGGER_HPP

src/modules/mavlink/mavlink_receiver.cpp

@@ -234,10 +234,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_battery_status(msg);
 		break;
 
-	case MAVLINK_MSG_ID_CAN_FRAME:
-		handle_message_can_frame(msg);
-		break;
-
 	case MAVLINK_MSG_ID_SERIAL_CONTROL:
 		handle_message_serial_control(msg);
 		break;
@@ -373,7 +369,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 }
 
 bool
-MavlinkReceiver::evaluate_target_ok(mavlink_message_t *msg, int command, int target_system, int target_component)
+MavlinkReceiver::evaluate_target_ok(int command, int target_system, int target_component)
 {
 	/* evaluate if this system should accept this command */
 	bool target_ok = false;
@@ -386,11 +382,6 @@ MavlinkReceiver::evaluate_target_ok(mavlink_message_t *msg, int command, int tar
 		target_ok = (target_system == 0) || (target_system == mavlink_system.sysid);
 		break;
 
-	case MAV_CMD_CAN_FORWARD:
-		// MAV_COMP_ID_MAVCAN
-		target_ok = ((target_system == 0) || (target_system == mavlink_system.sysid)) && (msg->compid == MAV_COMP_ID_MAVCAN);
-		break;
-
 	default:
 		target_ok = (target_system == mavlink_system.sysid) && ((target_component == mavlink_system.compid)
 				|| (target_component == MAV_COMP_ID_ALL));
@@ -491,18 +482,12 @@ template <class T>
 void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const T &cmd_mavlink,
 		const vehicle_command_s &vehicle_command)
 {
-	bool target_ok = evaluate_target_ok(msg, cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
+	bool target_ok = evaluate_target_ok(cmd_mavlink.command, cmd_mavlink.target_system, cmd_mavlink.target_component);
 	bool send_ack = true;
 	uint8_t result = vehicle_command_ack_s::VEHICLE_CMD_RESULT_ACCEPTED;
 	uint8_t progress = 0; // TODO: should be 255, 0 for backwards compatibility
 
 	if (!target_ok) {
-
-		if (cmd_mavlink.command == MAV_CMD_CAN_FORWARD) {
-			PX4_WARN("cmd:%d from SYSID:%d, COMPID:%d, target_ok:%d, target_system:%d, target_component:%d", cmd_mavlink.command,
-				 msg->sysid, msg->compid, target_ok, cmd_mavlink.target_system, cmd_mavlink.target_component);
-		}
-
 		// Reject alien commands only if there is no forwarding or we've never seen target component before
 		if (!_mavlink->get_forwarding_on()
 		    || !_mavlink->component_was_seen(cmd_mavlink.target_system, cmd_mavlink.target_component, _mavlink)) {
@@ -576,18 +561,6 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const
 			send_ack = true;
 		}
 
-	} else if (cmd_mavlink.command == MAV_CMD_CAN_FORWARD) {
-
-		//PX4_INFO("cmd MAV_CMD_CAN_FORWARD");
-
-		// 1: bus	Bus number (0 to disable forwarding, 1 for first bus, 2 for 2nd bus, 3 for 3rd bus).
-
-		if (!_can_frame_pub.advertised()) {
-			PX4_INFO("MAV_CMD_CAN_FORWARD: enabling CAN_FRAME stream");
-			const char stream_name[] {"CAN_FRAME"};
-			_mavlink->configure_stream_threadsafe(stream_name, -1.f);
-		}
-
 	} else if (cmd_mavlink.command == MAV_CMD_DO_AUTOTUNE_ENABLE) {
 
 		bool has_module = true;
@@ -1861,30 +1834,6 @@ MavlinkReceiver::handle_message_battery_status(mavlink_message_t *msg)
 	_battery_pub.publish(battery_status);
 }
 
-void
-MavlinkReceiver::handle_message_can_frame(mavlink_message_t *msg)
-{
-	if (!_can_frame_pub.advertised()) {
-		PX4_INFO("enabling CAN_FRAME stream");
-		const char stream_name[] {"CAN_FRAME"};
-		_mavlink->configure_stream_threadsafe(stream_name, -1.f);
-	}
-
-	mavlink_can_frame_t mavlink_can_frame;
-	mavlink_msg_can_frame_decode(msg, &mavlink_can_frame);
-
-	can_frame_s can_frame{};
-	//msg.target_system = 0;
-	//msg.target_component = 0;
-	//can_frame.bus = mavlink_can_frame.bus;
-	can_frame.id = mavlink_can_frame.id;
-	can_frame.dlc = mavlink_can_frame.len;
-	memcpy(can_frame.data, mavlink_can_frame.data, sizeof(mavlink_can_frame.data));
-
-	can_frame.timestamp = hrt_absolute_time();
-	_can_frame_pub.publish(can_frame);
-}
-
 void
 MavlinkReceiver::handle_message_serial_control(mavlink_message_t *msg)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -67,7 +67,6 @@
 #include <uORB/topics/autotune_attitude_control_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/camera_status.h>
-#include <uORB/topics/can_frame.h>
 #include <uORB/topics/cellular_status.h>
 #include <uORB/topics/collision_report.h>
 #include <uORB/topics/differential_pressure.h>
@@ -161,7 +160,6 @@ private:
 	void handle_message_adsb_vehicle(mavlink_message_t *msg);
 	void handle_message_att_pos_mocap(mavlink_message_t *msg);
 	void handle_message_battery_status(mavlink_message_t *msg);
-	void handle_message_can_frame(mavlink_message_t *msg);
 	void handle_message_cellular_status(mavlink_message_t *msg);
 	void handle_message_collision(mavlink_message_t *msg);
 	void handle_message_command_ack(mavlink_message_t *msg);
@@ -230,7 +228,7 @@ private:
 	int set_message_interval(int msgId, float interval, int data_rate = -1);
 	void get_message_interval(int msgId);
 
-	bool evaluate_target_ok(mavlink_message_t *msg, int command, int target_system, int target_component);
+	bool evaluate_target_ok(int command, int target_system, int target_component);
 
 	void fill_thrust(float *thrust_body_array, uint8_t vehicle_type, float thrust);
 
@@ -296,7 +294,6 @@ private:
 	uORB::Publication<airspeed_s>				_airspeed_pub{ORB_ID(airspeed)};
 	uORB::Publication<battery_status_s>			_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<camera_status_s>			_camera_status_pub{ORB_ID(camera_status)};
-	uORB::Publication<can_frame_s>                          _can_frame_pub{ORB_ID(can_frame_in)};
 	uORB::Publication<cellular_status_s>			_cellular_status_pub{ORB_ID(cellular_status)};
 	uORB::Publication<collision_report_s>			_collision_report_pub{ORB_ID(collision_report)};
 	uORB::Publication<differential_pressure_s>		_differential_pressure_pub{ORB_ID(differential_pressure)};

src/modules/sensors/module.yaml

@@ -96,7 +96,7 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: m/s^2
+            unit: m
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -107,7 +107,7 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: m/s^2
+            unit: m
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -118,7 +118,7 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: m/s^2
+            unit: m
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -288,7 +288,7 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: rad/s
+            unit: m
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -299,7 +299,7 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: rad/s
+            unit: m
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -310,7 +310,7 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: rad/s
+            unit: m
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -405,7 +405,7 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: gauss
+            unit: m
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -416,7 +416,7 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: gauss
+            unit: m
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0
@@ -427,7 +427,7 @@ parameters:
             category: System
             type: float
             default: 0.0
-            unit: gauss
+            unit: m
             volatile: true
             num_instances: *max_num_sensor_instances
             instance_start: 0

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

@@ -31,53 +31,52 @@
  *
  ****************************************************************************/
 
-#ifndef CAN_FRAME_HPP
-#define CAN_FRAME_HPP
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/px4_config.h>
 
-#include <uORB/topics/can_frame.h>
+#include <px4_platform_common/i2c_spi_buses.h>
 
-class MavlinkStreamCanFrame : public MavlinkStream
+
+// 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[])
 {
-public:
-	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamCanFrame(mavlink); }
+	px4_print_all_instances();
 
-	static constexpr const char *get_name_static() { return "CAN_FRAME"; }
-	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_CAN_FRAME; }
-
-	const char *get_name() const override { return get_name_static(); }
-	uint16_t get_id() override { return get_id_static(); }
-
-	unsigned get_size() override
-	{
-		return _can_frame_out_sub.advertised() ? MAVLINK_MSG_ID_CAN_FRAME_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
-	}
-
-private:
-	explicit MavlinkStreamCanFrame(Mavlink *mavlink) : MavlinkStream(mavlink) {}
-
-	uORB::Subscription _can_frame_out_sub{ORB_ID(can_frame_out)};
-
-	bool send() override
-	{
-		can_frame_s can_frame;
-
-		if (_can_frame_out_sub.update(&can_frame)) {
-			mavlink_can_frame_t msg{};
-
-			msg.target_system = 0;
-			msg.target_component = 0;
-			msg.bus = 0;
-			msg.len = can_frame.dlc;
-			msg.id = can_frame.id;
-			memcpy(msg.data, can_frame.data, sizeof(msg.data));
-
-			mavlink_msg_can_frame_send_struct(_mavlink->get_channel(), &msg);
-
-			return true;
-		}
-
-		return false;
-	}
-};
-
-#endif // CAN_FRAME_HPP
+	return 0;
+}