From 9700c54654665c12d5c7c55899338c271c4cd1cd Mon Sep 17 00:00:00 2001
From: Jeremiah the Great <jcoulson@ethz.ch>
Date: Tue, 26 Nov 2019 19:57:35 +0100
Subject: [PATCH] tried debugging. now back to weird controller
---
.../include/nodes/CsoneControllerService.h | 6 ++++++
.../src/nodes/CsoneControllerService.cpp | 16 +++++++++++-----
2 files changed, 17 insertions(+), 5 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
index 73fac830..ffa37f32 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
@@ -174,6 +174,12 @@ float m_A=0,m_B=0,m_C=0,m_D=1;
// 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);
+
// The location error of the Crazyflie at the "previous" time step
float m_previous_stateErrorInertial[9];
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index 922d8e9f..5cde749a 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -238,7 +238,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
float rollRate_forResponse = 0;
float pitchRate_forResponse = 0;
float yawRate_forResponse = 0;
- float beta_ref = 0;
// Perform the "-Kx" LQR computation for the yaw rate to respoond with
for(int i = 0; i < 9; ++i)
@@ -253,9 +252,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
yawRate_forResponse -= yaml_gainMatrixYawRate[i] * stateErrorBody[i];
}
- m_controller_state = m_A*m_controller_state + m_B*stateErrorBody[0];
- beta_ref = m_C*m_controller_state + m_D*stateErrorBody[0];
- pitchRate_forResponse = 5*(beta_ref-request.ownCrazyflie.pitch);
+ m_controller_state = m_A*m_controller_state - m_B*stateErrorBody[0];
+ m_beta_ref = m_C*m_controller_state - m_D*stateErrorBody[0];
+
+ // 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;
@@ -577,6 +579,7 @@ void requestControllerParametersCallback(const SetpointWithHeader& newSetpoint)
if (isRevelant)
{
convertIntoDiscreteTimeParameters(newSetpoint.x, newSetpoint.y, newSetpoint.z);
+ m_controller_state = 0;
}
}
@@ -606,6 +609,9 @@ void convertIntoDiscreteTimeParameters(float k, float T, float alpha)
m_C = k/(alpha*T)*(1-1/alpha);
m_D = k/alpha;
+ ROS_INFO_STREAM("Parameters changed to k="<<k<<", T="<<T<<", alpha="<<alpha);
+ ROS_INFO_STREAM("Matrices changed to A="<<m_A<<", B="<<m_B<<", C="<<m_C<<", D="<<m_D);
+
}
@@ -1006,7 +1012,7 @@ int main(int argc, char* argv[]) {
// the callback function. This subscriber will mainly receive
// messages from the "flying agent GUI" when the setpoint is changed
// by the user.
- ros::Subscriber requestControllerParametersSubscriber = nodeHandle.subscribe("requestControllerParameters", 1, requestControllerParametersCallback);
+ ros::Subscriber requestControllerParametersSubscriber = nodeHandle.subscribe("RequestControllerParametersChange", 1, requestControllerParametersCallback);
// Instantiate the class variable "m_setpointChangedPublisher" to
// be a "ros::Publisher". This variable advertises under the name
--
GitLab