Inline uORBDeviceNode::copy for performance improvement

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>

boards/holybro/kakutef7/default.px4board

@@ -43,4 +43,3 @@ CONFIG_SYSTEMCMDS_MIXER=y
 CONFIG_SYSTEMCMDS_PARAM=y
 CONFIG_SYSTEMCMDS_PWM=y
 CONFIG_SYSTEMCMDS_REBOOT=y
-CONFIG_SYSTEMCMDS_VER=y

boards/matek/h743-slim/src/spi.cpp

@@ -38,7 +38,7 @@
 
 constexpr px4_spi_bus_t px4_spi_buses[SPI_BUS_MAX_BUS_ITEMS] = {
 	initSPIBus(SPI::Bus::SPI1, {
-		initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin1}),
+		initSPIDevice(DRV_IMU_DEVTYPE_MPU6000, SPI::CS{GPIO::PortC, GPIO::Pin15}, SPI::DRDY{GPIO::PortB, GPIO::Pin2}),
 	}),
 	initSPIBus(SPI::Bus::SPI2, {
 		initSPIDevice(DRV_OSD_DEVTYPE_ATXXXX, SPI::CS{GPIO::PortB, GPIO::Pin12}),

platforms/common/uORB/Publication.hpp

@@ -42,7 +42,7 @@
 #include <systemlib/err.h>
 
 #include <uORB/uORB.h>
-#include "uORBDeviceNode.hpp"
+#include "uORBManager.hpp"
 #include <uORB/topics/uORBTopics.hpp>
 
 namespace uORB
@@ -72,7 +72,7 @@ public:
 
 	bool advertised() const { return _handle != nullptr; }
 
-	bool unadvertise() { return (DeviceNode::unadvertise(_handle) == PX4_OK); }
+	bool unadvertise() { return (Manager::orb_unadvertise(_handle) == PX4_OK); }
 
 	orb_id_t get_topic() const { return get_orb_meta(_orb_id); }
 
@@ -84,7 +84,7 @@ protected:
 	{
 		if (_handle != nullptr) {
 			// don't automatically unadvertise queued publications (eg vehicle_command)
-			if (static_cast<DeviceNode *>(_handle)->get_queue_size() == 1) {
+			if (Manager::orb_get_queue_size(_handle) == 1) {
 				unadvertise();
 			}
 		}
@@ -129,7 +129,7 @@ public:
 			advertise();
 		}
 
-		return (DeviceNode::publish(get_topic(), _handle, &data) == PX4_OK);
+		return (Manager::orb_publish(get_topic(), _handle, &data) == PX4_OK);
 	}
 };
 

platforms/common/uORB/PublicationMulti.hpp

@@ -96,7 +96,7 @@ public:
 	{
 		// advertise if not already advertised
 		if (advertise()) {
-			return static_cast<uORB::DeviceNode *>(_handle)->get_instance();
+			return Manager::orb_get_instance(_handle);
 		}
 
 		return -1;

platforms/common/uORB/Subscription.cpp

@@ -49,25 +49,14 @@ bool Subscription::subscribe()
 		return true;
 	}
 
-	if ((_orb_id != ORB_ID::INVALID) && uORB::Manager::get_instance()) {
-		DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
+	if (_orb_id != ORB_ID::INVALID && uORB::Manager::get_instance()) {
+		unsigned initial_generation;
+		void *node = uORB::Manager::orb_add_internal_subscriber(_orb_id, _instance, &initial_generation);
 
-		if (device_master != nullptr) {
-
-			if (!device_master->deviceNodeExists(_orb_id, _instance)) {
-				return false;
-			}
-
-			uORB::DeviceNode *node = device_master->getDeviceNode(get_topic(), _instance);
-
-			if (node != nullptr) {
-				_node = node;
-				_node->add_internal_subscriber();
-
-				_last_generation = _node->get_initial_generation();
-
-				return true;
-			}
+		if (node) {
+			_node = node;
+			_last_generation = initial_generation;
+			return true;
 		}
 	}
 
@@ -77,7 +66,7 @@ bool Subscription::subscribe()
 void Subscription::unsubscribe()
 {
 	if (_node != nullptr) {
-		_node->remove_internal_subscriber();
+		uORB::Manager::orb_remove_internal_subscriber(_node);
 	}
 
 	_node = nullptr;
@@ -87,13 +76,7 @@ void Subscription::unsubscribe()
 bool Subscription::ChangeInstance(uint8_t instance)
 {
 	if (instance != _instance) {
-		DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
-
-		if (device_master != nullptr) {
-			if (!device_master->deviceNodeExists(_orb_id, instance)) {
-				return false;
-			}
-
+		if (uORB::Manager::orb_device_node_exists(_orb_id, _instance)) {
 			// if desired new instance exists, unsubscribe from current
 			unsubscribe();
 			_instance = instance;

platforms/common/uORB/Subscription.hpp

@@ -44,7 +44,6 @@
 #include <px4_platform_common/defines.h>
 #include <lib/mathlib/mathlib.h>
 
-#include "uORBDeviceNode.hpp"
 #include "uORBManager.hpp"
 #include "uORBUtils.hpp"
 
@@ -120,14 +119,14 @@ public:
 	bool advertised()
 	{
 		if (valid()) {
-			return _node->is_advertised();
+			return Manager::is_advertised(_node);
 		}
 
 		// try to initialize
 		if (subscribe()) {
 			// check again if valid
 			if (valid()) {
-				return _node->is_advertised();
+				return Manager::is_advertised(_node);
 			}
 		}
 
@@ -137,19 +136,19 @@ public:
 	/**
 	 * Check if there is a new update.
 	 */
-	bool updated() { return advertised() && _node->updates_available(_last_generation); }
+	bool updated() { return advertised() && Manager::updates_available(_node, _last_generation); }
 
 	/**
 	 * Update the struct
 	 * @param dst The uORB message struct we are updating.
 	 */
-	bool update(void *dst) { return updated() && _node->copy(dst, _last_generation); }
+	bool update(void *dst) { return updated() && Manager::orb_data_copy(_node, dst, _last_generation); }
 
 	/**
 	 * Copy the struct
 	 * @param dst The uORB message struct we are updating.
 	 */
-	bool copy(void *dst) { return advertised() && _node->copy(dst, _last_generation); }
+	bool copy(void *dst) { return advertised() && Manager::orb_data_copy(_node, dst, _last_generation); }
 
 	/**
 	 * Change subscription instance
@@ -166,9 +165,9 @@ protected:
 	friend class SubscriptionCallback;
 	friend class SubscriptionCallbackWorkItem;
 
-	DeviceNode *get_node() { return _node; }
+	void *get_node() { return _node; }
 
-	DeviceNode *_node{nullptr};
+	void *_node{nullptr};
 
 	unsigned _last_generation{0}; /**< last generation the subscriber has seen */
 

platforms/common/uORB/SubscriptionCallback.hpp

@@ -69,7 +69,7 @@ public:
 	bool registerCallback()
 	{
 		if (!_registered) {
-			if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) {
+			if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) {
 				// registered
 				_registered = true;
 
@@ -79,7 +79,7 @@ public:
 
 				// try to register callback again
 				if (_subscription.subscribe()) {
-					if (_subscription.get_node() && _subscription.get_node()->register_callback(this)) {
+					if (_subscription.get_node() && Manager::register_callback(_subscription.get_node(), this)) {
 						_registered = true;
 					}
 				}
@@ -94,7 +94,7 @@ public:
 	void unregisterCallback()
 	{
 		if (_subscription.get_node()) {
-			_subscription.get_node()->unregister_callback(this);
+			Manager::unregister_callback(_subscription.get_node(), this);
 		}
 
 		_registered = false;
@@ -164,7 +164,7 @@ public:
 	{
 		// schedule immediately if updated (queue depth or subscription interval)
 		if ((_required_updates == 0)
-		    || (_subscription.get_node()->updates_available(_subscription.get_last_generation()) >= _required_updates)) {
+		    || (Manager::updates_available(_subscription.get_node(), _subscription.get_last_generation()) >= _required_updates)) {
 			if (updated()) {
 				_work_item->ScheduleNow();
 			}

platforms/common/uORB/uORB.cpp

@@ -94,7 +94,6 @@ int uorb_top(char **topic_filter, int num_filters)
 	return OK;
 }
 
-
 orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data)
 {
 	return uORB::Manager::get_instance()->orb_advertise(meta, data);
@@ -121,42 +120,42 @@ int orb_unadvertise(orb_advert_t handle)
 	return uORB::Manager::get_instance()->orb_unadvertise(handle);
 }
 
-int  orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
+int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
 {
 	return uORB::Manager::get_instance()->orb_publish(meta, handle, data);
 }
 
-int  orb_subscribe(const struct orb_metadata *meta)
+int orb_subscribe(const struct orb_metadata *meta)
 {
 	return uORB::Manager::get_instance()->orb_subscribe(meta);
 }
 
-int  orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
+int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance)
 {
 	return uORB::Manager::get_instance()->orb_subscribe_multi(meta, instance);
 }
 
-int  orb_unsubscribe(int handle)
+int orb_unsubscribe(int handle)
 {
 	return uORB::Manager::get_instance()->orb_unsubscribe(handle);
 }
 
-int  orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
+int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
 {
 	return uORB::Manager::get_instance()->orb_copy(meta, handle, buffer);
 }
 
-int  orb_check(int handle, bool *updated)
+int orb_check(int handle, bool *updated)
 {
 	return uORB::Manager::get_instance()->orb_check(handle, updated);
 }
 
-int  orb_exists(const struct orb_metadata *meta, int instance)
+int orb_exists(const struct orb_metadata *meta, int instance)
 {
 	return uORB::Manager::get_instance()->orb_exists(meta, instance);
 }
 
-int  orb_group_count(const struct orb_metadata *meta)
+int orb_group_count(const struct orb_metadata *meta)
 {
 	unsigned instance = 0;
 

platforms/common/uORB/uORBDeviceMaster.cpp

@@ -459,34 +459,6 @@ uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const char *nodepath)
 	return nullptr;
 }
 
-bool uORB::DeviceMaster::deviceNodeExists(ORB_ID id, const uint8_t instance)
-{
-	if ((id == ORB_ID::INVALID) || (instance > ORB_MULTI_MAX_INSTANCES - 1)) {
-		return false;
-	}
-
-	return _node_exists[instance][(uint8_t)id];
-}
-
-uORB::DeviceNode *uORB::DeviceMaster::getDeviceNode(const struct orb_metadata *meta, const uint8_t instance)
-{
-	if (meta == nullptr) {
-		return nullptr;
-	}
-
-	if (!deviceNodeExists(static_cast<ORB_ID>(meta->o_id), instance)) {
-		return nullptr;
-	}
-
-	lock();
-	uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance);
-	unlock();
-
-	//We can safely return the node that can be used by any thread, because
-	//a DeviceNode never gets deleted.
-	return node;
-}
-
 uORB::DeviceNode *uORB::DeviceMaster::getDeviceNodeLocked(const struct orb_metadata *meta, const uint8_t instance)
 {
 	for (uORB::DeviceNode *node : _node_list) {

platforms/common/uORB/uORBDeviceMaster.hpp

@@ -72,9 +72,34 @@ public:
 	 * @return node if exists, nullptr otherwise
 	 */
 	uORB::DeviceNode *getDeviceNode(const char *node_name);
-	uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance);
+	uORB::DeviceNode *getDeviceNode(const struct orb_metadata *meta, const uint8_t instance)
+	{
+		if (meta == nullptr) {
+			return nullptr;
+		}
 
-	bool deviceNodeExists(ORB_ID id, const uint8_t instance);
+		if (!deviceNodeExists(static_cast<ORB_ID>(meta->o_id), instance)) {
+			return nullptr;
+		}
+
+		lock();
+		uORB::DeviceNode *node = getDeviceNodeLocked(meta, instance);
+		unlock();
+
+		//We can safely return the node that can be used by any thread, because
+		//a DeviceNode never gets deleted.
+		return node;
+
+	}
+
+	bool deviceNodeExists(ORB_ID id, const uint8_t instance)
+	{
+		if ((id == ORB_ID::INVALID) || (instance > ORB_MULTI_MAX_INSTANCES - 1)) {
+			return false;
+		}
+
+		return _node_exists[instance][(uint8_t)id];
+	}
 
 	/**
 	 * Print statistics for each existing topic.

platforms/common/uORB/uORBDeviceNode.cpp

@@ -44,17 +44,6 @@
 
 static uORB::SubscriptionInterval *filp_to_subscription(cdev::file_t *filp) { return static_cast<uORB::SubscriptionInterval *>(filp->f_priv); }
 
-// Determine the data range
-static inline bool is_in_range(unsigned left, unsigned value, unsigned right)
-{
-	if (right > left) {
-		return (left <= value) && (value <= right);
-
-	} else {  // Maybe the data overflowed and a wraparound occurred
-		return (left <= value) || (value <= right);
-	}
-}
-
 // round up to nearest power of two
 // Such as 0 => 1, 1 => 1, 2 => 2 ,3 => 4, 10 => 16, 60 => 64, 65...255 => 128
 // Note: When the input value > 128, the output is always 128
@@ -151,46 +140,6 @@ uORB::DeviceNode::close(cdev::file_t *filp)
 	return CDev::close(filp);
 }
 
-bool
-uORB::DeviceNode::copy(void *dst, unsigned &generation)
-{
-	if ((dst != nullptr) && (_data != nullptr)) {
-		if (_queue_size == 1) {
-			ATOMIC_ENTER;
-			memcpy(dst, _data, _meta->o_size);
-			generation = _generation.load();
-			ATOMIC_LEAVE;
-			return true;
-
-		} else {
-			ATOMIC_ENTER;
-			const unsigned current_generation = _generation.load();
-
-			if (current_generation == generation) {
-				/* The subscriber already read the latest message, but nothing new was published yet.
-				* Return the previous message
-				*/
-				--generation;
-			}
-
-			// Compatible with normal and overflow conditions
-			if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) {
-				// Reader is too far behind: some messages are lost
-				generation = current_generation - _queue_size;
-			}
-
-			memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
-			ATOMIC_LEAVE;
-
-			++generation;
-
-			return true;
-		}
-	}
-
-	return false;
-}
-
 ssize_t
 uORB::DeviceNode::read(cdev::file_t *filp, char *buffer, size_t buflen)
 {
@@ -323,7 +272,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
 	}
 
 	/* check if the orb meta data matches the publication */
-	if (devnode->_meta != meta) {
+	if (devnode->_meta->o_id != meta->o_id) {
 		errno = EINVAL;
 		return PX4_ERROR;
 	}

platforms/common/uORB/uORBDeviceNode.hpp

@@ -225,7 +225,45 @@ public:
 	 * @return bool
 	 *   Returns true if the data was copied.
 	 */
-	bool copy(void *dst, unsigned &generation);
+	bool copy(void *dst, unsigned &generation)
+	{
+		if ((dst != nullptr) && (_data != nullptr)) {
+			if (_queue_size == 1) {
+				ATOMIC_ENTER;
+				memcpy(dst, _data, _meta->o_size);
+				generation = _generation.load();
+				ATOMIC_LEAVE;
+				return true;
+
+			} else {
+				ATOMIC_ENTER;
+				const unsigned current_generation = _generation.load();
+
+				if (current_generation == generation) {
+					/* The subscriber already read the latest message, but nothing new was published yet.
+					* Return the previous message
+					*/
+					--generation;
+				}
+
+				// Compatible with normal and overflow conditions
+				if (!is_in_range(current_generation - _queue_size, generation, current_generation - 1)) {
+					// Reader is too far behind: some messages are lost
+					generation = current_generation - _queue_size;
+				}
+
+				memcpy(dst, _data + (_meta->o_size * (generation % _queue_size)), _meta->o_size);
+				ATOMIC_LEAVE;
+
+				++generation;
+
+				return true;
+			}
+		}
+
+		return false;
+
+	}
 
 	// add item to list of work items to schedule on node update
 	bool register_callback(SubscriptionCallback *callback_sub);
@@ -253,4 +291,16 @@ private:
 	bool _advertised{false};  /**< has ever been advertised (not necessarily published data yet) */
 	uint8_t _queue_size; /**< maximum number of elements in the queue */
 	int8_t _subscriber_count{0};
+
+
+// Determine the data range
+	static inline bool is_in_range(unsigned left, unsigned value, unsigned right)
+	{
+		if (right > left) {
+			return (left <= value) && (value <= right);
+
+		} else {  // Maybe the data overflowed and a wraparound occurred
+			return (left <= value) || (value <= right);
+		}
+	}
 };

platforms/common/uORB/uORBManager.cpp

@@ -112,8 +112,10 @@ int uORB::Manager::orb_exists(const struct orb_metadata *meta, int instance)
 		return ret;
 	}
 
-	if (get_device_master()) {
-		uORB::DeviceNode *node = _device_master->getDeviceNode(meta, instance);
+	uORB::DeviceMaster *dev =  uORB::Manager::get_instance()->get_device_master();
+
+	if (dev) {
+		uORB::DeviceNode *node = dev->getDeviceNode(meta, instance);
 
 		if (node != nullptr) {
 			if (node->is_advertised()) {
@@ -319,6 +321,65 @@ int uORB::Manager::orb_get_interval(int handle, unsigned *interval)
 	return ret;
 }
 
+
+bool uORB::Manager::orb_device_node_exists(ORB_ID orb_id, uint8_t instance)
+{
+	DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
+
+	return device_master != nullptr &&
+	       device_master->deviceNodeExists(orb_id, instance);
+}
+
+void *uORB::Manager::orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation)
+{
+	uORB::DeviceNode *node = nullptr;
+	DeviceMaster *device_master = uORB::Manager::get_instance()->get_device_master();
+
+	if (device_master != nullptr) {
+		node = device_master->getDeviceNode(get_orb_meta(orb_id), instance);
+
+		if (node) {
+			node->add_internal_subscriber();
+			*initial_generation = node->get_initial_generation();
+		}
+	}
+
+	return node;
+}
+
+void uORB::Manager::orb_remove_internal_subscriber(void *node_handle)
+{
+	static_cast<DeviceNode *>(node_handle)->remove_internal_subscriber();
+}
+
+uint8_t uORB::Manager::orb_get_queue_size(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->get_queue_size(); }
+
+bool uORB::Manager::orb_data_copy(void *node_handle, void *dst, unsigned &generation)
+{
+	return static_cast<DeviceNode *>(node_handle)->copy(dst, generation);
+}
+
+// add item to list of work items to schedule on node update
+bool uORB::Manager::register_callback(void *node_handle, SubscriptionCallback *callback_sub)
+{
+	return static_cast<DeviceNode *>(node_handle)->register_callback(callback_sub);
+}
+
+// remove item from list of work items
+void uORB::Manager::unregister_callback(void *node_handle, SubscriptionCallback *callback_sub)
+{
+	static_cast<DeviceNode *>(node_handle)->unregister_callback(callback_sub);
+}
+
+uint8_t uORB::Manager::orb_get_instance(const void *node_handle)
+{
+	if (node_handle) {
+		return static_cast<const uORB::DeviceNode *>(node_handle)->get_instance();
+	}
+
+	return -1;
+}
+
 int uORB::Manager::node_open(const struct orb_metadata *meta, bool advertiser, int *instance)
 {
 	char path[orb_maxpath];

platforms/common/uORB/uORBManager.hpp

@@ -34,6 +34,7 @@
 #ifndef _uORBManager_hpp_
 #define _uORBManager_hpp_
 
+#include "uORBDeviceNode.hpp"
 #include "uORBCommon.hpp"
 #include "uORBDeviceMaster.hpp"
 
@@ -54,6 +55,7 @@
 namespace uORB
 {
 class Manager;
+class SubscriptionCallback;
 }
 
 /**
@@ -165,7 +167,7 @@ public:
 	 * @param handle  handle returned by orb_advertise or orb_advertise_multi.
 	 * @return 0 on success
 	 */
-	int orb_unadvertise(orb_advert_t handle);
+	static int orb_unadvertise(orb_advert_t handle);
 
 	/**
 	 * Publish new data to a topic.
@@ -180,7 +182,7 @@ public:
 	 * @param data    A pointer to the data to be published.
 	 * @return    OK on success, PX4_ERROR otherwise with errno set accordingly.
 	 */
-	int  orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data);
+	static int  orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data);
 
 	/**
 	 * Subscribe to a topic.
@@ -301,7 +303,7 @@ public:
 	 * @param instance  ORB instance
 	 * @return    OK if the topic exists, PX4_ERROR otherwise.
 	 */
-	int  orb_exists(const struct orb_metadata *meta, int instance);
+	static int  orb_exists(const struct orb_metadata *meta, int instance);
 
 	/**
 	 * Set the minimum interval between which updates are seen for a subscription.
@@ -335,6 +337,26 @@ public:
 	 */
 	int	orb_get_interval(int handle, unsigned *interval);
 
+	static bool orb_device_node_exists(ORB_ID orb_id, uint8_t instance);
+
+	static void *orb_add_internal_subscriber(ORB_ID orb_id, uint8_t instance, unsigned *initial_generation);
+
+	static void orb_remove_internal_subscriber(void *node_handle);
+
+	static uint8_t orb_get_queue_size(const void *node_handle);
+
+	static bool orb_data_copy(void *node_handle, void *dst, unsigned &generation);
+
+	static bool register_callback(void *node_handle, SubscriptionCallback *callback_sub);
+
+	static void unregister_callback(void *node_handle, SubscriptionCallback *callback_sub);
+
+	static uint8_t orb_get_instance(const void *node_handle);
+
+	static unsigned updates_available(const void *node_handle, unsigned last_generation) { return static_cast<const DeviceNode *>(node_handle)->updates_available(last_generation); }
+
+	static bool is_advertised(const void *node_handle) { return static_cast<const DeviceNode *>(node_handle)->is_advertised(); }
+
 #ifdef ORB_COMMUNICATOR
 	/**
 	 * Method to set the uORBCommunicator::IChannel instance.

posix-configs/SITL/init/test/test_imu_filtering

@@ -20,9 +20,9 @@ param set IMU_GYRO_FFT_MAX 1000
 param set IMU_GYRO_FFT_LEN 512
 
 # dynamic notches ESC/FFT/both
-#param set IMU_GYRO_DYN_NF 1
-#param set IMU_GYRO_DYN_NF 2
-param set IMU_GYRO_DYN_NF 3
+#param set IMU_GYRO_DNF_EN 1
+#param set IMU_GYRO_DNF_EN 2
+param set IMU_GYRO_DNF_EN 3
 
 # test values
 param set IMU_GYRO_CUTOFF 60

src/drivers/imu/invensense/mpu6000/MPU6000.cpp

@@ -256,6 +256,7 @@ void MPU6000::RunImpl()
 
 				// full reset if things are failing consistently
 				if (_failure_count > 10) {
+					PX4_DEBUG("Full reset because things are failing consistently");
 					Reset();
 					return;
 				}
@@ -270,6 +271,7 @@ void MPU6000::RunImpl()
 				} else {
 					// register check failed, force reset
 					perf_count(_bad_register_perf);
+					PX4_DEBUG("Force reset because register 0x%02hhX check failed ", (uint8_t)_register_cfg[_checked_register].reg);
 					Reset();
 				}
 
@@ -435,6 +437,7 @@ uint8_t MPU6000::RegisterRead(Register reg)
 	cmd[0] = static_cast<uint8_t>(reg) | DIR_READ;
 	set_frequency(SPI_SPEED); // low speed for regular registers
 	transfer(cmd, cmd, sizeof(cmd));
+	px4_udelay(10);
 	return cmd[1];
 }
 
@@ -443,6 +446,7 @@ void MPU6000::RegisterWrite(Register reg, uint8_t value)
 	uint8_t cmd[2] { (uint8_t)reg, value };
 	set_frequency(SPI_SPEED); // low speed for regular registers
 	transfer(cmd, cmd, sizeof(cmd));
+	px4_udelay(10);
 }
 
 void MPU6000::RegisterSetAndClearBits(Register reg, uint8_t setbits, uint8_t clearbits)

src/drivers/imu/invensense/mpu6000/MPU6000.hpp

@@ -158,12 +158,12 @@ private:
 	static constexpr uint8_t size_register_cfg{7};
 	register_config_t _register_cfg[size_register_cfg] {
 		// Register               | Set bits, Clear bits
-		{ Register::GYRO_CONFIG,   GYRO_CONFIG_BIT::FS_SEL_2000_DPS, GYRO_CONFIG_BIT::XG_ST | GYRO_CONFIG_BIT::YG_ST | GYRO_CONFIG_BIT::ZG_ST },
-		{ Register::ACCEL_CONFIG,  ACCEL_CONFIG_BIT::AFS_SEL_16G, ACCEL_CONFIG_BIT::XA_ST | ACCEL_CONFIG_BIT::YA_ST | ACCEL_CONFIG_BIT::ZA_ST },
+		{ Register::GYRO_CONFIG,   GYRO_CONFIG_BIT::FS_SEL_2000_DPS, 0 },
+		{ Register::ACCEL_CONFIG,  ACCEL_CONFIG_BIT::AFS_SEL_16G, 0 },
 		{ Register::FIFO_EN,       FIFO_EN_BIT::XG_FIFO_EN | FIFO_EN_BIT::YG_FIFO_EN | FIFO_EN_BIT::ZG_FIFO_EN | FIFO_EN_BIT::ACCEL_FIFO_EN, FIFO_EN_BIT::TEMP_FIFO_EN },
 		{ Register::INT_PIN_CFG,   INT_PIN_CFG_BIT::INT_LEVEL, 0 },
 		{ Register::INT_ENABLE,    INT_ENABLE_BIT::DATA_RDY_INT_EN, 0 },
-		{ Register::USER_CTRL,     USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, USER_CTRL_BIT::I2C_MST_EN },
+		{ Register::USER_CTRL,     USER_CTRL_BIT::FIFO_EN | USER_CTRL_BIT::I2C_IF_DIS, 0 },
 		{ Register::PWR_MGMT_1,    PWR_MGMT_1_BIT::CLKSEL_0, PWR_MGMT_1_BIT::SLEEP },
 	};
 };

src/lib/avoidance/ObstacleAvoidanceTest.cpp

@@ -68,7 +68,7 @@ TEST_F(ObstacleAvoidanceTest, instantiation) { ObstacleAvoidance oa(nullptr); }
 
 TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy)
 {
-	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message coming
+	// GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming
 	// from offboard
 	TestObstacleAvoidance oa;
 
@@ -104,7 +104,7 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy)
 
 TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier)
 {
-	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message coming
+	// GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming
 	// from offboard
 	TestObstacleAvoidance oa;
 
@@ -147,7 +147,7 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy_bezier)
 
 TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy)
 {
-	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message
+	// GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message
 	TestObstacleAvoidance oa;
 
 	vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint;
@@ -177,7 +177,7 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy)
 
 TEST_F(ObstacleAvoidanceTest, oa_desired)
 {
-	// GIVEN: the flight controller setpoints from FlightTaskAutoMapper and the waypoints from FLightTaskAuto
+	// GIVEN: the flight controller setpoints from FlightTaskAuto and the waypoints from FLightTaskAuto
 	TestObstacleAvoidance oa;
 
 	pos_sp = Vector3f(1.f, 1.2f, NAN);

src/lib/mathlib/math/filter/LowPassFilter2p.hpp

@@ -71,7 +71,7 @@ public:
 		_delay_element_1 = {};
 		_delay_element_2 = {};
 
-		_cutoff_freq = math::constrain(cutoff_freq, 5.f, sample_freq / 2); // TODO: min based on actual numerical limit
+		_cutoff_freq = math::max(cutoff_freq, sample_freq * 0.001f);
 		_sample_freq = sample_freq;
 
 		const float fr = _sample_freq / _cutoff_freq;

src/lib/mathlib/math/filter/NotchFilter.hpp

@@ -197,9 +197,39 @@ void NotchFilter<T>::setParameters(float sample_freq, float notch_freq, float ba
 		return;
 	}
 
-	_notch_freq = math::constrain(notch_freq, 5.f, sample_freq / 2); // TODO: min based on actual numerical limit
-	_bandwidth = math::constrain(bandwidth, 5.f, sample_freq / 2);
+	const float freq_min = sample_freq * 0.001f;
+
+	const float notch_freq_new = math::max(notch_freq, freq_min);
+	const float bandwidth_new = math::max(bandwidth, freq_min);
+
+	bool sample_freq_change = (fabsf(sample_freq - _sample_freq) > FLT_EPSILON);
+	bool bandwidth_change = (fabsf(bandwidth_new - _bandwidth) > FLT_EPSILON);
+
+	if (!sample_freq_change && !bandwidth_change) {
+		if (fabsf(notch_freq_new - _notch_freq) > FLT_EPSILON) {
+			// only notch frequency has changed
+			_notch_freq = notch_freq_new;
+
+			const float beta = -cosf(2.f * M_PI_F * _notch_freq / _sample_freq);
+
+			_b1 = 2.f * beta * _b0;
+			_a1 = _b1;
+
+			if (!isFinite(_b1)) {
+				disable();
+			}
+
+			return;
+
+		} else {
+			// no change, do nothing
+			return;
+		}
+	}
+
 	_sample_freq = sample_freq;
+	_notch_freq = notch_freq_new;
+	_bandwidth = bandwidth_new;
 
 	const float alpha = tanf(M_PI_F * _bandwidth / _sample_freq);
 	const float beta = -cosf(2.f * M_PI_F * _notch_freq / _sample_freq);

src/lib/version/CMakeLists.txt

@@ -79,6 +79,7 @@ add_library(version version.c)
 target_compile_definitions(version
 	PUBLIC
 		PX4_BOARD_NAME="${PX4_BOARD_NAME}"
+		PX4_BOARD_LABEL="${PX4_BOARD_LABEL}"
 	PRIVATE
 		BUILD_URI=${BUILD_URI}
 	)

src/lib/version/version.h

@@ -56,6 +56,14 @@ static inline const char *px4_board_name(void)
 	return PX4_BOARD_NAME;
 }
 
+/**
+ * get the board build target variant
+ */
+static inline const char *px4_board_target_label(void)
+{
+	return PX4_BOARD_LABEL;
+}
+
 /**
  * get the board sub type
  */

src/modules/ekf2/EKF2.cpp

@@ -1706,18 +1706,15 @@ void EKF2::UpdateMagSample(ekf2_timestamps_s &ekf2_timestamps)
 void EKF2::UpdateRangeSample(ekf2_timestamps_s &ekf2_timestamps)
 {
 	if (!_distance_sensor_selected) {
-		// get subscription index of first downward-facing range sensor
-		uORB::SubscriptionMultiArray<distance_sensor_s> distance_sensor_subs{ORB_ID::distance_sensor};
 
-		if (distance_sensor_subs.advertised()) {
-			for (unsigned i = 0; i < distance_sensor_subs.size(); i++) {
+		if (_distance_sensor_subs.advertised()) {
+			for (unsigned i = 0; i < _distance_sensor_subs.size(); i++) {
 				distance_sensor_s distance_sensor;
 
-				if (distance_sensor_subs[i].copy(&distance_sensor)) {
+				if (_distance_sensor_subs[i].copy(&distance_sensor)) {
 					// only use the first instace which has the correct orientation
 					if ((hrt_elapsed_time(&distance_sensor.timestamp) < 100_ms)
 					    && (distance_sensor.orientation == distance_sensor_s::ROTATION_DOWNWARD_FACING)) {
-
 						if (_distance_sensor_sub.ChangeInstance(i)) {
 							int ndist = orb_group_count(ORB_ID(distance_sensor));
 

src/modules/ekf2/EKF2.hpp

@@ -264,6 +264,8 @@ private:
 	uORB::SubscriptionCallbackWorkItem _sensor_combined_sub{this, ORB_ID(sensor_combined)};
 	uORB::SubscriptionCallbackWorkItem _vehicle_imu_sub{this, ORB_ID(vehicle_imu)};
 
+	uORB::SubscriptionMultiArray<distance_sensor_s> _distance_sensor_subs{ORB_ID::distance_sensor};
+
 	bool _callback_registered{false};
 
 	bool _distance_sensor_selected{false}; // because we can have several distance sensor instances with different orientations

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/CMakeLists.txt

@@ -35,5 +35,5 @@ px4_add_library(FlightTaskAutoLineSmoothVel
 	FlightTaskAutoLineSmoothVel.cpp
 )
 
-target_link_libraries(FlightTaskAutoLineSmoothVel PUBLIC FlightTaskAutoMapper FlightTaskUtility)
+target_link_libraries(FlightTaskAutoLineSmoothVel PUBLIC FlightTaskAuto FlightTaskUtility)
 target_include_directories(FlightTaskAutoLineSmoothVel PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp

@@ -36,13 +36,22 @@
  */
 
 #include "FlightTaskAutoLineSmoothVel.hpp"
+#include <mathlib/mathlib.h>
 
 using namespace matrix;
 
+FlightTaskAutoLineSmoothVel::FlightTaskAutoLineSmoothVel() :
+	_sticks(this),
+	_stick_acceleration_xy(this)
+{}
 
 bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
-	bool ret = FlightTaskAutoMapper::activate(last_setpoint);
+	bool ret = FlightTaskAuto::activate(last_setpoint);
+
+	// Set setpoints equal current state.
+	_velocity_setpoint = _velocity;
+	_position_setpoint = _position;
 
 	Vector3f vel_prev{last_setpoint.vx, last_setpoint.vy, last_setpoint.vz};
 	Vector3f pos_prev{last_setpoint.x, last_setpoint.y, last_setpoint.z};
@@ -70,7 +79,7 @@ bool FlightTaskAutoLineSmoothVel::activate(const vehicle_local_position_setpoint
 
 void FlightTaskAutoLineSmoothVel::reActivate()
 {
-	FlightTaskAutoMapper::reActivate();
+	FlightTaskAuto::reActivate();
 
 	// On ground, reset acceleration and velocity to zero
 	_position_smoothing.reset({0.f, 0.f, 0.f}, {0.f, 0.f, 0.7f}, _position);
@@ -106,6 +115,70 @@ void FlightTaskAutoLineSmoothVel::_ekfResetHandlerHeading(float delta_psi)
 	_yaw_sp_prev += delta_psi;
 }
 
+bool FlightTaskAutoLineSmoothVel::update()
+{
+	bool ret = FlightTaskAuto::update();
+	// always reset constraints because they might change depending on the type
+	_setDefaultConstraints();
+
+	// The only time a thrust set-point is sent out is during
+	// idle. Hence, reset thrust set-point to NAN in case the
+	// vehicle exits idle.
+
+	if (_type_previous == WaypointType::idle) {
+		_acceleration_setpoint.setNaN();
+	}
+
+	// during mission and reposition, raise the landing gears but only
+	// if altitude is high enough
+	if (_highEnoughForLandingGear()) {
+		_gear.landing_gear = landing_gear_s::GEAR_UP;
+	}
+
+	switch (_type) {
+	case WaypointType::idle:
+		_prepareIdleSetpoints();
+		break;
+
+	case WaypointType::land:
+		_prepareLandSetpoints();
+		break;
+
+	case WaypointType::loiter:
+
+	/* fallthrought */
+	case WaypointType::position:
+		_preparePositionSetpoints();
+		break;
+
+	case WaypointType::takeoff:
+		_prepareTakeoffSetpoints();
+		break;
+
+	case WaypointType::velocity:
+		_prepareVelocitySetpoints();
+		break;
+
+	default:
+		_preparePositionSetpoints();
+		break;
+	}
+
+	if (_param_com_obs_avoid.get()) {
+		_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
+		_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
+				_yawspeed_setpoint);
+	}
+
+
+	_generateSetpoints();
+
+	// update previous type
+	_type_previous = _type;
+
+	return ret;
+}
+
 void FlightTaskAutoLineSmoothVel::_generateSetpoints()
 {
 	_checkEmergencyBraking();
@@ -225,7 +298,7 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
 
 		// The constraints are broken because they are used as hard limits by the position controller, so put this here
 		// until the constraints don't do things like cause controller integrators to saturate. Once the controller
-		// doesn't use z speed constraints, this can go in AutoMapper::_prepareTakeoffSetpoints(). Accel limit is to
+		// doesn't use z speed constraints, this can go in _prepareTakeoffSetpoints(). Accel limit is to
 		// emulate the motor ramp (also done in the controller) so that the controller can actually track the setpoint.
 		if (_type == WaypointType::takeoff &&  _dist_to_ground < _param_mpc_land_alt1.get()) {
 			z_vel_constraint = _param_mpc_tko_speed.get();
@@ -245,3 +318,95 @@ void FlightTaskAutoLineSmoothVel::_updateTrajConstraints()
 		_position_smoothing.setMaxVelocityZ(_param_mpc_z_vel_max_dn.get());
 	}
 }
+
+void FlightTaskAutoLineSmoothVel::_prepareIdleSetpoints()
+{
+	// Send zero thrust setpoint
+	_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
+	_velocity_setpoint.setNaN();
+	_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
+}
+
+void FlightTaskAutoLineSmoothVel::_prepareLandSetpoints()
+{
+	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
+
+	// Slow down automatic descend close to ground
+	float land_speed = math::gradual(_dist_to_ground,
+					 _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
+					 _param_mpc_land_speed.get(), _constraints.speed_down);
+
+	if (_type_previous != WaypointType::land) {
+		// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
+		_land_position = Vector3f(_target(0), _target(1), NAN);
+		_land_heading = _yaw_setpoint;
+		_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
+	}
+
+	// User input assisted landing
+	if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) {
+		// Stick full up -1 -> stop, stick full down 1 -> double the speed
+		land_speed *= (1 + _sticks.getPositionExpo()(2));
+
+		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
+					       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
+		_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
+				_velocity_setpoint_feedback.xy(), _deltatime);
+		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
+
+		// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
+		if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
+			_yaw_sp_aligned = true;
+		}
+
+	} else {
+		// Make sure we have a valid land position even in the case we loose RC while amending it
+		if (!PX4_ISFINITE(_land_position(0))) {
+			_land_position.xy() = Vector2f(_position);
+		}
+	}
+
+	_position_setpoint = _land_position; // The last element of the land position has to stay NAN
+	_yaw_setpoint = _land_heading;
+	_velocity_setpoint(2) = land_speed;
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoLineSmoothVel::_prepareTakeoffSetpoints()
+{
+	// Takeoff is completely defined by target position
+	_position_setpoint = _target;
+	_velocity_setpoint = Vector3f(NAN, NAN, NAN);
+
+	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
+}
+
+void FlightTaskAutoLineSmoothVel::_prepareVelocitySetpoints()
+{
+	// XY Velocity waypoint
+	// TODO : Rewiew that. What is the expected behavior?
+	_position_setpoint = Vector3f(NAN, NAN, _position(2));
+	Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
+	_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
+}
+
+void FlightTaskAutoLineSmoothVel::_preparePositionSetpoints()
+{
+	// Simple waypoint navigation: go to xyz target, with standard limitations
+	_position_setpoint = _target;
+	_velocity_setpoint.setNaN(); // No special velocity limitations
+}
+
+void FlightTaskAutoLineSmoothVel::updateParams()
+{
+	FlightTaskAuto::updateParams();
+
+	// make sure that alt1 is above alt2
+	_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
+}
+
+bool FlightTaskAutoLineSmoothVel::_highEnoughForLandingGear()
+{
+	// return true if altitude is above two meters
+	return _dist_to_ground > 2.0f;
+}

src/modules/flight_mode_manager/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp

@@ -40,17 +40,21 @@
 
 #pragma once
 
-#include "FlightTaskAutoMapper.hpp"
+#include "FlightTaskAuto.hpp"
 #include <motion_planning/PositionSmoothing.hpp>
+#include "Sticks.hpp"
+#include "StickAccelerationXY.hpp"
+#include "StickYaw.hpp"
 
-class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper
+class FlightTaskAutoLineSmoothVel : public FlightTaskAuto
 {
 public:
-	FlightTaskAutoLineSmoothVel() = default;
+	FlightTaskAutoLineSmoothVel();
 	virtual ~FlightTaskAutoLineSmoothVel() = default;
 
 	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	void reActivate() override;
+	bool update() override;
 
 private:
 	PositionSmoothing _position_smoothing;
@@ -65,7 +69,7 @@ protected:
 	void _ekfResetHandlerVelocityZ(float delta_vz) override;
 	void _ekfResetHandlerHeading(float delta_psi) override;
 
-	void _generateSetpoints() override; /**< Generate setpoints along line. */
+	void _generateSetpoints(); /**< Generate setpoints along line. */
 	void _generateHeading();
 	void _checkEmergencyBraking();
 	bool _generateHeadingAlongTraj(); /**< Generates heading along trajectory. */
@@ -82,13 +86,39 @@ protected:
 	bool _checkTakeoff() override { return _want_takeoff; };
 	bool _want_takeoff{false};
 
-	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAutoMapper,
+	void _prepareIdleSetpoints();
+	void _prepareLandSetpoints();
+	void _prepareVelocitySetpoints();
+	void _prepareTakeoffSetpoints();
+	void _preparePositionSetpoints();
+	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
+
+	void updateParams() override; /**< See ModuleParam class */
+
+	Sticks _sticks;
+	StickAccelerationXY _stick_acceleration_xy;
+	StickYaw _stick_yaw;
+	matrix::Vector3f _land_position;
+	float _land_heading;
+	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
+
+	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
 					(ParamFloat<px4::params::MIS_YAW_ERR>) _param_mis_yaw_err, // yaw-error threshold
 					(ParamFloat<px4::params::MPC_ACC_HOR>) _param_mpc_acc_hor, // acceleration in flight
 					(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
 					(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max,
 					(ParamFloat<px4::params::MPC_JERK_AUTO>) _param_mpc_jerk_auto,
 					(ParamFloat<px4::params::MPC_XY_TRAJ_P>) _param_mpc_xy_traj_p,
-					(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max
+					(ParamFloat<px4::params::MPC_XY_ERR_MAX>) _param_mpc_xy_err_max,
+					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
+					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
+					(ParamFloat<px4::params::MPC_LAND_ALT1>)
+					_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
+					(ParamFloat<px4::params::MPC_LAND_ALT2>)
+					_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
+					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
+					(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
+					_param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp
+					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
 				       );
 };

src/modules/flight_mode_manager/tasks/AutoMapper/CMakeLists.txt

@@ -1,39 +0,0 @@
-############################################################################
-#
-#   Copyright (c) 2018 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_library(FlightTaskAutoMapper
-	FlightTaskAutoMapper.cpp
-)
-
-target_link_libraries(FlightTaskAutoMapper PUBLIC FlightTaskAuto)
-target_include_directories(FlightTaskAutoMapper PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.cpp

@@ -1,216 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
-
-/**
- * @file FlightAutoMapper.cpp
- */
-
-#include "FlightTaskAutoMapper.hpp"
-#include <mathlib/mathlib.h>
-
-using namespace matrix;
-
-FlightTaskAutoMapper::FlightTaskAutoMapper() :
-	_sticks(this),
-	_stick_acceleration_xy(this)
-{}
-
-bool FlightTaskAutoMapper::activate(const vehicle_local_position_setpoint_s &last_setpoint)
-{
-	bool ret = FlightTaskAuto::activate(last_setpoint);
-	_reset();
-	return ret;
-}
-
-bool FlightTaskAutoMapper::update()
-{
-	bool ret = FlightTaskAuto::update();
-	// always reset constraints because they might change depending on the type
-	_setDefaultConstraints();
-
-	// The only time a thrust set-point is sent out is during
-	// idle. Hence, reset thrust set-point to NAN in case the
-	// vehicle exits idle.
-
-	if (_type_previous == WaypointType::idle) {
-		_acceleration_setpoint.setNaN();
-	}
-
-	// during mission and reposition, raise the landing gears but only
-	// if altitude is high enough
-	if (_highEnoughForLandingGear()) {
-		_gear.landing_gear = landing_gear_s::GEAR_UP;
-	}
-
-	switch (_type) {
-	case WaypointType::idle:
-		_prepareIdleSetpoints();
-		break;
-
-	case WaypointType::land:
-		_prepareLandSetpoints();
-		break;
-
-	case WaypointType::loiter:
-
-	/* fallthrought */
-	case WaypointType::position:
-		_preparePositionSetpoints();
-		break;
-
-	case WaypointType::takeoff:
-		_prepareTakeoffSetpoints();
-		break;
-
-	case WaypointType::velocity:
-		_prepareVelocitySetpoints();
-		break;
-
-	default:
-		_preparePositionSetpoints();
-		break;
-	}
-
-	if (_param_com_obs_avoid.get()) {
-		_obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type);
-		_obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint,
-				_yawspeed_setpoint);
-	}
-
-
-	_generateSetpoints();
-
-	// update previous type
-	_type_previous = _type;
-
-	return ret;
-}
-
-void FlightTaskAutoMapper::_reset()
-{
-	// Set setpoints equal current state.
-	_velocity_setpoint = _velocity;
-	_position_setpoint = _position;
-}
-
-void FlightTaskAutoMapper::_prepareIdleSetpoints()
-{
-	// Send zero thrust setpoint
-	_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
-	_velocity_setpoint.setNaN();
-	_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
-}
-
-void FlightTaskAutoMapper::_prepareLandSetpoints()
-{
-	_velocity_setpoint.setNaN(); // Don't take over any smoothed velocity setpoint
-
-	// Slow down automatic descend close to ground
-	float land_speed = math::gradual(_dist_to_ground,
-					 _param_mpc_land_alt2.get(), _param_mpc_land_alt1.get(),
-					 _param_mpc_land_speed.get(), _constraints.speed_down);
-
-	if (_type_previous != WaypointType::land) {
-		// initialize xy-position and yaw to waypoint such that home is reached exactly without user input
-		_land_position = Vector3f(_target(0), _target(1), NAN);
-		_land_heading = _yaw_setpoint;
-		_stick_acceleration_xy.resetPosition(Vector2f(_target(0), _target(1)));
-	}
-
-	// User input assisted landing
-	if (_param_mpc_land_rc_help.get() && _sticks.checkAndSetStickInputs()) {
-		// Stick full up -1 -> stop, stick full down 1 -> double the speed
-		land_speed *= (1 + _sticks.getPositionExpo()(2));
-
-		_stick_yaw.generateYawSetpoint(_yawspeed_setpoint, _land_heading,
-					       _sticks.getPositionExpo()(3) * math::radians(_param_mpc_man_y_max.get()), _yaw, _deltatime);
-		_stick_acceleration_xy.generateSetpoints(_sticks.getPositionExpo().slice<2, 1>(0, 0), _yaw, _land_heading, _position,
-				_velocity_setpoint_feedback.xy(), _deltatime);
-		_stick_acceleration_xy.getSetpoints(_land_position, _velocity_setpoint, _acceleration_setpoint);
-
-		// Hack to make sure the MPC_YAW_MODE 4 alignment doesn't stop the vehicle from descending when there's yaw input
-		if (fabsf(_yawspeed_setpoint) > FLT_EPSILON) {
-			_yaw_sp_aligned = true;
-		}
-
-	} else {
-		// Make sure we have a valid land position even in the case we loose RC while amending it
-		if (!PX4_ISFINITE(_land_position(0))) {
-			_land_position.xy() = Vector2f(_position);
-		}
-	}
-
-	_position_setpoint = _land_position; // The last element of the land position has to stay NAN
-	_yaw_setpoint = _land_heading;
-	_velocity_setpoint(2) = land_speed;
-	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
-}
-
-void FlightTaskAutoMapper::_prepareTakeoffSetpoints()
-{
-	// Takeoff is completely defined by target position
-	_position_setpoint = _target;
-	_velocity_setpoint = Vector3f(NAN, NAN, NAN);
-
-	_gear.landing_gear = landing_gear_s::GEAR_DOWN;
-}
-
-void FlightTaskAutoMapper::_prepareVelocitySetpoints()
-{
-	// XY Velocity waypoint
-	// TODO : Rewiew that. What is the expected behavior?
-	_position_setpoint = Vector3f(NAN, NAN, _position(2));
-	Vector2f vel_sp_xy = Vector2f(_velocity).unit_or_zero() * _mc_cruise_speed;
-	_velocity_setpoint = Vector3f(vel_sp_xy(0), vel_sp_xy(1), NAN);
-}
-
-void FlightTaskAutoMapper::_preparePositionSetpoints()
-{
-	// Simple waypoint navigation: go to xyz target, with standard limitations
-	_position_setpoint = _target;
-	_velocity_setpoint.setNaN(); // No special velocity limitations
-}
-
-void FlightTaskAutoMapper::updateParams()
-{
-	FlightTaskAuto::updateParams();
-
-	// make sure that alt1 is above alt2
-	_param_mpc_land_alt1.set(math::max(_param_mpc_land_alt1.get(), _param_mpc_land_alt2.get()));
-}
-
-bool FlightTaskAutoMapper::_highEnoughForLandingGear()
-{
-	// return true if altitude is above two meters
-	return _dist_to_ground > 2.0f;
-}

src/modules/flight_mode_manager/tasks/AutoMapper/FlightTaskAutoMapper.hpp

@@ -1,90 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (c) 2018 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.
- *
- ****************************************************************************/
-
-/**
- * @file FlightTaskAutoMapper.hpp
- *
- * Abstract Flight task which generates local setpoints
- * based on the triplet type.
- */
-
-#pragma once
-
-#include "FlightTaskAuto.hpp"
-#include "Sticks.hpp"
-#include "StickAccelerationXY.hpp"
-#include "StickYaw.hpp"
-
-class FlightTaskAutoMapper : public FlightTaskAuto
-{
-public:
-	FlightTaskAutoMapper();
-	virtual ~FlightTaskAutoMapper() = default;
-	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
-	bool update() override;
-
-protected:
-
-	virtual void _generateSetpoints() = 0; /**< Generate velocity and position setpoint for following line. */
-
-	void _prepareIdleSetpoints();
-	void _prepareLandSetpoints();
-	void _prepareVelocitySetpoints();
-	void _prepareTakeoffSetpoints();
-	void _preparePositionSetpoints();
-
-	void updateParams() override; /**< See ModuleParam class */
-
-	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskAuto,
-					(ParamFloat<px4::params::MPC_LAND_SPEED>) _param_mpc_land_speed,
-					(ParamInt<px4::params::MPC_LAND_RC_HELP>) _param_mpc_land_rc_help,
-					(ParamFloat<px4::params::MPC_LAND_ALT1>)
-					_param_mpc_land_alt1, // altitude at which speed limit downwards reaches maximum speed
-					(ParamFloat<px4::params::MPC_LAND_ALT2>)
-					_param_mpc_land_alt2, // altitude at which speed limit downwards reached minimum speed
-					(ParamFloat<px4::params::MPC_TKO_SPEED>) _param_mpc_tko_speed,
-					(ParamFloat<px4::params::MPC_TKO_RAMP_T>)
-					_param_mpc_tko_ramp_t, // time constant for smooth takeoff ramp
-					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
-				       );
-
-private:
-	Sticks _sticks;
-	StickAccelerationXY _stick_acceleration_xy;
-	StickYaw _stick_yaw;
-	matrix::Vector3f _land_position;
-	float _land_heading;
-	void _reset(); /**< Resets member variables to current vehicle state */
-	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
-	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
-};

src/modules/flight_mode_manager/tasks/CMakeLists.txt

@@ -35,7 +35,6 @@
 add_subdirectory(FlightTask)
 add_subdirectory(Utility)
 add_subdirectory(Auto)
-add_subdirectory(AutoMapper)
 
 # add all additional flight tasks
 foreach(task ${flight_tasks_all})

src/modules/mavlink/mavlink_main.cpp

@@ -440,11 +440,11 @@ Mavlink::forward_message(const mavlink_message_t *msg, Mavlink *self)
 	if (meta) {
 		// Extract target system and target component if set
 		if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM) {
-			target_system_id = (_MAV_PAYLOAD(msg))[meta->target_system_ofs];
+			target_system_id = static_cast<uint8_t>((_MAV_PAYLOAD(msg))[meta->target_system_ofs]);
 		}
 
 		if (meta->flags & MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT) {
-			target_component_id = (_MAV_PAYLOAD(msg))[meta->target_component_ofs];
+			target_component_id = static_cast<uint8_t>((_MAV_PAYLOAD(msg))[meta->target_component_ofs]);
 		}
 	}
 

src/modules/mavlink/mavlink_messages.cpp

@@ -80,6 +80,7 @@
 #include "streams/GPS_GLOBAL_ORIGIN.hpp"
 #include "streams/GPS_RAW_INT.hpp"
 #include "streams/GPS_STATUS.hpp"
+#include "streams/GPS_RTCM_DATA.hpp"
 #include "streams/HEARTBEAT.hpp"
 #include "streams/HIGHRES_IMU.hpp"
 #include "streams/HIL_ACTUATOR_CONTROLS.hpp"
@@ -542,8 +543,11 @@ static const StreamListItem streams_list[] = {
 	create_stream_list_item<MavlinkStreamRawRpm>(),
 #endif // RAW_RPM_HPP
 #if defined(EFI_STATUS_HPP)
-	create_stream_list_item<MavlinkStreamEfiStatus>()
+	create_stream_list_item<MavlinkStreamEfiStatus>(),
 #endif // EFI_STATUS_HPP
+#if defined(GPS_RTCM_DATA_HPP)
+	create_stream_list_item<MavlinkStreamGPSRTCMData>()
+#endif // GPS_RTCM_DATA_HPP
 };
 
 const char *get_stream_name(const uint16_t msg_id)

src/modules/mavlink/streams/GPS_RTCM_DATA.hpp

@@ -0,0 +1,81 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 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
+ * 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.
+ *
+ ****************************************************************************/
+
+#ifndef GPS_RTCM_DATA_HPP
+#define GPS_RTCM_DATA_HPP
+
+#include <uORB/topics/gps_inject_data.h>
+
+class MavlinkStreamGPSRTCMData : public MavlinkStream
+{
+public:
+	static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamGPSRTCMData(mavlink); }
+
+	static constexpr const char *get_name_static() { return "GPS_RTCM_DATA"; }
+	static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_GPS_RTCM_DATA; }
+
+	const char *get_name() const override { return get_name_static(); }
+	uint16_t get_id() override { return get_id_static(); }
+
+	unsigned get_size() override
+	{
+		return _gps_inject_data_sub.advertised() ? (MAVLINK_MSG_ID_GPS_RTCM_DATA_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) : 0;
+	}
+
+private:
+	explicit MavlinkStreamGPSRTCMData(Mavlink *mavlink) : MavlinkStream(mavlink) {}
+
+	uORB::Subscription _gps_inject_data_sub{ORB_ID(gps_inject_data), 0};
+
+	bool send() override
+	{
+		gps_inject_data_s gps_inject_data;
+		bool sent = false;
+
+		while ((_mavlink->get_free_tx_buf() >= get_size()) && _gps_inject_data_sub.update(&gps_inject_data)) {
+			mavlink_gps_rtcm_data_t msg{};
+
+			msg.len = gps_inject_data.len;
+			msg.flags = gps_inject_data.flags;
+			memcpy(msg.data, gps_inject_data.data, sizeof(msg.data));
+
+			mavlink_msg_gps_rtcm_data_send_struct(_mavlink->get_channel(), &msg);
+
+			sent = true;
+		}
+
+		return sent;
+	}
+};
+
+#endif // GPS_RTCM_DATA_HPP

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -55,9 +55,14 @@ VehicleAngularVelocity::~VehicleAngularVelocity()
 	perf_free(_filter_reset_perf);
 	perf_free(_selection_changed_perf);
 #if !defined(CONSTRAINED_FLASH)
+	perf_free(_dynamic_notch_filter_esc_rpm_disable_perf);
+	perf_free(_dynamic_notch_filter_esc_rpm_reset_perf);
 	perf_free(_dynamic_notch_filter_esc_rpm_update_perf);
-	perf_free(_dynamic_notch_filter_fft_update_perf);
 	perf_free(_dynamic_notch_filter_esc_rpm_perf);
+
+	perf_free(_dynamic_notch_filter_fft_disable_perf);
+	perf_free(_dynamic_notch_filter_fft_reset_perf);
+	perf_free(_dynamic_notch_filter_fft_update_perf);
 	perf_free(_dynamic_notch_filter_fft_perf);
 #endif // CONSTRAINED_FLASH
 }
@@ -313,27 +318,26 @@ void VehicleAngularVelocity::ParametersUpdate(bool force)
 
 #if !defined(CONSTRAINED_FLASH)
 
-		if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm) {
-			if (_dynamic_notch_filter_esc_rpm_update_perf == nullptr) {
+		if (_param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm) {
+			if (_dynamic_notch_filter_esc_rpm_disable_perf == nullptr) {
+				_dynamic_notch_filter_esc_rpm_disable_perf = perf_alloc(PC_COUNT,
+						MODULE_NAME": gyro dynamic notch filter ESC RPM disable");
+				_dynamic_notch_filter_esc_rpm_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter ESC RPM reset");
 				_dynamic_notch_filter_esc_rpm_update_perf = perf_alloc(PC_COUNT,
 						MODULE_NAME": gyro dynamic notch filter ESC RPM update");
-			}
-
-			if (_dynamic_notch_filter_esc_rpm_perf == nullptr) {
-				_dynamic_notch_filter_esc_rpm_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch ESC RPM filter");
+				_dynamic_notch_filter_esc_rpm_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch filter ESC RPM elapsed");
 			}
 
 		} else {
 			DisableDynamicNotchEscRpm();
 		}
 
-		if (_param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT) {
-			if (_dynamic_notch_filter_fft_update_perf == nullptr) {
+		if (_param_imu_gyro_dnf_en.get() & DynamicNotch::FFT) {
+			if (_dynamic_notch_filter_fft_disable_perf == nullptr) {
+				_dynamic_notch_filter_fft_disable_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT disable");
+				_dynamic_notch_filter_fft_reset_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT reset");
 				_dynamic_notch_filter_fft_update_perf = perf_alloc(PC_COUNT, MODULE_NAME": gyro dynamic notch filter FFT update");
-			}
-
-			if (_dynamic_notch_filter_fft_perf == nullptr) {
-				_dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch FFT filter");
+				_dynamic_notch_filter_fft_perf = perf_alloc(PC_ELAPSED, MODULE_NAME": gyro dynamic notch filter FFT elapsed");
 			}
 
 		} else {
@@ -392,6 +396,7 @@ void VehicleAngularVelocity::DisableDynamicNotchEscRpm()
 				}
 
 				_esc_available.set(esc, false);
+				perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
 			}
 		}
 
@@ -413,6 +418,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT()
 		}
 
 		_dynamic_notch_fft_available = false;
+		perf_count(_dynamic_notch_filter_fft_disable_perf);
 	}
 
 #endif // !CONSTRAINED_FLASH
@@ -421,7 +427,7 @@ void VehicleAngularVelocity::DisableDynamicNotchFFT()
 void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 {
 #if !defined(CONSTRAINED_FLASH)
-	const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::EscRpm;
+	const bool enabled = _param_imu_gyro_dnf_en.get() & DynamicNotch::EscRpm;
 
 	if (enabled && (_esc_status_sub.updated() || force)) {
 
@@ -448,31 +454,32 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 					// for each ESC check determine if enabled/disabled from first notch (x axis, harmonic 0)
 					auto &nfx0 = _dynamic_notch_filter_esc_rpm[0][esc][0];
 
-					bool reset = (nfx0.getNotchFreq() <= FLT_EPSILON); // notch was previously disabled
+					bool reset = force || (nfx0.getNotchFreq() <= FLT_EPSILON); // notch was previously disabled
 
 					const float esc_hz = esc_rpm / 60.f;
+					const float notch_freq_diff = fabsf(nfx0.getNotchFreq() - esc_hz);
 
 					// update filter parameters if frequency changed or forced
-					if (force || reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > FLT_EPSILON)) {
-						static constexpr float ESC_NOTCH_BW_HZ = 8.f; // TODO: configurable bandwidth
+					if (reset || (notch_freq_diff > 0.1f)) {
 
 						// force reset if the notch frequency jumps significantly
-						if (!reset || (fabsf(nfx0.getNotchFreq() - esc_hz) > ESC_NOTCH_BW_HZ)) {
+						if (notch_freq_diff > _param_imu_gyro_dnf_bw.get()) {
 							reset = true;
 						}
 
-						for (int harmonic = MAX_NUM_ESC_RPM_HARMONICS; harmonic >= 0; harmonic--) {
+						for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
 							const float frequency_hz = esc_hz * (harmonic + 1);
 
 							for (int axis = 0; axis < 3; axis++) {
-								_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(_filter_sample_rate_hz, frequency_hz, ESC_NOTCH_BW_HZ);
+								_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(_filter_sample_rate_hz, frequency_hz,
+										_param_imu_gyro_dnf_bw.get());
 							}
 						}
 
 						perf_count(_dynamic_notch_filter_esc_rpm_update_perf);
 					}
 
-					if (force || reset) {
+					if (reset) {
 						const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
 
 						for (int axis = 0; axis < 3; axis++) {
@@ -480,6 +487,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 								_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].reset(reset_angular_velocity(axis));
 							}
 						}
+
+						perf_count(_dynamic_notch_filter_esc_rpm_reset_perf);
 					}
 
 					_dynamic_notch_esc_rpm_available = true;
@@ -491,6 +500,8 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 					// if this ESC was previously available disable all notch filters if forced or timeout
 					_esc_available.set(esc, false);
 
+					perf_count(_dynamic_notch_filter_esc_rpm_disable_perf);
+
 					for (int axis = 0; axis < 3; axis++) {
 						for (int harmonic = 0; harmonic < MAX_NUM_ESC_RPM_HARMONICS; harmonic++) {
 							_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].setParameters(0, 0, 0);
@@ -510,7 +521,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchEscRpm(bool force)
 void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
 {
 #if !defined(CONSTRAINED_FLASH)
-	const bool enabled = _param_imu_gyro_dyn_nf.get() & DynamicNotch::FFT;
+	const bool enabled = _param_imu_gyro_dnf_en.get() & DynamicNotch::FFT;
 
 	if (enabled && (_sensor_gyro_fft_sub.updated() || force)) {
 
@@ -556,6 +567,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
 						if (force || reset || (notch_freq_diff > bandwidth)) {
 							const Vector3f reset_angular_velocity{GetResetAngularVelocity()};
 							nf.reset(reset_angular_velocity(axis));
+							perf_count(_dynamic_notch_filter_fft_reset_perf);
 						}
 
 						_dynamic_notch_fft_available = true;
@@ -564,6 +576,7 @@ void VehicleAngularVelocity::UpdateDynamicNotchFFT(bool force)
 						// disable this notch filter (if it isn't already)
 						if (force || !reset) {
 							nf.setParameters(0, 0, 0);
+							perf_count(_dynamic_notch_filter_fft_disable_perf);
 						}
 					}
 				}
@@ -587,6 +600,7 @@ float VehicleAngularVelocity::FilterAngularVelocity(int axis, float data[], int
 
 		for (int esc = 0; esc < MAX_NUM_ESC_RPM; esc++) {
 			if (_esc_available[esc]) {
+				// apply notch filters higher -> lowest frequency
 				for (int harmonic = MAX_NUM_ESC_RPM_HARMONICS - 1; harmonic >= 0; harmonic--) {
 					_dynamic_notch_filter_esc_rpm[axis][esc][harmonic].applyArray(data, N);
 				}

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -157,8 +157,14 @@ private:
 	hrt_abstime _last_esc_rpm_notch_update[MAX_NUM_ESC_RPM] {};
 
 	perf_counter_t _dynamic_notch_filter_esc_rpm_update_perf{nullptr};
-	perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};
+	perf_counter_t _dynamic_notch_filter_esc_rpm_reset_perf{nullptr};
+	perf_counter_t _dynamic_notch_filter_esc_rpm_disable_perf{nullptr};
 	perf_counter_t _dynamic_notch_filter_esc_rpm_perf{nullptr};
+
+	perf_counter_t _dynamic_notch_filter_fft_disable_perf{nullptr};
+	perf_counter_t _dynamic_notch_filter_fft_reset_perf{nullptr};
+	perf_counter_t _dynamic_notch_filter_fft_update_perf{nullptr};
+
 	perf_counter_t _dynamic_notch_filter_fft_perf{nullptr};
 
 	bool _dynamic_notch_esc_rpm_available{false};
@@ -178,7 +184,8 @@ private:
 
 	DEFINE_PARAMETERS(
 #if !defined(CONSTRAINED_FLASH)
-		(ParamInt<px4::params::IMU_GYRO_DYN_NF>) _param_imu_gyro_dyn_nf,
+		(ParamInt<px4::params::IMU_GYRO_DNF_EN>) _param_imu_gyro_dnf_en,
+		(ParamFloat<px4::params::IMU_GYRO_DNF_BW>) _param_imu_gyro_dnf_bw,
 #endif // !CONSTRAINED_FLASH
 		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
 		(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,

src/modules/sensors/vehicle_angular_velocity/imu_gyro_parameters.c

@@ -130,10 +130,20 @@ PARAM_DEFINE_FLOAT(IMU_DGYRO_CUTOFF, 30.0f);
 * Enable bank of dynamically updating notch filters.
 * Requires ESC RPM feedback or onboard FFT (IMU_GYRO_FFT_EN).
 * @group Sensors
-* @category Developer
 * @min 0
 * @max 3
 * @bit 0 ESC RPM
 * @bit 1 FFT
 */
-PARAM_DEFINE_INT32(IMU_GYRO_DYN_NF, 0);
+PARAM_DEFINE_INT32(IMU_GYRO_DNF_EN, 0);
+
+/**
+* IMU gyro ESC notch filter bandwidth
+*
+* Bandwidth per notch filter when using dynamic notch filtering with ESC RPM.
+*
+* @group Sensors
+* @min 5
+* @max 30
+*/
+PARAM_DEFINE_FLOAT(IMU_GYRO_DNF_BW, 15.f);

src/systemcmds/ver/ver.c

@@ -217,6 +217,9 @@ int ver_main(int argc, char *argv[])
 
 			}
 
+			if (show_all) {
+				printf("Build variant: %s\n", px4_board_target_label());
+			}
 
 			if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
 				printf("Toolchain: %s, %s\n", px4_toolchain_name(), px4_toolchain_version());