2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
23 #include "unittest_macros.h"
24 #include "gtest/gtest.h"
25 #include "build/debug.h"
27 bool simulatedAirmodeEnabled
= true;
28 float simulatedSetpointRate
[3] = { 0,0,0 };
29 float simulatedRcDeflection
[3] = { 0,0,0 };
30 float simulatedThrottlePIDAttenuation
= 1.0f
;
31 float simulatedMotorMixRange
= 0.0f
;
33 int16_t debug
[DEBUG16_VALUE_COUNT
];
37 #include "build/debug.h"
38 #include "common/axis.h"
39 #include "common/maths.h"
40 #include "common/filter.h"
42 #include "config/config_reset.h"
44 #include "pg/pg_ids.h"
46 #include "drivers/sound_beeper.h"
47 #include "drivers/time.h"
52 #include "fc/rc_controls.h"
53 #include "fc/runtime_config.h"
55 #include "flight/pid.h"
56 #include "flight/imu.h"
57 #include "flight/mixer.h"
61 #include "sensors/gyro.h"
62 #include "sensors/acceleration.h"
65 attitudeEulerAngles_t attitude
;
67 PG_REGISTER(accelerometerConfig_t
, accelerometerConfig
, PG_ACCELEROMETER_CONFIG
, 0);
69 bool unitLaunchControlActive
= false;
70 launchControlMode_e unitLaunchControlMode
= LAUNCH_CONTROL_MODE_NORMAL
;
72 float getThrottlePIDAttenuation(void) { return simulatedThrottlePIDAttenuation
; }
73 float getMotorMixRange(void) { return simulatedMotorMixRange
; }
74 float getSetpointRate(int axis
) { return simulatedSetpointRate
[axis
]; }
75 bool isAirmodeActivated() { return simulatedAirmodeEnabled
; }
76 float getRcDeflectionAbs(int axis
) { return fabsf(simulatedRcDeflection
[axis
]); }
77 void systemBeep(bool) { }
78 bool gyroOverflowDetected(void) { return false; }
79 float getRcDeflection(int axis
) { return simulatedRcDeflection
[axis
]; }
80 void beeperConfirmationBeeps(uint8_t) { }
81 bool isLaunchControlActive(void) {return unitLaunchControlActive
; }
83 float applyFFLimit(int axis
, float value
, float Kp
, float currentPidSetpoint
) {
86 UNUSED(currentPidSetpoint
);
91 pidProfile_t
*pidProfile
;
95 // Always use same defaults for testing in future releases even when defaults change
96 void setDefaultTestSettings(void) {
98 pidProfile
= pidProfilesMutable(1);
99 pidProfile
->pid
[PID_ROLL
] = { 40, 40, 30, 65 };
100 pidProfile
->pid
[PID_PITCH
] = { 58, 50, 35, 60 };
101 pidProfile
->pid
[PID_YAW
] = { 70, 45, 20, 60 };
102 pidProfile
->pid
[PID_LEVEL
] = { 50, 50, 75, 0 };
104 // Compensate for the upscaling done without 'use_integrated_yaw'
105 pidProfile
->pid
[PID_YAW
].I
= pidProfile
->pid
[PID_YAW
].I
/ 2.5f
;
107 pidProfile
->pidSumLimit
= PIDSUM_LIMIT
;
108 pidProfile
->pidSumLimitYaw
= PIDSUM_LIMIT_YAW
;
109 pidProfile
->yaw_lowpass_hz
= 0;
110 pidProfile
->dterm_lowpass_hz
= 100;
111 pidProfile
->dterm_lowpass2_hz
= 0;
112 pidProfile
->dterm_notch_hz
= 260;
113 pidProfile
->dterm_notch_cutoff
= 160;
114 pidProfile
->dterm_filter_type
= FILTER_BIQUAD
;
115 pidProfile
->itermWindupPointPercent
= 50;
116 pidProfile
->vbatPidCompensation
= 0;
117 pidProfile
->pidAtMinThrottle
= PID_STABILISATION_ON
;
118 pidProfile
->levelAngleLimit
= 55;
119 pidProfile
->feedForwardTransition
= 100;
120 pidProfile
->yawRateAccelLimit
= 100;
121 pidProfile
->rateAccelLimit
= 0;
122 pidProfile
->antiGravityMode
= ANTI_GRAVITY_SMOOTH
;
123 pidProfile
->itermThrottleThreshold
= 250;
124 pidProfile
->itermAcceleratorGain
= 1000;
125 pidProfile
->crash_time
= 500;
126 pidProfile
->crash_delay
= 0;
127 pidProfile
->crash_recovery_angle
= 10;
128 pidProfile
->crash_recovery_rate
= 100;
129 pidProfile
->crash_dthreshold
= 50;
130 pidProfile
->crash_gthreshold
= 400;
131 pidProfile
->crash_setpoint_threshold
= 350;
132 pidProfile
->crash_recovery
= PID_CRASH_RECOVERY_OFF
;
133 pidProfile
->horizon_tilt_effect
= 75;
134 pidProfile
->horizon_tilt_expert_mode
= false;
135 pidProfile
->crash_limit_yaw
= 200;
136 pidProfile
->itermLimit
= 150;
137 pidProfile
->throttle_boost
= 0;
138 pidProfile
->throttle_boost_cutoff
= 15;
139 pidProfile
->iterm_rotation
= false;
140 pidProfile
->iterm_relax
= ITERM_RELAX_OFF
,
141 pidProfile
->iterm_relax_cutoff
= 11,
142 pidProfile
->iterm_relax_type
= ITERM_RELAX_SETPOINT
,
143 pidProfile
->abs_control_gain
= 0,
144 pidProfile
->launchControlMode
= LAUNCH_CONTROL_MODE_NORMAL
,
145 pidProfile
->launchControlGain
= 40,
147 gyro
.targetLooptime
= 4000;
150 timeUs_t
currentTestTime(void) {
151 return targetPidLooptime
* loopIter
++;
154 void resetTest(void) {
156 simulatedThrottlePIDAttenuation
= 1.0f
;
157 simulatedMotorMixRange
= 0.0f
;
159 pidStabilisationState(PID_STABILISATION_OFF
);
160 DISABLE_ARMING_FLAG(ARMED
);
162 setDefaultTestSettings();
163 for (int axis
= FD_ROLL
; axis
<= FD_YAW
; axis
++) {
168 pidData
[axis
].Sum
= 0;
169 simulatedSetpointRate
[axis
] = 0;
170 simulatedRcDeflection
[axis
] = 0;
171 gyro
.gyroADCf
[axis
] = 0;
173 attitude
.values
.roll
= 0;
174 attitude
.values
.pitch
= 0;
175 attitude
.values
.yaw
= 0;
178 unitLaunchControlActive
= false;
179 pidProfile
->launchControlMode
= unitLaunchControlMode
;
182 // Run pidloop for a while after reset
183 for (int loop
= 0; loop
< 20; loop
++) {
184 pidController(pidProfile
, currentTestTime());
188 void setStickPosition(int axis
, float stickRatio
) {
189 simulatedSetpointRate
[axis
] = 1998.0f
* stickRatio
;
190 simulatedRcDeflection
[axis
] = stickRatio
;
193 // All calculations will have 10% tolerance
194 float calculateTolerance(float input
) {
195 return fabs(input
* 0.1f
);
198 TEST(pidControllerTest
, testInitialisation
)
202 // In initial state PIDsums should be 0
203 for (int axis
= 0; axis
<= FD_YAW
; axis
++) {
204 EXPECT_FLOAT_EQ(0, pidData
[axis
].P
);
205 EXPECT_FLOAT_EQ(0, pidData
[axis
].I
);
206 EXPECT_FLOAT_EQ(0, pidData
[axis
].D
);
210 TEST(pidControllerTest
, testStabilisationDisabled
) {
211 ENABLE_ARMING_FLAG(ARMED
);
212 // Run few loops to make sure there is no error building up when stabilisation disabled
214 for (int loop
= 0; loop
< 10; loop
++) {
215 pidController(pidProfile
, currentTestTime());
217 // PID controller should not do anything, while stabilisation disabled
218 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].P
);
219 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].P
);
220 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].P
);
221 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].I
);
222 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].I
);
223 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].I
);
224 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].D
);
225 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].D
);
226 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].D
);
230 TEST(pidControllerTest
, testPidLoop
) {
231 // Make sure to start with fresh values
233 ENABLE_ARMING_FLAG(ARMED
);
234 pidStabilisationState(PID_STABILISATION_ON
);
236 pidController(pidProfile
, currentTestTime());
238 // Loop 1 - Expecting zero since there is no error
239 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].P
);
240 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].P
);
241 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].P
);
242 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].I
);
243 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].I
);
244 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].I
);
245 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].D
);
246 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].D
);
247 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].D
);
249 // Add some rotation on ROLL to generate error
250 gyro
.gyroADCf
[FD_ROLL
] = 100;
251 pidController(pidProfile
, currentTestTime());
253 // Loop 2 - Expect PID loop reaction to ROLL error
254 ASSERT_NEAR(-128.1, pidData
[FD_ROLL
].P
, calculateTolerance(-128.1));
255 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].P
);
256 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].P
);
257 ASSERT_NEAR(-7.8, pidData
[FD_ROLL
].I
, calculateTolerance(-7.8));
258 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].I
);
259 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].I
);
260 ASSERT_NEAR(-198.4, pidData
[FD_ROLL
].D
, calculateTolerance(-198.4));
261 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].D
);
262 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].D
);
264 // Add some rotation on PITCH to generate error
265 gyro
.gyroADCf
[FD_PITCH
] = -100;
266 pidController(pidProfile
, currentTestTime());
268 // Loop 3 - Expect PID loop reaction to PITCH error, ROLL is still in error
269 ASSERT_NEAR(-128.1, pidData
[FD_ROLL
].P
, calculateTolerance(-128.1));
270 ASSERT_NEAR(185.8, pidData
[FD_PITCH
].P
, calculateTolerance(185.8));
271 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].P
);
272 ASSERT_NEAR(-15.6, pidData
[FD_ROLL
].I
, calculateTolerance(-15.6));
273 ASSERT_NEAR(9.8, pidData
[FD_PITCH
].I
, calculateTolerance(9.8));
274 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].I
);
275 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].D
);
276 ASSERT_NEAR(231.4, pidData
[FD_PITCH
].D
, calculateTolerance(231.4));
277 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].D
);
279 // Add some rotation on YAW to generate error
280 gyro
.gyroADCf
[FD_YAW
] = 100;
281 pidController(pidProfile
, currentTestTime());
283 // Loop 4 - Expect PID loop reaction to PITCH error, ROLL and PITCH are still in error
284 ASSERT_NEAR(-128.1, pidData
[FD_ROLL
].P
, calculateTolerance(-128.1));
285 ASSERT_NEAR(185.8, pidData
[FD_PITCH
].P
, calculateTolerance(185.8));
286 ASSERT_NEAR(-224.2, pidData
[FD_YAW
].P
, calculateTolerance(-224.2));
287 ASSERT_NEAR(-23.5, pidData
[FD_ROLL
].I
, calculateTolerance(-23.5));
288 ASSERT_NEAR(19.6, pidData
[FD_PITCH
].I
, calculateTolerance(19.6));
289 ASSERT_NEAR(-8.7, pidData
[FD_YAW
].I
, calculateTolerance(-8.7));
290 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].D
);
291 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].D
);
292 ASSERT_NEAR(-132.25, pidData
[FD_YAW
].D
, calculateTolerance(-132.25));
294 // Simulate Iterm behaviour during mixer saturation
295 simulatedMotorMixRange
= 1.2f
;
296 pidController(pidProfile
, currentTestTime());
297 ASSERT_NEAR(-23.5, pidData
[FD_ROLL
].I
, calculateTolerance(-23.5));
298 ASSERT_NEAR(19.6, pidData
[FD_PITCH
].I
, calculateTolerance(19.6));
299 ASSERT_NEAR(-8.8, pidData
[FD_YAW
].I
, calculateTolerance(-8.8));
300 simulatedMotorMixRange
= 0;
302 // Match the stick to gyro to stop error
303 simulatedSetpointRate
[FD_ROLL
] = 100;
304 simulatedSetpointRate
[FD_PITCH
] = -100;
305 simulatedSetpointRate
[FD_YAW
] = 100;
307 for(int loop
= 0; loop
< 5; loop
++) {
308 pidController(pidProfile
, currentTestTime());
310 // Iterm is stalled as it is not accumulating anymore
311 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].P
);
312 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].P
);
313 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].P
);
314 ASSERT_NEAR(-23.5, pidData
[FD_ROLL
].I
, calculateTolerance(-23.5));
315 ASSERT_NEAR(19.6, pidData
[FD_PITCH
].I
, calculateTolerance(19.6));
316 ASSERT_NEAR(-10.6, pidData
[FD_YAW
].I
, calculateTolerance(-10.6));
317 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].D
);
318 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].D
);
319 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].D
);
321 // Now disable Stabilisation
322 pidStabilisationState(PID_STABILISATION_OFF
);
323 pidController(pidProfile
, currentTestTime());
325 // Should all be zero again
326 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].P
);
327 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].P
);
328 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].P
);
329 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].I
);
330 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].I
);
331 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].I
);
332 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].D
);
333 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].D
);
334 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].D
);
337 TEST(pidControllerTest
, testPidLevel
) {
338 // Make sure to start with fresh values
340 ENABLE_ARMING_FLAG(ARMED
);
341 pidStabilisationState(PID_STABILISATION_ON
);
343 // Test Angle mode response
344 enableFlightMode(ANGLE_MODE
);
345 float currentPidSetpoint
= 30;
346 rollAndPitchTrims_t angleTrim
= { { 0, 0 } };
348 currentPidSetpoint
= pidLevel(FD_ROLL
, pidProfile
, &angleTrim
, currentPidSetpoint
);
349 EXPECT_FLOAT_EQ(0, currentPidSetpoint
);
350 currentPidSetpoint
= pidLevel(FD_PITCH
, pidProfile
, &angleTrim
, currentPidSetpoint
);
351 EXPECT_FLOAT_EQ(0, currentPidSetpoint
);
353 // Test attitude response
354 setStickPosition(FD_ROLL
, 1.0f
);
355 setStickPosition(FD_PITCH
, -1.0f
);
356 currentPidSetpoint
= pidLevel(FD_ROLL
, pidProfile
, &angleTrim
, currentPidSetpoint
);
357 EXPECT_FLOAT_EQ(275, currentPidSetpoint
);
358 currentPidSetpoint
= pidLevel(FD_PITCH
, pidProfile
, &angleTrim
, currentPidSetpoint
);
359 EXPECT_FLOAT_EQ(-275, currentPidSetpoint
);
361 setStickPosition(FD_ROLL
, -0.5f
);
362 setStickPosition(FD_PITCH
, 0.5f
);
363 currentPidSetpoint
= pidLevel(FD_ROLL
, pidProfile
, &angleTrim
, currentPidSetpoint
);
364 EXPECT_FLOAT_EQ(-137.5, currentPidSetpoint
);
365 currentPidSetpoint
= pidLevel(FD_PITCH
, pidProfile
, &angleTrim
, currentPidSetpoint
);
366 EXPECT_FLOAT_EQ(137.5, currentPidSetpoint
);
368 attitude
.values
.roll
= -275;
369 attitude
.values
.pitch
= 275;
370 currentPidSetpoint
= pidLevel(FD_ROLL
, pidProfile
, &angleTrim
, currentPidSetpoint
);
371 EXPECT_FLOAT_EQ(0, currentPidSetpoint
);
372 currentPidSetpoint
= pidLevel(FD_PITCH
, pidProfile
, &angleTrim
, currentPidSetpoint
);
373 EXPECT_FLOAT_EQ(0, currentPidSetpoint
);
375 // Disable ANGLE_MODE
376 disableFlightMode(ANGLE_MODE
);
377 currentPidSetpoint
= pidLevel(FD_ROLL
, pidProfile
, &angleTrim
, currentPidSetpoint
);
378 EXPECT_FLOAT_EQ(0, currentPidSetpoint
);
379 currentPidSetpoint
= pidLevel(FD_PITCH
, pidProfile
, &angleTrim
, currentPidSetpoint
);
380 EXPECT_FLOAT_EQ(0, currentPidSetpoint
);
384 TEST(pidControllerTest
, testPidHorizon
) {
386 ENABLE_ARMING_FLAG(ARMED
);
387 pidStabilisationState(PID_STABILISATION_ON
);
388 enableFlightMode(HORIZON_MODE
);
390 // Test full stick response
391 setStickPosition(FD_ROLL
, 1.0f
);
392 setStickPosition(FD_PITCH
, -1.0f
);
393 EXPECT_FLOAT_EQ(0, calcHorizonLevelStrength());
395 // Expect full rate output on full stick
396 // Test zero stick response
397 setStickPosition(FD_ROLL
, 0);
398 setStickPosition(FD_PITCH
, 0);
399 EXPECT_FLOAT_EQ(1, calcHorizonLevelStrength());
401 // Test small stick response
402 setStickPosition(FD_ROLL
, 0.1f
);
403 setStickPosition(FD_PITCH
, -0.1f
);
404 ASSERT_NEAR(0.82, calcHorizonLevelStrength(), calculateTolerance(0.82));
407 TEST(pidControllerTest
, testMixerSaturation
) {
409 ENABLE_ARMING_FLAG(ARMED
);
410 pidStabilisationState(PID_STABILISATION_ON
);
412 // Test full stick response
413 setStickPosition(FD_ROLL
, 1.0f
);
414 setStickPosition(FD_PITCH
, -1.0f
);
415 setStickPosition(FD_YAW
, 1.0f
);
416 simulatedMotorMixRange
= 2.0f
;
417 pidController(pidProfile
, currentTestTime());
419 // Expect no iterm accumulation
420 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].I
);
421 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].I
);
422 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].I
);
424 // Test itermWindup limit
425 // First store values without exceeding iterm windup limit
427 ENABLE_ARMING_FLAG(ARMED
);
428 pidStabilisationState(PID_STABILISATION_ON
);
429 setStickPosition(FD_ROLL
, 0.1f
);
430 setStickPosition(FD_PITCH
, -0.1f
);
431 setStickPosition(FD_YAW
, 0.1f
);
432 simulatedMotorMixRange
= 0.0f
;
433 pidController(pidProfile
, currentTestTime());
434 float rollTestIterm
= pidData
[FD_ROLL
].I
;
435 float pitchTestIterm
= pidData
[FD_PITCH
].I
;
436 float yawTestIterm
= pidData
[FD_YAW
].I
;
438 // Now compare values when exceeding the limit
440 ENABLE_ARMING_FLAG(ARMED
);
441 pidStabilisationState(PID_STABILISATION_ON
);
442 setStickPosition(FD_ROLL
, 0.1f
);
443 setStickPosition(FD_PITCH
, -0.1f
);
444 setStickPosition(FD_YAW
, 0.1f
);
445 simulatedMotorMixRange
= (pidProfile
->itermWindupPointPercent
+ 1) / 100.0f
;
446 pidController(pidProfile
, currentTestTime());
447 ASSERT_LT(pidData
[FD_ROLL
].I
, rollTestIterm
);
448 ASSERT_GE(pidData
[FD_PITCH
].I
, pitchTestIterm
);
449 ASSERT_LT(pidData
[FD_YAW
].I
, yawTestIterm
);
452 // TODO - Add more scenarios
453 TEST(pidControllerTest
, testCrashRecoveryMode
) {
455 pidProfile
->crash_recovery
= PID_CRASH_RECOVERY_ON
;
457 ENABLE_ARMING_FLAG(ARMED
);
458 pidStabilisationState(PID_STABILISATION_ON
);
459 sensorsSet(SENSOR_ACC
);
461 EXPECT_FALSE(crashRecoveryModeActive());
463 int loopsToCrashTime
= (int)((pidProfile
->crash_time
* 1000) / targetPidLooptime
) + 1;
465 // generate crash detection for roll axis
466 gyro
.gyroADCf
[FD_ROLL
] = 800;
467 simulatedMotorMixRange
= 1.2f
;
468 for (int loop
=0; loop
<= loopsToCrashTime
; loop
++) {
469 gyro
.gyroADCf
[FD_ROLL
] += gyro
.gyroADCf
[FD_ROLL
];
470 // advance the time to avoid initialized state prevention of crash recovery
471 pidController(pidProfile
, currentTestTime() + 2000000);
474 EXPECT_TRUE(crashRecoveryModeActive());
475 // Add additional verifications
478 TEST(pidControllerTest
, testFeedForward
) {
480 ENABLE_ARMING_FLAG(ARMED
);
481 pidStabilisationState(PID_STABILISATION_ON
);
483 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].F
);
484 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].F
);
485 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].F
);
487 // Match the stick to gyro to stop error
488 setStickPosition(FD_ROLL
, 1.0f
);
489 setStickPosition(FD_PITCH
, -1.0f
);
490 setStickPosition(FD_YAW
, -1.0f
);
492 pidController(pidProfile
, currentTestTime());
494 ASSERT_NEAR(2232.78, pidData
[FD_ROLL
].F
, calculateTolerance(2232.78));
495 ASSERT_NEAR(-2061.03, pidData
[FD_PITCH
].F
, calculateTolerance(-2061.03));
496 ASSERT_NEAR(-82.52, pidData
[FD_YAW
].F
, calculateTolerance(-82.5));
498 // Match the stick to gyro to stop error
499 setStickPosition(FD_ROLL
, 0.5f
);
500 setStickPosition(FD_PITCH
, -0.5f
);
501 setStickPosition(FD_YAW
, -0.5f
);
503 pidController(pidProfile
, currentTestTime());
505 ASSERT_NEAR(-558.20, pidData
[FD_ROLL
].F
, calculateTolerance(-558.20));
506 ASSERT_NEAR(515.26, pidData
[FD_PITCH
].F
, calculateTolerance(515.26));
507 ASSERT_NEAR(-41.26, pidData
[FD_YAW
].F
, calculateTolerance(-41.26));
509 for (int loop
=0; loop
<= 15; loop
++) {
510 gyro
.gyroADCf
[FD_ROLL
] += gyro
.gyroADCf
[FD_ROLL
];
511 pidController(pidProfile
, currentTestTime());
514 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].F
);
515 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].F
);
516 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].F
);
520 TEST(pidControllerTest
, testItermRelax
) {
522 pidProfile
->iterm_relax
= ITERM_RELAX_RP
;
523 ENABLE_ARMING_FLAG(ARMED
);
524 pidStabilisationState(PID_STABILISATION_ON
);
526 pidProfile
->iterm_relax_type
= ITERM_RELAX_SETPOINT
;
529 float itermErrorRate
= 0;
530 float currentPidSetpoint
= 0;
533 applyItermRelax(FD_PITCH
, 0, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
534 EXPECT_FLOAT_EQ(itermErrorRate
, 0);
535 itermErrorRate
= -10;
536 currentPidSetpoint
= 10;
537 pidData
[FD_PITCH
].I
= 10;
539 applyItermRelax(FD_PITCH
, pidData
[FD_PITCH
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
541 ASSERT_NEAR(-8.16, itermErrorRate
, calculateTolerance(-8.16));
542 currentPidSetpoint
+= ITERM_RELAX_SETPOINT_THRESHOLD
;
543 applyItermRelax(FD_PITCH
, pidData
[FD_PITCH
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
544 ASSERT_NEAR(-2.69, itermErrorRate
, calculateTolerance(-2.69));
545 applyItermRelax(FD_PITCH
, pidData
[FD_PITCH
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
546 ASSERT_NEAR(-0.84, itermErrorRate
, calculateTolerance(-0.84));
548 pidProfile
->iterm_relax_type
= ITERM_RELAX_GYRO
;
551 currentPidSetpoint
= 100;
552 applyItermRelax(FD_PITCH
, pidData
[FD_PITCH
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
553 EXPECT_FLOAT_EQ(itermErrorRate
, 0);
555 itermErrorRate
= -10;
556 applyItermRelax(FD_PITCH
, pidData
[FD_PITCH
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
557 ASSERT_NEAR(7, itermErrorRate
, calculateTolerance(7));
559 applyItermRelax(FD_PITCH
, pidData
[FD_PITCH
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
560 ASSERT_NEAR(-10, itermErrorRate
, calculateTolerance(-10));
562 pidProfile
->iterm_relax
= ITERM_RELAX_RP_INC
;
565 itermErrorRate
= -10;
566 pidData
[FD_PITCH
].I
= 10;
567 currentPidSetpoint
= 10;
568 applyItermRelax(FD_PITCH
, pidData
[FD_PITCH
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
569 EXPECT_FLOAT_EQ(itermErrorRate
, -10);
571 pidData
[FD_PITCH
].I
= -10;
572 applyItermRelax(FD_PITCH
, pidData
[FD_PITCH
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
573 EXPECT_FLOAT_EQ(itermErrorRate
, 10);
574 itermErrorRate
= -10;
575 currentPidSetpoint
= 10;
576 applyItermRelax(FD_PITCH
, pidData
[FD_PITCH
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
577 EXPECT_FLOAT_EQ(itermErrorRate
, -100);
579 pidProfile
->iterm_relax_type
= ITERM_RELAX_SETPOINT
;
582 itermErrorRate
= -10;
583 currentPidSetpoint
= ITERM_RELAX_SETPOINT_THRESHOLD
;
584 applyItermRelax(FD_YAW
, pidData
[FD_YAW
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
585 EXPECT_FLOAT_EQ(itermErrorRate
, -10);
587 pidProfile
->iterm_relax
= ITERM_RELAX_RPY
;
589 applyItermRelax(FD_YAW
, pidData
[FD_YAW
].I
, gyroRate
, &itermErrorRate
, ¤tPidSetpoint
);
590 ASSERT_NEAR(-6.46, itermErrorRate
, calculateTolerance(-3.6));
593 // TODO - Add more tests
594 TEST(pidControllerTest
, testAbsoluteControl
) {
596 pidProfile
->abs_control_gain
= 10;
598 ENABLE_ARMING_FLAG(ARMED
);
599 pidStabilisationState(PID_STABILISATION_ON
);
603 float itermErrorRate
= 10;
604 float currentPidSetpoint
= 10;
606 applyAbsoluteControl(FD_PITCH
, gyroRate
, ¤tPidSetpoint
, &itermErrorRate
);
608 ASSERT_NEAR(10.8, itermErrorRate
, calculateTolerance(10.8));
609 ASSERT_NEAR(10.8, currentPidSetpoint
, calculateTolerance(10.8));
611 applyAbsoluteControl(FD_PITCH
, gyroRate
, ¤tPidSetpoint
, &itermErrorRate
);
612 ASSERT_NEAR(10.8, itermErrorRate
, calculateTolerance(10.8));
613 ASSERT_NEAR(10.8, currentPidSetpoint
, calculateTolerance(10.8));
616 axisError
[FD_PITCH
] = -60;
617 applyAbsoluteControl(FD_PITCH
, gyroRate
, ¤tPidSetpoint
, &itermErrorRate
);
618 ASSERT_NEAR(-79.2, itermErrorRate
, calculateTolerance(-79.2));
619 ASSERT_NEAR(-79.2, currentPidSetpoint
, calculateTolerance(-79.2));
622 TEST(pidControllerTest
, testDtermFiltering
) {
626 TEST(pidControllerTest
, testItermRotationHandling
) {
630 rotateItermAndAxisError();
631 EXPECT_FLOAT_EQ(pidData
[FD_ROLL
].I
, 0);
632 EXPECT_FLOAT_EQ(pidData
[FD_PITCH
].I
, 0);
633 EXPECT_FLOAT_EQ(pidData
[FD_YAW
].I
, 0);
635 pidProfile
->iterm_rotation
= true;
638 rotateItermAndAxisError();
639 EXPECT_FLOAT_EQ(pidData
[FD_ROLL
].I
, 0);
640 EXPECT_FLOAT_EQ(pidData
[FD_PITCH
].I
, 0);
641 EXPECT_FLOAT_EQ(pidData
[FD_YAW
].I
, 0);
643 pidData
[FD_ROLL
].I
= 10;
644 pidData
[FD_PITCH
].I
= 1000;
645 pidData
[FD_YAW
].I
= 1000;
646 gyro
.gyroADCf
[FD_ROLL
] = -1000;
647 rotateItermAndAxisError();
648 EXPECT_FLOAT_EQ(pidData
[FD_ROLL
].I
, 10);
649 ASSERT_NEAR(860.37, pidData
[FD_PITCH
].I
, calculateTolerance(860.37));
650 ASSERT_NEAR(1139.6, pidData
[FD_YAW
].I
, calculateTolerance(1139.6));
652 pidProfile
->abs_control_gain
= 10;
654 pidData
[FD_ROLL
].I
= 10;
655 pidData
[FD_PITCH
].I
= 1000;
656 pidData
[FD_YAW
].I
= 1000;
658 gyro
.gyroADCf
[FD_ROLL
] = -1000;
659 // FIXME - axisError changes don't affect the system. This is a potential bug or intendend behaviour?
660 axisError
[FD_PITCH
] = 1000;
661 axisError
[FD_YAW
] = 1000;
662 rotateItermAndAxisError();
663 EXPECT_FLOAT_EQ(pidData
[FD_ROLL
].I
, 10);
664 ASSERT_NEAR(860.37, pidData
[FD_PITCH
].I
, calculateTolerance(860.37));
665 ASSERT_NEAR(1139.6, pidData
[FD_YAW
].I
, calculateTolerance(1139.6));
668 TEST(pidControllerTest
, testLaunchControl
) {
669 // The launchControlGain is indirectly tested since when launch control is active the
670 // the gain overrides the PID settings. If the logic to use launchControlGain wasn't
671 // working then any I calculations would be incorrect.
674 unitLaunchControlActive
= true;
675 ENABLE_ARMING_FLAG(ARMED
);
676 pidStabilisationState(PID_STABILISATION_ON
);
678 // test that feedforward and D are disabled (always zero) when launch control is active
680 pidController(pidProfile
, currentTestTime());
682 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].F
);
683 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].F
);
684 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].F
);
685 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].D
);
686 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].D
);
687 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].D
);
689 // Move the sticks to induce feedforward
690 setStickPosition(FD_ROLL
, 0.5f
);
691 setStickPosition(FD_PITCH
, -0.5f
);
692 setStickPosition(FD_YAW
, -0.5f
);
694 // add gyro activity to induce D
695 gyro
.gyroADCf
[FD_ROLL
] = -1000;
696 gyro
.gyroADCf
[FD_PITCH
] = 1000;
697 gyro
.gyroADCf
[FD_YAW
] = -1000;
699 pidController(pidProfile
, currentTestTime());
701 // validate that feedforwad is still 0
702 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].F
);
703 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].F
);
704 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].F
);
706 // validate that D is still 0
707 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].D
);
708 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].D
);
709 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].D
);
711 // test NORMAL mode - expect P/I on roll and pitch, P on yaw but I == 0
712 unitLaunchControlMode
= LAUNCH_CONTROL_MODE_NORMAL
;
714 unitLaunchControlActive
= true;
715 ENABLE_ARMING_FLAG(ARMED
);
716 pidStabilisationState(PID_STABILISATION_ON
);
718 pidController(pidProfile
, currentTestTime());
720 gyro
.gyroADCf
[FD_ROLL
] = -20;
721 gyro
.gyroADCf
[FD_PITCH
] = 20;
722 gyro
.gyroADCf
[FD_YAW
] = -20;
723 pidController(pidProfile
, currentTestTime());
725 ASSERT_NEAR(25.62, pidData
[FD_ROLL
].P
, calculateTolerance(25.62));
726 ASSERT_NEAR(1.56, pidData
[FD_ROLL
].I
, calculateTolerance(1.56));
727 ASSERT_NEAR(-37.15, pidData
[FD_PITCH
].P
, calculateTolerance(-37.15));
728 ASSERT_NEAR(-1.56, pidData
[FD_PITCH
].I
, calculateTolerance(-1.56));
729 ASSERT_NEAR(44.84, pidData
[FD_YAW
].P
, calculateTolerance(44.84));
730 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].I
);
732 // test PITCHONLY mode - expect P/I only on pitch; I cannot go negative
733 unitLaunchControlMode
= LAUNCH_CONTROL_MODE_PITCHONLY
;
735 unitLaunchControlActive
= true;
736 ENABLE_ARMING_FLAG(ARMED
);
737 pidStabilisationState(PID_STABILISATION_ON
);
739 pidController(pidProfile
, currentTestTime());
741 // first test that pitch I is prevented from going negative
742 gyro
.gyroADCf
[FD_ROLL
] = 0;
743 gyro
.gyroADCf
[FD_PITCH
] = 20;
744 gyro
.gyroADCf
[FD_YAW
] = 0;
745 pidController(pidProfile
, currentTestTime());
747 EXPECT_FLOAT_EQ(0, pidData
[FD_PITCH
].I
);
749 gyro
.gyroADCf
[FD_ROLL
] = 20;
750 gyro
.gyroADCf
[FD_PITCH
] = -20;
751 gyro
.gyroADCf
[FD_YAW
] = 20;
752 pidController(pidProfile
, currentTestTime());
754 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].P
);
755 EXPECT_FLOAT_EQ(0, pidData
[FD_ROLL
].I
);
756 ASSERT_NEAR(37.15, pidData
[FD_PITCH
].P
, calculateTolerance(37.15));
757 ASSERT_NEAR(1.56, pidData
[FD_PITCH
].I
, calculateTolerance(1.56));
758 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].P
);
759 EXPECT_FLOAT_EQ(0, pidData
[FD_YAW
].I
);
761 // test FULL mode - expect P/I on all axes
762 unitLaunchControlMode
= LAUNCH_CONTROL_MODE_FULL
;
764 unitLaunchControlActive
= true;
765 ENABLE_ARMING_FLAG(ARMED
);
766 pidStabilisationState(PID_STABILISATION_ON
);
768 pidController(pidProfile
, currentTestTime());
770 gyro
.gyroADCf
[FD_ROLL
] = -20;
771 gyro
.gyroADCf
[FD_PITCH
] = 20;
772 gyro
.gyroADCf
[FD_YAW
] = -20;
773 pidController(pidProfile
, currentTestTime());
775 ASSERT_NEAR(25.62, pidData
[FD_ROLL
].P
, calculateTolerance(25.62));
776 ASSERT_NEAR(1.56, pidData
[FD_ROLL
].I
, calculateTolerance(1.56));
777 ASSERT_NEAR(-37.15, pidData
[FD_PITCH
].P
, calculateTolerance(-37.15));
778 ASSERT_NEAR(-1.56, pidData
[FD_PITCH
].I
, calculateTolerance(-1.56));
779 ASSERT_NEAR(44.84, pidData
[FD_YAW
].P
, calculateTolerance(44.84));
780 ASSERT_NEAR(1.56, pidData
[FD_YAW
].I
, calculateTolerance(1.56));