flight/libraries/pid/pidcontroldown.cpp

   1 /**
   2  ******************************************************************************
   3  * @addtogroup OpenPilotModules OpenPilot Modules
   4  * @{
   5  * @addtogroup PathFollower PID interface class
   6  * @brief PID loop for down control
   7  * @{
   8  *
   9  * @file       pidcontroldown.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 for down direction
  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 <stabilizationdesired.h>
  43 #include <vtolselftuningstats.h>
  44 }
  45
  46 #include "pidcontroldown.h"
  47
  48 #define NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT 0.5f
  49 #define NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT      0.2f
  50 #define NEUTRALTHRUST_PH_VEL_STATE_LIMIT        0.2f
  51 #define NEUTRALTHRUST_PH_VEL_ERROR_LIMIT        0.1f
  52
  53 #define NEUTRALTHRUST_START_DELAY               (2 * 20) // 2 seconds at rate of 20Hz (50ms update rate)
  54 #define NEUTRALTHRUST_END_COUNT                 (NEUTRALTHRUST_START_DELAY + (4 * 20))  // 4 second sample
  55
  56
  57 PIDControlDown::PIDControlDown()
  58     : deltaTime(0.0f), mVelocitySetpointTarget(0.0f), mVelocitySetpointCurrent(0.0f), mVelocityState(0.0f), mDownCommand(0.0f),
  59     mCallback(NULL), mNeutral(0.5f), mVelocityMax(1.0f), mPositionSetpointTarget(0.0f), mPositionState(0.0f),
  60     mMinThrust(0.1f), mMaxThrust(0.6f), mActive(false), mAllowNeutralThrustCalc(true)
  61 {
  62     Deactivate();
  63 }
  64
  65 PIDControlDown::~PIDControlDown() {}
  66
  67 void PIDControlDown::Initialize(PIDControlDownCallback *callback)
  68 {
  69     mCallback = callback;
  70 }
  71
  72 void PIDControlDown::SetThrustLimits(float min_thrust, float max_thrust)
  73 {
  74     mMinThrust = min_thrust;
  75     mMaxThrust = max_thrust;
  76 }
  77
  78 void PIDControlDown::Deactivate()
  79 {
  80     mActive = false;
  81 }
  82
  83 void PIDControlDown::Activate()
  84 {
  85     float currentThrust;
  86
  87     StabilizationDesiredThrustGet(&currentThrust);
  88     pid2_transfer(&PID, currentThrust);
  89     mActive = true;
  90 }
  91
  92 void PIDControlDown::UpdateParameters(float kp, float ki, float kd, float beta, float dT, float velocityMax)
  93 {
  94     float Td; // Derivative time constant
  95     float Ti; // Integral time constant
  96     float kt; // Feedback gain for integral windup avoidance
  97     float N = 10.0f; // N is the factor used to determine the
  98                      // time constant for derivative filtering
  99                      // Why 10? Maybe should be configurable?
 100     float Tf; // Low pass filter time constant for derivative filtering
 101
 102     // Define Td, handling zero kp term (for I or ID controller)
 103     if (kp < 1e-6f) {
 104         Td = 1e6f;
 105     } else {
 106         Td = kd / kp;
 107     }
 108
 109     // Define Ti, Tt and kt, handling zero ki term (for P or PD controller)
 110     if (ki < 1e-6f) { // Avoid Ti being infinite
 111         kt = 0.0f;
 112     } else {
 113         Ti = kp / ki;
 114         kt = 1.0f / Ti;
 115     }
 116
 117     // Define Tf, according to controller type
 118     if (kd < 1e-6f) {
 119         // PI Controller or P Controller
 120         Tf = 0;
 121     } else {
 122         Tf = Td / N;
 123     }
 124
 125     if (beta > 1.0f) {
 126         beta = 1.0f;
 127     } else if (beta < 0.4f) {
 128         beta = 0.4f;
 129     }
 130
 131     pid2_configure(&PID, kp, ki, kd, Tf, kt, dT, beta, mNeutral, mNeutral, -1.0f);
 132     deltaTime    = dT;
 133     mVelocityMax = velocityMax;
 134 }
 135
 136
 137 void PIDControlDown::UpdatePositionalParameters(float kp)
 138 {
 139     pid_configure(&PIDpos, kp, 0.0f, 0.0f, 0.0f);
 140 }
 141 void PIDControlDown::UpdatePositionSetpoint(float setpointDown)
 142 {
 143     mPositionSetpointTarget = setpointDown;
 144 }
 145 void PIDControlDown::UpdatePositionState(float pvDown)
 146 {
 147     mPositionState = pvDown;
 148     setup_neutralThrustCalc();
 149 }
 150 // This is a pure position hold position control
 151 void PIDControlDown::ControlPosition()
 152 {
 153     // Current progress location relative to end
 154     float velDown = 0.0f;
 155
 156     velDown = pid_apply(&PIDpos, mPositionSetpointTarget - mPositionState, deltaTime);
 157     UpdateVelocitySetpoint(velDown);
 158
 159     run_neutralThrustCalc();
 160 }
 161
 162
 163 void PIDControlDown::ControlPositionWithPath(struct path_status *progress)
 164 {
 165     // Current progress location relative to end
 166     float velDown = progress->path_vector[2];
 167
 168     velDown += pid_apply(&PIDpos, progress->correction_vector[2], deltaTime);
 169     UpdateVelocitySetpoint(velDown);
 170 }
 171
 172
 173 void PIDControlDown::run_neutralThrustCalc(void)
 174 {
 175     // if auto thrust and we have not run a correction calc check for PH and stability to then run an assessment
 176     // note that arming into this flight mode is not allowed, so assumption here is that
 177     // we have not landed.  If GPSAssist+Manual/Cruise control thrust mode is used, a user overriding the
 178     // altitude maintaining PID will have moved the throttle state to FLIGHTSTATUS_ASSISTEDTHROTTLESTATE_MANUAL.
 179     // In manualcontrol.c the state will stay in manual throttle until the throttle command exceeds the vtol thrust min,
 180     // avoiding auto-takeoffs.  Therefore no need to check that here.
 181
 182     if (neutralThrustEst.have_correction != true) {
 183         // Make FSM specific
 184         bool stable = (fabsf(mPositionSetpointTarget - mPositionState) < NEUTRALTHRUST_PH_POSITIONAL_ERROR_LIMIT &&
 185                        fabsf(mVelocitySetpointCurrent) < NEUTRALTHRUST_PH_VEL_DESIRED_LIMIT &&
 186                        fabsf(mVelocityState) < NEUTRALTHRUST_PH_VEL_STATE_LIMIT &&
 187                        fabsf(mVelocitySetpointCurrent - mVelocityState) < NEUTRALTHRUST_PH_VEL_ERROR_LIMIT);
 188
 189         if (stable) {
 190             if (neutralThrustEst.start_sampling) {
 191                 neutralThrustEst.count++;
 192
 193
 194                 // delay count for 2 seconds into hold allowing for stablisation
 195                 if (neutralThrustEst.count > NEUTRALTHRUST_START_DELAY) {
 196                     neutralThrustEst.sum += PID.I;
 197                     if (PID.I < neutralThrustEst.min) {
 198                         neutralThrustEst.min = PID.I;
 199                     }
 200                     if (PID.I > neutralThrustEst.max) {
 201                         neutralThrustEst.max = PID.I;
 202                     }
 203                 }
 204
 205                 if (neutralThrustEst.count >= NEUTRALTHRUST_END_COUNT) {
 206                     // 6 seconds have past
 207                     // lets take an average
 208                     neutralThrustEst.average    = neutralThrustEst.sum / (float)(NEUTRALTHRUST_END_COUNT - NEUTRALTHRUST_START_DELAY);
 209                     neutralThrustEst.correction = neutralThrustEst.average;
 210
 211                     PID.I -= neutralThrustEst.average;
 212
 213                     neutralThrustEst.start_sampling  = false;
 214                     neutralThrustEst.have_correction = true;
 215
 216                     // Write a new adjustment value
 217                     // vtolSelfTuningStats.NeutralThrustOffset  was incremental adjusted above
 218                     VtolSelfTuningStatsData vtolSelfTuningStats;
 219                     VtolSelfTuningStatsGet(&vtolSelfTuningStats);
 220                     // add the average remaining i value to the
 221                     vtolSelfTuningStats.NeutralThrustOffset     += neutralThrustEst.correction;
 222                     vtolSelfTuningStats.NeutralThrustCorrection  = neutralThrustEst.correction; // the i term thrust correction value applied
 223                     vtolSelfTuningStats.NeutralThrustAccumulator = PID.I; // the actual iaccumulator value after correction
 224                     vtolSelfTuningStats.NeutralThrustRange = neutralThrustEst.max - neutralThrustEst.min;
 225                     VtolSelfTuningStatsSet(&vtolSelfTuningStats);
 226                 }
 227             } else {
 228                 // start a tick count
 229                 neutralThrustEst.start_sampling = true;
 230                 neutralThrustEst.count = 0;
 231                 neutralThrustEst.sum   = 0.0f;
 232                 neutralThrustEst.max   = 0.0f;
 233                 neutralThrustEst.min   = 0.0f;
 234             }
 235         } else {
 236             // reset sampling as we did't get 6 continuous seconds
 237             neutralThrustEst.start_sampling = false;
 238         }
 239     } // else we already have a correction for this PH run
 240 }
 241
 242
 243 void PIDControlDown::setup_neutralThrustCalc(void)
 244 {
 245     // reset neutral thrust assessment.
 246     // and do once for each position hold engagement
 247     neutralThrustEst.start_sampling  = false;
 248     neutralThrustEst.count = 0;
 249     neutralThrustEst.sum = 0.0f;
 250     neutralThrustEst.have_correction = false;
 251     neutralThrustEst.average    = 0.0f;
 252     neutralThrustEst.correction = 0.0f;
 253     neutralThrustEst.min = 0.0f;
 254     neutralThrustEst.max = 0.0f;
 255 }
 256
 257
 258 void PIDControlDown::UpdateNeutralThrust(float neutral)
 259 {
 260     if (mActive) {
 261         // adjust neutral and achieve bumpless transfer
 262         PID.va = neutral;
 263         pid2_transfer(&PID, mDownCommand);
 264     }
 265     mNeutral = neutral;
 266 }
 267
 268 void PIDControlDown::UpdateVelocitySetpoint(float setpoint)
 269 {
 270     mVelocitySetpointTarget = setpoint;
 271     if (fabsf(mVelocitySetpointTarget) > mVelocityMax) {
 272         // maintain sign but set to max
 273         mVelocitySetpointTarget *= mVelocityMax / fabsf(mVelocitySetpointTarget);
 274     }
 275 }
 276
 277 void PIDControlDown::RateLimit(float *spDesired, float *spCurrent, float rateLimit)
 278 {
 279     float velocity_delta = *spDesired - *spCurrent;
 280
 281     if (fabsf(velocity_delta) < 1e-6f) {
 282         *spCurrent = *spDesired;
 283         return;
 284     }
 285
 286     // Calculate the rate of change
 287     float accelerationDesired = velocity_delta / deltaTime;
 288
 289     if (fabsf(accelerationDesired) > rateLimit) {
 290         accelerationDesired *= rateLimit / accelerationDesired;
 291     }
 292
 293     if (fabsf(accelerationDesired) < 0.1f) {
 294         *spCurrent = *spDesired;
 295     } else {
 296         *spCurrent += accelerationDesired * deltaTime;
 297     }
 298 }
 299
 300 void PIDControlDown::UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity)
 301 {
 302     if (startingVelocity >= 0.0f) {
 303         *updatedVelocity = startingVelocity - dT * brakeRate;
 304         if (*updatedVelocity > currentVelocity) {
 305             *updatedVelocity = currentVelocity;
 306         }
 307         if (*updatedVelocity < 0.0f) {
 308             *updatedVelocity = 0.0f;
 309         }
 310     } else {
 311         *updatedVelocity = startingVelocity + dT * brakeRate;
 312         if (*updatedVelocity < currentVelocity) {
 313             *updatedVelocity = currentVelocity;
 314         }
 315         if (*updatedVelocity > 0.0f) {
 316             *updatedVelocity = 0.0f;
 317         }
 318     }
 319 }
 320
 321 void PIDControlDown::UpdateVelocityStateWithBrake(float pvDown, float path_time, float brakeRate)
 322 {
 323     mVelocityState = pvDown;
 324
 325     float velocitySetpointDesired;
 326
 327     UpdateBrakeVelocity(mVelocitySetpointTarget, path_time, brakeRate, pvDown, &velocitySetpointDesired);
 328
 329     // Calculate the rate of change
 330     // RateLimit(velocitySetpointDesired[iaxis], mVelocitySetpointCurrent[iaxis], 2.0f );
 331
 332     mVelocitySetpointCurrent = velocitySetpointDesired;
 333 }
 334
 335
 336 // Update velocity state called per dT. Also update current
 337 // desired velocity
 338 void PIDControlDown::UpdateVelocityState(float pv)
 339 {
 340     mVelocityState = pv;
 341
 342     if (mCallback) {
 343         // The FSM controls the actual descent velocity and introduces step changes as required
 344         float velocitySetpointDesired = mCallback->BoundVelocityDown(mVelocitySetpointTarget);
 345         // RateLimit(velocitySetpointDesired, mVelocitySetpointCurrent, 2.0f );
 346         mVelocitySetpointCurrent = velocitySetpointDesired;
 347     } else {
 348         mVelocitySetpointCurrent = mVelocitySetpointTarget;
 349     }
 350 }
 351
 352 float PIDControlDown::GetVelocityDesired(void)
 353 {
 354     return mVelocitySetpointCurrent;
 355 }
 356
 357 float PIDControlDown::GetDownCommand(void)
 358 {
 359     float ulow  = mMinThrust;
 360     float uhigh = mMaxThrust;
 361
 362     if (mCallback) {
 363         mCallback->BoundThrust(ulow, uhigh);
 364     }
 365     float downCommand = pid2_apply(&PID, mVelocitySetpointCurrent, mVelocityState, ulow, uhigh);
 366     mDownCommand = downCommand;
 367     return mDownCommand;
 368 }