diff --git a/crazyflie_sim/crazyflie_sim/crazyflie_sil.py b/crazyflie_sim/crazyflie_sim/crazyflie_sil.py
index 9b5f2e774..47569c3c8 100644
--- a/crazyflie_sim/crazyflie_sim/crazyflie_sil.py
+++ b/crazyflie_sim/crazyflie_sim/crazyflie_sil.py
@@ -185,6 +185,10 @@ def cmdFullState(self, pos, vel, acc, yaw, omega):
         self.setpoint.acceleration.x = acc[0]
         self.setpoint.acceleration.y = acc[1]
         self.setpoint.acceleration.z = acc[2]
+                
+        self.cmdHl_pos = copy_svec(self.setpoint.pos)
+        self.cmdHl_vel = copy_svec(self.setpoint.vel)
+        self.cmdHl_yaw = yaw
 
     # def cmdPosition(self, pos, yaw = 0):
     #     self.mode = CrazyflieSIL.MODE_LOW_POSITION