From 00c5f331d07eae6f9a8bba963ac5e679241dbaed Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Thu, 28 Nov 2019 06:32:48 +0100
Subject: [PATCH] Small changes to the CS1 controller service, mainly adding
comments and re-formatting
---
.../include/nodes/CsoneControllerService.h | 5 +-
.../src/nodes/CsoneControllerService.cpp | 49 ++++++++++---------
2 files changed, 28 insertions(+), 26 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
index ffa37f32..085cdc5a 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
@@ -169,14 +169,11 @@ std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00,
float m_cf_weight_in_newtons = 0.0;
// The state space matrices of lead compensator controller
-float m_A=0,m_B=0,m_C=0,m_D=1;
+float m_A=0.0,m_B=0.0,m_C=0.0,m_D=1.0;
// The state of lead compensator controller
float m_controller_state = 0.0;
-// previous beta_ref given by controller
-float m_beta_ref = 0;
-
// The inertial x measurement during a time window of 200 measurements
std::vector<float> m_x (200,0);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index 5cde749a..1f90286f 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -252,13 +252,32 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
yawRate_forResponse -= yaml_gainMatrixYawRate[i] * stateErrorBody[i];
}
+
+
+ // For the x-controller:
+ // > Execute the discrete-time state space equivalent of the lead
+ // compensator specified.
+ // > NOTE: the new pitch reference, i.e., the output to the state
+ // space, MUST be calucated before the state update equation
+ // is evaulated, in equations:
+ // FIRST: y(t) = C x(t) + D u(t)
+ // SECOND: x(t+1) = A x(t) + B u(t)
+
+ // > FIRST: compute the new pitch reference
+ float pitch_ref = m_C*m_controller_state - m_D*stateErrorBody[0];
+
+ // > SECOND: evaluate the state update equation
m_controller_state = m_A*m_controller_state - m_B*stateErrorBody[0];
- m_beta_ref = m_C*m_controller_state - m_D*stateErrorBody[0];
+
+ // > THIRD: perform the inner controller
+ pitchRate_forResponse = 5.0*(pitch_ref-request.ownCrazyflie.pitch);
+
+
// m_beta_ref = 0.99*m_beta_ref - 1.71/5*stateErrorBody[0] - 1.33/5*m_controller_state;
- // pitchRate_forResponse = 5*(m_beta_ref-request.ownCrazyflie.pitch);
// m_controller_state = -stateErrorBody[0];
+
// Put the computed body rate commands into the "response" variable
response.controlOutput.roll = rollRate_forResponse;
response.controlOutput.pitch = pitchRate_forResponse;
@@ -579,34 +598,20 @@ void requestControllerParametersCallback(const SetpointWithHeader& newSetpoint)
if (isRevelant)
{
convertIntoDiscreteTimeParameters(newSetpoint.x, newSetpoint.y, newSetpoint.z);
- m_controller_state = 0;
+ m_controller_state = 0.0;
}
}
// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
void convertIntoDiscreteTimeParameters(float k, float T, float alpha)
{
- if (alpha > 1)
- {
- alpha = 1;
- }
- else if (alpha<0.1)
- {
- alpha = 0.1;
- }
+ if (alpha > 1){alpha = 1;} else if (alpha<0.1){alpha = 0.1;}
- if (T > 100)
- {
- T = 100;
- }
- else if (T<0.1)
- {
- T = 0.1;
- }
+ if (T > 100){T = 100;} else if (T<0.1){T = 0.1;}
- m_A = pow(2.71828,(-1/(yaml_control_frequency*alpha*T)));
- m_B = -alpha*T*(m_A-1);
- m_C = k/(alpha*T)*(1-1/alpha);
+ m_A = pow(2.71828,(-1.0/(yaml_control_frequency*alpha*T)));
+ m_B = -alpha*T*(m_A-1.0);
+ m_C = k/(alpha*T)*(1.0-1.0/alpha);
m_D = k/alpha;
ROS_INFO_STREAM("Parameters changed to k="<<k<<", T="<<T<<", alpha="<<alpha);
--
GitLab