move I2C bus clock defaults to board px4_i2c_buses

- constructor use available setters

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

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/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

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

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-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
@@ -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, 45);
+PARAM_DEFINE_INT32(COM_ARM_MAG_ANG, 60);
 
 /**
  * Enable mag strength preflight check

src/modules/sensors/module.yaml

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

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -80,14 +80,14 @@ int GZBridge::init()
 		if (!_model_pose.empty()) {
 			PX4_INFO("Requested Model Position: %s", _model_pose.c_str());
 
-			std::vector<float> model_pose_v;
+			std::vector<double> model_pose_v;
 
 			std::stringstream ss(_model_pose);
 
 			while (ss.good()) {
 				std::string substr;
 				std::getline(ss, substr, ',');
-				model_pose_v.push_back(std::stof(substr));
+				model_pose_v.push_back(std::stod(substr));
 			}
 
 			while (model_pose_v.size() < 6) {