flight/modules/PathFollower/pidcontrolne.cpp

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup PathFollower CONTROL interface class
   6  * @brief PID controller for NE
   7  * @{
   8  *
   9  * @file       PIDControlNE.h
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2017.
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  12  * @brief      Executes PID control loops for NE directions
  13  *
  14  * @see        The GNU Public License (GPL) Version 3
  15  *
  16  *****************************************************************************/
  17 /*
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by
  20  * the Free Software Foundation; either version 3 of the License, or
  21  * (at your option) any later version.
  22  *
  23  * This program is distributed in the hope that it will be useful, but
  24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  26  * for more details.
  27  *
  28  * You should have received a copy of the GNU General Public License along
  29  * with this program; if not, write to the Free Software Foundation, Inc.,
  30  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  31  */
  32 extern "C" {
  33 #include <openpilot.h>
  34
  35 #include <math.h>
  36 #include <pid.h>
  37 #include <CoordinateConversions.h>
  38 #include <sin_lookup.h>
  39 #include <pathdesired.h>
  40 #include <paths.h>
  41 #include "plans.h"
  42 #include <pidstatus.h>
  43 }
  44 #include "pathfollowerfsm.h"
  45 #include "pidcontrolne.h"
  46
  47 PIDControlNE::PIDControlNE()
  48     : deltaTime(0), mNECommand(0), mNeutral(0), mVelocityMax(0), mMinCommand(0), mMaxCommand(0), mVelocityFeedforward(0), mActive(false)
  49 {
  50     memset((void *)PIDvel, 0, 2 * sizeof(pid2));
  51     memset((void *)PIDposH, 0, 2 * sizeof(pid));
  52
  53     mVelocitySetpointTarget[0]  = 0.0f;
  54     mVelocitySetpointTarget[1]  = 0.0f;
  55     mVelocityState[0] = 0.0f;
  56     mVelocityState[1] = 0.0f;
  57     mPositionSetpointTarget[0]  = 0.0f;
  58     mPositionSetpointTarget[1]  = 0.0f;
  59     mPositionState[0] = 0.0f;
  60     mPositionState[1] = 0.0f;
  61     mVelocitySetpointCurrent[0] = 0.0f;
  62     mVelocitySetpointCurrent[1] = 0.0f;
  63 }
  64
  65 PIDControlNE::~PIDControlNE() {}
  66
  67 void PIDControlNE::Deactivate()
  68 {
  69     mActive = false;
  70 }
  71
  72 void PIDControlNE::Activate()
  73 {
  74     mActive = true;
  75     pid2_transfer(&PIDvel[0], 0.0f);
  76     pid2_transfer(&PIDvel[1], 0.0f);
  77     pid_zero(&PIDposH[0]);
  78     pid_zero(&PIDposH[1]);
  79 }
  80
  81 void PIDControlNE::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
  82 {
  83     float Td; // Derivative time constant
  84     float Ti; // Integral time constant
  85     float kt; // Feedback gain for integral windup avoidance
  86     float N = 10.0f; // N is the factor used to determine the
  87                      // time constant for derivative filtering
  88                      // Why 10? Maybe should be configurable?
  89     float Tf; // Low pass filter time constant for derivative filtering
  90
  91     float u0 = 0.0f;
  92
  93     // Define Td, handling zero kp term (for I or ID controller)
  94     if (kp < 1e-6f) {
  95         Td = 1e6f;
  96     } else {
  97         Td = kd / kp;
  98     }
  99
 100     // Define Ti, Tt and kt, handling zero ki term (for P or PD controller)
 101     if (ki < 1e-6f) { // Avoid Ti being infinite
 102         kt = 0.0f;
 103     } else {
 104         Ti = kp / ki;
 105         kt = 1.0f / Ti;
 106     }
 107
 108     // Define Tf, according to controller type
 109     if (kd < 1e-6f) {
 110         // PI Controller or P Controller
 111         Tf = 0;
 112     } else {
 113         Tf = Td / N;
 114     }
 115
 116     if (beta > 1.0f) {
 117         beta = 1.0f;
 118     } else if (beta < 0.4f) {
 119         beta = 0.4f;
 120     }
 121
 122     pid2_configure(&PIDvel[0], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
 123     pid2_configure(&PIDvel[1], kp, ki, kd, Tf, kt, dT, beta, u0, 0.0f, 1.0f);
 124     deltaTime    = dT;
 125     mVelocityMax = velocityMax;
 126 }
 127
 128 void PIDControlNE::UpdatePositionalParameters(float kp)
 129 {
 130     pid_configure(&PIDposH[0], kp, 0.0f, 0.0f, 0.0f);
 131     pid_configure(&PIDposH[1], kp, 0.0f, 0.0f, 0.0f);
 132 }
 133 void PIDControlNE::UpdatePositionSetpoint(float setpointNorth, float setpointEast)
 134 {
 135     mPositionSetpointTarget[0] = setpointNorth;
 136     mPositionSetpointTarget[1] = setpointEast;
 137 }
 138 void PIDControlNE::UpdatePositionState(float pvNorth, float pvEast)
 139 {
 140     mPositionState[0] = pvNorth;
 141     mPositionState[1] = pvEast;
 142 }
 143 // This is a pure position hold position control
 144 void PIDControlNE::ControlPosition()
 145 {
 146     // Current progress location relative to end
 147     float velNorth = 0.0f;
 148     float velEast  = 0.0f;
 149
 150     velNorth = pid_apply(&PIDposH[0], mPositionSetpointTarget[0] - mPositionState[0], deltaTime);
 151     velEast  = pid_apply(&PIDposH[1], mPositionSetpointTarget[1] - mPositionState[1], deltaTime);
 152     UpdateVelocitySetpoint(velNorth, velEast);
 153 }
 154
 155 void PIDControlNE::ControlPositionWithPath(struct path_status *progress)
 156 {
 157     // Current progress location relative to end
 158     float velNorth = progress->path_vector[0];
 159     float velEast  = progress->path_vector[1];
 160
 161     velNorth += pid_apply(&PIDposH[0], progress->correction_vector[0], deltaTime);
 162     velEast  += pid_apply(&PIDposH[1], progress->correction_vector[1], deltaTime);
 163     UpdateVelocitySetpoint(velNorth, velEast);
 164 }
 165
 166 void PIDControlNE::UpdateVelocitySetpoint(float setpointNorth, float setpointEast)
 167 {
 168     // scale velocity if it is above configured maximum
 169     // for braking, we can not help it if initial velocity was greater
 170     float velH = sqrtf(setpointNorth * setpointNorth + setpointEast * setpointEast);
 171
 172     if (velH > mVelocityMax) {
 173         setpointNorth *= mVelocityMax / velH;
 174         setpointEast  *= mVelocityMax / velH;
 175     }
 176
 177     mVelocitySetpointTarget[0] = setpointNorth;
 178     mVelocitySetpointTarget[1] = setpointEast;
 179 }
 180
 181
 182 void PIDControlNE::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
 183 {
 184     if (startingVelocity >= 0.0f) {
 185         *updatedVelocity = startingVelocity - dT * brakeRate;
 186         if (*updatedVelocity > currentVelocity) {
 187             *updatedVelocity = currentVelocity;
 188         }
 189         if (*updatedVelocity < 0.0f) {
 190             *updatedVelocity = 0.0f;
 191         }
 192     } else {
 193         *updatedVelocity = startingVelocity + dT * brakeRate;
 194         if (*updatedVelocity < currentVelocity) {
 195             *updatedVelocity = currentVelocity;
 196         }
 197         if (*updatedVelocity > 0.0f) {
 198             *updatedVelocity = 0.0f;
 199         }
 200     }
 201 }
 202
 203 void PIDControlNE::UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate)
 204 {
 205     mVelocityState[0] = pvNorth;
 206     mVelocityState[1] = pvEast;
 207
 208     float velocitySetpointDesired[2];
 209
 210     UpdateBrakeVelocity(mVelocitySetpointTarget[0], path_time, brakeRate, pvNorth, &velocitySetpointDesired[0]);
 211     UpdateBrakeVelocity(mVelocitySetpointTarget[1], path_time, brakeRate, pvEast, &velocitySetpointDesired[1]);
 212
 213     // If rate of change limits required, add here
 214     for (int iaxis = 0; iaxis < 2; iaxis++) {
 215         mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
 216     }
 217 }
 218
 219 void PIDControlNE::UpdateVelocityState(float pvNorth, float pvEast)
 220 {
 221     mVelocityState[0] = pvNorth;
 222     mVelocityState[1] = pvEast;
 223
 224     // The FSM controls the actual descent velocity and introduces step changes as required
 225     float velocitySetpointDesired[2];
 226     velocitySetpointDesired[0] = mVelocitySetpointTarget[0];
 227     velocitySetpointDesired[1] = mVelocitySetpointTarget[1];
 228
 229     // If rate of change limits required, add here
 230     for (int iaxis = 0; iaxis < 2; iaxis++) {
 231         mVelocitySetpointCurrent[iaxis] = velocitySetpointDesired[iaxis];
 232     }
 233 }
 234
 235
 236 void PIDControlNE::UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward)
 237 {
 238     mMinCommand = minCommand;
 239     mMaxCommand = maxCommand;
 240     mVelocityFeedforward = velocityFeedforward;
 241 }
 242
 243
 244 void PIDControlNE::GetNECommand(float *northCommand, float *eastCommand)
 245 {
 246     PIDvel[0].va  = mVelocitySetpointCurrent[0] * mVelocityFeedforward;
 247     *northCommand = pid2_apply(&(PIDvel[0]), mVelocitySetpointCurrent[0], mVelocityState[0], mMinCommand, mMaxCommand);
 248     PIDvel[1].va  = mVelocitySetpointCurrent[1] * mVelocityFeedforward;
 249     *eastCommand  = pid2_apply(&(PIDvel[1]), mVelocitySetpointCurrent[1], mVelocityState[1], mMinCommand, mMaxCommand);
 250
 251     PIDStatusData pidStatus;
 252     pidStatus.setpoint = mVelocitySetpointCurrent[0];
 253     pidStatus.actual   = mVelocityState[0];
 254     pidStatus.error    = mVelocitySetpointCurrent[0] - mVelocityState[0];
 255     pidStatus.setpoint = mVelocitySetpointCurrent[0];
 256     pidStatus.ulow     = mMinCommand;
 257     pidStatus.uhigh    = mMaxCommand;
 258     pidStatus.command  = *northCommand;
 259     pidStatus.P = PIDvel[0].P;
 260     pidStatus.I = PIDvel[0].I;
 261     pidStatus.D = PIDvel[0].D;
 262     PIDStatusSet(&pidStatus);
 263 }
 264
 265 void PIDControlNE::GetVelocityDesired(float *north, float *east)
 266 {
 267     *north = mVelocitySetpointCurrent[0];
 268     *east  = mVelocitySetpointCurrent[1];
 269 }