From ba848ac3f423c71636f9d994393cf39312a2bfaa Mon Sep 17 00:00:00 2001
From: Jeremiah the Great <jcoulson@ethz.ch>
Date: Tue, 26 Nov 2019 17:25:54 +0100
Subject: [PATCH] finished writing CS1 controller without time delayz
---
.../include/nodes/CsoneControllerService.h | 12 ++++
.../src/nodes/CsoneControllerService.cpp | 61 ++++++++++++++++++-
2 files changed, 71 insertions(+), 2 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
index ea482a79..73fac830 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
@@ -168,6 +168,12 @@ std::vector<float> yaml_gainMatrixYawRate = { 0.00, 0.00, 0.00,
// The weight of the Crazyflie in Newtons, i.e., mg
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;
+
+// The state of lead compensator controller
+float m_controller_state = 0.0;
+
// The location error of the Crazyflie at the "previous" time step
float m_previous_stateErrorInertial[9];
@@ -223,9 +229,15 @@ float computeMotorPolyBackward(float thrust);
// REQUEST SETPOINT CHANGE CALLBACK
void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);
+// REQUEST CONTROLLER PARAMETERS CALLBACK
+void requestControllerParametersCallback(const SetpointWithHeader& newSetpoint);
+
// CHANGE SETPOINT FUNCTION
void setNewSetpoint(float x, float y, float z, float yaw);
+// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
+void convertIntoDiscreteTimeParameters(float k, float T, float alpha);
+
// GET CURRENT SETPOINT SERVICE CALLBACK
bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index fb2aa311..922d8e9f 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -238,20 +238,25 @@ 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)
{
// For the z-controller
thrustAdjustment -= yaml_gainMatrixThrust_NineStateVector[i] * stateErrorBody[i];
- // For the x-controller
- pitchRate_forResponse -= yaml_gainMatrixPitchRate[i] * stateErrorBody[i];
+ // // For the x-controller
+ // pitchRate_forResponse -= yaml_gainMatrixPitchRate[i] * stateErrorBody[i];
// For the y-controller
rollRate_forResponse -= yaml_gainMatrixRollRate[i] * stateErrorBody[i];
// For the yaw-controller
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);
+
// Put the computed body rate commands into the "response" variable
response.controlOutput.roll = rollRate_forResponse;
response.controlOutput.pitch = pitchRate_forResponse;
@@ -561,6 +566,48 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
return true;
}
+// REQUEST CONTROLLER PARAMETERS CALLBACK
+// Using x y z from SetpointWithHeader for k T alpha controller parameters
+void requestControllerParametersCallback(const SetpointWithHeader& newSetpoint)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ convertIntoDiscreteTimeParameters(newSetpoint.x, newSetpoint.y, newSetpoint.z);
+ }
+}
+
+// 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 (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_D = k/alpha;
+
+
+}
@@ -951,6 +998,16 @@ int main(int argc, char* argv[]) {
// And now we can instantiate the subscriber:
ros::Subscriber requestSetpointChangeSubscriber_from_coord = nodeHandle_to_coordinator.subscribe("CsoneControllerService/RequestSetpointChange", 1, requestSetpointChangeCallback);
+ // Instantiate the local variable "requestSetpointChangeSubscriber"
+ // to be a "ros::Subscriber" type variable that subscribes to the
+ // "RequestSetpointChange" topic and calls the class function
+ // "requestSetpointChangeCallback" each time a messaged is received
+ // on this topic and the message is passed as an input argument to
+ // 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);
+
// Instantiate the class variable "m_setpointChangedPublisher" to
// be a "ros::Publisher". This variable advertises under the name
// "SetpointChanged" and is a message with the structure defined
--
GitLab