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

commenting

Browse Source
This commit is contained in:
bpmcgeeney 2021-09-12 14:23:35 -07:00
parent c6b1f0fc2c
commit 0c8703ac73
1 changed files with 2 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/sim.cpp
View File

@ -198,6 +198,7 @@ void lqrCalc(struct sVars &Vars)
*/ */
// Paste in Values from gainCalc.m
double K11 = 39.54316; double K11 = 39.54316;
double K12 = 0.00000; double K12 = 0.00000;
double K13 = -0.00000; double K13 = -0.00000;
@ -217,7 +218,7 @@ void lqrCalc(struct sVars &Vars)
double K35 = 0.00000; double K35 = 0.00000;
double K36 = 39.54394; double K36 = 39.54394;
double gain = 0.25 * pow(10, -4); double gain = 0.25 * pow(10, -4); // changing exponenet drastically changes results of LQR
// Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm // Matrix Multiply K with [YPR/2; w123] column vector and divide by moment arm
Vars.LQRx = gain * ((K12 * Vars.pitch) / 2 + K15 * Vars.pitchdot + (K13 * Vars.roll) / 2 + K16 * Vars.rolldot + (K11 * Vars.yaw) / 2 + K14 * Vars.yawdot) / -Vars.momentArm; Vars.LQRx = gain * ((K12 * Vars.pitch) / 2 + K15 * Vars.pitchdot + (K13 * Vars.roll) / 2 + K16 * Vars.rolldot + (K11 * Vars.yaw) / 2 + K14 * Vars.yawdot) / -Vars.momentArm;