From 8535e668a1a85eee5a951f636c3c2af183a0b63a Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
Date: Tue, 18 Jul 2023 13:24:15 +0200
Subject: [PATCH] mc-atune: use matrix lib to compare against threshold

---
 .../mc_autotune_attitude_control.cpp              | 15 +++------------
 .../mc_autotune_attitude_control.hpp              |  1 -
 2 files changed, 3 insertions(+), 13 deletions(-)

diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
index ea370851a1..d1f1d52efc 100644
--- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
+++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
@@ -294,7 +294,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		break;
 
 	case state::roll:
-		if (areAllSmallerThan(_sys_id.getVariances(), converged_thr)
+		if ((_sys_id.getVariances().max() < converged_thr)
 		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(0);
 
@@ -316,7 +316,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		break;
 
 	case state::pitch:
-		if (areAllSmallerThan(_sys_id.getVariances(), converged_thr)
+		if ((_sys_id.getVariances().max() < converged_thr)
 		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(1);
 			_state = state::pitch_pause;
@@ -336,7 +336,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		break;
 
 	case state::yaw:
-		if (areAllSmallerThan(_sys_id.getVariances(), converged_thr)
+		if ((_sys_id.getVariances().max() < converged_thr)
 		    && ((now - _state_start_time) > (_param_mc_at_sysid_time.get() * 1_s))) {
 			copyGains(2);
 			_state = state::yaw_pause;
@@ -486,15 +486,6 @@ bool McAutotuneAttitudeControl::registerActuatorControlsCallback()
 	return true;
 }
 
-bool McAutotuneAttitudeControl::areAllSmallerThan(const Vector<float, 5> &vect, float threshold) const
-{
-	return (vect(0) < threshold)
-	       && (vect(1) < threshold)
-	       && (vect(2) < threshold)
-	       && (vect(3) < threshold)
-	       && (vect(4) < threshold);
-}
-
 void McAutotuneAttitudeControl::copyGains(int index)
 {
 	if (index <= 2) {
diff --git a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp
index 5f1987bfae..8c5c2bb67b 100644
--- a/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp
+++ b/src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp
@@ -94,7 +94,6 @@ private:
 	void updateStateMachine(hrt_abstime now);
 	bool registerActuatorControlsCallback();
 	void stopAutotune();
-	bool areAllSmallerThan(const matrix::Vector<float, 5> &vect, float threshold) const;
 	void copyGains(int index);
 	bool areGainsGood() const;
 	void saveGainsToParams();