You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
71 lines
2.7 KiB
71 lines
2.7 KiB
/* |
|
* 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; |
|
}
|
|
|