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

more cleanup

Browse Source
This commit is contained in:
bpmcgeeney
2021-09-21 22:14:10 -07:00
parent 9eb9bce77c
commit 1b13a88c7d
4 changed files with 16 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/Vehicle.h
View File

@@ -35,6 +35,7 @@ struct Vehicle {
double yError, yPrevError;
double pError, pPrevError;
double i_yError, i_pError = 0;
int pidTest = 0;
double simTime;
int stepSize;

18
include/sim.h
View File

@@ -190,19 +190,25 @@ void pidController(Vehicle &State, struct Vehicle &PrevState) {
// Make sure we start reacting when we start burning
if (State.thrust > 0.01) {
State.yError = State.yaw;
State.pError = State.pitch;
// Integral of Error
State.i_yError = integral(State.yaw, State.i_yError, State.stepSize);
State.i_pError = integral(State.pitch, State.i_pError, State.stepSize);
State.i_yError = integral(State.yError, State.i_yError, State.stepSize);
State.i_pError = integral(State.pError, State.i_pError, State.stepSize);
// Derivative of Error
double d_yError = derivative(State.yaw, PrevState.yaw, State.stepSize);
double d_pError = derivative(State.pitch, PrevState.pitch, State.stepSize);
double d_yError =
derivative(State.yError, PrevState.yError, State.stepSize);
double d_pError =
derivative(State.pError, PrevState.pError, State.stepSize);
// PID Function - it says LQR but this is just so that it gets passed to the
// TVC block properly
State.LQRx = (State.Kp * State.yaw + State.Ki * State.i_yError +
State.LQRx = (State.Kp * State.yError + State.Ki * State.i_yError +
State.Kd * d_yError);
State.LQRy = (State.Kp * State.pitch + State.Ki * State.i_pError +
State.LQRy = (State.Kp * State.pError + State.Ki * State.i_pError +
State.Kd * d_pError);
} else {