diff --git a/include/teensy.h b/include/teensy.h
index 1a97f62..e5a1c2e 100644
--- a/include/teensy.h
+++ b/include/teensy.h
@@ -214,12 +214,12 @@ void thrustInfo(Vehicle &State) {
     double R = 5.0;
 
     // Vector math to aqcuire thrust vector components
-    State.thrust = State.lc0_processed + State.lc1_processed +
-                   State.lc2_processed + State.lc3_processed;
+    State.Fz = State.lc0_processed + State.lc1_processed + State.lc2_processed +
+               State.lc3_processed;
     State.Fx = (State.lc1_processed - State.lc2_processed) * r / R;
     State.Fy = (State.lc0_processed - State.lc3_processed) * r / R;
-    State.Fz =
-        sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) +
+    State.thrust =
+        sqrt(pow(State.Fz, 2) + pow(State.Fx, 2) + pow(State.Fy, 2)) +
         (State.mass * g);
 
   } else {