tried debugging. now back to weird controller

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

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