From 1d65be9158cf42816e1322c1ae6992f6fab03845 Mon Sep 17 00:00:00 2001
From: agent03 <jcoulson@ethz.ch>
Date: Thu, 5 Dec 2019 17:47:42 +0100
Subject: [PATCH] switched order of read write indexes for lead lag controller
 with time delay

---
 .../dfall_pkg/src/nodes/CsoneControllerService.cpp    | 11 +++++------
 1 file changed, 5 insertions(+), 6 deletions(-)

diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
index 9de709b0..7a39c37a 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/CsoneControllerService.cpp
+++ b/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
-- 
GitLab