From 91a11def6badf2bab80293b7579d7469c5fe5954 Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Tue, 5 Feb 2019 11:33:53 +0100
Subject: [PATCH] Fixed small error in Tuning Controller relating to gain
multipliers
---
.../src/nodes/TuningControllerService.cpp | 14 +++++++++-----
1 file changed, 9 insertions(+), 5 deletions(-)
diff --git a/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
index 289e3adb..05ff0300 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/TuningControllerService.cpp
@@ -914,7 +914,7 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
for(int i = 0; i < 6; ++i)
{
// BODY FRAME Y CONTROLLER
- lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
+ //lqr_angleRateNested_prev_rollAngle -= gainMatrixRollAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
// BODY FRAME X CONTROLLER
//lqr_angleRateNested_prev_pitchAngle -= gainMatrixPitchAngle_50Hz[i] * stateErrorBody[i] * multiplier_horizontal;
// > ALITUDE CONTROLLER (i.e., z-controller):
@@ -926,7 +926,7 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// BODY FRAME Z CONTROLLER
//lqr_angleRateNested_prev_yawAngle = setpoint[3];
- lqr_angleRateNested_prev_yawAngle = stateErrorBody[8];
+ //lqr_angleRateNested_prev_yawAngle = stateErrorBody[8];
}
@@ -954,11 +954,11 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
for(int i = 0; i < 4; ++i)
{
// BODY FRAME Y CONTROLLER
- rollRate_forResponse -= gainMatrixRollRate_Nested[i] * temp_stateAngleError[i];
+ //rollRate_forResponse -= gainMatrixRollRate_Nested[i] * temp_stateAngleError[i];
// BODY FRAME X CONTROLLER
pitchRate_forResponse -= gainMatrixPitchRate_Nested[i] * temp_stateAngleError[i];
// BODY FRAME Z CONTROLLER
- yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i] * multiplier_heading;
+ //yawRate_forResponse -= gainMatrixYawRate_Nested[i] * temp_stateAngleError[i] * multiplier_heading;
}
@@ -967,8 +967,12 @@ void calculateControlOutput_viaLQRforAnglesRatesNested( float stateErrorBody[12]
// Perform the "-Kx" LQR computation for the rates and thrust:
for(int i = 0; i < 9; ++i)
{
+ // BODY FRAME Y CONTROLLER
+ rollRate_forResponse -= gainMatrixRollRate[i] * stateErrorBody[i];
// > ALITUDE CONTROLLER (i.e., z-controller):
- thrustAdjustment_200Hz -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i] * multiplier_vertical;
+ thrustAdjustment_200Hz -= gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
+ // BODY FRAME YAW CONTROLLER
+ yawRate_forResponse -= gainMatrixYawRate[i] * stateErrorBody[i];
}
--
GitLab