mirror of https://github.com/ArduPilot/ardupilot
72 lines
2.7 KiB
C++
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;
|
|
}
|