Anson/Lander-Embedded
1
0
Fork 0
mirror of https://gitlab.com/lander-team/lander-cpp.git synced 2025-06-16 07:06:51 +00:00
Code Releases Activity

fixed missing gravity during burn

Browse Source
This commit is contained in:
bpmcgeeney 2021-11-22 16:25:49 -07:00
parent 8bc356892e
commit c4f90c82ce
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
include/teensy.h
View File

@ -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));