src/main/navigation/sqrt_controller.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21
  22 #include "platform.h"
  23
  24 #include "common/maths.h"
  25
  26 #include "navigation/sqrt_controller.h"
  27
  28 /*
  29     Square Root Controller calculates the correction based on a proportional controller (kP).
  30     Used only in AutoPilot System for Vertical (Z) control.
  31 */
  32
  33 // Inverse of the sqrt controller. Calculates the input (aka error) to the sqrt_controller required to achieve a given output
  34 static float sqrtControllerInverse(float kp, float derivative_max, float output)
  35 {
  36     if ((derivative_max > 0.0f) && (kp == 0.0f)) {
  37         return (output * output) / (2.0f * derivative_max);
  38     }
  39
  40     if (((derivative_max < 0.0f) ||(derivative_max == 0.0f)) && (kp != 0.0f)) {
  41         return output / kp;
  42     }
  43
  44     if (((derivative_max < 0.0f) || (derivative_max == 0.0f)) && (kp == 0.0f)) {
  45         return 0.0f;
  46     }
  47
  48     // Calculate the velocity at which we switch from calculating the stopping point using a linear function to a sqrt function
  49     const float linear_velocity = derivative_max / kp;
  50
  51     if (fabsf(output) < linear_velocity) {
  52         // If our current velocity is below the cross-over point we use a linear function
  53         return output / kp;
  54     }
  55
  56     const float linear_dist = derivative_max / sq(kp);
  57     const float stopping_dist = (linear_dist * 0.5f) + sq(output) / (2.0f * derivative_max);
  58     return (output > 0.0f) ? stopping_dist : -stopping_dist;
  59 }
  60
  61 // Proportional controller with piecewise sqrt sections to constrain derivative
  62 float sqrtControllerApply(sqrt_controller_t *sqrt_controller_pointer, float target, float measurement, float deltaTime)
  63 {
  64     float correction_rate;
  65
  66     // Calculate distance error
  67     sqrt_controller_pointer->error = target - measurement;
  68
  69     if ((sqrt_controller_pointer->error_min < 0.0f) && (sqrt_controller_pointer->error < sqrt_controller_pointer->error_min)) {
  70         sqrt_controller_pointer->error = sqrt_controller_pointer->error_min;
  71     } else if ((sqrt_controller_pointer->error_max > 0.0f) && (sqrt_controller_pointer->error > sqrt_controller_pointer->error_max)) {
  72         sqrt_controller_pointer->error = sqrt_controller_pointer->error_max;
  73     }
  74
  75     if ((sqrt_controller_pointer->derivative_max < 0.0f) || sqrt_controller_pointer->derivative_max == 0.0f) {
  76         // Derivative Max is zero or negative.
  77         correction_rate = sqrt_controller_pointer->error * sqrt_controller_pointer->kp;
  78     } else if (sqrt_controller_pointer->kp == 0.0f) {
  79         // Proportional term is zero but we have a Derivative Max.
  80         if (sqrt_controller_pointer->error > 0.0f) {
  81             correction_rate = fast_fsqrtf(2.0f * sqrt_controller_pointer->derivative_max * (sqrt_controller_pointer->error));
  82         } else if (sqrt_controller_pointer->error < 0.0f) {
  83             correction_rate = -fast_fsqrtf(2.0f * sqrt_controller_pointer->derivative_max * (-sqrt_controller_pointer->error));
  84         } else {
  85             correction_rate = 0.0f;
  86         }
  87     } else {
  88         // Both the Proportional and Derivative Max have been defined.
  89         const float linear_dist = sqrt_controller_pointer->derivative_max / sq(sqrt_controller_pointer->kp);
  90         if (sqrt_controller_pointer->error > linear_dist) {
  91             correction_rate = fast_fsqrtf(2.0f * sqrt_controller_pointer->derivative_max * (sqrt_controller_pointer->error - (linear_dist / 2.0f)));
  92         } else if (sqrt_controller_pointer->error < -linear_dist) {
  93             correction_rate = -fast_fsqrtf(2.0f * sqrt_controller_pointer->derivative_max * (-sqrt_controller_pointer->error - (linear_dist / 2.0f)));
  94         } else {
  95             correction_rate = sqrt_controller_pointer->error * sqrt_controller_pointer->kp;
  96         }
  97     }
  98
  99     if (deltaTime != 0.0f) {
 100         // This ensures we do not get small oscillations by over shooting the error correction in the last time step.
 101         return constrainf(correction_rate, -fabsf(sqrt_controller_pointer->error) / deltaTime, fabsf(sqrt_controller_pointer->error) / deltaTime);
 102     }
 103
 104     return correction_rate;
 105 }
 106
 107 // Sets the maximum error to limit output and derivative of output
 108 void sqrtControllerInit(sqrt_controller_t *sqrt_controller_pointer,const float kp,const float output_min,const float output_max,const float derivative_out_max)
 109 {
 110
 111     // Reset the variables
 112     sqrt_controller_pointer->kp = kp;
 113     sqrt_controller_pointer->derivative_max = 0.0f;
 114     sqrt_controller_pointer->error_min = 0.0f;
 115     sqrt_controller_pointer->error_max = 0.0f;
 116
 117     if (derivative_out_max > 0.0f) {
 118         sqrt_controller_pointer->derivative_max = derivative_out_max;
 119     }
 120
 121     if ((output_min < 0.0f) && (sqrt_controller_pointer->kp > 0.0f)) {
 122         sqrt_controller_pointer->error_min = sqrtControllerInverse(sqrt_controller_pointer->kp, sqrt_controller_pointer->derivative_max, output_min);
 123     }
 124
 125     if ((output_max > 0.0f) && (sqrt_controller_pointer->kp > 0.0f)) {
 126         sqrt_controller_pointer->error_max = sqrtControllerInverse(sqrt_controller_pointer->kp, sqrt_controller_pointer->derivative_max, output_max);
 127     }
 128 }