OP-1900 added deceleration check to autotakeoff failsafe
[librepilot.git] / flight / modules / PathFollower / inc / pidcontrolne.h
blob8d2fa0d3bc89b523ab68ff70b57402e898c28679
1 /**
2 ******************************************************************************
3 * @addtogroup OpenPilotModules OpenPilot Modules
4 * @{
5 * @addtogroup PathFollower CONTROL interface class
6 * @brief PID Controler for NE Class definition
7 * @{
9 * @file pidcontrolne.h
10 * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
11 * @brief Executes pid control loop for NE
13 * @see The GNU Public License (GPL) Version 3
15 *****************************************************************************/
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.
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.
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
31 #ifndef PIDCONTROLNE_H
32 #define PIDCONTROLNE_H
33 extern "C" {
34 #include <pid.h>
36 #include "pathfollowerfsm.h"
38 class PIDControlNE {
39 public:
40 PIDControlNE();
41 ~PIDControlNE();
42 void Initialize();
43 void Deactivate();
44 void Activate();
45 void UpdateParameters(float kp, float ki, float kd, __attribute__((unused)) float ilimit, float dT, float velocityMax);
46 void UpdatePositionalParameters(float kp);
47 void UpdatePositionState(float pvNorth, float pvEast);
48 void UpdatePositionSetpoint(float setpointNorth, float setpointEast);
49 void UpdateVelocitySetpoint(float setpointNorth, float setpointEast);
50 // void RateLimit(float *spDesired, float *spCurrent, float rateLimit);
51 void UpdateVelocityState(float pvNorth, float pvEast);
52 void UpdateCommandParameters(float minCommand, float maxCommand, float velocityFeedforward);
53 void ControlPosition();
54 void ControlPositionWithPath(struct path_status *progress);
55 void GetNECommand(float *northCommand, float *eastCommand);
56 void GetVelocityDesired(float *north, float *east);
57 void UpdateBrakeVelocity(float startingVelocity, float dT, float brakeRate, float currentVelocity, float *updatedVelocity);
58 void UpdateVelocityStateWithBrake(float pvNorth, float pvEast, float path_time, float brakeRate);
60 private:
61 struct pid2 PIDvel[2]; // North, East
62 float mVelocitySetpointTarget[2];
63 float mVelocityState[2];
64 struct pid PIDposH[2];
65 float mPositionSetpointTarget[2];
66 float mPositionState[2];
69 float deltaTime;
70 float mVelocitySetpointCurrent[2];
71 float mNECommand;
72 float mNeutral;
73 float mVelocityMax;
74 float mMinCommand;
75 float mMaxCommand;
76 float mVelocityFeedforward;
77 uint8_t mActive;
80 #endif // PIDCONTROLNE_H