diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
index 1dee19c3ba760af3ab5edeac0eb446d38bac3113..cdaa0bfd6d967b3acf51c11da88505a0fc629a5f 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CsoneControllerService.h
@@ -191,8 +191,11 @@ float m_D=2;
// The state of lead compensator controller
float m_controller_state = 0.0;
-// The inertial x measurement during a time window of 200 measurements
-std::vector<float> m_x (200,0);
+// The inertial x measurement during a time window of 300 measurements
+std::vector<float> m_x_inertial_buffer (300,0);
+
+// The time delay as an integer of how many measurements to delay by
+int m_time_delay_in_steps = 0;
// The location error of the Crazyflie at the "previous" time step
float m_previous_stateErrorInertial[9];
@@ -259,17 +262,23 @@ void setNewSetpoint(float x, float y, float z, float yaw);
// GET CURRENT SETPOINT SERVICE CALLBACK
bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
-// REQUEST CONTROLLER PARAMETERS CALLBACK
+// REQUEST INTEGRATOR STATE CHANGE CALLBACK
void requestIntegratorStateChangeCallback(const IntWithHeader& msg);
-// CHANGE SETPOINT FUNCTION
+// CHANGE INTEGRATOR STATE FUNCTION
void setNewIntegratorState(int newIntegratorState);
// GET CURRENT INTEGRATOR STATE SERVICE CALLBACK
bool getCurrentIntegratorStateCallback(IntIntService::Request &request, IntIntService::Response &response);
// REQUEST CONTROLLER PARAMETERS CALLBACK
-void requestControllerParametersCallback(const SetpointWithHeader& newSetpoint);
+void requestTimeDelayChangeCallback(const IntWithHeader& msg);
+
+// CHANGE TIME DELAY FUNCTION
+void setNewTimeDelay(int newTimeDelay);
+
+// REQUEST CONTROLLER PARAMETERS CALLBACK
+void requestControllerParametersChangeCallback(const SetpointWithHeader& newSetpoint);
// CHANGE CONTROLLER PARAMETERS INTO DISCRETE TIME FUNCTION
void convertIntoDiscreteTimeParameters(float k, float T, float alpha);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index cbf315497e9ac351c396c267a4e4d1c98f5c4685..9de709b0cbfc8cb04a09dc3f7339587d582173e0 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
@@ -181,12 +181,40 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// computation here, or you may find it convienient to create functions
// to keep you code cleaner
+ // Initialise static variables for handling the circular buffer
+ // of inertial x measurement
+ static int write_index = 0;
+ static int read_index = 0;
+
+ // Write the new data to the buffer
+ m_x_inertial_buffer[write_index] = request.ownCrazyflie.x;
+
+ // Read the data for this time step from the buffer
+ float inertial_x_for_this_time_step = m_x_inertial_buffer[read_index];
+
+ // Increment the write index
+ write_index += 1;
+ // And wrap it back into range if necessary
+ if (write_index>=m_x_inertial_buffer.size())
+ {
+ write_index = 0;
+ }
+
+ // Compute the next read index based on the delay
+ read_index = write_index - m_time_delay_in_steps;
+ // And wrap it back into range if necessary
+ if (read_index<0)
+ {
+ read_index += m_x_inertial_buffer.size();
+ }
+
+
// Define a local array to fill in with the state error
float stateErrorInertial[9];
// Fill in the (x,y,z) position error
- stateErrorInertial[0] = request.ownCrazyflie.x - m_setpoint[0];
+ stateErrorInertial[0] = inertial_x_for_this_time_step - m_setpoint[0];
stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1];
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
@@ -600,7 +628,7 @@ bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpoin
-// REQUEST SETPOINT CHANGE CALLBACK
+// REQUEST INTEGRATOR STATE CHANGE CALLBACK
void requestIntegratorStateChangeCallback(const IntWithHeader& msg)
{
// Check whether the message is relevant
@@ -653,7 +681,7 @@ void requestIntegratorStateChangeCallback(const IntWithHeader& msg)
}
}
-// CHANGE SETPOINT FUNCTION
+// CHANGE INTEGRATOR STATE FUNCTION
void setNewIntegratorState(int newIntegratorState){
// Directly set this to the class variable
m_integratorState = newIntegratorState;
@@ -673,7 +701,7 @@ void setNewIntegratorState(int newIntegratorState){
-// GET CURRENT SETPOINT SERVICE CALLBACK
+// GET CURRENT INTEGRATOR STATE SERVICE CALLBACK
bool getCurrentIntegratorStateCallback(IntIntService::Request &request, IntIntService::Response &response)
{
// Directly put the current integrator state into the response
@@ -684,8 +712,51 @@ bool getCurrentIntegratorStateCallback(IntIntService::Request &request, IntIntSe
// REQUEST CONTROLLER PARAMETERS CALLBACK
+void requestTimeDelayChangeCallback(const IntWithHeader& msg)
+{
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+ // Continue if the message is relevant
+ if (isRevelant)
+ {
+ // Extract the request change
+ int requested_time_delay = msg.data;
+
+ // Call the function for setting the time delay
+ setNewTimeDelay(requested_time_delay);
+ }
+}
+
+// CHANGE TIME DELAY FUNCTION
+void setNewTimeDelay(int newTimeDelay)
+{
+
+ // ASSUMPTION: that the time delay is in MILLISECONDS
+
+ // Compute the number of milliseconds per time step
+ float delta_T_in_milliseconds = 1000.0 / yaml_control_frequency;
+
+ // Convert the time delay to a number of time steps
+ int time_delay_in_steps = int( float(newTimeDelay) / delta_T_in_milliseconds );
+
+ // Wrap this value into the allowed limits
+ if (time_delay_in_steps<0)
+ time_delay_in_steps=0;
+ if(time_delay_in_steps>(m_x_inertial_buffer.size()-1))
+ time_delay_in_steps=m_x_inertial_buffer.size()-1;
+
+ // Put the time delay into the class variable
+ m_time_delay_in_steps = time_delay_in_steps;
+
+ // Inform the user about the change
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Time delay changed to " << time_delay_in_steps << " steps, i.e., " << float(time_delay_in_steps)*1000.0/yaml_control_frequency << " milliseconds" );
+}
+
+
+// REQUEST CONTROLLER PARAMETERS CHANGE CALLBACK
// Using x y z from SetpointWithHeader for k T alpha controller parameters
-void requestControllerParametersCallback(const SetpointWithHeader& newSetpoint)
+void requestControllerParametersChangeCallback(const SetpointWithHeader& newSetpoint)
{
// Check whether the message is relevant
bool isRevelant = checkMessageHeader( m_agentID , newSetpoint.shouldCheckForAgentID , newSetpoint.agentIDs );
@@ -710,8 +781,8 @@ void convertIntoDiscreteTimeParameters(float k, float T, float alpha)
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);
- ROS_INFO_STREAM("Matrices changed to A="<<m_A<<", B="<<m_B<<", C="<<m_C<<", D="<<m_D);
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Parameters changed to k="<<k<<", T="<<T<<", alpha="<<alpha);
+ ROS_INFO_STREAM("[CSONE CONTROLLER] Matrices changed to A="<<m_A<<", B="<<m_B<<", C="<<m_C<<", D="<<m_D);
}
@@ -1119,7 +1190,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("RequestControllerParametersChange", 1, requestControllerParametersCallback);
+ ros::Subscriber requestControllerParametersChangeSubscriber = nodeHandle.subscribe("RequestControllerParametersChange", 1, requestControllerParametersChangeCallback);
// Instantiate the class variable "m_setpointChangedPublisher" to
// be a "ros::Publisher". This variable advertises under the name
@@ -1173,6 +1244,17 @@ int main(int argc, char* argv[]) {
ros::ServiceServer getCurrentIntegratorStateService = nodeHandle.advertiseService("GetCurrentIntegratorState", getCurrentIntegratorStateCallback);
+ // Instantiate the local variable:
+ // variable name: "requestTimeDelayChangeSubscriber"
+ // variable type: ros::Subscriber
+ // name of message: "RequestTimeDelayChange"
+ // The message has the structure defined in the file:
+ // "IntWithHeader.msg" (located in the "msg" folder).
+ // The messages received by this subscriber typically come from
+ // the "flying agent GUI".
+ ros::Subscriber requestTimeDelayChangeSubscriber = nodeHandle.subscribe("RequestTimeDelayChange", 1, requestTimeDelayChangeCallback);
+
+
// Instantiate the local variable "service" to be a "ros::ServiceServer" type
// variable that advertises the service called "CsoneController". This service has