From ee9b411ba8807efef8a645116afae2b673d1a4d8 Mon Sep 17 00:00:00 2001
From: David Wehr <dawehr@iastate.edu>
Date: Sun, 12 Feb 2017 00:05:45 -0600
Subject: [PATCH] WIP - generating graph of quad controller
---
quad/sw/modular_quad_pid/gen_diagram/Makefile | 10 +
.../modular_quad_pid/gen_diagram/gen_diagram | Bin 0 -> 13746 bytes
.../modular_quad_pid/gen_diagram/generate.c | 185 ++++++++++++++++++
.../modular_quad_pid/gen_diagram/local_PID.h | 85 ++++++++
4 files changed, 280 insertions(+)
create mode 100644 quad/sw/modular_quad_pid/gen_diagram/Makefile
create mode 100644 quad/sw/modular_quad_pid/gen_diagram/gen_diagram
create mode 100644 quad/sw/modular_quad_pid/gen_diagram/generate.c
create mode 100644 quad/sw/modular_quad_pid/gen_diagram/local_PID.h
diff --git a/quad/sw/modular_quad_pid/gen_diagram/Makefile b/quad/sw/modular_quad_pid/gen_diagram/Makefile
new file mode 100644
index 000000000..610e289a0
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/Makefile
@@ -0,0 +1,10 @@
+
+COMP_GRAPH = ../../../computation_graph
+QUAD_BLOCKS = ../src/graph_blocks
+
+gen_diagram: generate.c
+ gcc -o gen_diagram -I. -I$(COMP_GRAPH)/src generate.c $(COMP_GRAPH)/src/computation_graph.c $(QUAD_BLOCKS)/node_bounds.c $(QUAD_BLOCKS)/node_pid.c $(QUAD_BLOCKS)/node_mixer.c
+
+.PHONY: clean
+clean:
+ rm gen_diagram
diff --git a/quad/sw/modular_quad_pid/gen_diagram/gen_diagram b/quad/sw/modular_quad_pid/gen_diagram/gen_diagram
new file mode 100644
index 0000000000000000000000000000000000000000..ae1620225638cc42929390bc1646225a8b1cbbf3
GIT binary patch
literal 13746
zcmeHOdvILUc|W_W7c$nv7!Xfi983+3*75^KU~H~rt$oEvwy-4=2F%r3yRuZI-R(a7
zpx83AHq5dZdjgqpGyTJVkSR^4joTSAjytub*hQw}Nkj)4wNW!JW6~9J5@8ggP{a23
zoyXmKue2%cO#dp!-us>JdA{?VbG~!Wy;nnlwoP7-hpFOa8yI27%XCf>SH^Ppa8%M7
z*$qr*ce6WK87QUr>6}AQ)D)bRrWLG^az4Nk{2IN2(CF29MZr4-nJK6|BuMQQiY}`H
zF0K=(f{baLKzihB&D|UY?B*2(W73b4<QOj+zY>}BmP@_mQcuA#>6n75f7CbnbxOOP
zUL6?WIsw+guLVf5R60IPhN#|fGe-e+UQzHBL1qdny~m+Pety|fBL%m~_KL-2TJ}#t
ziLw;~{kzt#Suqf*ALx&y_SNreTwA|(jXx3fuj2hC`y{)0OBZiR#gT@Gd|r$ng_`&)
zj&CXdn>SwjcCzm=U&WQby72ajtzRQu^+WYbmo<=FG>^1agzhgwf4m62wFtcjbPYdu
zSqH*={?rzsFDOE9E<(Sp2%TYkdT2P-vfgMc9AWWra9|+X!xG7OC>3L-8B8R?@ub-s
z>>psgu~f2$_4W)z6Ja(e1bg?!`;%d?;{B0iFBHh4nMekqG#KoUfU<iq8j%!}ZEkCA
z-e|7!ukt?#pS;#TXWfHQrEyjBFLitW4I~i<jmqqjZVqWK+vjtgCg|m$lfEjNjV_lO
zXDzqUeM%Ux!A4hWhuDoax;?+>HoDy(!$v2cROztMRW2ajH*Iuu*ecyNdZk5Wtj|WL
zIiZS?zFJ{qOV{77G2_T^vNZQBI7a$H#d#r-e{dC8`DJTxtzM#|MtC1FCoktwmOVf?
zg>Z6=<4Xyr&`l0={C2{9grDO0BEl(DlP5V|NjQaQ@;Jx6gi~lH4{`jn+kjI@CSx4G
zMmU9HvYX={5l$hP?BMtZgj3fib>J(1*_wIx86)$7k-jqB(b+mWJbjbSjM0%p9E`qL
zuCv?|5S;0)UedzY>6Z|wE~EYsfRUc9GBO|Tx#Kjf@$l5{JV%WDWe6W8BHMYsmx^EE
zXNlisyznb-VB`xQ3>cYdV<h*e;d$G5_lxAsmbC@STJ;jL!`nZ!{vr6!Qng)B%theX
z&bLeN1IhErd5Zrz4I74$ew3w3r(T1eW$a^8$dB!ud=5HP$H>;4Em;heGF)=!zsTo}
zOkfs+eFEl;Y}c%j`DY_Dn)4xeMiv~@?}8LJ`ObM|;~XumTu7;>56rSuIhUDgKBp7U
z;JhCR8y%QV)QVn#`J@DIBiblrMN6Z42~8LxTJ2~?=0Gl!x{~R-oIY@QarLnu3yB<=
z%645bdj826`Jl}3jEP<ZX0y7Mp7m58c~hFo>J3MRtB?Gt)kp(1G8&l1l})XaZZ&4R
zriCpZY-L#{Fq1wolTY574O~9j;mNkoKrom2F-0SYP^JUZ9;sT9)$7&;rib2AI9zL>
zRl~c-yG#U%Y<AmnBlFYz7)g%m4YVAlN{&x?j?d{YwU%Y;0#}C0Pouf~)Yj3!b?&i7
zEYbVxh2Fy&anph8<Q7-GKJ<RJ{R&}ch$Vdg?wpa-5#jMn>b*?YSf>3_OD1p;D&y4l
zdj}V&>ZPGd?#AQ-1doWqf%ggyApFw>#w7;>9VaFRj3PC5um(7v3t9cnb%Be@_V?kX
zl*Dk1IfvtF`oPtEa&@7Jt5y><#e_rcAiiUa{5$5uMVbdWPo|mTMUw)fNVQA{5y%@y
z8$ZehE@iw$I2SX`CE2b^;Es#6v333^8krQPVb?_?(>@GgxKNj#U3~DiA2Qrto!Noz
zoz-aNZKsv@FP~xM@knQAJP~reWL+RP)GLUXKb4~98J;RVO>;IAxS9=IHBJ~Km&=Td
z)oZz4C&?ZSLi)foOUdDSNe)-G>uRQ>gzRA+P8%7%97W8!a#kSK8Uh^6y~FKMD@Xxy
z#Hed(C3g*@zMR;jx(4rP=+NUkxqrxSqq)s?3ms&F2BpCN0|&oO@aqcvatFUr+DBj(
zqB@Hatf5ubAnn@dbwpS8P+ctL8%6Hn>nS!GxI$h}Em2G>n@|QAWh9jjT%k!r14&Eq
zeblis#7r7jK|xCUZx=ikp<Lpb1GAabOr~r4T6HZS_gPwMGY9XHZXl`6*an0z8jaG6
z%mZtR$<;YB*tP1KKn_iaonWU(+g96T<4ZPM2}`A6Y_wAkeCd|4ZSig5f7l*!!SqmB
zZ}qQ-i?>zd?xkugeYK&G$kGSKZo;ywp=I|P23ah-K3aCWeyf%p5?uAG2Y*jK!xW>;
z!MBemg&bvV)UbWz_~fx|A95L~?L*`%Y#(EM`?v~Q<01vO^NE$CiA9sRf={m8&|A{x
zWEPes4=KuJ7Kw{%xj1?W->|0h$%m;SZdPq*d~?KNu_hL|h*4#<kDS09Dfx$!xI39z
z#BmD0MN0?VZdM<o-A}oWc9zpx^E(J)r^9-0NB{n~PQd-x`QAN44gJ3DkH#>EG<rGH
zH8C2P<uRswjRc86VF$Igduzj+r9koxZ+7T?rL%#ebiz2g_z0PRz-76sUsuA{58je(
z=a_AG{M*lI&ChzEy)H2E{F%bO+yTds{8GcvN8%efb0h4YrKxZ(O4BO)8y*$TT4=GJ
zCsqus(ZD!}Y_b_!J}g`m-5D=*2bmjvq#xzU;j=G86FHRMQ00C;!;M?a@7kFoV2%bR
z$o!NpOi)H**{%s;o`xG{vw?}DF^Uy@d^dCL>|fiBJP#x3zyuE^_l*+X#Ly{aI4TVz
z$vCW{Au%>k!US{h3N6BMKFx8b%bRaMP3h^h@Z;~L9~8^n%qMO?kozWvUJ!aCXM7M6
zQU3|layJ+HKKT={q&HX`Hc#kgj&2_F)rbFF$XpcRzw8J<tt@iXrA#Y}Zlb{Ei5|01
z-gJt%Vw7k>ay}y9;58uTEk|`3Rf-j?)EL<Vq<~>ai8!=oes^}<9)(|Bb4P(jqx#rg
zJadeby^EB-$E%NhA2U`Md;8o0ir}}P%V*6y=L^S=<4sRAW%5lsS~H(Eb+u;RZ|ZD2
zx@=#ih9k+*dV<^A?*DM*&y11JeRT56ef}}59{FYRZgqZX%Y4+9`LqQ#@{1>o^m&i*
z&{XOpIuJg!v+3!kolVa)nK<%T=bcZ^V=(9pDUXaEoW1ul|I?9ha&I*LtUnY@!r4Oc
z`&WqnJ<<BnT$^apd0}561ddB;q|Q=1~1TEV(}xuHO(b`y)_DXpgMdRyDAd+WPfc
z0}J(YU2U`WCzUK7j656akFQ_awry!8Te>W<^r=K&Fcw}P5BEglp~`{auJFKmupa4c
z*q|*-RI=wE>0K7upw-tuy}^b)wJfoInSXWfP8h+D7qX_+Z_o%n3?=O;X-LGF$j
z428@{G!$k`;G$b$PBY<+ipT7b=MLXGdiF+#tdrCEJY|n=3<@1;c3_0*;ly_UH9#-c
zj1G7Wup2OhMR5r5d&oN{0pG`tHw^grY(9S(a0C#;!RmhpeZY@#o23K3{GZSV?7+?K
zA;4k4i-1(<IT{tubK96_UybLE8_O$>d&+BwPR~wH!N&(kK$iLC8M_yJk{|vxd?MMZ
znoU)8k5w<&TXBdzdi%NuR^Pjn$RvYfH`{_f6J$+QH7|NME-V{D!$652!Y>YfAE3qe
z_HY5>AI9$t`23)!^u1FS{|)e8Mhy8gA&Wm~@h^Zs0sbb$%(Cxm@{j=8zk;6^vHnAf
zk<$0w=T&?<%r5*ipMM@~i|^fQ*}oV3e(+-s{<9Xp1^gd^Kj7f+w)i3NOEI<=04@8z
zbC!RHLH`SkHJyAdzV~%Y{|)e$Vh;SB!~Y*}|68C!b}wR1V7fBaTvc<-8>p&#)fcGJ
zj+O+fmcLSJR5iR<W>ht%%iF8;fvU!)s)nYj<;_)Eb5$MWo2x4Lytxtfr(uVh`Ko*s
zfv+O)RRq3@!2j<EbmRFt6<tD(;6hs&6$R;(Mdehf&LJJGsLbFoJr#N{MMb?|y+fdk
z?h&ZmE&!{;<8dmqZ&OkH-+Yme5`My~6R_U-j{5|z-o>2u3S7NAIS5%Q3j|<w63W-9
z5)<zpIf8R96}1Oe$PTLeBs^c?<yHybEYW#Q(T_`9`K{Vh?|2lBTQ=UGn3TtXf#WKk
zxJ~2u4#}6z{htH%9^2i}FXXs>Dq&ff&P8sK@IDFGO1MeF9TN6RxJSYlC3Ls5dE>^1
zwdGyAQjugzTZ@;64fU&299y~a;L0`rhBfyKG7b?d*ET%Vu#!1c*7zI!YwA}?6`|o@
zeZR)(jQJD$2a~~FfXTRkeX54Hn&Ehi`6JO}*x%ILTAvK=me}q{%D*eskC)s1A?7En
zFPP|K{?Psi><O5R3(lT!Jb^Qj12MsihX;bBAZxLKB=h%2`f*K$_u<+L1iWY{m<%$1
zxX<j32M5DuUkJJi6B=eP9uMvp8mj(BJrINS;9!3bn#Q4xE`HvMu(&IcV1Arq2E&o0
zJLvxloYpVRRkcnUy*jYM_Zw>caw0BjUMu`-sL+~G`kh`KSmFB!jbnvNNukPXD{-~!
zC%rnSC#AuAIaav1^;^KgEqhe8f5NMC;S-Xe{4eFyLLQPR+ky(^6s3RKs{_OPM@i6R
zhs!{tsyiR-0L_P=(pPzGoQNnYZwYK}{GI`Wa)#1Zd18m`sLC_Se<i2jv(Tp;q;Qpo
zVp3m|4ZHo9WcC~?B&zgP-aB5TeRuy4yYy8aeN*bIyi5Mm-s0AO6*P(~`A0?VJEvra
z)qWzl!dF!%n8DAkKPL0qnA9I8AzPvIJrxCCN8PTkzK=RC9onTfK(1BLlPLR+OJD7i
zGdBqdwO=TC85V`Ilc-VqD*g<QrFdB^0Dt%`s{E?_R-`vkvFoeviPU#SuP8wY-R=Lt
zr9UjcOB$9NtNPAKnNj1X@L^P{@RQTsx-U@Q9l7P*_$BC8+w|3aL!JCC%PsH5$DvF8
zSMAelc`DkM)c-fg(oidXwQs8L!p6urikhFQeKiih0MFjO`p#u`30Je`yTA(bU)h;*
z=`*cch-w1F1hB43Pr(^*?Cr;-eoPZ8%1>oL=_&j(REVbfPoGOrIizvG-1AS81rygi
ze$;-0MG+OHPx;MWNS2C%wWzDMIYVB4u8E4uABv~+?6|dd6cMoRet~>bcK9x!tMF6(
zS9z7z=X~uC)5e5y&Bb)LFQn$b`q0ZpuRe$A!)ouGm+oW9dFdtA{j%Md4|mU}=anyG
zY9DgSmov4$IO!Ek?Hf+|4NUC^PI@I%dEQB1VC8Qo{YIwpvXj2h`X1Fuud>b?PC7D$
z%8xdN57}JhJsTP0TFX>EbJ7=CdCEz@$vU?<>5Gw1y6q%h_pzJt{pfu3TbRlNb|oLf
zX+y3rC%ulTb?c<xhIKffefO>?)>8@ln3kiX(4OnTQ$e@?bdK|~g>1M?cgXuCos~Q3
zO_J_DKRyAv2d6^ir&=GqoIW?6N2Q%^mwkF>>cJP4>Rbtwm(#e;*RS(M=syLW{5dJ(
zb`m(1|G<y>yQ)Gbluh$1?i~R=v;)a~kM<%e)#wP(>5Vp(Tltk}^ZT=$%h$4+I$cT%
zu$I&3#-F(J^{bg0KpfOQNEnsv+|Jzj_Oz6D$1Td`;k9f2907e1;y_>DQBk?`+mimT
zm4eQnv4j7Hq`!WHpz~+*puYvW2IwyDa{Ao)JR$9rF4VaSe@2hmH0U)r&8zvN?ERL@
zE35z)z~M!><52fmic`z!3M;^`bNWK|v21rhverxbQrR!{ovKm9&QnF`&w)<<yVvha
zMdbesbgkf?Rr^cOYrS=BpKHA698yd3Q|3K&|2E3)FJ$-B=-dW>=YZPJB%MC1q!Q*=
zys74NiP!+WB&fl9IdxCBjf%PH6Z1sf4=d&ZPFGj~YM_%H_c(rC+M(||skHDbV=E>7
zA0@p{(l->b|2XKHl(B;_=(T89m;S5!CHm5p=J^>JAO6k-iisljUm`n*zuW)Uq@A5r
zI&by@<|UQyfv)kI^KzkxeL4@4KQG96sQkRj>GSNHJ@I5BnZg_19(<7f*v4&UTkG~t
z(`2D=JiNO<foB|Ma?r$=-H~ttAA!#ynPw<z?jDHl3J#c|WHg>IgQ<OtUVsjSli`s6
z8xOAj1}x7bWYQxH@j$}FLx%W%CLUpgQiFs0p<=`EF1SDG5ZxV)gyTUxdFkP8Io>RT
z7Ty=`5igL<J;4ETJP}S7UdNhd$F@Lc=ac59t}PooTeog8P5+vP2Unxj`52AF*tD&w
zJzxg5v>-SlQmXE7F2u~1C$}`Uw{C=#jl(~DXXfU%t<6ns=GIM{wg)=R&Zg$J0GZ?8
z#`h#rJSOrbvh`9~yi=wQB%-N!Pnemlo$Y2}fI8bZ((rT!cMXIYJ-rd%)Jrp@7YpLy
z5JiHSVg7hWe5$YO_K)sG+l~+WodUv--LMx;CGl?F7FF>V08a5aU;Mk-Hdp0e3AiO3
ze;|NKl!%&r!AJ<*G+Vc#m!bZMnM#C1^SpLX*rQ70Apb7G)kHXg0TW{*|H8o~VB$fT
z6ySfg&;zk#SbV}D{xrg+@Yo)6n_MOv2k4I^5-gU$gp5V;hY$35g!sb<mzIe^l#{E-
aCj@-Z)dF-3x%iU|r@_Kl*~XrS;lBWQ8cwAE
literal 0
HcmV?d00001
diff --git a/quad/sw/modular_quad_pid/gen_diagram/generate.c b/quad/sw/modular_quad_pid/gen_diagram/generate.c
new file mode 100644
index 000000000..b7cc7966c
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/generate.c
@@ -0,0 +1,185 @@
+#include <stdio.h>
+#include "computation_graph.h"
+#include "../src/graph_blocks/node_pid.h"
+#include "../src/graph_blocks/node_bounds.h"
+#include "graph_blocks/node_constant.h"
+#include "../src/graph_blocks/node_mixer.h"
+#include "local_PID.h"
+
+#define ROLL_PITCH_MAX_ANGLE 0.3490 // 20 degrees
+
+typedef struct parameter_t {
+ struct computation_graph* graph;
+ // PID blocks
+ int roll_pid;
+ int pitch_pid;
+ int yaw_pid;
+ int roll_r_pid;
+ int pitch_r_pid;
+ int yaw_r_pid;
+ int x_pos_pid;
+ int y_pos_pid;
+ int alt_pid;
+ // Sensor blocks
+ int cur_pitch;
+ int cur_roll;
+ int cur_yaw;
+ int theta_dot;
+ int phi_dot;
+ int psi_dot;
+ // RC blocks
+ int rc_pitch;
+ int rc_roll;
+ int rc_yaw;
+ int rc_throttle;
+ // Loop time
+ int dt;
+ // Signal mixer
+ int mixer;
+ // Clamping blocks
+ int clamp_pitch;
+ int clamp_roll;
+ int clamp_d_pwmR; // Clamp the change in PWM values for roll
+ int clamp_d_pwmP; // ... pitch
+ int clamp_d_pwmY; // ... yaw
+} parameter_t;
+
+parameter_t ps;
+
+ int control_algorithm_init(parameter_t * ps);
+
+int main() {
+
+ FILE* dot_fp;
+ dot_fp = fopen(".\\network.dot", "w");
+ export_dot(ps.graph, dot_fp);
+ fclose(dot_fp);
+}
+
+
+ int control_algorithm_init(parameter_t * ps)
+ {
+ struct computation_graph* graph = create_graph();
+ ps->graph = graph;
+
+ // Create all the PID blocks
+ ps->roll_pid = graph_add_node_pid(graph, "Roll PID");
+ ps->pitch_pid = graph_add_node_pid(graph, "Pitch PID");
+ ps->yaw_pid = graph_add_node_pid(graph, "Yaw PID");
+ ps->roll_r_pid = graph_add_node_pid(graph, "Roll Rate PID");
+ ps->pitch_r_pid = graph_add_node_pid(graph, "Pitch Rate PID");
+ ps->yaw_r_pid = graph_add_node_pid(graph, "Yaw Rate PID");
+ ps->x_pos_pid = graph_add_node_pid(graph, "X pos PID PID");
+ ps->y_pos_pid = graph_add_node_pid(graph, "Y pos PID");
+ ps->alt_pid = graph_add_node_pid(graph, "Altitude");
+
+ // Create blocks for bounds checking
+ ps->clamp_d_pwmP = graph_add_node_bounds(graph, "P PWM Clamp");
+ ps->clamp_d_pwmR = graph_add_node_bounds(graph, "R PWM Clamp");
+ ps->clamp_d_pwmY = graph_add_node_bounds(graph, "Y PWM Clamp");
+ ps->clamp_pitch = graph_add_node_bounds(graph, "Pitch Clamp");
+ ps->clamp_roll= graph_add_node_bounds(graph, "Roll Clamp");
+
+ // Create blocks for sensor inputs
+ ps->cur_pitch = graph_add_node_const(graph, "Pitch");
+ ps->cur_roll = graph_add_node_const(graph, "Roll");
+ ps->cur_yaw = graph_add_node_const(graph, "Yaw");
+ // Yaw angular velocity PID
+ // theta_dot is the angular velocity about the y-axis
+ // phi_dot is the angular velocity about the x-axis
+ // psi_dot is the angular velocity about the z-axis
+ // These are calculated from using the gimbal equations
+ ps->theta_dot = graph_add_node_const(graph, "dTheta");
+ ps->phi_dot = graph_add_node_const(graph, "dPhi");
+ ps->psi_dot = graph_add_node_const(graph, "dPsi");
+
+ // Create blocks for RC controller
+ ps->rc_pitch = graph_add_node_const(graph, "RC Pitch");
+ ps->rc_roll = graph_add_node_const(graph, "RC Roll");
+ ps->rc_yaw = graph_add_node_const(graph, "RC Yaw");
+ ps->rc_throttle = graph_add_node_const(graph, "RC Throttle");
+
+ ps->mixer = graph_add_node_mixer(graph, "Signal Mixer");
+
+ ps->dt = graph_add_node_const(graph, "dT");
+
+ // Connect pitch PID chain
+ graph_set_source(graph, ps->pitch_r_pid, PID_SETPOINT, ps->pitch_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->pitch_r_pid, PID_CUR_POINT, ps->theta_dot, CONST_VAL);
+ graph_set_source(graph, ps->pitch_r_pid, PID_DT, ps->dt, CONST_VAL);
+ graph_set_source(graph, ps->pitch_pid, PID_SETPOINT, ps->rc_pitch, CONST_VAL);
+ graph_set_source(graph, ps->pitch_pid, PID_CUR_POINT, ps->cur_pitch, CONST_VAL);
+ graph_set_source(graph, ps->pitch_pid, PID_DT, ps->dt, CONST_VAL);
+
+ // Connect roll PID chain
+ graph_set_source(graph, ps->roll_r_pid, PID_SETPOINT, ps->roll_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->roll_r_pid, PID_CUR_POINT, ps->phi_dot, CONST_VAL);
+ graph_set_source(graph, ps->roll_r_pid, PID_DT, ps->dt, CONST_VAL);
+ graph_set_source(graph, ps->roll_pid, PID_SETPOINT, ps->rc_roll, CONST_VAL);
+ graph_set_source(graph, ps->roll_pid, PID_CUR_POINT, ps->cur_roll, CONST_VAL);
+ graph_set_source(graph, ps->roll_pid, PID_DT, ps->dt, CONST_VAL);
+
+ // Connect yaw PID chain
+ graph_set_source(graph, ps->yaw_r_pid, PID_SETPOINT, ps->rc_yaw, PID_CORRECTION);
+ graph_set_source(graph, ps->yaw_r_pid, PID_CUR_POINT, ps->psi_dot, CONST_VAL);
+ graph_set_source(graph, ps->yaw_r_pid, PID_DT, ps->dt, CONST_VAL);
+ /*
+ graph_set_source(graph, ps->yaw_pid, PID_SETPOINT, ps->rc_yaw, CONST_VAL);
+ graph_set_source(graph, ps->yaw_pid, PID_CUR_POINT, ps->yaw, CONST_VAL);
+ graph_set_source(graph, ps->yaw_pid, PID_DT, ps->dt, CONST_VAL);
+ */
+
+ // Connect PWM clamping blocks
+ graph_set_source(graph, ps->clamp_d_pwmP, BOUNDS_IN, ps->pitch_r_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->clamp_d_pwmR, BOUNDS_IN, ps->roll_r_pid, PID_CORRECTION);
+ graph_set_source(graph, ps->clamp_d_pwmY, BOUNDS_IN, ps->yaw_r_pid, PID_CORRECTION);
+
+ // Connect signal mixer
+ graph_set_source(graph, ps->mixer, MIXER_THROTTLE, ps->rc_throttle, CONST_VAL);
+ graph_set_source(graph, ps->mixer, MIXER_PITCH, ps->clamp_d_pwmP, BOUNDS_OUT);
+ graph_set_source(graph, ps->mixer, MIXER_ROLL, ps->clamp_d_pwmR, BOUNDS_OUT);
+ graph_set_source(graph, ps->mixer, MIXER_YAW, ps->clamp_d_pwmY, BOUNDS_OUT);
+
+ // Set pitch PID constants
+ graph_set_param_val(graph, ps->pitch_pid, PID_KP, PITCH_ANGLE_KP);
+ graph_set_param_val(graph, ps->pitch_pid, PID_KI, PITCH_ANGLE_KI);
+ graph_set_param_val(graph, ps->pitch_pid, PID_KD, PITCH_ANGLE_KD);
+ graph_set_param_val(graph, ps->pitch_r_pid, PID_KP, PITCH_ANGULAR_VELOCITY_KP);
+ graph_set_param_val(graph, ps->pitch_r_pid, PID_KI, PITCH_ANGULAR_VELOCITY_KI);
+ graph_set_param_val(graph, ps->pitch_r_pid, PID_KD, PITCH_ANGULAR_VELOCITY_KD);
+
+ // Set roll PID constants
+ graph_set_param_val(graph, ps->roll_pid, PID_KP, ROLL_ANGLE_KP);
+ graph_set_param_val(graph, ps->roll_pid, PID_KI, ROLL_ANGLE_KI);
+ graph_set_param_val(graph, ps->roll_pid, PID_KD, ROLL_ANGLE_KD);
+ graph_set_param_val(graph, ps->roll_r_pid, PID_KP, ROLL_ANGULAR_VELOCITY_KP);
+ graph_set_param_val(graph, ps->roll_r_pid, PID_KI, ROLL_ANGULAR_VELOCITY_KI);
+ graph_set_param_val(graph, ps->roll_r_pid, PID_KD, ROLL_ANGULAR_VELOCITY_KD);
+
+ // Set yaw PID constants
+ graph_set_param_val(graph, ps->yaw_pid, PID_KP, YAW_ANGLE_KP);
+ graph_set_param_val(graph, ps->yaw_pid, PID_KI, YAW_ANGLE_KI);
+ graph_set_param_val(graph, ps->yaw_pid, PID_KD, YAW_ANGLE_KD);
+ graph_set_param_val(graph, ps->yaw_r_pid, PID_KP, YAW_ANGULAR_VELOCITY_KP);
+ graph_set_param_val(graph, ps->yaw_r_pid, PID_KI, YAW_ANGULAR_VELOCITY_KI);
+ graph_set_param_val(graph, ps->yaw_r_pid, PID_KD, YAW_ANGULAR_VELOCITY_KD);
+
+ // Set angle clamping limits
+ graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MIN, -ROLL_PITCH_MAX_ANGLE);
+ graph_set_param_val(graph, ps->clamp_pitch, BOUNDS_MAX, ROLL_PITCH_MAX_ANGLE);
+ graph_set_param_val(graph, ps->clamp_roll, BOUNDS_MIN, -ROLL_PITCH_MAX_ANGLE);
+ graph_set_param_val(graph, ps->clamp_roll, BOUNDS_MAX, ROLL_PITCH_MAX_ANGLE);
+
+ // Set PWM difference clamping limits
+ graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MIN, -20000);
+ graph_set_param_val(graph, ps->clamp_d_pwmP, BOUNDS_MAX, 20000);
+ graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MIN, -20000);
+ graph_set_param_val(graph, ps->clamp_d_pwmR, BOUNDS_MAX, 20000);
+ graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MIN, -20000);
+ graph_set_param_val(graph, ps->clamp_d_pwmY, BOUNDS_MAX, 20000);
+
+ // TODO: Change this to use LOOP_TIME
+ graph_set_param_val(graph, ps->dt, CONST_SET, 0.005);
+
+ return 0;
+ }
diff --git a/quad/sw/modular_quad_pid/gen_diagram/local_PID.h b/quad/sw/modular_quad_pid/gen_diagram/local_PID.h
new file mode 100644
index 000000000..376b1f1b9
--- /dev/null
+++ b/quad/sw/modular_quad_pid/gen_diagram/local_PID.h
@@ -0,0 +1,85 @@
+/*
+ * PID.h
+ *
+ * Created on: Nov 10, 2014
+ * Author: ucart
+ */
+
+#ifndef PID_H_
+#define PID_H_
+
+// Yaw constants
+
+// when using units of degrees
+//#define YAW_ANGULAR_VELOCITY_KP 40.0f
+//#define YAW_ANGULAR_VELOCITY_KI 0.0f
+//#define YAW_ANGULAR_VELOCITY_KD 0.0f
+//#define YAW_ANGLE_KP 2.6f
+//#define YAW_ANGLE_KI 0.0f
+//#define YAW_ANGLE_KD 0.0f
+
+// when using units of radians
+#define YAW_ANGULAR_VELOCITY_KP 190.0f * 2292.0f//200.0f * 2292.0f
+#define YAW_ANGULAR_VELOCITY_KI 0.0f
+#define YAW_ANGULAR_VELOCITY_KD 0.0f
+#define YAW_ANGLE_KP 2.6f
+#define YAW_ANGLE_KI 0.0f
+#define YAW_ANGLE_KD 0.0f
+
+// Roll constants
+//#define ROLL_ANGULAR_VELOCITY_KP 0.95f
+//#define ROLL_ANGULAR_VELOCITY_KI 0.0f
+//#define ROLL_ANGULAR_VELOCITY_KD 0.13f//0.4f//0.7f
+//#define ROLL_ANGLE_KP 17.0f //9.0f
+//#define ROLL_ANGLE_KI 0.0f
+//#define ROLL_ANGLE_KD 0.3f // 0.2f
+//#define YPOS_KP 0.0f
+//#define YPOS_KI 0.0f
+//#define YPOS_KD 0.0f
+
+// when using units of radians
+#define ROLL_ANGULAR_VELOCITY_KP 100.0f*46.0f//102.0f*46.0f//9384.0f//204.0f * 46.0f
+#define ROLL_ANGULAR_VELOCITY_KI 0.0f
+#define ROLL_ANGULAR_VELOCITY_KD 100.f*5.5f//102.0f*6.8f//1387.2//204.0f * 6.8f
+#define ROLL_ANGLE_KP 15.0f
+#define ROLL_ANGLE_KI 0.0f
+#define ROLL_ANGLE_KD 0.2f
+#define YPOS_KP 0.015f
+#define YPOS_KI 0.005f
+#define YPOS_KD 0.03f
+
+
+//Pitch constants
+
+// when using units of degrees
+//#define PITCH_ANGULAR_VELOCITY_KP 0.95f
+//#define PITCH_ANGULAR_VELOCITY_KI 0.0f
+//#define PITCH_ANGULAR_VELOCITY_KD 0.13f//0.35f//0.7f
+//#define PITCH_ANGLE_KP 17.0f // 7.2f
+//#define PITCH_ANGLE_KI 0.0f
+//#define PITCH_ANGLE_KD 0.3f //0.3f
+//#define XPOS_KP 40.0f
+//#define XPOS_KI 0.0f
+//#define XPOS_KD 10.0f//0.015f
+
+// when using units of radians
+#define PITCH_ANGULAR_VELOCITY_KP 100.0f*46.0f//101.0f*46.0f//9292.0f//202.0f * 46.0f
+#define PITCH_ANGULAR_VELOCITY_KI 0.0f
+#define PITCH_ANGULAR_VELOCITY_KD 100.0f*5.5f//101.0f*6.8f//1373.6//202.0f * 6.8f
+#define PITCH_ANGLE_KP 15.0f
+#define PITCH_ANGLE_KI 0.0f
+#define PITCH_ANGLE_KD 0.2f
+#define XPOS_KP -0.015f
+#define XPOS_KI -0.005f
+#define XPOS_KD -0.03f
+
+
+//Throttle constants
+#define ALT_ZPOS_KP 9804.0f
+#define ALT_ZPOS_KI 817.0f
+#define ALT_ZPOS_KD 7353.0f
+
+// Computes control error and correction
+PID_values pid_computation(PID_t *pid);
+
+#endif /* PID_H_ */
--
GitLab