From e788abd5a9de83d07e1aebe88bcdcd88a8ff8f61 Mon Sep 17 00:00:00 2001 From: Cyrille Nofficial Date: Fri, 2 Sep 2022 11:44:14 +0200 Subject: [PATCH] feat: publish pwm feedback --- cmd/rc-arduino/rc-arduino.go | 8 +- doc/profil-pwm-feedback.ods | Bin 0 -> 21804 bytes pkg/arduino/arduino.go | 98 ++++++++++++---- pkg/arduino/arduino_test.go | 187 +++++++++++++++++++++--------- pkg/tools/test_data/config.json | 5 + pkg/tools/thresholdconfig.go | 65 +++++++++++ pkg/tools/thresholdconfig_test.go | 135 +++++++++++++++++++++ 7 files changed, 423 insertions(+), 75 deletions(-) create mode 100644 doc/profil-pwm-feedback.ods create mode 100644 pkg/tools/test_data/config.json create mode 100644 pkg/tools/thresholdconfig.go create mode 100644 pkg/tools/thresholdconfig_test.go diff --git a/cmd/rc-arduino/rc-arduino.go b/cmd/rc-arduino/rc-arduino.go index 461317c..e876ec3 100644 --- a/cmd/rc-arduino/rc-arduino.go +++ b/cmd/rc-arduino/rc-arduino.go @@ -27,7 +27,8 @@ var ( func main() { var mqttBroker, username, password, clientId string - var throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic string + var throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic string + var feedbackConfig string var device string var baud int var pubFrequency float64 @@ -75,8 +76,10 @@ func main() { flag.StringVar(&steeringTopic, "mqtt-topic-steering", os.Getenv("MQTT_TOPIC_STEERING"), "Mqtt topic where to publish steering values, use MQTT_TOPIC_STEERING if args not set") flag.StringVar(&driveModeTopic, "mqtt-topic-drive-mode", os.Getenv("MQTT_TOPIC_DRIVE_MODE"), "Mqtt topic where to publish drive mode state, use MQTT_TOPIC_DRIVE_MODE if args not set") flag.StringVar(&switchRecordTopic, "mqtt-topic-switch-record", os.Getenv("MQTT_TOPIC_SWITCH_RECORD"), "Mqtt topic where to publish switch record state, use MQTT_TOPIC_SWITCH_RECORD if args not set") + flag.StringVar(&throttleFeedbackTopic, "mqtt-topic-throttle-feedback", os.Getenv("MQTT_TOPIC_THROTTLE_FEEDBACK"), "Mqtt topic where to publish throttle feedback, use MQTT_TOPIC_THROTTLE_FEEDBACK if args not set") flag.StringVar(&device, "device", "/dev/serial0", "Serial device") flag.IntVar(&baud, "baud", 115200, "Serial baud") + flag.StringVar(&feedbackConfig, "throttle-feedback-config", "", "config file that described thresholds to map pwm to percent the throttle feedback") flag.IntVar(&steeringLeftPWM, "steering-left-pwm", steeringLeftPWM, "maxPwm left value for steering PWM, STEERING_LEFT_PWM env if args not set") flag.IntVar(&steeringRightPWM, "steering-right-pwm", steeringRightPWM, "maxPwm right value for steering PWM, STEERING_RIGHT_PWM env if args not set") @@ -124,8 +127,9 @@ func main() { tc := arduino.NewAsymetricPWMConfig(throttleMinPWM, throttleMaxPWM, throttleZeroPWM) secondaryTc := arduino.NewAsymetricPWMConfig(secondaryThrottleMinPWM, secondaryThrottleMaxPWM, secondaryThrottleMaxPWM) - a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, + a := arduino.NewPart(client, device, baud, throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic, pubFrequency, + arduino.WithThrottleFeedbackConfig(feedbackConfig), arduino.WithThrottleConfig(tc), arduino.WithSteeringConfig(sc), arduino.WithSecondaryRC(secondaryTc, secondarySc), diff --git a/doc/profil-pwm-feedback.ods b/doc/profil-pwm-feedback.ods new file mode 100644 index 0000000000000000000000000000000000000000..d3740d7c939002df51639f1094001d9f9688f032 GIT binary patch literal 21804 zcmb5V1yo$YmM%E#siJJI|L0*aCZv?_&ayz&YL&u z-g)cq#aUFHQ{Ud@RlDljstPc$xKL1tP*7Cy)POKM{%AHRD5$>=GtgPEhLo4vV%E1Qdxv$?UEinMag-l|JjNZfGg6TRW(FoZ}Qe?oux$0 zXti4WJ0bFA*&%r*({3_JvKo!C8rw!=78mpg6CJl8zuBn5n1TpbK!_v-E5&3&!Gc%M zATH&r>N_)wP?3*umF;t|f3E0W#(S3S-Z#O*y*bh9aUl5cd_FO?w!Gf8J4Mg6`o0cp z7dHrbB=`6pnkEvHVuv~ri>{V3oIrxCmt(M_unNa!;2NiBFZzh}SZY50BIS3<$HNwdhT3 z%aZjpPeLVsi3^~bY586^iR>($gCh?n zHXbFdP+wowB!g*{S=&3u(shs8!#2TVZM6emet&Sj->1?2>HMNkr6)c@r~PrXFYRPH z(hi9#OMjULMVrm0w+nwi0cjo6!q3{GJby(kwAw8e@P74|8bUA~)ACR@om z`S|q@n&a)K&6RsEGe+ORF><{VwN! zeQUvB|5d6L*n@GU?l6rV$D}-Bn$Ia15mdVS`tg#s<6b0sX+5;4@{-smOd3xVn3(nJ z9_@q00mn?r+9rIiKGIBt54~dTJ3fN~wLv5=)c^XVY%+wyhm+gaz8)q2Kcptx2rZiq zh5BGaQ7l<$ei0{!iCgAJ{XqNB9F%VJj=Zl5X=$cq7Wh9<@HM_sy4e2V%Z9ac<=de` z@Wc%+gjVwBjxadYQ0*MHePkb!R!jfyOt{-a^Cdt)q098VwwhNrcK-)7EC3}B0rB~J zQ=HEK-}30ap+Fo8eaI89n!uuH?Rp}si7DfJ#@24Mj-ME+F}m1v9M32)#DI>p$^UGf zi>e?^{-8Qic&>_|2_tU>ZI9As7X^Wmu;L@Ksyu9>t#9P`{Zqd>pD{4Vh(tal9g-0f zC8vf{-!{wM(!wviVyAwAwJ>QwB5w|j>(NWo;=Xm1-^qXTvc_eeA@h9N z2R>_7EySE2RT4>Q-B;wAH6h;@=KPMCU6zt;cda4S-TtlnL15OOF|}|o)aqm)&vtMC zA4xkTRQo+{{n-9{6RxCghRkLLzF7j+W`)fyYCj2v&h%N{ntFxJLuxq6yRo)&8C1#&Cs4q}miSBY9YMZd9p?u>b`u2sf9l6>BH}hcb_Gzvjrk$HE zq#Y)boLIjLJy!i421Mb!76*jkytEh*zpM*!@b9f2p$H}`j{@OkO|-q=9boyM4OcZo z4_53FPgy4Zp~x zQ}1|)Q!;uOV+(|)gfK60WbB?l(VfD5NCcD1?aRD9>8*a1E6))a8dJ1;0@x_4Voq01JCgZyC{o!AG)-*xSyB3tWZo1PdRGy<+)(2hhlgWdXm3z%$=z`ey4& zHSJ5q7no0^S;^~XKYxNH*qXIT7gf|;zVCNt-}Lb0AssRqX`tr3NHm}wQKb(Z>HAt4 zEObYB;oKtb)1f?{TI~k3IHrAbc=P|kCra!O zDSwqw(lH`1>o#Xq#A{I23WGd!8BUqxj#BPWK7zk8k<_kCd{_2tL?p_)IB4CUF)7kK z(vxU-z1_N3Y`@tIso&DW^dDKfB+tFWYkSb(cyZ_moCsfNnV{Kq1$`oz*|LRCY1k_h z1||!>zE^XcAw*?@eT^nmM5298C42YOBk3BM`F&7#nXnFSVBv+yyOO4*ec&!HngV+m zRShaxdAJ-=9zWuiA!AF$*!0}WF$+aM!Kk;xKDU}5M_^cIX!2k;S0sX%xIbrB-|Umn%rVz4h^xXYCLgF#(zlR9-N?mhI~FG}MZ&fl zGihj@+kj~%QSUyR9YVpJf6OSDVI$qvt_vE4pK%#ksKiE;5^=%$&4irR93wIjG%J;A~B(0{kJf^LAW(_>1?F(tw z9utNdl%Mnx@28*R)0id=oNYa(zTd0GH5L2%zkSkvZprmI|J3-p9&zTqLH{=O_ILVG z`~FKz3Kk0L1LZ%bpMTl_UzJH;wWy(>{yv0|=||Jb&ECYp*xJs8-SwYAHYW$mC{<+{ zG-P6AND?$TSxI#$sCNL!Gl>WTdGqankPGt9LRC>y3K|+31qB5M2Zw}&gocKOiHV7u zn_Ey&P(ngNK|w)7Lqktb&&0&U*4Ea=#l^?RCnO{!CMG61IXNpUtEi}`va+(Vv9Z0q zy}!R7GONtZ&8@7g?Ck8EoSfX=-oC!RLiD`7y_E>lbwG@mN;%0-nw~3XIT4x10@$HB z@2B|XF5c2A9pr|3LK}Wg+@V%GK9KiN3Wc%kZc2fBE66rclL9qq0dfGFVx|gtkotQ_ zP(76x5=;Wk%R+*hCDM>!F%y^qkUB}RzBaeN7xGH+l)HRa`o}Ucfn_`*$G_#v+-QX+ ztKAY=EiZzBZv4*5@*LTH?!i#3Pt*ISZ<1y4ylWk3O=Iq7;u}?iIC@hEOZDur>;+p9SyrfO7sWnU>?C-zSXR84%DL0!s|tkV)*oXI~he9d7L#X?AcBSd`k;aglqEpVD+R4i`}T-i9lEpb6e1x4qWtPMieK3<}{v#a0aA!g+3 z$hY#;>()p8f-TK5wzg=OD*G{(u$)s#+nDbF2qEVFSo<6iQ~5TQNTV)vqHpHo!sNNeS;tRc;13l>CoMv&f zyt92*ji3pZ$I}%)l$hs=jTU$3uKKao^q6klbzDP@n03+3UnVg>q9oW+YF>O&eX}N; zJq(wffsuzQbF5w9w9#m=`ozC>b8ao8mIYcpT1$@BFx`e z?3^B|D*Bj+={igQ$Z0A#&ax*kR4YG6U7P0jUC_R4Gd{Ar5*lj3)Sa3WA0QVk8Bnj} z$gbRS$IVd%Jk}ge61`5WrSkr!UHf6|sO*fK5~6rNjlACMulKk$0F`4YLKmAb^ns9D zB%7DPgC+hOQP-v=ibWtSU2;-7n?H;54wT@;%VQ{rYe`|yJ6sRXP+yei#x2vYDNH%B z@5gfQ!^e9^i>0nR9dPG7fp=Js1E zzBlFYAxe~fJXTir#IM6eV)10#^G@b{?$SD;2|?fYUhO{j@mu9$9N4PbJ}DI#Tym2` zH}#&t=Cm2^s2bCceSK2roXY%;8CMZEBPVjh4FUmIZr%z3w4%0x;tNM3WKR#tH&OPv z(_I&#FK>>YCZc62E9B4oG|dBa7G!KDv;b=6Za5>( zdymRpwh<;@99lzb4pjcgS1&auDliTI=wfTX@3U#my^4GOU7ru-v~MY zJt1*@;em8LoAsrWEL8k0#xDl!)*if#VtVUnpHG(_yzl3m4-a`RHI&03Mfj zOG#o%)mST0S<0=AK6P$7uIN37*68e@xmj($O;S3jY$Enfw&=m?%kPwD`{3%Xabh1V z84-!-@Wzw&6Mn zWtI$zPWDp}`gF*`&XNyORJWz7rV>rs1(05H*u-(CXvc1|XXoZsTX<~65z0W-aHk7m zYx^AAf1FO9&qM+Y`XCTM`C*?>6@(EP3Vn@6Sgc28#wlLhe!S2?phIsLEdgQ7w@HOk zh{oyQaL4;{lN+&24&nMLR5K$;o2=C~k_>?e>o3Fjvp(VKqNBRD%0vmW>Gp^E4e9^k4A6xk{WHYrY?F&%&9jvJ*bW$}MUF?E9<6+3Vm{ z=ZR~Fq53-|4>a#`ftJ4R!PMW~IayjHle2Uk=-UEbKAv0!lq{O$q&#gqq32Lz$>Rmt zhEv7F@gc$pbr~CUhBo2Rt+~I?U(CTO_2ijRQTE2al&u~V9n94r7@`*&^J^Pi$WycB zqgyEsMEW#){*k@1(CD*nid1}_J&Glr6kw01HVBxGl6PGkthS0y4*;k{ykUXa9(7lb z%I~`8l1Xe{(2~21S0$^XVE?Wc3uXK3hLy)M^WK}UETyI_Oc#VXmX!6qEGB#NeM8alxu zP_uz;_&jmPv`TC>YMDwhZ(LGqULm8JTD?>K!*NXORYsK@>#G@^ zFqW*VdD*G*<3(H6cP_uE+PpJ$3oHe;KKY-${jlv+w;l4)QzA@GF9dx$j(4`4D7fAo z&E#@|fjY0sG~6LByvk%gb!79;1cw4{!Wn{QgC@A~Wpiq{gg+ zcMWu%+@mGv$x=iuw(#41c+gjJP!wiUUvt$Ld~DA4ita^3SEWhN6*i~1Z>}*6P8K7_ zR7Xmz9;{Sic#d*fGpx_rQBl_{D8o-_0@}9bNA$NJ!{(1wZE|krU5gIn@*&mqs8Bwo z(^a&1tgzAUWSP>~jxFJ*E~p{x)VW{>RZ+k79g0@J?|MSPTvE;IvB>)~wB|wu?QGMxg_xtIV^*_ex2*xJ8gAvX zV;Haj-K9vD%KN6Nlw9lD5O%k?zV7@*TB{Q?7r;W)=L97Hp4QO(2{DRDmcje8t>E-` zc;n}l->*8kU)i>P*iPDsu5&}BSPF0^#ZYyDXprCfY=wdT`)w_V08!bD!48nYFbsWz zL`~wo(GY<~4UIlObDj&@B`0`er+&uZgiR^J*6~!K)@}6i51w_i&dJia8#}0Y!&P8Z zxfh3PTjA+E1q4d_y7UAGQkF4ENa&cOc!-+Me8CAZ7L7HFg#Eiesc7qln8;RubkZ1Ofum(a%o>mu(D^29K~r6u(A}h*+#b7 zbT}S=U_tpMu#pF>%)wji{Lt11>D4BChsFcz57okzvQ$T&C5t88_x{^Dc*-Ey{RRHa^2Ut}XpRO3<{$FBLKn6;<0_qG=gbo{rc;n)Qz{n8}h zG-|&P@dfhmg$PV6T(5+$ei+U$@0JZ+7mxAYmO^ngFSMByz{fV7`Oh85L{$a1vbV{$ z%9%Gs!H^C^Yp6vpOqYXt|Tfrv1F+1 z!C3ws!E@{a4Iz&mpy)88eJHZ{N7C?85~1o7D_Awk*C*U^w}NDmpJd=grQzTa3Yd4` zMI+$$@#uAvtPjWgpNOqgpJuoP89X2@>*o-LMY`o@5M9W0Q)Yark0R2sxU=M)8c7=}y|U6HY(7&lfkrf)hgqmn`ATwe^YwBL3G z^wV?MxKmnGRAztTfp0v3+9q=mKCYDhKvi`3{FY5BsBo_;Br4hJ2!z@@Lx3BY^@b;< z=pFutxW%sDfG{tp2b+mhprJ_dVt~>DGn6##qAI$APu>FuoY)o<)vk}tOS)z!v=J@c zQUU48^PfNR(EUP`R0Ek>HlO{(tKafLP=U)9RVEAw8u^dE!$F`>Or>fH=jyj&(8Mg1 z4#LQYDmt#$kD#~d1b~+t(rltbInVPds0NBKJuyoa{m9S#fwbxp1(6O%JWb&q5B%x^ zVuBKi{G|z-`>uf0f;9A%Bg=nZU_5?jvuR){MIN!DyL2(&oJ9=U2%Y&3Qj$PaYqaRR z&$@d6K#!P`#@76%>Ph$LJDm?EV@PhVpBshFvdZAcnHyE$wkk<7dv6Hy}V5hQP;Q17cq)#cH@t}2G)F6w-~IO zu~Nr}bT7i8ee?aCxr(+?aBgvk7noPn$APxOjo&z%DG472Bb>n}i^wP$vryH&JqvgO zGG&SNJh@dkr|Ngxv@}?!3AF;GcK-f-!&Jd^gNC917H+xadz|_wXXtnVmi0uA{8M#7 zdjW=cZaa!&uieV>sh{RuGu@%w=V5_BU{Q=Wm&NYemRf+h&h?SzXB=@Onu3tuls5MA zHe^P*fQGs5m({RAwAwVAbt-e6Jgbp~ZoK?}d@$a6V3Y7Pjg{y%_n{%!2ItePmnCw-j})g>x5& z>(WceLdCG`FEUUi0CJZ7jVyCJe|nI(tX+AD*O=?1w^~|*E+!OdJ4fFFPZ2H2)u0|$ zEMp}y59mIV+oig&8j7hqYMTez+9gHZw;ZE*{CUJ8JW8mvMPD>}*?gp&iXdJ-&rU7t zq*j5)lp|%oYg=A^zMBnlz!4|3+%We@@JAf*W~>TJmxkO>-1$fFuMjxk`?`}_w0qgh ztQ1uP0G?R9+bWVX7QPdI_1TM$m{EK+w7|8Ur@*;R7@3?R^xUeiGdFFIfLbTg);)~?fy$J-|@4u3?lR*v}ycD zL-?O)kp#onP>KdBSv97qzocEiD8P$2#O&v&F7Qy9@kg^CpbZ7_@@qnQoocQVT=1I2 z-;X3?ooI;<04tNF1f*tLXR+ASK5)F-8DCbAd#p#I3d0I3==KRfd5<=r-oU#Zqr;x+ zk#qd~X@FpOT(Mp~vTEYa*aA`g{n98kS<1s`J*@*}cru`eQ zE&ZIJ8TOrDnWyN5YmJ-FI!xCtl8}mbqZ_^~ASJSIL0U^<>3wE@J9j11s>*@l+m*iU z0^NPc*RXYM(?7e*qWa;9e{CF)`ZEs_)#+NhM>!~Afe^!{neHT_tG4Iccs9@BFN(L=8ls4P{WGqJjsEdEmY=w{Bd$rXV+}Df2l~ zFPdX*Z9 zF&!@6b4q9jVVwrz`?)lrB`qr*knn*jOA9Uzz2iq9q%LcOF<$?!E0f z)ejdS1|D7Xj5rY_xj1EL9 z{`^+PSGK!Wa`^bMc(Dz?C$Rk&++(i^&yc|x>KwU!RHGMlcy~snztaOnJ!>6X6c!!Hgr~0u`e5E7?`DwOia_hR# zXNWjG%O>^*ClVFtlXxWU`@nk2>f-G-VfB+wd&VxhaabO7hhP~!D*M$q@%rrJ)F44_ z?oC}t-45BVr%CDJ?ikd99mZ^{k?C#dsY?#Om-#7=RfH=Fv7EC)A-`2)&OPHTI!J#jtHdNTMe~+q;Uv)3bDtLTC z>b+Z*o|OA)A{1OY)MxWOt-TLjG zU&(j8i_I{GSY|eBTU`P6#NCz#N$CHA*u|~tdsF2JP8+ z4SwpNjP$})df_;_j{DBAQ)nuK3)U(jem1?oh1lhq->$m$5j4?LEcK_0hW)Ck9-n){g zbQTb<=9fxIeDht~Q$+O>S)oieg!7{q&CY_cRBKYTSjTXM=)-UB5DRQ6Gdg3U5X5Iv zQKDSGLL5WA2p_8VyHO$=War_WJHvxVAEsGg zw1rEl^fG8&4CuzgUS&b^Y69dTQz7 zrN6SOk&+bu8Hg!fY|fYufeqI@#oz?C+zm54hH1eQlGV#Kn>(OHT>pEWOBR9`rX}L1 zO}lx6lWS+cZUef8a;Er|pwG0IZEfYC4y}3bUiG*?b;5r9pQ&o^m^E_@zMl1V{k;73 zT#s>HpTN4L8Fxh{N6DjpM^NGITzglmerIlQ(ylD}ZXzr5>&^Ac_`#l=k9R(Zq}I(l ze+j5|A~$!SyjDSx=gy;ZvvIRlxzwg-%{%KFi80#Ba>Zxg*YYhZM-4JF2{gc`W%@eu zNLLf6+G?j8>z|i{7dN``9g9zw)8Jm9NeIf#1bM{7M#1zeDyqKQ6?It>YS}*uzS024 zj>SVukrCTTTJTT|vT~}QShd^D?k7_HG%R0-S=*eaLHTy}A4a2(m>f8#LGXm@MmaUI~I2u?EJ=p4Ecy70D7*((+o*{Sg|97j?opeexH*f1AS^6VhZ% zU<*i@jD<)m=1j7lX&Kl8w!dhsUS-<`#w}&4@C#&MTmzYHI!E6gxA+N3)vvHD)&xf3 zvrRZ*p5xt_E8X)<96=hKa<}V@008TYic57wXkHE30ob1wT=hK;V7Q9Bm|8T$lCx~0qWp^d6@g2{Y<75|_6n$g-|y0iX!A{F-8ZJs1@K)pkl@Ez2nJyvc=8SdHUWBJ6FVnrh9^kiBW@7b4BG0&o_bh$^_tTUHtr+ zycd!dJXUtz{pnXphx;I1DvQYk(~P$k73J#VS6_9K2E$#jv>ZA|-8iC+R30Q%6AZb{ zN8)7oxg2(wYHnsUyLzg~K3m^N#rE@M_G_R7$21m;=f=K9KwMu*M4QnXh1>T?)W%#6 zSTi+OpE!&k)=LVE4Gq)E+OK*S+%YNxFz64!Zs_c~0MA?+gwy+0!|-6pfyF=P4S9o; z9e^Jmxa;MhQ?t{%0E z;#2OebZG>^)`O@qg(u45DX_&iJ}uatgozB;+B4g*-c&TGbSwtwEhvd2?gK9#fR%kw zOo}CM1GX>2OO<1Dq|zqDxAB*dK3#oNap7FOxwj_ooe6Loc;B)3-KlV@tZN+X&)(*| zJrHkLb$q+?@x(**A9qGw)u2;6?q3Q5FT#)#-4#G(m4oMRzY<3X^p8CNvRm5DAdcY5^WHDm`^+Ktt(ym2lg%ULeIbt*H@7n| z!5H9S!+JKkJjEjj)9h}4{V{|h7Ym%mgYHqg3=na193W`;%|f+1ou|iWgLcEXlJ$_M zJ>q*T_j>b1d?afo6|^;vuI*+r{N-yN$_-!pKQ6@X>;bg3cUPdP+LxluY@jxNsF#_K z-~=l{`JL45{tBAo=Y#5sJ0aA5s|$ngm5`(9!>W3^`wauz<>(js^j^0{nPtB>$HHTK z@WAqhC`c0^g>k-RQ05 zh0NWy2l;!9>b5M85BCPP^Tyr9NkDd{4}Z=ocy>0hHW#9S60a@iZ+@MNveg(!8|w4K z5}@B%KGjjcZn1^;or|&maiMon!=wi3l8{r;d*r}Li+g`lw;gbe{^qEMM|ruM?Kmhp z^|8#87r;C3%>mpXIolw|1GDASQ^0Q!@zln>PHIBT;Q6MOqf>J2Xy8Fo#r+*s&MOyy zSJTTKRI;3h6$-hGm}f2kQ8zA0spbJ7`wcd<>7UcuDNDpI)dic{(yDn{3y_78K*DAt zfJar$_M0th{99|S0sbdHEr=K*=E| z$FnNh3Kko7(i6dgGxBP-skXx1AA+Ac4z#pw{8WLBg^L19PpMe*mSxP6)rJM2;>F?Y z`tI+II~V_P!3pWCvBg6A3#?%OUga(yaoIB??7OMba-Imni2HKofNvn)`L^vau#Xv| z4J&+wg(EX6^t9?a_->nuwNI?0eauEeKd9v%$a;BD-| zT9{J!S{ArD7LI>qBp~hVyqfvQRUb5gYy`>6Bm`a{`yytr0VzB8gCW|yfo;47bHmI0 z6F{_CAR9<9ign_42Hx)=!#KJ1n8hnxtW~{(n}2HlPV%2;QtCbu0E&*qgpCs^k?#}D zwHhSp0Y|q3AT60cTetDRO_G+Z1)tAR^lk2AK9Eb=Z5CeFP9uoD%c?>;jmMtni^{5& z`-p_C{6GV z?hpiT+ydl!OBW{30V2OM_)Ds6U%1u;mJ<;jOfxfG%*IlE31`6{qtrQGj6#MJ`4-qexm6%V zh#|v8Xz2@(Tqt1xac#c2Br}5@N+G9+s(4aV8)jLuvou6&hZ?)9OuA-c*oyc^*nbul zLxS2pzq#xXxCG*8TqlE`Siu$^Q8aI6Q(b5q7LH|1oXr1%BT&pFC-x7101t#JKntiy z{CgDtj}xf>H>KeJ1<|BM`tJ;c|5x&VL0b5?kR%YzJIJBM|KK?M+vNYA7V$42>Zvr8 z|BglRpUH(6UJ=NHkjm_w@S7n74k$S(Wyuz7pQ~%@; z!hzZGfHyLUhmR~D0w;M~P$t{isP33(>KLh4m&h4(z{~fw6rg#xpK9suZo#XE;kHNi zqfxywTk?~vkfuIL#aMPnr^qw&ceZn`&P|1QO{`CTYf%5TVpfIyK}KPVzyce}7L_3h5r zu5*!&__(!OeUf*_8F;Ion|?(@i2(i?v)8 zGQ)j-ArD(ZG@lXT{7e}!;&oBTizRICHX%^Wf%zU)tN0hZUUb18lSNFr6{(HuZx{}r zn@}VyVSkA^56L<1AMUB@r>am=(r^{^ank#EySSNYI#-O2eW;W2VB8)LIrK_hbf8Yn zfrVUwzp!IkKu%W4=w+?9r{aJQQAz2qdR|@4BYXFb^kw=lgZdXYaZUkpL!YbPo4e4VFYxAs_4|x_8q1cxToJLt`J-TT1;R;T?_yIGF z&5~>4?HN0!!Uzz(C$Dq@gSR#gO&@D+P5L0S2P z^3}QG0elYn;T4s@%e%jwV~0!$Zw=BpMj&L=e?R&lJ|h2T^qD$3xc>F|fDFKOprOm^ zN9=%Zl>)T`a>nQshD2hSZ^hhRHs`-Gvs$$?LK!|#_QFsq9G7*&iHTDxjk?+oeaRxC zM>!S!<4(|?@AGh_IWr2gA6$3c{~=?t3`f6|<-Pf<@l&3oVTb3_m*)h*rqx7TN3Jw- zWPN#d^ODeRddknyN=yUd{u$>bRUf+ogvc$22V<|+-tHX@&%ChnibbniU2@GE69luT z3H;A3WvMTMwszR#2CUg*45BFIunjMOTAyml(f_>pznnd|CZOyls>^DYK2oO&vzmYN zh$YxY4miv@3+Fc-1Wfk)`h^hq!#O4;8TqTwrE~vRN)wGj=$JS~-l{%xn75(5jl=wJ zc;?>ajoWVKC^Qe+9Pux;brk`=FV(+z89C>HIb!j`Lm50}u&6R2Q?tCT{W<4Y>AX_G zt+n6SbRc9xn(}7TuX9piG_7Tyc@ubFgq*24n@2y%{eJNVfj(yulal2LByN8Zl5_M< zqRs^F5iB0}R9K9uQ)pD7v;8r`{~4_5teVfGFVt+_Wg3AE!m0@;7WD;j(A%J>u{K#- zJAJ{qRz$Fjuy8Ng8_c5*Gau$!MN^Ugic+lvThN`88ddvk4G|q&;`vM6jcoFfnuM)u zL|@lnl*C_JWEjuIHi)2XCB^(ahSBs`-DDZPMs~zT>vJp1fP@HN6UOh<+1Wh-%{U8( zlE~}7(dniZtVY1Q4yT75<|Mnm4t(& z#FO!~D%dS^a6iT1XGgh?Fz0O?fh<04BgYdgO>;SLZ?iGg(qd1Q0Sw$dQiY3!_jq$lH^{; zI1_~%A&zI(*|%i}Ym~X#VB#ia#PB&@1ID;o+{28}tnOsL_5JvX7>b^wBFtJX>N`_Xw4HxZPg#>3OI`Zn+O zEEV(V9+vv>gZkNZ7>tS9U1L4SkXJa!(saA z)skrVIN$K{U2s^)a+#Cq+Y5hW^*9GcJ=TPl-0b&yMf}esH@ewX6w6RzrhB=A;&=8txrEkWG4z&Q<#@+C} zDaT@ixp7b7Ji`vnc}#Xe-Z@ov3WR3>G+{h`PYYg;C#m50Pqd)ITwTsu=xvh^n92B& zqkQi8i717I8_)xLI+XBi;qO1|7~}F|G99y0#!829!~!Y9-|Hfq5J*dP!o^npFtRJ+ z!;g&lX`LJGje_Y&;`c7HAm*3s8M7QIvsedQ;Cna4JKCzSEm>Vc_Vs0}-|tbD1F(cp z2yzkyo>w~e7e#N=e5m-U*K}i31gl%UUnGVT(|a3Se-K&c>q;r3272;tHL6<{Pw0jz z??YWV^ys#3#eN8sjDkVOynxSMYz|C>kFh!nZtxe~YTQ_UkA|kU>ncULfyP(;a3ec? zV0EPPZaXJ%M<_Ry(v$uaN z;o@x%+Cd0VP$v+-&40gy|MexYH+HbLFn4ifb2hV>OrHF^cMiP}3NxSjDaH;5=gctH zYLnN)6Rrctew~TE(G{PprB)0&&YRz=ivZ_GL~CLM|SUf!$u!IY79Ox6l~#4OOx z)>^VEu=2BsnIekij$aMo6WDx_5!^i!AC;V>}`f zd7QZQ-L5Y&|I{{;g|?dON&Z)_2TI|Xb%Bcf@5shY>YWiyl!-xNaY%||p{^mMZORIR z+-Wyz{JjNd+rvhkb0LL7MwiU-(?2&NQA_s>wmR$1^+Ps&Q?0zuO}*Xb2Cp{Xrx;~( zovR$T3wpgBys}Nd5pCBtx=Y%LO%+)_eLZNrrU1y){@npJK3L7dL3ThP5a#;7ufBiy z!~WCkNLF%OR~eKqsriJ!EyFS)41C4Wd~(>dq5QcAqrn zP4+iPVz<`zC<}=<=Vcto(9Sc z;b2{09m@DneW^~gOQq?_?N3=`RATtVVcn!(VZzqtuliXm%;VSzjs){SANUAHy6zaX z=(0z}gdcCf3=iJSjJ!=2U%VUh_7H7@xcIOTfm_()W`ykG(ELV8#$}6?AHj*#1);nA zR8m^Mn4{&9?>_;#fZtPSh$fFRaR=M_P(HfBRfq?8(&+InZi_bk=Dqeyeap`Bh1xh7 zsGQm2{~eJ#hqD?OWMD_ut?h$=QRL>EHB&6QKH152X8q8}<_;IP0o7jA(ESNkaW0F! zdKR0ti~X+j=|E*Z!e5diC348O{DJsuT{Y6n$_^u=*a@x7QFh~V!5rLRN8bfoXzdHy zANIdZIt9Owodao7h5u^Of4Vn72El**h#?^fM+XaQOE>3#I5)d+vAa4t+Bq3JnA<_* z{CC&r{{le*r2hhEY-(z5XATkL=={GRDgFi0+TPgG+=X4-+ST6J$>o0m|MkZBpV_sr zb2N4}|KC*pt3o45P0Ia`VsT3tGmys-cN&O)jG@5%Sdm@UW^3QzS)n( zDmtlEwNOm`%WSv*aJoSn?x5EM9C3z6$I`{Qr z;k>uIJwGR|?vBUvSbC&EGcp%_3mWYpO^?_SBD<-*HcZJrYISL8#%ms#q%pU6F6t%Z zTCY#e-EN)TmiF^2ZY+E9In&MZcqbfdXYMlX3s?l!k9!*B)NPhYy<5#5Zkg$fhzRBWGFM=MEo!R;7?Ypklv&DW#7o>Y#8zTy2eX9ci!89O7%vALlH78 z;b?}!-BQn`!RN35Nlh{COhXKq>I%FW=rifhK*@rAYn@;j*51fp@}85xlwhJ#!iZ3; zvldhWnHZ*=I}uq&0=7i0yR=KL1pDfrkBO_nh#Jk9dH zUCT!#K;~5{AiZIlAz}yax~IWBWj$v)KnyNj^rPo#pYI}eg;z+*u92uKr58QkD3ApX zK&Tu?LQ~FI;!(o#cZ<~(HrP~fvI+UzDVesy$QoV94=)^I)(SH&pk<}y zG@+CwWtc}2MYeP3!!>4A4)B@q(&1)XWlZ~6nN0@qe-q-H#{;a@Cv@PXq})DKKF(#s zl6=CPFx%oE#N+a|3%TJN@kHH5=_!X}oHQ*IZ0&L*q*_6D5tPGBE{^`rj-&g2>0rTg zG$jd^gY-~`Xb&BlR-R*wTtvT8TOf4vnYJIVj%!Zju<_*mr_(cXL+^eiluUK50$Zfq z0^dekw{hR!fn1i7z5E3FIEh(StAs+Oa4dOkaIrSk?P7Tw!|v|{JzB1c7U1mqJEQl> zO20j){aA~|WIT}$K#Fcr0`*yr$ zi{h)nkBB{O7HNqbi3hP`)!%KgU2GjXd^afJ=fU{Hc3&`^$?vdsGbgu;Py8Fwe4^Y& zt*@#4*@s^YxR^(|G*JT%$J z%WZ{xiKCGBir1+#4#IBJasDo&Ay#7rWFn~VnvjxlEV#Ew;wGLp$dP>7ysTTb;$yz~ z7T@DD1$h0CNc?eZdGpK@wp)!oo`axY3%`ATo<1Sw zQE>2*o!{8ZA1o+5lV(l3%7|d``$zfv(%#4WO!MI{Txe-H%ncR%in{4SX;|JMlcm^D z>6^l7dj{o9el6s?giVl^hnD?l0K(6th_U0mFkT5bu&vj~d_k{c$*R$=g09DEFb?I3}Bs-LYg_Dj~|= zrB4{u=Ieu+TPW)2|JTTw$3xk@f83fSGe))y#vYQjq#kPw#n>8KMxtbl?0XF=N|q5N z6f#9amMq2eNK7b8vXng|+holWLI~fPr=Mp=<$3)+^UvI`*L{8NnKS3y*L~jS%w+_P zz}s~s)b+ngMJ-ES12om9tN;RrE;q`HC9e(7&(9y2QYcT#WYm-H`O9}2GVYN5k}b(x zY}N^X_6f(j#mi{|igTJcSX-0Ub&6Qhg}E?vuo_W+)YGFZ(iGs$QeV936{)cC&%e|4 zlAc`#kY^OZlK=HcyqlavX;@$+&0A75jn*)Zc8qbE~7oX&_EW8V-73se}3c(?}}mB%NmuQdi$IofJy zFmhVo@67v(&UEdL6q~>=b0>^!?Bi{Wc>UfXin{OM)rZcxhORnJ9L0@SK0{1oMUng; zlsXX$@ulZ7S<9XL*PZfxUco<}0SMj5uxbv>Ob|W=w%e?DxxTjmWdM4YC4LF|H)4*2 zNcXPM-^baYZms!=yB); zCXP4&S+LmYD8BQd{;;0srN*3w{yc`zwj8?O;3)c$0S#F0?g`|#bJt@CSjmeLjR7A*nMLp}`5 zoSCWoEO3W@`I|2)?I0%#Q2N)E-z9w&g&yLrQ`QZ1cd_KMaF!=_o2+m@%bL676|s?U zDMda<3ApRXECgpiBa?qZrQRQcxemH*yGov@COiWLWh}D zO+9g$A9Zk!`@{Vv;#jSv@&kn0?T$^*>s02##LJwkLV1tWYnDj345zE!nO8=2iS0}C zf$s+%*Tk6PO^^F5s`s3DQ#=o_&jXeHMz!QxrUJ7;T7OsZD3g< z&z$blSX@p~0o1VBNA8(Okovu2QV$t;&l{dhJI^L-fiKDJzcG1k??GGK`Sw9q>-J`j z`*0bO@{>e%9N(z<-C_EUEYaE+ec|NM7x4xnvBmcW9UP|DnYlChx-@M9KVbUJX3K#ptW?DHP zXXrHcW2H(ho%gG0sHo7qX)CWwHA=W?3VE)Y)cpMOYnV{pC)TZ;Ky-2Qr706+kJs^PR-kmDwg=1M)kMJQf-X7|rWKTge|ujp*&Xz~{J`LN zWL|>kv3aQ^qP^K=zfs~VAEil1|F?bpgLJ*j>w8oj*w{no-=G}?Yo^P3d5UG0-<6VCB~pWfavH9?zm)jP@?YfB1_xUK}v z16*dj?uaBBfJz5lL6h|By^L1u400(B1B6XRqW0!)w#5NOC#TpMNq;?&$hcW!AEkiU zujMv+S&w>NaW03+E>}C3Ei>rN;0IUAg3L&l!H)H>pb1mW?DuPaDv$R@Yy1?AZXbsFa5 z!H0K&?bYssRVYSH>IB*4xGMrnK8_6pJ`J{-6eJhg|-bSIm`_3L&RK~UlO$GN%o%b!gVbofrC2;pK%<6H~!4QGDmWwJ8 z(kinTTMqb#=K^MV>zX(du_6nFVskUj7as^fj7t0hC401Pey4k4_Vi#xKSLUC$k>I4 z3mmR$CKs1=*}Twe-d*olV$CG`7p6g}M#MU-Qx}F|fiY|Bc%@52MUcSoV}k8?Sf4<& z_CzmRRJ}zg22`W1aZr9yk92aKG-L*cwA_ zW=RL#i-Sud(q~kf`p6491d)qjW2ppRjwI75?Ny2C(L*17HYKk0vzT4u6zr{DZI4`X zUAFk1_LKg>zCkDa$hjJ(_+Po2mz=9j^evCc8=4v@{)++0D|mRTYEMBiQ017MwUBQv z-|#m$SCQX|q@2h+coJ5w3Fs!yC&LlFz8n$I706>dZ!iZyA_syO?tN(S>aNViib1E{ z^^o6UEFH7l@YCS=u{pwqfhRxz!kJSAqT(W)RvRsKpYUR`r)HoXx9vXYeU^E#;Als_ zQS+@3(OOE=o-7Q61RE5|QFHi4T)a-B6p_8Dvj|$e1oAgzdz}tPFlg)Sy~oGjNnp+b zbiB}TUt+LS$zEcx0h~P^Fl}NdQJCq0}-84ALgsqO>WP- ztuXDma!pNFH!Q|oFb^r6W81A?+jYC#twdjjeW)4J(tBpuVMJH4xUA>&1`Oy0A#8q6 z!OZx5IILvi$@E4;?KW~Pe*Ea(?0Bgw3nO&RE_@ zT9uK)=r=dRT9;9gis`i~0WK2Xzl%2{v2xou#(sF8#~tO@dG~F4`bg#MeXDZV+4sCf zW&!g1>~QaKxf;-WWzl0PPhB6Wy3B5XG5=KPxS0Zelt@Nc-v6x2(lhYT?Odmi{M*uK zt7@M)>}S*#|DY{7M5)9_l_mM@f7Ips)rhuu2!$$B6+!NYwDpR%BDd(Vvgv`UIr0$tH=K%xel?)w?UaRa zs(|FGC;uAIJR1M%K#QEWx8R4P+bR98k+fu!f~2VesgooB4^VGKZM8e~EIK+0GNX#5 z@jFzcWuV(T9yRVK1*IMfYX0uCor&)3M4_ToS^dH3H%7X%8HGSnmGB2MDl^^LWqTtv z{K18mq3-On{nOBC{oY7aruzSNIs69~Dr4Q*g+f`Wa{q(NcILXX)ArWM{(}=OgZ=5V zHF#;KmJ|w0RX^Eh=RrrC<|q?Ssw51yO+&Z({?-L)Z={qgNL8!J4mr^r#!SBJ($Vpf OU*61gblGN\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P-?\d+),(?P\d+),(?P\d+),(?P\d+)`) + serialLineRegex = regexp.MustCompile(`(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+),(?P-?\d+),(?P\d+),(?P\d+),(?P\d+),(?P\d+)`) DefaultPwmThrottle = PWMConfig{ Min: MinPwmThrottle, Max: MaxPwmThrottle, @@ -30,22 +31,25 @@ var ( ) type Part struct { - client mqtt.Client - throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic string - pubFrequency float64 - serial io.Reader - mutex sync.Mutex - steering, secondarySteering float32 - throttle, secondaryThrottle float32 - ctrlRecord bool - driveMode events.DriveMode - cancel chan interface{} + client mqtt.Client + throttleTopic, steeringTopic, driveModeTopic, switchRecordTopic, throttleFeedbackTopic string + pubFrequency float64 + serial io.Reader + mutex sync.Mutex + steering, secondarySteering float32 + throttle, secondaryThrottle float32 + throttleFeedback float32 + ctrlRecord bool + driveMode events.DriveMode + cancel chan interface{} useSecondaryRc bool pwmSteeringConfig *PWMConfig pwmSecondarySteeringConfig *PWMConfig pwmThrottleConfig *PWMConfig pwmSecondaryThrottleConfig *PWMConfig + + throttleFeedbackThresholds *tools.ThresholdConfig } type PWMConfig struct { @@ -89,28 +93,46 @@ func WithSecondaryRC(throttleConfig, steeringConfig *PWMConfig) Option { } } +func WithThrottleFeedbackConfig(filename string) Option { + return func(p *Part) { + if filename == "" { + p.throttleFeedbackThresholds = tools.NewThresholdConfig() + return + } + tc, err := tools.NewThresholdConfigFromJson(filename) + if err != nil { + zap.S().Panicf("unable to load ThresholdConfig from file %v: %v", filename, err) + } + p.throttleFeedbackThresholds = tc + + } +} + func NewPart(client mqtt.Client, name string, baud int, throttleTopic, steeringTopic, driveModeTopic, - switchRecordTopic string, pubFrequency float64, options ...Option) *Part { + switchRecordTopic, throttleFeedbackTopic string, pubFrequency float64, options ...Option) *Part { c := &serial.Config{Name: name, Baud: baud} s, err := serial.OpenPort(c) if err != nil { zap.S().Panicw("unable to open serial port: %v", err) } p := &Part{ - client: client, - serial: s, - throttleTopic: throttleTopic, - steeringTopic: steeringTopic, - driveModeTopic: driveModeTopic, - switchRecordTopic: switchRecordTopic, - pubFrequency: pubFrequency, - driveMode: events.DriveMode_INVALID, - cancel: make(chan interface{}), + client: client, + serial: s, + throttleTopic: throttleTopic, + steeringTopic: steeringTopic, + driveModeTopic: driveModeTopic, + switchRecordTopic: switchRecordTopic, + throttleFeedbackTopic: throttleFeedbackTopic, + pubFrequency: pubFrequency, + driveMode: events.DriveMode_INVALID, + cancel: make(chan interface{}), pwmSteeringConfig: &DefaultPwmThrottle, pwmSecondarySteeringConfig: &DefaultPwmThrottle, pwmThrottleConfig: &DefaultPwmThrottle, pwmSecondaryThrottleConfig: &DefaultPwmThrottle, + + throttleFeedbackThresholds: tools.NewThresholdConfig(), } for _, o := range options { @@ -153,6 +175,7 @@ func (a *Part) updateValues(values []string) { a.processChannel6(values[6]) a.processChannel7(values[7]) a.processChannel8(values[8]) + a.processChannel9(values[9]) } func (a *Part) Stop() { @@ -225,6 +248,11 @@ func (a *Part) processChannel3(v string) { func (a *Part) processChannel4(v string) { zap.L().Debug("process new value for channel4", zap.String("value", v)) + value, err := strconv.Atoi(v) + if err != nil { + zap.S().Errorf("invalid throttle value for channel2, should be an int: %v", err) + } + a.throttleFeedback = a.convertPwmFeedBackToPercent(value) } func (a *Part) processChannel5(v string) { @@ -295,6 +323,10 @@ func (a *Part) processChannel8(v string) { a.secondaryThrottle = ((float32(value)-float32(a.pwmSecondaryThrottleConfig.Min))/float32(a.pwmSecondaryThrottleConfig.Max-a.pwmSecondaryThrottleConfig.Min))*2.0 - 1.0 } +func (a *Part) processChannel9(v string) { + zap.L().Debug("process new value for channel9", zap.String("value", v)) +} + func (a *Part) Throttle() float32 { a.mutex.Lock() defer a.mutex.Unlock() @@ -304,6 +336,12 @@ func (a *Part) Throttle() float32 { return a.throttle } +func (a *Part) ThrottleFeedback() float32 { + a.mutex.Lock() + defer a.mutex.Unlock() + return a.throttleFeedback +} + func (a *Part) Steering() float32 { a.mutex.Lock() defer a.mutex.Unlock() @@ -341,6 +379,7 @@ func (a *Part) publishLoop() { func (a *Part) publishValues() { a.publishThrottle() + a.publishThrottleFeedback() a.publishSteering() a.publishDriveMode() a.publishSwitchRecord() @@ -374,6 +413,19 @@ func (a *Part) publishSteering() { publish(a.client, a.steeringTopic, steeringMessage) } +func (a *Part) publishThrottleFeedback() { + tm := events.ThrottleMessage{ + Throttle: a.ThrottleFeedback(), + Confidence: 1., + } + tfMessage, err := proto.Marshal(&tm) + if err != nil { + zap.S().Errorf("unable to marshal protobuf throttleFeedback message: %v", err) + return + } + publish(a.client, a.throttleFeedbackTopic, tfMessage) +} + func (a *Part) publishDriveMode() { dm := events.DriveModeMessage{ DriveMode: a.DriveMode(), @@ -398,6 +450,10 @@ func (a *Part) publishSwitchRecord() { publish(a.client, a.switchRecordTopic, switchRecordMessage) } +func (a *Part) convertPwmFeedBackToPercent(value int) float32 { + return float32(a.throttleFeedbackThresholds.ValueOf(value)) +} + var publish = func(client mqtt.Client, topic string, payload []byte) { client.Publish(topic, 0, false, payload) } diff --git a/pkg/arduino/arduino_test.go b/pkg/arduino/arduino_test.go index 7fa2f16..87f5ec2 100644 --- a/pkg/arduino/arduino_test.go +++ b/pkg/arduino/arduino_test.go @@ -3,6 +3,7 @@ package arduino import ( "bufio" "fmt" + "github.com/cyrilix/robocar-arduino/pkg/tools" "github.com/cyrilix/robocar-protobuf/go/events" mqtt "github.com/eclipse/paho.mqtt.golang" "google.golang.org/protobuf/proto" @@ -59,8 +60,13 @@ func TestArduinoPart_Update(t *testing.T) { }() defaultPwmThrottleConfig := NewPWMConfig(MinPwmThrottle, MaxPwmThrottle) - a := Part{client: nil, serial: conn, pubFrequency: 100, pwmSteeringConfig: NewAsymetricPWMConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle), - pwmThrottleConfig: &DefaultPwmThrottle, pwmSecondaryThrottleConfig: &DefaultPwmThrottle, pwmSecondarySteeringConfig: NewPWMConfig(MinPwmThrottle, MaxPwmThrottle)} + a := Part{client: nil, serial: conn, pubFrequency: 100, + pwmSteeringConfig: NewAsymetricPWMConfig(MinPwmAngle, MaxPwmAngle, MiddlePwmAngle), + pwmThrottleConfig: &DefaultPwmThrottle, + pwmSecondaryThrottleConfig: &DefaultPwmThrottle, + pwmSecondarySteeringConfig: NewPWMConfig(MinPwmThrottle, MaxPwmThrottle), + throttleFeedbackThresholds: tools.NewThresholdConfig(), + } go func() { err := a.Start() if err != nil { @@ -69,7 +75,7 @@ func TestArduinoPart_Update(t *testing.T) { } }() - channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8 := 678, 910, 1012, 1678, 1910, 112, 0, 0 + channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9 := 678, 910, 1012, 1678, 1910, 112, 0, 0, 0 cases := []struct { name, content string throttlePwmConfig *PWMConfig @@ -78,86 +84,86 @@ func TestArduinoPart_Update(t *testing.T) { expectedSwitchRecord bool }{ {"Good value", - fmt.Sprintf("12345,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12345,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, {"Invalid line", "12350,invalid line\n", defaultPwmThrottleConfig, -1., -1., events.DriveMode_INVALID, false}, {"Switch record on", - fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 998, channel6, channel7, channel8), + fmt.Sprintf("12355,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 998, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true}, {"Switch record off", - fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1987, channel6, channel7, channel8), + fmt.Sprintf("12360,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1987, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, {"Switch record off", - fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1850, channel6, channel7, channel8), + fmt.Sprintf("12365,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1850, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, {"Switch record on", - fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1003, channel6, channel7, channel8), + fmt.Sprintf("12370,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, 1003, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, true}, {"DriveMode: user", - fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 998, channel7, channel8), + fmt.Sprintf("12375,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 998, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, {"DriveMode: pilot", - fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1987, channel7, channel8), + fmt.Sprintf("12380,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1987, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false}, {"DriveMode: pilot", - fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1850, channel7, channel8), + fmt.Sprintf("12385,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1850, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false}, // DriveMode: user {"DriveMode: user", - fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1003, channel7, channel8), + fmt.Sprintf("12390,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel2, channel3, channel4, channel5, 1003, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, - {"Sterring: over left", fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 99, channel2, channel3, channel4, channel5, channel6, channel7, channel8), + {"Sterring: over left", fmt.Sprintf("12395,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 99, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, {"Sterring: left", - fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,%d,%d,50\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12400,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", int(MinPwmAngle+40), channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -0.92, events.DriveMode_USER, false}, {"Sterring: middle", - fmt.Sprintf("12405,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1450, channel2, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12405,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1450, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -0.09, events.DriveMode_USER, false}, {"Sterring: right", - fmt.Sprintf("12410,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1958, channel2, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12410,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1958, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., 0.95, events.DriveMode_USER, false}, {"Sterring: over right", - fmt.Sprintf("12415,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 2998, channel2, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12415,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 2998, channel2, channel3, channel4, channel5, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., 1., events.DriveMode_USER, false}, {"Throttle: over down", - fmt.Sprintf("12420,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 99, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12420,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 99, channel3, channel4, channel5, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, {"Throttle: down", - fmt.Sprintf("12425,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 998, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12425,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 998, channel3, channel4, channel5, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, -0.95, -1., events.DriveMode_USER, false}, {"Throttle: stop", - fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1450, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1450, channel3, channel4, channel5, channel6, channel7, channel8, channel9), NewPWMConfig(1000, 1900), 0.0, -1., events.DriveMode_USER, false}, {"Throttle: up", - fmt.Sprintf("12435,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1948, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12435,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1948, channel3, channel4, channel5, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, 0.99, -1., events.DriveMode_USER, false}, {"Throttle: over up", - fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 2998, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 2998, channel3, channel4, channel5, channel6, channel7, channel8, channel9), defaultPwmThrottleConfig, 1., -1., events.DriveMode_USER, false}, {"Throttle: zero not middle", - fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1600, channel3, channel4, channel5, channel6, channel7, channel8), + fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, 1600, channel3, channel4, channel5, channel6, channel7, channel8, channel9), &PWMConfig{1000, 1700, 1500}, 0.5, -1., events.DriveMode_USER, false}, {"Use 2nd rc: use channels 7 and 8", - fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1000, 1000, 1950, channel4, channel5, channel6, 2000, 2008), + fmt.Sprintf("12440,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", 1000, 1000, 1950, channel4, channel5, channel6, 2000, 2008, channel9), defaultPwmThrottleConfig, 1., 1, events.DriveMode_USER, false}, {"Drive Mode: user", - fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 900, channel7, channel8), + fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 900, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_USER, false}, {"Drive Mode: pilot", - fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 1950, channel7, channel8), + fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, 1950, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_PILOT, false}, {"Drive Mode: no value", - fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, -1, channel7, channel8), + fmt.Sprintf("12430,%d,%d,%d,%d,%d,%d,%d,%d,%d,50\n", channel1, channel6, channel3, channel4, channel5, -1, channel7, channel8, channel9), defaultPwmThrottleConfig, -1., -1., events.DriveMode_INVALID, false}, } @@ -227,44 +233,50 @@ func TestPublish(t *testing.T) { pubFrequency := 100. a := Part{ - client: nil, - serial: conn, - pubFrequency: pubFrequency, - throttleTopic: "car/part/arduino/throttle", - steeringTopic: "car/part/arduino/steering", - driveModeTopic: "car/part/arduino/drive_mode", - switchRecordTopic: "car/part/arduino/switch_record", - cancel: make(chan interface{}), + client: nil, + serial: conn, + pubFrequency: pubFrequency, + throttleTopic: "car/part/arduino/throttle/target", + steeringTopic: "car/part/arduino/steering", + driveModeTopic: "car/part/arduino/drive_mode", + switchRecordTopic: "car/part/arduino/switch_record", + throttleFeedbackTopic: "car/part/arduino/throttle/feedback", + cancel: make(chan interface{}), } go a.Start() defer a.Stop() cases := []struct { - throttle, steering float32 - driveMode events.DriveMode - switchRecord bool - expectedThrottle events.ThrottleMessage - expectedSteering events.SteeringMessage - expectedDriveMode events.DriveModeMessage - expectedSwitchRecord events.SwitchRecordMessage + throttle, steering float32 + driveMode events.DriveMode + throttleFeedback float32 + switchRecord bool + expectedThrottle events.ThrottleMessage + expectedSteering events.SteeringMessage + expectedDriveMode events.DriveModeMessage + expectedSwitchRecord events.SwitchRecordMessage + expectedThrottleFeedback events.ThrottleMessage }{ - {-1, 1, events.DriveMode_USER, true, + {-1, 1, events.DriveMode_USER, 0.3, true, events.ThrottleMessage{Throttle: -1., Confidence: 1.}, events.SteeringMessage{Steering: 1.0, Confidence: 1.}, events.DriveModeMessage{DriveMode: events.DriveMode_USER}, events.SwitchRecordMessage{Enabled: false}, + events.ThrottleMessage{Throttle: 0.3, Confidence: 1.}, }, - {0, 0, events.DriveMode_PILOT, false, + {0, 0, events.DriveMode_PILOT, 0.4, false, events.ThrottleMessage{Throttle: 0., Confidence: 1.}, events.SteeringMessage{Steering: 0., Confidence: 1.}, events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.SwitchRecordMessage{Enabled: true}, + events.ThrottleMessage{Throttle: 0.4, Confidence: 1.}, }, - {0.87, -0.58, events.DriveMode_PILOT, false, + {0.87, -0.58, events.DriveMode_PILOT, 0.5, false, events.ThrottleMessage{Throttle: 0.87, Confidence: 1.}, events.SteeringMessage{Steering: -0.58, Confidence: 1.}, events.DriveModeMessage{DriveMode: events.DriveMode_PILOT}, events.SwitchRecordMessage{Enabled: true}, + events.ThrottleMessage{Throttle: 0.5, Confidence: 1.}, }, } @@ -274,17 +286,17 @@ func TestPublish(t *testing.T) { a.steering = c.steering a.driveMode = c.driveMode a.ctrlRecord = c.switchRecord + a.throttleFeedback = c.throttleFeedback a.mutex.Unlock() - time.Sleep(time.Second / time.Duration(int(pubFrequency))) - time.Sleep(500 * time.Millisecond) + time.Sleep(time.Second / time.Duration(int(pubFrequency)) * 2) var throttleMsg events.ThrottleMessage muPublishedEvents.Lock() - unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle"], &throttleMsg) + unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle/target"], &throttleMsg) muPublishedEvents.Unlock() if throttleMsg.String() != c.expectedThrottle.String() { - t.Errorf("msg(car/part/arduino/throttle): %v, wants %v", throttleMsg, c.expectedThrottle) + t.Errorf("msg(car/part/arduino/throttle/target): %v, wants %v", throttleMsg.String(), c.expectedThrottle.String()) } var steeringMsg events.SteeringMessage @@ -292,7 +304,7 @@ func TestPublish(t *testing.T) { unmarshalMsg(t, pulishedEvents["car/part/arduino/steering"], &steeringMsg) muPublishedEvents.Unlock() if steeringMsg.String() != c.expectedSteering.String() { - t.Errorf("msg(car/part/arduino/steering): %v, wants %v", steeringMsg, c.expectedSteering) + t.Errorf("msg(car/part/arduino/steering): %v, wants %v", steeringMsg.String(), c.expectedSteering.String()) } var driveModeMsg events.DriveModeMessage @@ -300,7 +312,7 @@ func TestPublish(t *testing.T) { unmarshalMsg(t, pulishedEvents["car/part/arduino/drive_mode"], &driveModeMsg) muPublishedEvents.Unlock() if driveModeMsg.String() != c.expectedDriveMode.String() { - t.Errorf("msg(car/part/arduino/drive_mode): %v, wants %v", driveModeMsg, c.expectedDriveMode) + t.Errorf("msg(car/part/arduino/drive_mode): %v, wants %v", driveModeMsg.String(), c.expectedDriveMode.String()) } var switchRecordMsg events.SwitchRecordMessage @@ -308,7 +320,15 @@ func TestPublish(t *testing.T) { unmarshalMsg(t, pulishedEvents["car/part/arduino/switch_record"], &switchRecordMsg) muPublishedEvents.Unlock() if switchRecordMsg.String() != c.expectedSwitchRecord.String() { - t.Errorf("msg(car/part/arduino/switch_record): %v, wants %v", switchRecordMsg, c.expectedSwitchRecord) + t.Errorf("msg(car/part/arduino/switch_record): %v, wants %v", switchRecordMsg.String(), c.expectedSwitchRecord.String()) + } + + var throttleFeedbackMsg events.ThrottleMessage + muPublishedEvents.Lock() + unmarshalMsg(t, pulishedEvents["car/part/arduino/throttle/feedback"], &throttleFeedbackMsg) + muPublishedEvents.Unlock() + if throttleFeedbackMsg.String() != c.expectedThrottleFeedback.String() { + t.Errorf("msg(car/part/arduino/throttle/feedback): %v, wants %v", throttleFeedbackMsg.String(), c.expectedThrottleFeedback.String()) } } } @@ -483,3 +503,66 @@ func Test_convertPwmSteeringToPercent(t *testing.T) { }) } } + +func TestPart_convertPwmFeedBackToPercent(t *testing.T) { + type fields struct { + } + type args struct { + value int + } + tests := []struct { + name string + fields fields + args args + want float32 + }{ + { + name: "big value -> 0%", + args: args{value: 1234567}, + want: 0., + }, + { + name: "very slow", + args: args{10000}, + want: 0., + }, + { + name: "0.07 limit", + args: args{8700}, + want: 0.07, + }, + { + name: "0.075", + args: args{value: 8700 - (8700-4800)/2}, + want: 0.075, + }, + { + name: "0.08", + args: args{value: 4800}, + want: 0.08, + }, + { + name: "1.0", + args: args{value: 548}, + want: 1., + }, + { + name: "under lower limit", + args: args{value: 520}, + want: 1., + }, + { + name: "invalid value", + args: args{value: 499}, + want: 0., + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + a := &Part{throttleFeedbackThresholds: tools.NewThresholdConfig()} + if got := a.convertPwmFeedBackToPercent(tt.args.value); got != tt.want { + t.Errorf("convertPwmFeedBackToPercent() = %v, want %v", got, tt.want) + } + }) + } +} diff --git a/pkg/tools/test_data/config.json b/pkg/tools/test_data/config.json new file mode 100644 index 0000000..c7b2dcf --- /dev/null +++ b/pkg/tools/test_data/config.json @@ -0,0 +1,5 @@ +{ + "threshold_steps": [ 0.07, 0.08, 0.09, 0.1, 0.125, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 ], + "min_valid": 500, + "data": [ 8700, 4800, 3500, 2550, 1850, 1387, 992, 840, 750, 700, 655, 620, 590, 570, 553, 549, 548 ] +} diff --git a/pkg/tools/thresholdconfig.go b/pkg/tools/thresholdconfig.go new file mode 100644 index 0000000..b232376 --- /dev/null +++ b/pkg/tools/thresholdconfig.go @@ -0,0 +1,65 @@ +package tools + +import ( + "encoding/json" + "fmt" + "os" +) + +var ( + defaultThresholdConfig = ThresholdConfig{ + ThresholdSteps: []float64{0.07, 0.08, 0.09, 0.1, 0.125, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0}, + MinValid: 500, + Data: []int{8700, 4800, 3500, 2550, 1850, 1387, 992, 840, 750, 700, 655, 620, 590, 570, 553, 549, 548}, + } +) + +func NewThresholdConfig() *ThresholdConfig { + return &defaultThresholdConfig +} + +func NewThresholdConfigFromJson(fileName string) (*ThresholdConfig, error) { + content, err := os.ReadFile(fileName) + if err != nil { + return nil, fmt.Errorf("unable to read content from %s file: %w", fileName, err) + } + var ft ThresholdConfig + err = json.Unmarshal(content, &ft) + if err != nil { + return nil, fmt.Errorf("unable to unmarshal json content from %s file: %w", fileName, err) + } + return &ft, nil +} + +type ThresholdConfig struct { + ThresholdSteps []float64 `json:"threshold_steps"` + MinValid int `json:"min_valid"` + Data []int `json:"data"` +} + +func (tc *ThresholdConfig) ValueOf(pwm int) float64 { + if pwm < tc.MinValid || pwm > tc.Data[0] { + return 0. + } + if pwm == tc.Data[0] { + return tc.ThresholdSteps[0] + } + + if pwm < tc.Data[len(tc.Data)-1] && pwm >= tc.MinValid { + return 1. + } + // search column index + var idx int + // Start loop at 1 because first column should be skipped + for i := 1; i < len(tc.ThresholdSteps); i++ { + if pwm == tc.Data[i] { + return tc.ThresholdSteps[i] + } + if pwm > tc.Data[i] { + idx = i - 1 + break + } + } + + return tc.ThresholdSteps[idx] - (tc.ThresholdSteps[idx]-tc.ThresholdSteps[idx+1])/2. +} diff --git a/pkg/tools/thresholdconfig_test.go b/pkg/tools/thresholdconfig_test.go new file mode 100644 index 0000000..af788f7 --- /dev/null +++ b/pkg/tools/thresholdconfig_test.go @@ -0,0 +1,135 @@ +package tools + +import ( + "reflect" + "testing" +) + +func TestNewThresholdConfigFromJson(t *testing.T) { + type args struct { + fileName string + } + tests := []struct { + name string + args args + want *ThresholdConfig + wantErr bool + }{ + { + name: "default config", + args: args{ + fileName: "test_data/config.json", + }, + want: &defaultThresholdConfig, + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + got, err := NewThresholdConfigFromJson(tt.args.fileName) + if (err != nil) != tt.wantErr { + t.Errorf("NewThresholdConfigFromJson() error = %v, wantErr %v", err, tt.wantErr) + return + } + if !reflect.DeepEqual(*got, *tt.want) { + t.Errorf("NewThresholdConfigFromJson() got = %v, want %v", got, tt.want) + } + if !reflect.DeepEqual(got.MinValid, tt.want.MinValid) { + t.Errorf("NewThresholdConfigFromJson(), bad minValid value: got = %v, want %v", got.MinValid, tt.want.MinValid) + } + if !reflect.DeepEqual(got.ThresholdSteps, tt.want.ThresholdSteps) { + t.Errorf("NewThresholdConfigFromJson(), bad ThresholdSteps: got = %v, want %v", got.ThresholdSteps, tt.want.ThresholdSteps) + } + }) + } +} + +func TestThresholdConfig_ValueOf(t *testing.T) { + type fields struct { + ThresholdSteps []float64 + MinValue int + Data []int + } + type args struct { + pwm int + } + tests := []struct { + name string + fields fields + args args + want float64 + }{ + { + name: "big value", + fields: fields{ + ThresholdSteps: defaultThresholdConfig.ThresholdSteps, + MinValue: defaultThresholdConfig.MinValid, + Data: defaultThresholdConfig.Data, + }, + args: args{ + pwm: 11000., + }, + want: 0, + }, + { + name: "little value", + fields: fields{ + ThresholdSteps: defaultThresholdConfig.ThresholdSteps, + MinValue: defaultThresholdConfig.MinValid, + Data: defaultThresholdConfig.Data, + }, + args: args{ + pwm: defaultThresholdConfig.MinValid - 1, + }, + want: 0, + }, + { + name: "pwm at limit", + fields: fields{ + ThresholdSteps: defaultThresholdConfig.ThresholdSteps, + MinValue: defaultThresholdConfig.MinValid, + Data: defaultThresholdConfig.Data, + }, + args: args{ + pwm: defaultThresholdConfig.Data[2], + }, + want: defaultThresholdConfig.ThresholdSteps[2], + }, + { + name: "between 2 limits", + fields: fields{ + ThresholdSteps: defaultThresholdConfig.ThresholdSteps, + MinValue: defaultThresholdConfig.MinValid, + Data: defaultThresholdConfig.Data, + }, + args: args{ + pwm: 800, + }, + want: 0.275, + }, + { + name: "over last value and > minValue", + fields: fields{ + ThresholdSteps: defaultThresholdConfig.ThresholdSteps, + MinValue: defaultThresholdConfig.MinValid, + Data: defaultThresholdConfig.Data, + }, + args: args{ + pwm: defaultThresholdConfig.MinValid + 3, + }, + want: 1., + }, + } + for _, tt := range tests { + t.Run(tt.name, func(t *testing.T) { + f := &ThresholdConfig{ + ThresholdSteps: tt.fields.ThresholdSteps, + MinValid: tt.fields.MinValue, + Data: tt.fields.Data, + } + got := f.ValueOf(tt.args.pwm) + if !reflect.DeepEqual(got, tt.want) { + t.Errorf("ValueOf() = %v, want %v", got, tt.want) + } + }) + } +}