From c4f90c82cefcede17ecfc192f563839b95f20143 Mon Sep 17 00:00:00 2001 From: bpmcgeeney Date: Mon, 22 Nov 2021 16:25:49 -0700 Subject: [PATCH] fixed missing gravity during burn --- include/teensy.h | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/include/teensy.h b/include/teensy.h index cdfe15a..7cd0c84 100644 --- a/include/teensy.h +++ b/include/teensy.h @@ -220,12 +220,10 @@ void thrustInfo(Vehicle &State) { // Vector math to aqcuire thrust vector components State.Fz = State.lc0_processed + State.lc1_processed + State.lc2_processed + - State.lc3_processed; + State.lc3_processed + (State.mass * g); State.Fx = (State.lc1_processed - State.lc2_processed) * r / R; State.Fy = (State.lc0_processed - State.lc3_processed) * r / R; - State.thrust = - sqrt(pow(State.Fz, 2) + pow(State.Fx, 2) + pow(State.Fy, 2)) + - (State.mass * g); + State.thrust = sqrt(pow(State.Fz, 2) + pow(State.Fx, 2) + pow(State.Fy, 2)); } else { State.thrust = 0.0; @@ -233,10 +231,12 @@ void thrustInfo(Vehicle &State) { } void processTVC(Vehicle &State, PWMServo &yaw, PWMServo &pitch) { - State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0)); - State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0)); - State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) + - (State.mass * g); + /* + State.Fx = State.thrust * sin(State.xServoDegs * (M_PI / 180.0)); + State.Fy = State.thrust * sin(State.yServoDegs * (M_PI / 180.0)); + State.Fz = sqrt(pow(State.thrust, 2) - pow(State.Fx, 2) - pow(State.Fy, 2)) + + (State.mass * g); + */ yaw.write(yaw_conv(State.xServoDegs)); pitch.write(pitch_conv(State.yServoDegs));