switched order of read write indexes for lead lag controller with time delay

dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp

@@ -186,12 +186,6 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 	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
@@ -208,6 +202,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 		read_index += m_x_inertial_buffer.size();
 	}
 
+	// 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];
 
 
 	// Define a local array to fill in with the state error