Merge branch 'cs1' of gitlab.ethz.ch:dfall/dfall-system into cs1

Forgot to pull before committing

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);
 

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