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