fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 13:21:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc-atune: use matrix lib to compare against threshold

Browse Source
This commit is contained in:
bresch
2024-01-24 10:03:52 +01:00
parent 8d9b378cab
commit 8535e668a1
2 changed files with 3 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp
+3 -12
View File
@@ -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) {
src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp
-1
View File
@@ -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();