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/sensor_calibration/Accelerometer.hpp

@@ -76,7 +76,6 @@ public:
 	const matrix::Dcmf &rotation() const { return _rotation; }
 	const Rotation &rotation_enum() const { return _rotation_enum; }
 	const matrix::Vector3f &scale() const { return _scale; }
-	const matrix::Vector3f &thermal_offset() const { return _thermal_offset; }
 
 	// apply offsets and scale
 	// rotate corrected measurements from sensor to body frame

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/commander/accelerometer_calibration.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2020 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
@@ -130,17 +130,13 @@
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/time.h>
 
-#include "lm_fit.hpp"
-
 #include <drivers/drv_hrt.h>
-#include <include/containers/Bitset.hpp>
 #include <lib/sensor_calibration/Accelerometer.hpp>
 #include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <matrix/math.hpp>
 #include <lib/conversion/rotation.h>
-#include <lib/mathlib/math/WelfordMean.hpp>
 #include <lib/parameters/param.h>
 #include <lib/systemlib/err.h>
 #include <lib/systemlib/mavlink_log.h>
@@ -154,174 +150,191 @@ using namespace matrix;
 using namespace time_literals;
 
 static constexpr char sensor_name[] {"accel"};
-static constexpr uint8_t MAX_ACCEL_SENS = 4;
+static constexpr unsigned MAX_ACCEL_SENS = 4;
 
 /// Data passed to calibration worker routine
 struct accel_worker_data_s {
-	orb_advert_t *mavlink_log_pub{nullptr};
-	unsigned done_count{0};
-	matrix::Vector3f accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count] {};
-	float accel_temperature_ref[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN};
-
-	unsigned calibration_sides{0}; ///< The total number of sides
-	bool side_data_collected[detect_orientation_side_count] {};
+	orb_advert_t	*mavlink_log_pub{nullptr};
+	unsigned	done_count{0};
+	float		accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {};
+	float		accel_temperature_ref[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN};
 };
 
 // Read specified number of accelerometer samples, calculate average and dispersion.
-static calibrate_return read_accelerometer_avg(Vector3f(&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count],
-		float (&accel_temperature_avg)[MAX_ACCEL_SENS], unsigned orient)
+static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3],
+		float (&accel_temperature_avg)[MAX_ACCEL_SENS],	unsigned orient, unsigned samples_num)
 {
-	calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {};
-	uORB::SubscriptionMultiArray<sensor_accel_s, MAX_ACCEL_SENS> accel_subs{ORB_ID::sensor_accel};
-	math::WelfordMean<Vector3f> mean[MAX_ACCEL_SENS] {};
-
-	Vector3f accel_prev[MAX_ACCEL_SENS] {};
-
+	Vector3f accel_sum[MAX_ACCEL_SENS] {};
 	float temperature_sum[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN};
-	unsigned temperature_count[MAX_ACCEL_SENS] {};
+	unsigned counts[MAX_ACCEL_SENS] {};
+
+	unsigned errcount = 0;
+
+	// sensor thermal corrections
+	uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
+	sensor_correction_s sensor_correction{};
+	sensor_correction_sub.copy(&sensor_correction);
+
+	uORB::SubscriptionBlocking<sensor_accel_s> accel_sub[MAX_ACCEL_SENS] {
+		{ORB_ID(sensor_accel), 0, 0},
+		{ORB_ID(sensor_accel), 0, 1},
+		{ORB_ID(sensor_accel), 0, 2},
+		{ORB_ID(sensor_accel), 0, 3},
+	};
 
 	/* use the first sensor to pace the readout, but do per-sensor counts */
-	bool done = false;
+	while (counts[0] < samples_num) {
+		if (accel_sub[0].updatedBlocking(100000)) {
+			for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
+				sensor_accel_s arp;
 
-	const unsigned num_accels = math::min(accel_subs.advertised_count(), MAX_ACCEL_SENS);
+				while (accel_sub[accel_index].update(&arp)) {
+					// fetch optional thermal offset corrections in sensor/board frame
+					Vector3f offset{0, 0, 0};
+					sensor_correction_sub.update(&sensor_correction);
 
-	const hrt_abstime timestamp_start = hrt_absolute_time();
+					if (sensor_correction.timestamp > 0 && arp.device_id != 0) {
+						for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) {
+							if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) {
+								switch (correction_index) {
+								case 0:
+									offset = Vector3f{sensor_correction.accel_offset_0};
+									break;
+								case 1:
+									offset = Vector3f{sensor_correction.accel_offset_1};
+									break;
+								case 2:
+									offset = Vector3f{sensor_correction.accel_offset_2};
+									break;
+								case 3:
+									offset = Vector3f{sensor_correction.accel_offset_3};
+									break;
+								}
+							}
+						}
+					}
 
-	while (!done && (hrt_elapsed_time(&timestamp_start) < 5_s)) {
-		unsigned good_count = 0;
+					accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset;
 
-		for (unsigned accel_index = 0; accel_index < num_accels; accel_index++) {
-			sensor_accel_s arp;
+					counts[accel_index]++;
 
-			while (accel_subs[accel_index].update(&arp)) {
-				calibrations[accel_index].set_device_id(arp.device_id);
-				calibrations[accel_index].SensorCorrectionsUpdate();
+					if (!PX4_ISFINITE(temperature_sum[accel_index])) {
+						// set first valid value
+						temperature_sum[accel_index] = (arp.temperature * counts[accel_index]);
 
-				// fetch optional thermal offset corrections in sensor/board frame
-				const Vector3f accel{Vector3f{arp.x, arp.y, arp.z} - calibrations[accel_index].thermal_offset()};
-
-				if ((accel - accel_prev[accel_index]).longerThan(0.5f)) {
-					mean[accel_index].reset();
-					accel_prev[accel_index] = accel;
-
-				} else {
-					mean[accel_index].update(accel);
-				}
-
-				if (!PX4_ISFINITE(temperature_sum[accel_index])) {
-					// set first valid value
-					temperature_sum[accel_index] = arp.temperature;
-					temperature_count[accel_index] = 1;
-
-				} else {
-					temperature_sum[accel_index] += arp.temperature;
-					temperature_count[accel_index]++;
+					} else {
+						temperature_sum[accel_index] += arp.temperature;
+					}
 				}
 			}
 
-			if ((mean[accel_index].count() > 1000) && !mean[accel_index].variance().longerThan(0.01)) {
-				good_count++;
-			}
-		}
-
-		if ((good_count > 0) && (good_count == num_accels)) {
-			PX4_INFO("finished early: %d", mean[0].count());
-			done = true;
-
 		} else {
-			px4_usleep(2000);
+			errcount++;
+			continue;
+		}
+
+		if (errcount > samples_num / 10) {
+			return calibrate_return_error;
 		}
 	}
 
-	unsigned good_count = 0;
+	// rotate sensor measurements from sensor to body frame using board rotation matrix
+	const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
 
-	for (unsigned s = 0; s < num_accels; s++) {
-		if (mean[s].valid()) {
-			const Vector3f avg = mean[s].mean();
-			const Vector3f var = mean[s].variance();
-
-			PX4_INFO("O: %d, Accel: %d, Mean: [%.6f, %.6f, %.6f], Variance: [%.6f, %.6f, %.6f]", orient, s,
-				 (double)avg(0), (double)avg(1), (double)avg(2),
-				 (double)var(0), (double)var(1), (double)var(2)
-				);
-
-			if (!var.longerThan(0.01f)) {
-				good_count++;
-
-			} else {
-				break;
-			}
-
-			accel_avg[s][orient] = mean[s].mean();
-
-		} else {
-			accel_avg[s][orient].zero();
-		}
+	for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
+		accel_sum[s] = board_rotation * accel_sum[s];
 	}
 
 	for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) {
-		if (temperature_count[s] > 0) {
-			accel_temperature_avg[s] = temperature_sum[s] / temperature_count[s];
+		const Vector3f avg{accel_sum[s] / counts[s]};
+		avg.copyTo(accel_avg[s][orient]);
 
-		} else {
-			accel_temperature_avg[s] = NAN;
-		}
+		accel_temperature_avg[s] = temperature_sum[s] /  counts[s];
 	}
 
-	return (good_count == num_accels) ? calibrate_return_ok : calibrate_return_error;
+	return calibrate_return_ok;
 }
 
 static calibrate_return accel_calibration_worker(detect_orientation_return orientation, void *data)
 {
+	static constexpr unsigned samples_num = 750;
 	accel_worker_data_s *worker_data = (accel_worker_data_s *)(data);
 
-	int attempt = 0;
+	calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
+			     detect_orientation_str(orientation));
 
-	while (attempt < 10) {
-		calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side",
-				     detect_orientation_str(orientation));
+	read_accelerometer_avg(worker_data->accel_ref, worker_data->accel_temperature_ref, orientation, samples_num);
 
-		if (read_accelerometer_avg(worker_data->accel_ref, worker_data->accel_temperature_ref,
-					   orientation) == calibrate_return_ok) {
+	// check accel
+	for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
+		switch (orientation) {
+		case ORIENTATION_TAIL_DOWN:    // [ g, 0, 0 ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_TAIL_DOWN][0] < 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid X-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
 
-			calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%.3f %.3f %.3f]",
-					     detect_orientation_str(orientation),
-					     (double)worker_data->accel_ref[0][orientation](0),
-					     (double)worker_data->accel_ref[0][orientation](1),
-					     (double)worker_data->accel_ref[0][orientation](2));
+			break;
 
-			worker_data->done_count++;
-			calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
+		case ORIENTATION_NOSE_DOWN:    // [ -g, 0, 0 ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_NOSE_DOWN][0] > 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid X-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
 
-			return calibrate_return_ok;
+			break;
+
+		case ORIENTATION_LEFT:         // [ 0, g, 0 ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_LEFT][1] < 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Y-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
+
+			break;
+
+		case ORIENTATION_RIGHT:        // [ 0, -g, 0 ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_RIGHT][1] > 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Y-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
+
+			break;
+
+		case ORIENTATION_UPSIDE_DOWN:  // [ 0, 0, g ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_UPSIDE_DOWN][2] < 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Z-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
+
+			break;
+
+		case ORIENTATION_RIGHTSIDE_UP: // [ 0, 0, -g ]
+			if (worker_data->accel_ref[accel_index][ORIENTATION_RIGHTSIDE_UP][2] > 0.f) {
+				calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Z-axis, check rotation", accel_index);
+				return calibrate_return_error;
+			}
+
+			break;
+
+		default:
+			break;
 		}
-
-		attempt++;
 	}
 
-	return calibrate_return_error;
+	calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%.3f %.3f %.3f]",
+			     detect_orientation_str(orientation),
+			     (double)worker_data->accel_ref[0][orientation][0],
+			     (double)worker_data->accel_ref[0][orientation][1],
+			     (double)worker_data->accel_ref[0][orientation][2]);
+
+	worker_data->done_count++;
+	calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
+
+	return calibrate_return_ok;
 }
 
 int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 {
-	// We should not try to subscribe if the topic doesn't actually exist and can be counted.
-	const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel));
-
-	// Warn that we will not calibrate more than MAX_GYROS gyroscopes
-	if (orb_accel_count > MAX_ACCEL_SENS) {
-		calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count,
-					 MAX_ACCEL_SENS);
-
-	} else if (orb_accel_count < 1) {
-		calibration_log_critical(mavlink_log_pub, "No accels found");
-		return PX4_ERROR;
-	}
-
-	// Collect: As defined by configuration
-	// start with a full mask, all six bits set
-	int32_t cal_mask = (1 << 6) - 1;
-	param_get(param_find("CAL_ACC_SIDES"), &cal_mask);
-
 	calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name);
 
 	calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {};
@@ -357,222 +370,83 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 	/* measure and calculate offsets & scales */
 	accel_worker_data_s worker_data{};
 	worker_data.mavlink_log_pub = mavlink_log_pub;
+	bool data_collected[detect_orientation_side_count] {};
 
-	for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) / sizeof(worker_data.side_data_collected[0])); i++) {
-		if ((cal_mask & (1 << i)) > 0) {
-			// mark as missing
-			worker_data.side_data_collected[i] = false;
-			worker_data.calibration_sides++;
+	if (calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, &worker_data,
+				       false) == calibrate_return_ok) {
 
-		} else {
-			// mark as completed from the beginning
-			worker_data.side_data_collected[i] = true;
-
-			calibration_log_info(mavlink_log_pub,
-					     "[cal] %s side done, rotate to a different side",
-					     detect_orientation_str(static_cast<enum detect_orientation_return>(i)));
-			px4_usleep(100000);
-		}
-	}
-
-	if (calibrate_from_orientation(mavlink_log_pub, worker_data.side_data_collected, accel_calibration_worker,
-				       &worker_data) == calibrate_return_ok) {
-
-		const Rotation board_rotation = calibration::GetBoardRotation();
+		const Dcmf board_rotation = calibration::GetBoardRotationMatrix();
+		const Dcmf board_rotation_t = board_rotation.transpose();
 
 		bool param_save = false;
 		bool failed = true;
 
-		int internal_rotation_count{0};
-		Rotation internal_rotations[MAX_ACCEL_SENS] {};
+		for (unsigned i = 0; i < MAX_ACCEL_SENS; i++) {
+			if (i < active_sensors) {
+				// calculate offsets
+				Vector3f offset{};
 
-		for (uint8_t cur_accel = 0; cur_accel < active_sensors; cur_accel++) {
-			int accel_data_count = 0;
+				// X offset: average X from TAIL_DOWN + NOSE_DOWN
+				const Vector3f accel_tail_down{worker_data.accel_ref[i][ORIENTATION_TAIL_DOWN]};
+				const Vector3f accel_nose_down{worker_data.accel_ref[i][ORIENTATION_NOSE_DOWN]};
+				offset(0) = (accel_tail_down(0) + accel_nose_down(0)) * 0.5f;
 
-			matrix::Vector3f accel_data[6] {};
+				// Y offset: average Y from LEFT + RIGHT
+				const Vector3f accel_left{worker_data.accel_ref[i][ORIENTATION_LEFT]};
+				const Vector3f accel_right{worker_data.accel_ref[i][ORIENTATION_RIGHT]};
+				offset(1) = (accel_left(1) + accel_right(1)) * 0.5f;
 
-			for (int orientation = 0; orientation < 6; orientation++) {
-				if ((cal_mask & (1 << orientation)) > 0) {
-					accel_data[accel_data_count] = worker_data.accel_ref[cur_accel][orientation];
-					accel_data_count++;
-				}
-			}
+				// Z offset: average Z from UPSIDE_DOWN + RIGHTSIDE_UP
+				const Vector3f accel_upside_down{worker_data.accel_ref[i][ORIENTATION_UPSIDE_DOWN]};
+				const Vector3f accel_rightside_up{worker_data.accel_ref[i][ORIENTATION_RIGHTSIDE_UP]};
+				offset(2) = (accel_upside_down(2) + accel_rightside_up(2)) * 0.5f;
 
-			bool sphere_fit_only = false;
-			bool sphere_fit_success = false;
-			bool ellipsoid_fit_success = false;
+				// transform matrix
+				Matrix3f mat_A;
+				mat_A.row(0) = accel_tail_down - offset;
+				mat_A.row(1) = accel_left - offset;
+				mat_A.row(2) = accel_upside_down - offset;
 
-			sphere_params sphere_data{};
-			sphere_data.radius = CONSTANTS_ONE_G;
-			int ret = lm_fit(worker_data.accel_ref[cur_accel], 6, sphere_data, false);
+				// calculate inverse matrix for A: simplify matrices mult because b has only one non-zero element == g at index i
+				const Matrix3f accel_T = mat_A.I() * CONSTANTS_ONE_G;
 
-			if (ret == PX4_OK) {
-				sphere_fit_success = true;
+				// update calibration
+				const Vector3f accel_offs_rotated{board_rotation_t *offset};
+				calibrations[i].set_offset(accel_offs_rotated);
 
-				if (!sphere_fit_only) {
-					int ellipsoid_ret = lm_fit(accel_data, accel_data_count, sphere_data, true);
+				const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation};
+				calibrations[i].set_scale(accel_T_rotated.diag());
 
-					if (ellipsoid_ret == PX4_OK) {
-						ellipsoid_fit_success  = true;
-					}
-				}
-
-				PX4_INFO("Accel: %" PRIu8 " sphere radius: %.4f", cur_accel, (double)sphere_data.radius);
-			}
-
-			static constexpr float scale_min = 0.9f;
-			static constexpr float scale_max = 1.1f;
-
-			for (int axis = 0; axis < 3; axis++) {
-				if ((sphere_data.diag(axis) < scale_min) || (sphere_data.diag(axis) > scale_max)) {
-					PX4_WARN("bad scale factor"); // TODO, abort and report failure
-				}
-			}
-
-			bool calibration_valid = sphere_fit_success || ellipsoid_fit_success; // TODO: scale range, offset range, etc
+				calibrations[i].set_temperature(worker_data.accel_temperature_ref[i]);
 
 #if defined(DEBUD_BUILD)
-			PX4_INFO("accel %d offset", cur_accel);
-			sphere_data.offset.print();
+				PX4_INFO("accel %d: offset", i);
+				offset.print();
+				PX4_INFO("accel %d: bT * offset", i);
+				accel_offs_rotated.print();
 
-			PX4_INFO("accel %d scale", cur_accel);
-			sphere_data.diag.print();
-
-			PX4_INFO("accel %d offdiagonal", cur_accel);
-			sphere_data.offdiag.print();
+				PX4_INFO("accel %d: mat_A", i);
+				mat_A.print();
+				PX4_INFO("accel %d: accel_T", i);
+				accel_T.print();
+				PX4_INFO("accel %d: bT * accel_T * b", i);
+				accel_T_rotated.print();
 #endif // DEBUD_BUILD
-
-			if (!calibration_valid) {
-				failed = true;
-				break;
-			}
-
-			// determine best rotation
-			float calibration_metric[ROTATION_MAX] {};
-			float min_error = FLT_MAX;
-			Rotation best_rotation = ROTATION_NONE;
-
-			for (int r = ROTATION_NONE; r < ROTATION_MAX; r++) {
-				calibration_metric[r] = FLT_MAX;
-
-				// try all rotations
-				switch (r) {
-				case ROTATION_ROLL_90_PITCH_68_YAW_293: // skip
-				case ROTATION_PITCH_180_YAW_90:  // skip 26, same as 14 ROTATION_ROLL_180_YAW_270
-				case ROTATION_PITCH_180_YAW_270: // skip 27, same as 10 ROTATION_ROLL_180_YAW_90
-					break;
-
-				default: {
-						matrix::Matrix<float, 6, 3> Y;
-						static constexpr float g = CONSTANTS_ONE_G;
-						Y.row(0) = Vector3f{ g,  0,  0}; // ORIENTATION_TAIL_DOWN
-						Y.row(1) = Vector3f{-g,  0,  0}; // ORIENTATION_NOSE_DOWN,
-						Y.row(2) = Vector3f{ 0,  g,  0}; // ORIENTATION_LEFT,
-						Y.row(3) = Vector3f{ 0, -g,  0}; // ORIENTATION_RIGHT,
-						Y.row(4) = Vector3f{ 0,  0,  g}; // ORIENTATION_UPSIDE_DOWN,
-						Y.row(5) = Vector3f{ 0,  0, -g}; // ORIENTATION_RIGHTSIDE_UP
-
-						// apply calibration and compare data with expected direction
-						for (int orientation = 0; orientation < 6; orientation++) {
-
-							if ((cal_mask & (1 << orientation)) > 0) {
-								Vector3f calibrated_accel{sphere_data.diag.emult(worker_data.accel_ref[cur_accel][orientation] - sphere_data.offset)};
-
-								const Vector3f rotated_data{get_rot_matrix((Rotation)r) *calibrated_accel};
-
-								calibration_metric[r] += (rotated_data - Y.row(orientation)).norm();
-							}
-						}
-					}
-				}
+				calibrations[i].PrintStatus();
 
 
-				if (calibration_metric[r] < min_error) {
-					min_error = calibration_metric[r];
-					best_rotation = (Rotation)r;
-				}
+				if (calibrations[i].ParametersSave()) {
+					param_save = true;
+					failed = false;
 
-			}
-
-			PX4_INFO("best rotation: %d", best_rotation);
-
-			if (!calibrations[cur_accel].external()) {
-				internal_rotations[internal_rotation_count] = best_rotation;
-				internal_rotation_count++;
-			}
-
-			bool print_all_errors = true;
-
-			if (calibrations[cur_accel].external()) {
-				switch (calibrations[cur_accel].rotation_enum()) {
-				case ROTATION_ROLL_90_PITCH_68_YAW_293:
-					PX4_INFO("[cal] External Accel: %" PRIu8 " (%" PRIu32 "), keeping manually configured rotation %" PRIu8,
-						 cur_accel, calibrations[cur_accel].device_id(), calibrations[cur_accel].rotation_enum());
-					continue;
-
-				default: {
-						if (best_rotation != calibrations[cur_accel].rotation_enum()) {
-							calibration_log_info(mavlink_log_pub, "[cal] External Accel: %" PRIu8 " (%" PRIu32 "), determined rotation: %" PRIu8,
-									     cur_accel, calibrations[cur_accel].device_id(), best_rotation);
-							calibrations[cur_accel].set_rotation(best_rotation);
-
-						} else {
-							PX4_INFO("[cal] External Accel: %" PRIu8 " (%" PRIu32 "), no rotation change: %" PRIu8,
-								 cur_accel, calibrations[cur_accel].device_id(), best_rotation);
-						}
-					}
+				} else {
+					failed = true;
+					calibration_log_critical(mavlink_log_pub, "calibration save failed");
 					break;
 				}
 			}
-
-			if (print_all_errors) {
-				for (int r = ROTATION_NONE; r < ROTATION_MAX; r++) {
-					if (calibration_metric[r] < FLT_MAX) {
-						PX4_ERR("Accel: %" PRIu8 " (%" PRIu32 "), rotation: %" PRIu32 ", error: %.3f",
-							cur_accel, calibrations[cur_accel].device_id(), (uint32_t)r, (double)calibration_metric[r]);
-					}
-				}
-			}
-
-			// update calibration
-			calibrations[cur_accel].set_offset(sphere_data.offset);
-			calibrations[cur_accel].set_scale(sphere_data.diag);
-			calibrations[cur_accel].set_temperature(worker_data.accel_temperature_ref[cur_accel]);
-
-			calibrations[cur_accel].PrintStatus();
-
-			// save all calibrations including empty slots
-			if (calibrations[cur_accel].ParametersSave()) {
-				param_save = true;
-				failed = false;
-
-			} else {
-				failed = true;
-				calibration_log_critical(mavlink_log_pub, "calibration save failed");
-				break;
-			}
 		}
 
-		// review SENS_BOARD_ROT, do all internal accels agree on SENS_BOARD_ROT?
-		if (!failed && (internal_rotation_count > 0)) {
-			Rotation best_rotation = internal_rotations[0];
-			bool same = true;
-
-			for (unsigned i = 0; i < MAX_ACCEL_SENS; i++) {
-				if (internal_rotations[0] != internal_rotations[i]) {
-					same = false;
-					break;
-				}
-			}
-
-			if (same && (best_rotation != board_rotation)) {
-				PX4_ERR("Incorrect board rotation: %d", board_rotation);
-				int32_t sens_board_rot = best_rotation;
-				param_set_no_notification(param_find("SENS_BOARD_ROT"), &sens_board_rot);
-			}
-		}
-
-
 		if (!failed && factory_storage.store() != PX4_OK) {
 			failed = true;
 		}
@@ -598,6 +472,7 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 #if !defined(CONSTRAINED_FLASH)
 	PX4_INFO("Accelerometer quick calibration");
 
+	bool param_save = false;
 	bool failed = true;
 
 	FactoryCalibrationStorage factory_storage;
@@ -607,113 +482,135 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 		return PX4_ERROR;
 	}
 
-	calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {};
-	math::WelfordMean<Vector3f> mean[MAX_ACCEL_SENS] {};
+	// sensor thermal corrections (optional)
+	uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
+	sensor_correction_s sensor_correction{};
+	sensor_correction_sub.copy(&sensor_correction);
+
 	uORB::SubscriptionMultiArray<sensor_accel_s, MAX_ACCEL_SENS> accel_subs{ORB_ID::sensor_accel};
-	px4::Bitset<MAX_ACCEL_SENS> valid_cal;
-
-	const hrt_abstime start_time = hrt_absolute_time();
-
-	Vector3f accel_prev[MAX_ACCEL_SENS] {};
-
-	while ((hrt_elapsed_time(&start_time) < 3_s) && (valid_cal.count() < accel_subs.advertised_count())) {
-		for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
-			sensor_accel_s sensor_accel;
-
-			while (accel_subs[accel_index].update(&sensor_accel)) {
-				if ((sensor_accel.timestamp > 0) && (sensor_accel.device_id != 0)) {
-					calibrations[accel_index].set_device_id(sensor_accel.device_id);
-					calibrations[accel_index].SensorCorrectionsUpdate();
-
-					const Vector3f accel{Vector3f{sensor_accel.x, sensor_accel.y, sensor_accel.z} - calibrations[accel_index].thermal_offset()};
-
-					if ((accel - accel_prev[accel_index]).longerThan(0.5f)) {
-						mean[accel_index].reset();
-						accel_prev[accel_index] = accel;
-
-					} else {
-						mean[accel_index].update(accel);
-					}
-
-					if (mean[accel_index].count() > 300 && !mean[accel_index].variance().longerThan(0.01f)) {
-						valid_cal.set(accel_index, true);
-					}
-				}
-			}
-		}
-
-		px4_usleep(1000);
-
-		for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
-			if (mean[accel_index].count() > 0) {
-				PX4_DEBUG("accel %d/%d valid %d var: %.6f", accel_index, accel_subs.advertised_count(), mean[accel_index].count(),
-					  (double)mean[accel_index].variance().length());
-			}
-		}
-	}
-
-	bool calibration_updated = false;
-
-	px4::Bitset<MAX_ACCEL_SENS> calibrated;
-
-	static constexpr float g = CONSTANTS_ONE_G;
-	matrix::Matrix<float, 6, 3> Y;
-	Y.row(0) = Vector3f{ g,  0,  0}; // ORIENTATION_TAIL_DOWN
-	Y.row(1) = Vector3f{-g,  0,  0}; // ORIENTATION_NOSE_DOWN,
-	Y.row(2) = Vector3f{ 0,  g,  0}; // ORIENTATION_LEFT,
-	Y.row(3) = Vector3f{ 0, -g,  0}; // ORIENTATION_RIGHT,
-	Y.row(4) = Vector3f{ 0,  0,  g}; // ORIENTATION_UPSIDE_DOWN,
-	Y.row(5) = Vector3f{ 0,  0, -g}; // ORIENTATION_RIGHTSIDE_UP
 
+	/* use the first sensor to pace the readout, but do per-sensor counts */
 	for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
-		if (valid_cal[accel_index]) {
+		sensor_accel_s arp{};
+		Vector3f accel_sum{};
+		float temperature_sum{NAN};
+		unsigned count = 0;
 
-			const Vector3f accel_avg = mean[accel_index].mean();
+		while (accel_subs[accel_index].update(&arp)) {
+			// fetch optional thermal offset corrections in sensor/board frame
+			if ((arp.timestamp > 0) && (arp.device_id != 0)) {
+				Vector3f offset{0, 0, 0};
 
-			int closest_orientation = -1;
-			float smallest_orientation_error = INFINITY;
+				if (sensor_correction.timestamp > 0) {
+					for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) {
+						if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) {
+							switch (correction_index) {
+							case 0:
+								offset = Vector3f{sensor_correction.accel_offset_0};
+								break;
+							case 1:
+								offset = Vector3f{sensor_correction.accel_offset_1};
+								break;
+							case 2:
+								offset = Vector3f{sensor_correction.accel_offset_2};
+								break;
+							case 3:
+								offset = Vector3f{sensor_correction.accel_offset_3};
+								break;
+							}
+						}
+					}
+				}
 
-			for (int i = 0; i < 6; i++) {
-				// assume sensor/board is in one of these orientations
-				float accel_error = (Vector3f{Y.row(i)} - accel_avg).length();
+				const Vector3f accel{Vector3f{arp.x, arp.y, arp.z} - offset};
 
-				if (accel_error < smallest_orientation_error) {
-					closest_orientation = i;
-					smallest_orientation_error = accel_error;
+				if (count > 0) {
+					const Vector3f diff{accel - (accel_sum / count)};
+
+					if (diff.norm() < 1.f) {
+						accel_sum += Vector3f{arp.x, arp.y, arp.z} - offset;
+
+						count++;
+
+						if (!PX4_ISFINITE(temperature_sum)) {
+							// set first valid value
+							temperature_sum = (arp.temperature * count);
+
+						} else {
+							temperature_sum += arp.temperature;
+						}
+					}
+
+				} else {
+					accel_sum = accel;
+					temperature_sum = arp.temperature;
+					count = 1;
 				}
 			}
+		}
+
+		if ((count > 0) && (arp.device_id != 0)) {
+
+			bool calibrated = false;
+			const Vector3f accel_avg = accel_sum / count;
+			const float temperature_avg = temperature_sum / count;
 
 			Vector3f offset{0.f, 0.f, 0.f};
 
-			if (closest_orientation != -1) {
-				PX4_INFO("assuming accel %d is orientation %s (%d)", accel_index,
-					 detect_orientation_str((enum detect_orientation_return)closest_orientation),
-					 closest_orientation);
+			uORB::SubscriptionData<vehicle_attitude_s> attitude_sub{ORB_ID(vehicle_attitude)};
+			attitude_sub.update();
 
-				offset = accel_avg - (calibrations[accel_index].scale().emult(Vector3f{Y.row(closest_orientation)}));
-				calibrated.set(accel_index, true);
+			if (attitude_sub.advertised() && attitude_sub.get().timestamp != 0) {
+				// use vehicle_attitude if available
+				const vehicle_attitude_s &att = attitude_sub.get();
+				const matrix::Quatf q{att.q};
+				const Vector3f accel_ref = q.conjugate_inversed(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G});
+
+				// sanity check angle between acceleration vectors
+				const float angle = AxisAnglef(Quatf(accel_avg, accel_ref)).angle();
+
+				if (angle <= math::radians(10.f)) {
+					offset = accel_avg - accel_ref;
+					calibrated = true;
+				}
 			}
 
-			const Vector3f accel_avg_calibrated = calibrations[accel_index].scale().emult(accel_avg - offset);
+			if (!calibrated) {
+				// otherwise simply normalize to gravity and remove offset
+				Vector3f accel{accel_avg};
+				accel.normalize();
+				accel = accel * CONSTANTS_ONE_G;
 
-			if (!calibrated[accel_index] || !accel_avg_calibrated.longerThan(CONSTANTS_ONE_G * 0.9f)
-			    || accel_avg_calibrated.longerThan(CONSTANTS_ONE_G * 1.1f)
-			    || !PX4_ISFINITE(offset(0)) || !PX4_ISFINITE(offset(1)) || !PX4_ISFINITE(offset(2))) {
+				offset = accel_avg - accel;
+				calibrated = true;
+			}
+
+			calibration::Accelerometer calibration{arp.device_id};
+
+			// reset cal index to uORB
+			calibration.set_calibration_index(accel_index);
+
+			if (!calibrated || (offset.norm() > CONSTANTS_ONE_G)
+			    || !PX4_ISFINITE(offset(0))
+			    || !PX4_ISFINITE(offset(1))
+			    || !PX4_ISFINITE(offset(2))) {
 
 				PX4_ERR("accel %d quick calibrate failed", accel_index);
-				failed = true;
-				break;
 
 			} else {
-				// reset cal index to uORB
-				calibrations[accel_index].set_calibration_index(accel_index);
+				calibration.set_offset(offset);
+				calibration.set_temperature(temperature_avg);
 
-				if (calibrations[accel_index].set_offset(offset)) {
-					calibrations[accel_index].PrintStatus();
-					calibration_updated = true;
+				if (calibration.ParametersSave()) {
+					calibration.PrintStatus();
+					param_save = true;
+					failed = false;
+
+				} else {
+					failed = true;
+					calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "calibration save failed");
+					break;
 				}
-
-				failed = false;
 			}
 		}
 	}
@@ -722,29 +619,15 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub)
 		failed = true;
 	}
 
+	if (param_save) {
+		param_notify_changes();
+	}
+
 	if (!failed) {
-		if (calibration_updated) {
-			bool param_save = false;
-
-			for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) {
-				if (calibrated[accel_index]) {
-					if (calibrations[accel_index].ParametersSave()) {
-						param_save = true;
-					}
-				}
-			}
-
-			if (param_save) {
-				param_notify_changes();
-			}
-		}
-
-		calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name);
 		return PX4_OK;
 	}
 
 #endif // !CONSTRAINED_FLASH
 
-	calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name);
 	return PX4_ERROR;
 }

src/modules/commander/calibration_routines.cpp

@@ -63,16 +63,17 @@
 
 using namespace time_literals;
 
-enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub)
+enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, bool lenient_still_position)
 {
 	static constexpr unsigned ndim = 3;
 
 	float accel_ema[ndim] {};                       // exponential moving average of accel
 	float accel_disp[3] {};                         // max-hold dispersion of accel
 	static constexpr float ema_len = 0.5f;          // EMA time constant in seconds
-	float still_thr2 = powf(0.25f * 3, 2);
+	static constexpr float normal_still_thr = 0.25; // normal still threshold
+	float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
 	static constexpr float accel_err_thr = 5.0f;    // set accel error threshold to 5m/s^2
-	static constexpr hrt_abstime still_time = 500000;
+	const hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us
 
 	/* set timeout to 90s */
 	static constexpr hrt_abstime timeout = 90_s;
@@ -218,7 +219,7 @@ const char *detect_orientation_str(enum detect_orientation_return orientation)
 
 calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 		bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker,
-		void *worker_data)
+		void *worker_data, bool lenient_still_position)
 {
 	const hrt_abstime calibration_started = hrt_absolute_time();
 	calibrate_return result = calibrate_return_ok;
@@ -267,7 +268,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,
 		px4_usleep(20000);
 		calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side");
 		px4_usleep(20000);
-		enum detect_orientation_return orient = detect_orientation(mavlink_log_pub);
+		enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, lenient_still_position);
 
 		if (orient == ORIENTATION_ERROR) {
 			orientation_failures++;

src/modules/commander/calibration_routines.h

@@ -54,7 +54,8 @@ static constexpr unsigned detect_orientation_side_count = 6;
 /// Wait for vehicle to become still and detect it's orientation
 ///	@return Returns detect_orientation_return according to orientation when vehicle
 ///		and ready for measurements
-enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub);
+enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub,	///< uORB handle to write output to
+		bool lenient_still_detection);	///< true: Use more lenient still position detection
 
 /// Returns the human readable string representation of the orientation
 ///	@param orientation Orientation to return string for, "error" if buffer is too small
@@ -66,15 +67,17 @@ enum calibrate_return {
 	calibrate_return_cancelled
 };
 
-typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return orientation,
-		void *worker_data);
+typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return
+		orientation,	///< Orientation which was detected
+		void *worker_data);	///< Opaque worker data
 
 /// Perform calibration sequence which require a rest orientation detection prior to calibration.
 ///	@return OK: Calibration succeeded, ERROR: Calibration failed
 calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub,	///< uORB handle to write output to
 		bool side_data_collected[detect_orientation_side_count],	///< Sides for which data still needs calibration
 		calibration_from_orientation_worker_t calibration_worker,	///< Worker routine which performs the actual calibration
-		void *worker_data);						///< Opaque data passed to worker routine
+		void *worker_data,						///< Opaque data passed to worker routine
+		bool lenient_still_detection);					///< true: Use more lenient still position detection
 
 /// Used to periodically check for a cancel command
 bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started);

src/modules/commander/lm_fit.cpp

@@ -41,43 +41,43 @@ struct iteration_result {
 	} result = STATUS::SUCCESS;
 };
 
-static void lm_sphere_fit_iteration(const matrix::Vector3f data[], unsigned samples_collected, sphere_params &p,
-				    iteration_result &result)
+void lm_sphere_fit_iteration(const float x[], const float y[], const float z[],
+			     unsigned int samples_collected, sphere_params &params, iteration_result &result)
 {
 	// Run Sphere Fit using Levenberg Marquardt LSq Fit
 	const float lma_damping = 10.f;
+	float fitness = result.cost;
 	float fit1 = 0.f;
 	float fit2 = 0.f;
 
 	matrix::SquareMatrix<float, 4> JTJ;
 	float JTFI[4] {};
+	float residual = 0.0f;
 
 	// Gauss Newton Part common for all kind of extensions including LM
-	for (unsigned k = 0; k < samples_collected; k++) {
-		float x = data[k](0);
-		float y = data[k](1);
-		float z = data[k](2);
-
-		// Calculate Jacobian
-		float A = (p.diag(0)    * (x - p.offset(0))) + (p.offdiag(0) * (y - p.offset(1))) + (p.offdiag(1) * (z - p.offset(2)));
-		float B = (p.offdiag(0) * (x - p.offset(0))) + (p.diag(1)    * (y - p.offset(1))) + (p.offdiag(2) * (z - p.offset(2)));
-		float C = (p.offdiag(1) * (x - p.offset(0))) + (p.offdiag(2) * (y - p.offset(1))) + (p.diag(2)    * (z - p.offset(2)));
-
-		float length = sqrtf(A * A + B * B + C * C);
+	for (uint16_t k = 0; k < samples_collected; k++) {
 
 		float sphere_jacob[4];
+		//Calculate Jacobian
+		float A = (params.diag(0)    * (x[k] - params.offset(0))) + (params.offdiag(0) * (y[k] - params.offset(1))) +
+			  (params.offdiag(1) * (z[k] - params.offset(2)));
+		float B = (params.offdiag(0) * (x[k] - params.offset(0))) + (params.diag(1)    * (y[k] - params.offset(1))) +
+			  (params.offdiag(2) * (z[k] - params.offset(2)));
+		float C = (params.offdiag(1) * (x[k] - params.offset(0))) + (params.offdiag(2) * (y[k] - params.offset(1))) +
+			  (params.diag(2)    * (z[k] - params.offset(2)));
+		float length = sqrtf(A * A + B * B + C * C);
+
 		// 0: partial derivative (radius wrt fitness fn) fn operated on sample
-		sphere_jacob[0] = 1.f;
+		sphere_jacob[0] = 1.0f;
 		// 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample
-		sphere_jacob[1] = 1.f * (((p.diag(0)    * A) + (p.offdiag(0) * B) + (p.offdiag(1) * C)) / length);
-		sphere_jacob[2] = 1.f * (((p.offdiag(0) * A) + (p.diag(1)    * B) + (p.offdiag(2) * C)) / length);
-		sphere_jacob[3] = 1.f * (((p.offdiag(1) * A) + (p.offdiag(2) * B) + (p.diag(2)    * C)) / length);
+		sphere_jacob[1] = 1.0f * (((params.diag(0)    * A) + (params.offdiag(0) * B) + (params.offdiag(1) * C)) / length);
+		sphere_jacob[2] = 1.0f * (((params.offdiag(0) * A) + (params.diag(1)    * B) + (params.offdiag(2) * C)) / length);
+		sphere_jacob[3] = 1.0f * (((params.offdiag(1) * A) + (params.offdiag(2) * B) + (params.diag(2)    * C)) / length);
+		residual = params.radius - length;
 
-		float residual = p.radius - length;
-
-		for (int i = 0; i < 4; i++) {
+		for (uint8_t i = 0; i < 4; i++) {
 			// compute JTJ
-			for (int j = 0; j < 4; j++) {
+			for (uint8_t j = 0; j < 4; j++) {
 				JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j];
 			}
 
@@ -87,12 +87,12 @@ static void lm_sphere_fit_iteration(const matrix::Vector3f data[], unsigned samp
 
 
 	//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
-	// refer: https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
-	float fit1_p[4] {p.radius, p.offset(0), p.offset(1), p.offset(2)};
-	float fit2_p[4] {p.radius, p.offset(0), p.offset(1), p.offset(2)};
-
+	// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
+	float fit1_params[4] = {params.radius, params.offset(0), params.offset(1), params.offset(2)};
+	float fit2_params[4];
+	memcpy(fit2_params, fit1_params, sizeof(fit1_params));
 	JTJ = (JTJ + JTJ.transpose()) * 0.5f; // fix numerical issues
-	matrix::SquareMatrix<float, 4> JTJ2{JTJ};
+	matrix::SquareMatrix<float, 4> JTJ2 = JTJ;
 
 	for (uint8_t i = 0; i < 4; i++) {
 		JTJ(i, i) += result.gradient_damping;
@@ -107,39 +107,42 @@ static void lm_sphere_fit_iteration(const matrix::Vector3f data[], unsigned samp
 	if (!JTJ2.I(JTJ2)) {
 		result.result = iteration_result::STATUS::FAILURE;
 		return;
+
 	}
 
-	for (int row = 0; row < 4; row++) {
-		for (int col = 0; col < 4; col++) {
-			fit1_p[row] -= JTFI[col] * JTJ(row, col);
-			fit2_p[row] -= JTFI[col] * JTJ2(row, col);
+	for (uint8_t row = 0; row < 4; row++) {
+		for (uint8_t col = 0; col < 4; col++) {
+			fit1_params[row] -= JTFI[col] * JTJ(row, col);
+			fit2_params[row] -= JTFI[col] * JTJ2(row, col);
 		}
 	}
 
 	// Calculate mean squared residuals
-	for (unsigned k = 0; k < samples_collected; k++) {
-		float x = data[k](0);
-		float y = data[k](1);
-		float z = data[k](2);
-
-		float A = (p.diag(0)    * (x - fit1_p[1])) + (p.offdiag(0) * (y - fit1_p[2])) + (p.offdiag(1) * (z + fit1_p[3]));
-		float B = (p.offdiag(0) * (x - fit1_p[1])) + (p.diag(1)    * (y - fit1_p[2])) + (p.offdiag(2) * (z + fit1_p[3]));
-		float C = (p.offdiag(1) * (x - fit1_p[1])) + (p.offdiag(2) * (y - fit1_p[2])) + (p.diag(2)    * (z - fit1_p[3]));
-
+	for (uint16_t k = 0; k < samples_collected; k++) {
+		float A = (params.diag(0)    * (x[k] - fit1_params[1])) + (params.offdiag(0) * (y[k] - fit1_params[2])) +
+			  (params.offdiag(1) *
+			   (z[k] + fit1_params[3]));
+		float B = (params.offdiag(0) * (x[k] - fit1_params[1])) + (params.diag(1)    * (y[k] - fit1_params[2])) +
+			  (params.offdiag(2) *
+			   (z[k] + fit1_params[3]));
+		float C = (params.offdiag(1) * (x[k] - fit1_params[1])) + (params.offdiag(2) * (y[k] - fit1_params[2])) + (params.diag(
+					2)    *
+				(z[k] - fit1_params[3]));
 		float length = sqrtf(A * A + B * B + C * C);
-		float residual = fit1_p[0] - length;
+		residual = fit1_params[0] - length;
 		fit1 += residual * residual;
 
-		A = (p.diag(0)    * (x - fit2_p[1])) + (p.offdiag(0) * (y - fit2_p[2])) + (p.offdiag(1) * (z - fit2_p[3]));
-		B = (p.offdiag(0) * (x - fit2_p[1])) + (p.diag(1)    * (y - fit2_p[2])) + (p.offdiag(2) * (z - fit2_p[3]));
-		C = (p.offdiag(1) * (x - fit2_p[1])) + (p.offdiag(2) * (y - fit2_p[2])) + (p.diag(2)    * (z - fit2_p[3]));
+		A = (params.diag(0)    * (x[k] - fit2_params[1])) + (params.offdiag(0) * (y[k] - fit2_params[2])) + (params.offdiag(1) *
+				(z[k] - fit2_params[3]));
+		B = (params.offdiag(0) * (x[k] - fit2_params[1])) + (params.diag(1)    * (y[k] - fit2_params[2])) + (params.offdiag(2) *
+				(z[k] - fit2_params[3]));
+		C = (params.offdiag(1) * (x[k] - fit2_params[1])) + (params.offdiag(2) * (y[k] - fit2_params[2])) + (params.diag(2)    *
+				(z[k] - fit2_params[3]));
 		length = sqrtf(A * A + B * B + C * C);
-		residual = fit2_p[0] - length;
+		residual = fit2_params[0] - length;
 		fit2 += residual * residual;
 	}
 
-	float fitness = result.cost;
-
 	fit1 = sqrtf(fit1) / samples_collected;
 	fit2 = sqrtf(fit2) / samples_collected;
 
@@ -148,7 +151,7 @@ static void lm_sphere_fit_iteration(const matrix::Vector3f data[], unsigned samp
 
 	} else if (fit2 < result.cost && fit2 < fit1) {
 		result.gradient_damping /= lma_damping;
-		memcpy(fit1_p, fit2_p, sizeof(fit1_p));
+		memcpy(fit1_params, fit2_params, sizeof(fit1_params));
 		fitness = fit2;
 
 	} else if (fit1 < result.cost) {
@@ -156,15 +159,13 @@ static void lm_sphere_fit_iteration(const matrix::Vector3f data[], unsigned samp
 	}
 
 	//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
+
 	if (PX4_ISFINITE(fitness) && fitness <= result.cost) {
 		result.cost = fitness;
-
-		p.radius    = fit1_p[0];
-
-		p.offset(0) = fit1_p[1];
-		p.offset(1) = fit1_p[2];
-		p.offset(2) = fit1_p[3];
-
+		params.radius = fit1_params[0];
+		params.offset(0) = fit1_params[1];
+		params.offset(1) = fit1_params[2];
+		params.offset(2) = fit1_params[3];
 		result.result = iteration_result::STATUS::SUCCESS;
 
 	} else {
@@ -172,49 +173,49 @@ static void lm_sphere_fit_iteration(const matrix::Vector3f data[], unsigned samp
 	}
 }
 
-static void lm_ellipsoid_fit_iteration(const matrix::Vector3f data[], unsigned samples_collected, sphere_params &p,
-				       iteration_result &result)
+void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[],
+				unsigned int samples_collected, sphere_params &params, iteration_result &result)
 {
 	// Run Sphere Fit using Levenberg Marquardt LSq Fit
-	const float lma_damping = 10.f;
+	const float lma_damping = 10.0f;
+	float fitness = result.cost;
+	float fit1 = 0.0f;
+	float fit2 = 0.0f;
 
-	float fit1 = 0.f;
-	float fit2 = 0.f;
-
-	matrix::SquareMatrix<float, 9> JTJ{};
+	matrix::SquareMatrix<float, 9> JTJ;
 	float JTFI[9] {};
+	float residual = 0.0f;
+	float ellipsoid_jacob[9];
 
 	// Gauss Newton Part common for all kind of extensions including LM
-	for (unsigned k = 0; k < samples_collected; k++) {
-		float x = data[k](0);
-		float y = data[k](1);
-		float z = data[k](2);
+	for (uint16_t k = 0; k < samples_collected; k++) {
 
 		// Calculate Jacobian
-		float A = (p.diag(0)    * (x - p.offset(0))) + (p.offdiag(0) * (y - p.offset(1))) + (p.offdiag(1) * (z - p.offset(2)));
-		float B = (p.offdiag(0) * (x - p.offset(0))) + (p.diag(1)    * (y - p.offset(1))) + (p.offdiag(2) * (z - p.offset(2)));
-		float C = (p.offdiag(1) * (x - p.offset(0))) + (p.offdiag(2) * (y - p.offset(1))) + (p.diag(2)    * (z - p.offset(2)));
+		float A = (params.diag(0)    * (x[k] - params.offset(0))) + (params.offdiag(0) * (y[k] - params.offset(1))) +
+			  (params.offdiag(1) * (z[k] - params.offset(2)));
+		float B = (params.offdiag(0) * (x[k] - params.offset(0))) + (params.diag(1)    * (y[k] - params.offset(1))) +
+			  (params.offdiag(2) * (z[k] - params.offset(2)));
+		float C = (params.offdiag(1) * (x[k] - params.offset(0))) + (params.offdiag(2) * (y[k] - params.offset(1))) +
+			  (params.diag(2)    * (z[k] - params.offset(2)));
 		float length = sqrtf(A * A + B * B + C * C);
-		float residual = p.radius - length;
+		residual = params.radius - length;
 		fit1 += residual * residual;
-
-		float ellipsoid_jacob[9];
 		// 0-2: partial derivative (offset wrt fitness fn) fn operated on sample
-		ellipsoid_jacob[0] =  1.f * (((p.diag(0)    * A) + (p.offdiag(0) * B) + (p.offdiag(1) * C)) / length);
-		ellipsoid_jacob[1] =  1.f * (((p.offdiag(0) * A) + (p.diag(1)    * B) + (p.offdiag(2) * C)) / length);
-		ellipsoid_jacob[2] =  1.f * (((p.offdiag(1) * A) + (p.offdiag(2) * B) + (p.diag(2)    * C)) / length);
+		ellipsoid_jacob[0] = 1.0f * (((params.diag(0)    * A) + (params.offdiag(0) * B) + (params.offdiag(1) * C)) / length);
+		ellipsoid_jacob[1] = 1.0f * (((params.offdiag(0) * A) + (params.diag(1)    * B) + (params.offdiag(2) * C)) / length);
+		ellipsoid_jacob[2] = 1.0f * (((params.offdiag(1) * A) + (params.offdiag(2) * B) + (params.diag(2)    * C)) / length);
 		// 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample
-		ellipsoid_jacob[3] = -1.f * ((x - p.offset(0)) * A) / length;
-		ellipsoid_jacob[4] = -1.f * ((y - p.offset(1)) * B) / length;
-		ellipsoid_jacob[5] = -1.f * ((z - p.offset(2)) * C) / length;
+		ellipsoid_jacob[3] = -1.0f * ((x[k] - params.offset(0)) * A) / length;
+		ellipsoid_jacob[4] = -1.0f * ((y[k] - params.offset(1)) * B) / length;
+		ellipsoid_jacob[5] = -1.0f * ((z[k] - params.offset(2)) * C) / length;
 		// 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample
-		ellipsoid_jacob[6] = -1.f * (((y - p.offset(1)) * A) + ((x - p.offset(0)) * B)) / length;
-		ellipsoid_jacob[7] = -1.f * (((z - p.offset(2)) * A) + ((x - p.offset(0)) * C)) / length;
-		ellipsoid_jacob[8] = -1.f * (((z - p.offset(2)) * B) + ((y - p.offset(1)) * C)) / length;
+		ellipsoid_jacob[6] = -1.0f * (((y[k] - params.offset(1)) * A) + ((x[k] - params.offset(0)) * B)) / length;
+		ellipsoid_jacob[7] = -1.0f * (((z[k] - params.offset(2)) * A) + ((x[k] - params.offset(0)) * C)) / length;
+		ellipsoid_jacob[8] = -1.0f * (((z[k] - params.offset(2)) * B) + ((y[k] - params.offset(1)) * C)) / length;
 
-		for (int i = 0; i < 9; i++) {
+		for (uint8_t i = 0; i < 9; i++) {
 			// compute JTJ
-			for (int j = 0; j < 9; j++) {
+			for (uint8_t j = 0; j < 9; j++) {
 				JTJ(i, j) += ellipsoid_jacob[i] * ellipsoid_jacob[j];
 			}
 
@@ -225,13 +226,12 @@ static void lm_ellipsoid_fit_iteration(const matrix::Vector3f data[], unsigned s
 
 	//------------------------Levenberg-Marquardt-part-starts-here---------------------------------//
 	// refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter
-	float fit1_p[9] {p.offset(0), p.offset(1), p.offset(2), p.diag(0), p.diag(1), p.diag(2), p.offdiag(0), p.offdiag(1), p.offdiag(2)};
-	float fit2_p[9];
-	memcpy(fit2_p, fit1_p, sizeof(fit1_p));
+	float fit1_params[9] = {params.offset(0), params.offset(1), params.offset(2), params.diag(0), params.diag(1), params.diag(2), params.offdiag(0), params.offdiag(1), params.offdiag(2)};
+	float fit2_params[9];
+	memcpy(fit2_params, fit1_params, sizeof(fit1_params));
+	matrix::SquareMatrix<float, 9> JTJ2 = JTJ;
 
-	matrix::SquareMatrix<float, 9> JTJ2{JTJ};
-
-	for (int i = 0; i < 9; i++) {
+	for (uint8_t i = 0; i < 9; i++) {
 		JTJ(i, i) += result.gradient_damping;
 		JTJ2(i, i) += result.gradient_damping / lma_damping;
 	}
@@ -247,36 +247,36 @@ static void lm_ellipsoid_fit_iteration(const matrix::Vector3f data[], unsigned s
 		return;
 	}
 
-	for (int row = 0; row < 9; row++) {
-		for (int col = 0; col < 9; col++) {
-			fit1_p[row] -= JTFI[col] * JTJ(row, col);
-			fit2_p[row] -= JTFI[col] * JTJ2(row, col);
+	for (uint8_t row = 0; row < 9; row++) {
+		for (uint8_t col = 0; col < 9; col++) {
+			fit1_params[row] -= JTFI[col] * JTJ(row, col);
+			fit2_params[row] -= JTFI[col] * JTJ2(row, col);
 		}
 	}
 
 	// Calculate mean squared residuals
-	for (unsigned k = 0; k < samples_collected; k++) {
-		float x = data[k](0);
-		float y = data[k](1);
-		float z = data[k](2);
-
-		float A = (fit1_p[3] * (x - fit1_p[0])) + (fit1_p[6] * (y - fit1_p[1])) + (fit1_p[7] * (z - fit1_p[2]));
-		float B = (fit1_p[6] * (x - fit1_p[0])) + (fit1_p[4] * (y - fit1_p[1])) + (fit1_p[8] * (z - fit1_p[2]));
-		float C = (fit1_p[7] * (x - fit1_p[0])) + (fit1_p[8] * (y - fit1_p[1])) + (fit1_p[5] * (z - fit1_p[2]));
+	for (uint16_t k = 0; k < samples_collected; k++) {
+		float A = (fit1_params[3]    * (x[k] - fit1_params[0])) + (fit1_params[6] * (y[k] - fit1_params[1])) + (fit1_params[7] *
+				(z[k] - fit1_params[2]));
+		float B = (fit1_params[6] * (x[k] - fit1_params[0])) + (fit1_params[4]   * (y[k] - fit1_params[1])) + (fit1_params[8] *
+				(z[k] - fit1_params[2]));
+		float C = (fit1_params[7] * (x[k] - fit1_params[0])) + (fit1_params[8] * (y[k] - fit1_params[1])) + (fit1_params[5]    *
+				(z[k] - fit1_params[2]));
 		float length = sqrtf(A * A + B * B + C * C);
-		float residual = p.radius - length;
+		residual = params.radius - length;
 		fit1 += residual * residual;
 
-		A = (fit2_p[3] * (x - fit2_p[0])) + (fit2_p[6] * (y - fit2_p[1])) + (fit2_p[7] * (z - fit2_p[2]));
-		B = (fit2_p[6] * (x - fit2_p[0])) + (fit2_p[4] * (y - fit2_p[1])) + (fit2_p[8] * (z - fit2_p[2]));
-		C = (fit2_p[7] * (x - fit2_p[0])) + (fit2_p[8] * (y - fit2_p[1])) + (fit2_p[5] * (z - fit2_p[2]));
+		A = (fit2_params[3]    * (x[k] - fit2_params[0])) + (fit2_params[6] * (y[k] - fit2_params[1])) + (fit2_params[7] *
+				(z[k] - fit2_params[2]));
+		B = (fit2_params[6] * (x[k] - fit2_params[0])) + (fit2_params[4]   * (y[k] - fit2_params[1])) + (fit2_params[8] *
+				(z[k] - fit2_params[2]));
+		C = (fit2_params[7] * (x[k] - fit2_params[0])) + (fit2_params[8] * (y[k] - fit2_params[1])) + (fit2_params[5]    *
+				(z[k] - fit2_params[2]));
 		length = sqrtf(A * A + B * B + C * C);
-		residual = p.radius - length;
+		residual = params.radius - length;
 		fit2 += residual * residual;
 	}
 
-	float fitness = result.cost;
-
 	fit1 = sqrtf(fit1) / samples_collected;
 	fit2 = sqrtf(fit2) / samples_collected;
 
@@ -285,7 +285,7 @@ static void lm_ellipsoid_fit_iteration(const matrix::Vector3f data[], unsigned s
 
 	} else if (fit2 < result.cost && fit2 < fit1) {
 		result.gradient_damping /= lma_damping;
-		memcpy(fit1_p, fit2_p, sizeof(fit1_p));
+		memcpy(fit1_params, fit2_params, sizeof(fit1_params));
 		fitness = fit2;
 
 	} else if (fit1 < result.cost) {
@@ -295,17 +295,15 @@ static void lm_ellipsoid_fit_iteration(const matrix::Vector3f data[], unsigned s
 	//--------------------Levenberg-Marquardt-part-ends-here--------------------------------//
 	if (PX4_ISFINITE(fitness) && fitness <= result.cost) {
 		result.cost = fitness;
-
-		p.offset(0)  = fit1_p[0];
-		p.offset(1)  = fit1_p[1];
-		p.offset(2)  = fit1_p[2];
-		p.diag(0)    = fit1_p[3];
-		p.diag(1)    = fit1_p[4];
-		p.diag(2)    = fit1_p[5];
-		p.offdiag(0) = fit1_p[6];
-		p.offdiag(1) = fit1_p[7];
-		p.offdiag(2) = fit1_p[8];
-
+		params.offset(0) = fit1_params[0];
+		params.offset(1) = fit1_params[1];
+		params.offset(2) = fit1_params[2];
+		params.diag(0) = fit1_params[3];
+		params.diag(1) = fit1_params[4];
+		params.diag(2) = fit1_params[5];
+		params.offdiag(0) = fit1_params[6];
+		params.offdiag(1) = fit1_params[7];
+		params.offdiag(2) = fit1_params[8];
 		result.result = iteration_result::STATUS::SUCCESS;
 
 	} else {
@@ -313,13 +311,19 @@ static void lm_ellipsoid_fit_iteration(const matrix::Vector3f data[], unsigned s
 	}
 }
 
-int lm_fit(const matrix::Vector3f data[], unsigned samples_collected, sphere_params &params, bool full_ellipsoid)
-{
-	static constexpr int min_iterations = 10;
-	static constexpr int max_iterations = 100;
 
-	static constexpr float cost_threshold = 0.02f;
-	static constexpr float step_threshold = 0.001f;
+
+int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params &params,
+	       bool full_ellipsoid)
+{
+
+	const int max_iterations = 100;
+	const int min_iterations = 10;
+	const float cost_threshold = 0.01;
+	const float step_threshold = 0.001;
+
+	const float min_radius = 0.2;
+	const float max_radius = 0.7;
 
 	iteration_result iter;
 	iter.cost = 1e30f;
@@ -329,27 +333,15 @@ int lm_fit(const matrix::Vector3f data[], unsigned samples_collected, sphere_par
 
 	for (int i = 0; i < max_iterations; i++) {
 		if (full_ellipsoid) {
-			lm_ellipsoid_fit_iteration(data, samples_collected, params, iter);
-
-			PX4_INFO("[%d] O:[%.5f, %.5f, %.5f], S:[%.3f, %.3f, %.3f], Result: %d, Cost: %.6f, Damping: %.6f\n",
-				 i,
-				 (double)params.offset(0), (double)params.offset(1), (double)params.offset(2),
-				 (double)params.diag(0), (double)params.diag(1), (double)params.diag(2),
-				 iter.result == iteration_result::STATUS::SUCCESS, (double)iter.cost, (double)iter.gradient_damping);
+			lm_ellipsoid_fit_iteration(x, y, z, samples_collected, params, iter);
 
 		} else {
-			lm_sphere_fit_iteration(data, samples_collected, params, iter);
-
-			PX4_INFO("[%d] O:[%.5f, %.5f, %.5f], Result: %d, Cost: %.6f, Damping: %.6f, Radius: %.4f\n",
-				 i,
-				 (double)params.offset(0), (double)params.offset(1), (double)params.offset(2),
-				 iter.result == iteration_result::STATUS::SUCCESS, (double)iter.cost, (double)iter.gradient_damping,
-				 (double)params.radius);
+			lm_sphere_fit_iteration(x, y, z, samples_collected, params, iter);
 		}
 
 		if (iter.result == iteration_result::STATUS::SUCCESS
+		    && min_radius < params.radius && params.radius < max_radius
 		    && i > min_iterations && (iter.cost < cost_threshold || iter.gradient_damping < step_threshold)) {
-
 			success = true;
 			break;
 		}

src/modules/commander/lm_fit.hpp

@@ -37,9 +37,7 @@
 #include <px4_platform_common/defines.h>
 
 struct sphere_params {
-	matrix::Vector3f offset{0.f, 0.f, 0.f};
-	matrix::Vector3f diag{1.f, 1.f, 1.f};
-	matrix::Vector3f offdiag{0.f, 0.f, 0.f};
+	matrix::Vector3f offset, diag{1.f, 1.f, 1.f}, offdiag;
 	float radius{0.2f};
 };
 
@@ -49,7 +47,9 @@ struct sphere_params {
  *
  * Fits a sphere to a set of points on the sphere surface.
  *
- * @param data x,y,z point coordinates
+ * @param x point coordinates on the X axis
+ * @param y point coordinates on the Y axis
+ * @param z point coordinates on the Z axis
  * @param samples_collected number of points
  * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100.
  * @param params the values to be optimized
@@ -59,4 +59,5 @@ struct sphere_params {
  *
  * @return 0 on success, 1 on failure
  */
-int lm_fit(const matrix::Vector3f data[], unsigned samples_collected, sphere_params &params, bool full_ellipsoid);
+int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params &params,
+	       bool full_ellipsoid);

src/modules/commander/mag_calibration.cpp

@@ -90,7 +90,9 @@ struct mag_worker_data_t {
 	uint64_t	calibration_interval_perside_us;
 	unsigned int	calibration_counter_total[MAX_MAGS];
 
-	matrix::Vector3f *data[MAX_MAGS] {nullptr, nullptr, nullptr, nullptr};
+	float		*x[MAX_MAGS];
+	float		*y[MAX_MAGS];
+	float		*z[MAX_MAGS];
 
 	float		temperature[MAX_MAGS] {NAN, NAN, NAN, NAN};
 
@@ -139,20 +141,19 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub)
 	return result;
 }
 
-static bool reject_sample(Vector3f s, Vector3f data[], unsigned count,
+static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count,
 			  unsigned max_count, float mag_sphere_radius)
 {
 	float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
 
 	for (size_t i = 0; i < count; i++) {
-		float dx = s(0) - data[i](0);
-		float dy = s(1) - data[i](1);
-		float dz = s(2) - data[i](2);
+		float dx = sx - x[i];
+		float dy = sy - y[i];
+		float dz = sz - z[i];
 		float dist = sqrtf(dx * dx + dy * dy + dz * dz);
 
 		if (dist < min_sample_dist) {
-			PX4_DEBUG("rejected X: %.3f Y: %.3f Z: %.3f (%.3f < %.3f) (%u/%u) ", (double)s(0), (double)s(1), (double)s(2),
-				  (double)dist,
+			PX4_DEBUG("rejected X: %.3f Y: %.3f Z: %.3f (%.3f < %.3f) (%u/%u) ", (double)sx, (double)sy, (double)sz, (double)dist,
 				  (double)min_sample_dist, count, max_count);
 
 			return true;
@@ -362,7 +363,8 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 						}
 
 						// Check if this measurement is good to go in
-						bool reject = reject_sample(Vector3f{mag.x, mag.y, mag.z}, worker_data->data[cur_mag],
+						bool reject = reject_sample(mag.x, mag.y, mag.z,
+									    worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
 									    worker_data->calibration_counter_total[cur_mag],
 									    worker_data->calibration_sides * worker_data->calibration_points_perside,
 									    mag_sphere_radius);
@@ -386,9 +388,9 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 			if (!rejected) {
 				for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
 					if (worker_data->calibration[cur_mag].device_id() != 0) {
-
-						worker_data->data[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag];
-						worker_data->calibration_counter_total[cur_mag]++;
+						worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](0);
+						worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](1);
+						worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](2);
 
 						if (!PX4_ISFINITE(worker_data->temperature[cur_mag])) {
 							// set first valid value
@@ -397,11 +399,13 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 						} else {
 							worker_data->temperature[cur_mag] = 0.5f * (worker_data->temperature[cur_mag] + new_temperature[cur_mag]);
 						}
+
+						worker_data->calibration_counter_total[cur_mag]++;
 					}
 				}
 
 				hrt_abstime now = hrt_absolute_time();
-				mag_worker_data_s status{};
+				mag_worker_data_s status;
 				status.timestamp = now;
 				status.timestamp_sample = now;
 				status.done_count = worker_data->done_count;
@@ -414,9 +418,9 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 
 					if (worker_data->calibration[cur_mag].device_id() != 0) {
 						const unsigned int sample = worker_data->calibration_counter_total[cur_mag] - 1;
-						status.x[cur_mag] = worker_data->data[cur_mag][sample](0);
-						status.y[cur_mag] = worker_data->data[cur_mag][sample](1);
-						status.z[cur_mag] = worker_data->data[cur_mag][sample](2);
+						status.x[cur_mag] = worker_data->x[cur_mag][sample];
+						status.y[cur_mag] = worker_data->y[cur_mag][sample];
+						status.z[cur_mag] = worker_data->z[cur_mag][sample];
 
 					} else {
 						status.x[cur_mag] = 0.f;
@@ -523,6 +527,9 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 
 	for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
 		// Initialize to no memory allocated
+		worker_data.x[cur_mag] = nullptr;
+		worker_data.y[cur_mag] = nullptr;
+		worker_data.z[cur_mag] = nullptr;
 		worker_data.calibration_counter_total[cur_mag] = 0;
 	}
 
@@ -540,10 +547,11 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 		worker_data.calibration[cur_mag].set_calibration_index(cur_mag);
 
 		if (worker_data.calibration[cur_mag].device_id() != 0) {
+			worker_data.x[cur_mag] = static_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+			worker_data.y[cur_mag] = static_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
+			worker_data.z[cur_mag] = static_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
 
-			worker_data.data[cur_mag] = new matrix::Vector3f[calibration_points_maxcount];
-
-			if (worker_data.data[cur_mag] == nullptr) {
+			if (worker_data.x[cur_mag] == nullptr || worker_data.y[cur_mag] == nullptr || worker_data.z[cur_mag] == nullptr) {
 				calibration_log_critical(mavlink_log_pub, "ERROR: out of memory");
 				result = calibrate_return_error;
 				break;
@@ -558,7 +566,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 		result = calibrate_from_orientation(mavlink_log_pub,                    // uORB handle to write output
 						    worker_data.side_data_collected,    // Sides to calibrate
 						    mag_calibration_worker,             // Calibration worker
-						    &worker_data);			// Opaque data for calibration worked
+						    &worker_data,			// Opaque data for calibration worked
+						    true);				// true: lenient still detection
 	}
 
 	// calibration values for each mag
@@ -585,26 +594,26 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 				// Estimate only the offsets if two-sided calibration is selected, as the problem is not constrained
 				// enough to reliably estimate both scales and offsets with 2 sides only (even if the existing calibration
 				// is already close)
-				const bool sphere_fit_only = worker_data.calibration_sides <= 2;
+				bool sphere_fit_only = worker_data.calibration_sides <= 2;
 
-				sphere_params sphere_data{};
+				sphere_params sphere_data;
 				sphere_data.radius = sphere_radius[cur_mag];
-				sphere_data.offset = sphere[cur_mag];
-				sphere_data.diag = diag[cur_mag];
-				sphere_data.offdiag = offdiag[cur_mag];
+				sphere_data.offset = matrix::Vector3f(sphere[cur_mag](0), sphere[cur_mag](1), sphere[cur_mag](2));
+				sphere_data.diag = matrix::Vector3f(diag[cur_mag](0), diag[cur_mag](1), diag[cur_mag](2));
+				sphere_data.offdiag = matrix::Vector3f(offdiag[cur_mag](0), offdiag[cur_mag](1), offdiag[cur_mag](2));
 
 				bool sphere_fit_success = false;
 				bool ellipsoid_fit_success = false;
-
-				int ret = lm_fit(worker_data.data[cur_mag], worker_data.calibration_counter_total[cur_mag], sphere_data, false);
+				int ret = lm_mag_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
+						     worker_data.calibration_counter_total[cur_mag], sphere_data, false);
 
 				if (ret == PX4_OK) {
 					sphere_fit_success = true;
 					PX4_INFO("Mag: %" PRIu8 " sphere radius: %.4f", cur_mag, (double)sphere_data.radius);
 
 					if (!sphere_fit_only) {
-						int ellipsoid_ret = lm_fit(worker_data.data[cur_mag], worker_data.calibration_counter_total[cur_mag], sphere_data,
-									   true);
+						int ellipsoid_ret = lm_mag_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
+									       worker_data.calibration_counter_total[cur_mag], sphere_data, true);
 
 						if (ellipsoid_ret == PX4_OK) {
 							ellipsoid_fit_success  = true;
@@ -612,14 +621,13 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 					}
 				}
 
-
-				// TODO: verify sphere radius
-
 				sphere_radius[cur_mag] = sphere_data.radius;
 
-				sphere[cur_mag] = sphere_data.offset;
-				diag[cur_mag] = sphere_data.diag;
-				offdiag[cur_mag] = sphere_data.offdiag;
+				for (int i = 0; i < 3; i++) {
+					sphere[cur_mag](i) = sphere_data.offset(i);
+					diag[cur_mag](i) = sphere_data.diag(i);
+					offdiag[cur_mag](i) = sphere_data.offdiag(i);
+				}
 
 				if (!sphere_fit_success && !ellipsoid_fit_success) {
 					calibration_log_emergency(mavlink_log_pub, "Retry calibration (unable to fit mag %" PRIu8 ")", cur_mag);
@@ -720,7 +728,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 
 						for (unsigned i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
 
-							const Vector3f sample{worker_data.data[cur_mag][i]};
+							const Vector3f sample{worker_data.x[cur_mag][i], worker_data.y[cur_mag][i], worker_data.z[cur_mag][i]};
 
 							// apply calibration
 							Vector3f m{scale *(sample - offset)};
@@ -731,7 +739,9 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 							}
 
 							// store back in worker_data
-							worker_data.data[cur_mag][i] = m;
+							worker_data.x[cur_mag][i] = m(0);
+							worker_data.y[cur_mag][i] = m(1);
+							worker_data.z[cur_mag][i] = m(2);
 						}
 					}
 				}
@@ -765,8 +775,14 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 								float diff_sum = 0.f;
 
 								for (int i = 0; i < last_sample_index; i++) {
-									const Vector3f &m{worker_data.data[cur_mag][i]};
-									diff_sum += Vector3f((get_rot_matrix((enum Rotation)r) * m) - m).norm_squared();
+									float x = worker_data.x[cur_mag][i];
+									float y = worker_data.y[cur_mag][i];
+									float z = worker_data.z[cur_mag][i];
+									rotate_3f((enum Rotation)r, x, y, z);
+
+									Vector3f diff = Vector3f{x, y, z} - Vector3f{worker_data.x[internal_index][i], worker_data.y[internal_index][i], worker_data.z[internal_index][i]};
+
+									diff_sum += diff.norm_squared();
 								}
 
 								// compute mean squared error
@@ -862,7 +878,9 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 
 	// Data points are no longer needed
 	for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
-		delete[] worker_data.data[cur_mag];
+		free(worker_data.x[cur_mag]);
+		free(worker_data.y[cur_mag]);
+		free(worker_data.z[cur_mag]);
 	}
 
 	FactoryCalibrationStorage factory_storage;

src/modules/commander/mag_calibration_test.cpp

@@ -50,38 +50,38 @@ using matrix::Vector3f;
 class MagCalTest : public ::testing::Test
 {
 public:
-	void generate2SidesMagData(matrix::Vector3f mag_data[], unsigned int n_samples, float mag_str);
+	void generate2SidesMagData(float *x, float *y, float *z, unsigned int n_samples, float mag_str);
 
 	/* Generate regularly spaced data on a sphere
 	 * Ref.: How to generate equidistributed points on the surface of a sphere, Markus Deserno, 2004
 	 */
-	void generateRegularData(matrix::Vector3f mag_data[], unsigned int n_samples, float mag_str);
+	void generateRegularData(float *x, float *y, float *z, unsigned int n_samples, float mag_str);
 
-	void modifyOffsetScale(matrix::Vector3f mag_data[], unsigned int n_samples, Vector3f offsets, Vector3f scale_factors);
+	void modifyOffsetScale(float *x, float *y, float *z, unsigned int n_samples, Vector3f offsets, Vector3f scale_factors);
 };
 
-void MagCalTest::generate2SidesMagData(matrix::Vector3f data[], unsigned int n_samples, float mag_str)
+void MagCalTest::generate2SidesMagData(float *x, float *y, float *z, unsigned int n_samples, float mag_str)
 {
 	float psi = 0.f;
 	float theta = 0.f;
 	const float d_angle = 2.f * M_PI_F / float(n_samples / 2);
 
 	for (int i = 0; i < int(n_samples / 2); i++) {
-		data[i](0) = mag_str * sinf(psi);
-		data[i](1) = mag_str * cosf(psi);
-		data[i](2) = 0.f;
+		x[i] = mag_str * sinf(psi);
+		y[i] = mag_str * cosf(psi);
+		z[i] = 0.f;
 		psi += d_angle;
 	}
 
 	for (int i = int(n_samples / 2); i < int(n_samples); i++) {
-		data[i](0) = mag_str * sinf(theta);
-		data[i](1) = 0.f;
-		data[i](2) = mag_str * cosf(theta);
+		x[i] = mag_str * sinf(theta);
+		y[i] = 0.f;
+		z[i] = mag_str * cosf(theta);
 		theta += d_angle;
 	}
 }
 
-void MagCalTest::generateRegularData(matrix::Vector3f mag_data[], unsigned int n_samples, float mag_str)
+void MagCalTest::generateRegularData(float *x, float *y, float *z, unsigned int n_samples, float mag_str)
 {
 	const float a = 4.f * M_PI_F * mag_str * mag_str / n_samples;
 	const float d = sqrtf(a);
@@ -97,9 +97,9 @@ void MagCalTest::generateRegularData(matrix::Vector3f mag_data[], unsigned int n
 
 		for (int n = 0; n < m_phi; n++) {
 			const float phi = 2.f * M_PI_F * n / static_cast<float>(m_phi);
-			mag_data[n_count](0) = mag_str * sinf(theta) * cosf(phi);
-			mag_data[n_count](1) = mag_str * sinf(theta) * sinf(phi);
-			mag_data[n_count](2) = mag_str * cosf(theta);
+			x[n_count] = mag_str * sinf(theta) * cosf(phi);
+			y[n_count] = mag_str * sinf(theta) * sinf(phi);
+			z[n_count] = mag_str * cosf(theta);
 			n_count++;
 		}
 	}
@@ -111,16 +111,20 @@ void MagCalTest::generateRegularData(matrix::Vector3f mag_data[], unsigned int n
 
 	// Padd with constant data
 	while (n_count < n_samples) {
-		mag_data[n_count] = mag_data[n_count - 1];
+		x[n_count] = x[n_count - 1];
+		y[n_count] = y[n_count - 1];
+		z[n_count] = z[n_count - 1];
 		n_count++;
 	}
 }
 
-void MagCalTest::modifyOffsetScale(matrix::Vector3f mag_data[], unsigned int n_samples, Vector3f offsets,
+void MagCalTest::modifyOffsetScale(float *x, float *y, float *z, unsigned int n_samples, Vector3f offsets,
 				   Vector3f scale_factors)
 {
 	for (unsigned int k = 0; k < n_samples; k++) {
-		mag_data[k] = mag_data[k].emult(scale_factors) + offsets;
+		x[k] = x[k] * scale_factors(0) + offsets(0);
+		y[k] = y[k] * scale_factors(1) + offsets(1);
+		z[k] = z[k] * scale_factors(2) + offsets(2);
 	}
 }
 
@@ -134,13 +138,17 @@ TEST_F(MagCalTest, sphere2Sides)
 	const Vector3f offset_true;
 	const Vector3f scale_true = {1.f, 1.f, 1.f};
 
-	Vector3f mag_data[N_SAMPLES];
-	generate2SidesMagData(mag_data, N_SAMPLES, mag_str_true);
+	float x[N_SAMPLES];
+	float y[N_SAMPLES];
+	float z[N_SAMPLES];
+
+	generate2SidesMagData(x, y, z, N_SAMPLES, mag_str_true);
 
 	// WHEN: fitting a sphere with the data and given a wrong initial radius
-	sphere_params sphere{};
+	sphere_params sphere;
+	sphere.diag = {1.f, 1.f, 1.f};
 	sphere.radius = 0.2;
-	int success = lm_fit(mag_data, N_SAMPLES, sphere, false);
+	int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false);
 
 	// THEN: the algorithm should converge in a single step
 	EXPECT_EQ(success, PX4_OK);
@@ -163,15 +171,17 @@ TEST_F(MagCalTest, sphereRegularlySpaced)
 	const Vector3f offset_true = {-1.07f, 0.35f, -0.78f};
 	const Vector3f scale_true = {1.f, 1.f, 1.f};
 
-	Vector3f mag_data[N_SAMPLES];
-	generateRegularData(mag_data, N_SAMPLES, mag_str_true);
-	modifyOffsetScale(mag_data, N_SAMPLES, offset_true, scale_true);
+	float x[N_SAMPLES];
+	float y[N_SAMPLES];
+	float z[N_SAMPLES];
+	generateRegularData(x, y, z, N_SAMPLES, mag_str_true);
+	modifyOffsetScale(x, y, z, N_SAMPLES, offset_true, scale_true);
 
 	// WHEN: fitting a sphere to the data
-	sphere_params sphere{};
+	sphere_params sphere;
 	sphere.diag = {1.f, 1.f, 1.f};
 	sphere.radius = 0.2;
-	int success = lm_fit(mag_data, N_SAMPLES, sphere, false);
+	int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false);
 
 	// THEN: the algorithm should converge in a few iterations and
 	// find the correct parameters
@@ -190,7 +200,7 @@ TEST_F(MagCalTest, replayTestData)
 	// GIVEN: a real test dataset with large offsets
 	// and where the two first iterations of the LM algorithm
 	// produces a negative radius and a constant fitness value
-	constexpr unsigned N_SAMPLES = 231;
+	constexpr unsigned int N_SAMPLES = 231;
 
 	const float mag_str_true = 0.4f;
 	const Vector3f offset_true = {-0.18f, 0.05f, -0.58f};
@@ -199,16 +209,7 @@ TEST_F(MagCalTest, replayTestData)
 	sphere_params sphere;
 	sphere.diag = {1.f, 1.f, 1.f};
 	sphere.radius = 0.2;
-
-	matrix::Vector3f mag_data1[231];
-
-	for (unsigned i = 0; i < N_SAMPLES; i++) {
-		mag_data1[i](0) = mag_data1_x[i];
-		mag_data1[i](1) = mag_data1_y[i];
-		mag_data1[i](2) = mag_data1_z[i];
-	}
-
-	int sphere_success = lm_fit(mag_data1, N_SAMPLES, sphere, false);
+	int sphere_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, sphere, false);
 
 	// THEN: the algorithm should converge and find the correct parameters
 	EXPECT_EQ(sphere_success, PX4_OK);
@@ -221,8 +222,8 @@ TEST_F(MagCalTest, replayTestData)
 	sphere_params ellipsoid;
 	ellipsoid.diag = {1.f, 1.f, 1.f};
 	ellipsoid.radius = 0.2;
-	int ellipsoid_step_1_success = lm_fit(mag_data1, N_SAMPLES, ellipsoid, false);
-	int ellipsoid_success = lm_fit(mag_data1, N_SAMPLES, ellipsoid, true);
+	int ellipsoid_step_1_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, ellipsoid, false);
+	int ellipsoid_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, ellipsoid, true);
 	const Vector3f scale_true = {1.f, 1.06f, 0.94f};
 
 	EXPECT_EQ(ellipsoid_step_1_success, PX4_OK);

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

@@ -31,36 +31,51 @@
  *
  ****************************************************************************/
 
-/**
- * Bitfield selecting mag sides for calibration
- *
- * If set to two side calibration, only the offsets are estimated, the scale
- * calibration is left unchanged. Thus an initial six side calibration is
- * recommended.
- *
- * Bits:
- * ORIENTATION_TAIL_DOWN = 1
- * ORIENTATION_NOSE_DOWN = 2
- * ORIENTATION_LEFT = 4
- * ORIENTATION_RIGHT = 8
- * ORIENTATION_UPSIDE_DOWN = 16
- * ORIENTATION_RIGHTSIDE_UP = 32
- *
- * @min 34
- * @max 63
- * @value 34 Two side calibration
- * @value 38 Three side calibration
- * @value 63 Six side calibration
- * @group Sensors
- */
-PARAM_DEFINE_INT32(CAL_ACC_SIDES, 63);
+#ifndef GPS_RTCM_DATA_HPP
+#define GPS_RTCM_DATA_HPP
 
-/**
- * Automatically set external rotations.
- *
- * During calibration attempt to automatically determine the rotation of external magnetometers.
- *
- * @boolean
- * @group Sensors
- */
-PARAM_DEFINE_INT32(CAL_CAL_ROT_AUTO, 1);
+#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());