ardupilot/libraries/AP_Math/control.cpp

72 lines
2.7 KiB
C++

/*
* control.cpp
* Copyright (C) Leonard Hall 2020
*
* This file is free software: you can redistribute it and/or modify it
* under the terms of the GNU General Public License as published by the
* Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This file is distributed in the hope that it will be useful, but
* WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License along
* with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
* this module provides common controller functions
*/
#include "AP_Math.h"
#include "vector2.h"
#include "vector3.h"
// Proportional controller with piecewise sqrt sections to constrain second derivative
float sqrt_controller(float error, float p, float second_ord_lim, float dt)
{
float correction_rate;
if (is_negative(second_ord_lim) || is_zero(second_ord_lim)) {
// second order limit is zero or negative.
correction_rate = error * p;
} else if (is_zero(p)) {
// P term is zero but we have a second order limit.
if (is_positive(error)) {
correction_rate = safe_sqrt(2.0f * second_ord_lim * (error));
} else if (is_negative(error)) {
correction_rate = -safe_sqrt(2.0f * second_ord_lim * (-error));
} else {
correction_rate = 0.0f;
}
} else {
// Both the P and second order limit have been defined.
float linear_dist = second_ord_lim / sq(p);
if (error > linear_dist) {
correction_rate = safe_sqrt(2.0f * second_ord_lim * (error - (linear_dist / 2.0f)));
} else if (error < -linear_dist) {
correction_rate = -safe_sqrt(2.0f * second_ord_lim * (-error - (linear_dist / 2.0f)));
} else {
correction_rate = error * p;
}
}
if (!is_zero(dt)) {
// this ensures we do not get small oscillations by over shooting the error correction in the last time step.
return constrain_float(correction_rate, -fabsf(error) / dt, fabsf(error) / dt);
} else {
return correction_rate;
}
}
// limit vector to a given length, returns true if vector was limited
bool limit_vector_length(float &vector_x, float &vector_y, float max_length)
{
const float vector_length = norm(vector_x, vector_y);
if ((vector_length > max_length) && is_positive(vector_length)) {
vector_x *= (max_length / vector_length);
vector_y *= (max_length / vector_length);
return true;
}
return false;
}