-
Notifications
You must be signed in to change notification settings - Fork 1k
/
controller_pid.c
230 lines (198 loc) · 6.37 KB
/
controller_pid.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
#include "stabilizer_types.h"
#include "attitude_controller.h"
#include "position_controller.h"
#include "controller_pid.h"
#include "log.h"
#include "param.h"
#include "math3d.h"
#define ATTITUDE_UPDATE_DT (float)(1.0f/ATTITUDE_RATE)
static attitude_t attitudeDesired;
static attitude_t rateDesired;
static float actuatorThrust;
static float cmd_thrust;
static float cmd_roll;
static float cmd_pitch;
static float cmd_yaw;
static float r_roll;
static float r_pitch;
static float r_yaw;
static float accelz;
void controllerPidInit(void)
{
attitudeControllerInit(ATTITUDE_UPDATE_DT);
positionControllerInit();
}
bool controllerPidTest(void)
{
bool pass = true;
pass &= attitudeControllerTest();
return pass;
}
static float capAngle(float angle) {
float result = angle;
while (result > 180.0f) {
result -= 360.0f;
}
while (result < -180.0f) {
result += 360.0f;
}
return result;
}
void controllerPid(control_t *control, const setpoint_t *setpoint,
const sensorData_t *sensors,
const state_t *state,
const stabilizerStep_t stabilizerStep)
{
control->controlMode = controlModeLegacy;
if (RATE_DO_EXECUTE(ATTITUDE_RATE, stabilizerStep)) {
// Rate-controled YAW is moving YAW angle setpoint
if (setpoint->mode.yaw == modeVelocity) {
attitudeDesired.yaw = capAngle(attitudeDesired.yaw + setpoint->attitudeRate.yaw * ATTITUDE_UPDATE_DT);
float yawMaxDelta = attitudeControllerGetYawMaxDelta();
if (yawMaxDelta != 0.0f)
{
float delta = capAngle(attitudeDesired.yaw-state->attitude.yaw);
// keep the yaw setpoint within +/- yawMaxDelta from the current yaw
if (delta > yawMaxDelta)
{
attitudeDesired.yaw = state->attitude.yaw + yawMaxDelta;
}
else if (delta < -yawMaxDelta)
{
attitudeDesired.yaw = state->attitude.yaw - yawMaxDelta;
}
}
} else if (setpoint->mode.yaw == modeAbs) {
attitudeDesired.yaw = setpoint->attitude.yaw;
} else if (setpoint->mode.quat == modeAbs) {
struct quat setpoint_quat = mkquat(setpoint->attitudeQuaternion.x, setpoint->attitudeQuaternion.y, setpoint->attitudeQuaternion.z, setpoint->attitudeQuaternion.w);
struct vec rpy = quat2rpy(setpoint_quat);
attitudeDesired.yaw = degrees(rpy.z);
}
attitudeDesired.yaw = capAngle(attitudeDesired.yaw);
}
if (RATE_DO_EXECUTE(POSITION_RATE, stabilizerStep)) {
positionController(&actuatorThrust, &attitudeDesired, setpoint, state);
}
if (RATE_DO_EXECUTE(ATTITUDE_RATE, stabilizerStep)) {
// Switch between manual and automatic position control
if (setpoint->mode.z == modeDisable) {
actuatorThrust = setpoint->thrust;
}
if (setpoint->mode.x == modeDisable || setpoint->mode.y == modeDisable) {
attitudeDesired.roll = setpoint->attitude.roll;
attitudeDesired.pitch = setpoint->attitude.pitch;
}
attitudeControllerCorrectAttitudePID(state->attitude.roll, state->attitude.pitch, state->attitude.yaw,
attitudeDesired.roll, attitudeDesired.pitch, attitudeDesired.yaw,
&rateDesired.roll, &rateDesired.pitch, &rateDesired.yaw);
// For roll and pitch, if velocity mode, overwrite rateDesired with the setpoint
// value. Also reset the PID to avoid error buildup, which can lead to unstable
// behavior if level mode is engaged later
if (setpoint->mode.roll == modeVelocity) {
rateDesired.roll = setpoint->attitudeRate.roll;
attitudeControllerResetRollAttitudePID();
}
if (setpoint->mode.pitch == modeVelocity) {
rateDesired.pitch = setpoint->attitudeRate.pitch;
attitudeControllerResetPitchAttitudePID();
}
// TODO: Investigate possibility to subtract gyro drift.
attitudeControllerCorrectRatePID(sensors->gyro.x, -sensors->gyro.y, sensors->gyro.z,
rateDesired.roll, rateDesired.pitch, rateDesired.yaw);
attitudeControllerGetActuatorOutput(&control->roll,
&control->pitch,
&control->yaw);
control->yaw = -control->yaw;
cmd_thrust = control->thrust;
cmd_roll = control->roll;
cmd_pitch = control->pitch;
cmd_yaw = control->yaw;
r_roll = radians(sensors->gyro.x);
r_pitch = -radians(sensors->gyro.y);
r_yaw = radians(sensors->gyro.z);
accelz = sensors->acc.z;
}
control->thrust = actuatorThrust;
if (control->thrust == 0)
{
control->thrust = 0;
control->roll = 0;
control->pitch = 0;
control->yaw = 0;
cmd_thrust = control->thrust;
cmd_roll = control->roll;
cmd_pitch = control->pitch;
cmd_yaw = control->yaw;
attitudeControllerResetAllPID();
positionControllerResetAllPID();
// Reset the calculated YAW angle for rate control
attitudeDesired.yaw = state->attitude.yaw;
}
}
/**
* Logging variables for the command and reference signals for the
* altitude PID controller
*/
LOG_GROUP_START(controller)
/**
* @brief Thrust command
*/
LOG_ADD(LOG_FLOAT, cmd_thrust, &cmd_thrust)
/**
* @brief Roll command
*/
LOG_ADD(LOG_FLOAT, cmd_roll, &cmd_roll)
/**
* @brief Pitch command
*/
LOG_ADD(LOG_FLOAT, cmd_pitch, &cmd_pitch)
/**
* @brief yaw command
*/
LOG_ADD(LOG_FLOAT, cmd_yaw, &cmd_yaw)
/**
* @brief Gyro roll measurement in radians
*/
LOG_ADD(LOG_FLOAT, r_roll, &r_roll)
/**
* @brief Gyro pitch measurement in radians
*/
LOG_ADD(LOG_FLOAT, r_pitch, &r_pitch)
/**
* @brief Yaw measurement in radians
*/
LOG_ADD(LOG_FLOAT, r_yaw, &r_yaw)
/**
* @brief Acceleration in the zaxis in G-force
*/
LOG_ADD(LOG_FLOAT, accelz, &accelz)
/**
* @brief Thrust command without (tilt)compensation
*/
LOG_ADD(LOG_FLOAT, actuatorThrust, &actuatorThrust)
/**
* @brief Desired roll setpoint
*/
LOG_ADD(LOG_FLOAT, roll, &attitudeDesired.roll)
/**
* @brief Desired pitch setpoint
*/
LOG_ADD(LOG_FLOAT, pitch, &attitudeDesired.pitch)
/**
* @brief Desired yaw setpoint
*/
LOG_ADD(LOG_FLOAT, yaw, &attitudeDesired.yaw)
/**
* @brief Desired roll rate setpoint
*/
LOG_ADD(LOG_FLOAT, rollRate, &rateDesired.roll)
/**
* @brief Desired pitch rate setpoint
*/
LOG_ADD(LOG_FLOAT, pitchRate, &rateDesired.pitch)
/**
* @brief Desired yaw rate setpoint
*/
LOG_ADD(LOG_FLOAT, yawRate, &rateDesired.yaw)
LOG_GROUP_STOP(controller)