diff --git a/A_H_30_REV16_9.HEX b/A_H_30_REV16_9.HEX
new file mode 100644
index 0000000..37fe15c
--- /dev/null
+++ b/A_H_30_REV16_9.HEX
@@ -0,0 +1,450 @@
+:020000040000FA
+:060000000219FD02079841
+:03000B00020305E8
+:030013000207A53C
+:03001B000204F3E9
+:03002B000203517C
+:03005B000207B7E2
+:100073000203850406080C10182030406080A0C0DD
+:10008300C2AFC08290008B7301AB01BC01D001E40D
+:1000930001F52109211A21312145215C216A217EA3
+:1000A300218F21A921BA21D1D082D2AFA6F008A6EF
+:1000B3006F08A66D08856DF022D082D2AFA6F00836
+:1000C300A66D08A66E08A66D08856DF022D082D2B3
+:1000D300AFE56EB5F002E56FF608A66F08856DF023
+:1000E30022D082D2AFA6F008A66D08A66F08856D50
+:1000F300F022D082D2AFA6F008A66E08A66D08A69D
+:100103006D08856DF022D082D2AFA6F008A66E08E6
+:10011300A66E08856DF022D082D2AFE56EB5F002EF
+:10012300E56FF608A66D08A66E08856DF022D082ED
+:10013300D2AFA6F008A66D08A66D08A66E08856D59
+:10014300F022D082D2AFE56EB5F002E56FF608A6D5
+:100153006E08A66D08856DF022D082D2AFA6F00896
+:10016300A66F08856EF022D082D2AFE56EB5F0029D
+:10017300E56FF608A66E08856EF022D082D2AFA690
+:10018300F008A66D08A66E08856EF022D082D2AF65
+:10019300E56EB5F002E56FF608A66D08A66D08A634
+:1001A3006D08856DF022D082D2AFA6F008A66E0846
+:1001B300A66D08856EF022D082D2AFE56EB5F0024F
+:1001C300E56FF608A66D08A66D08856EF022D0824D
+:1001D300D2AFA6F008A66D08A66D08A66D08856EB9
+:1001E300F022E52BA25733F52A752B0F8075E52BEB
+:1001F300A25733A25633F52A752B0D8066E52BA241
+:100203005733A25633A25533F52A752B0B8054E589
+:100213002A54F0C25C452BC4F52A752B098044E5AA
+:100223002AA25813A25913A25A13F52A752B078031
+:1002330032E52AA25813A25913F52A752B058023F8
+:10024300E52AA25813F52A752B038017205F932004
+:100253005E9C205DA8205CB7205BC4205AD3205944
+:10026300DFE52AC4652A652BF4F57022C22E78E9EE
+:10027300302D0278D1856DF0E57023541E118385EE
+:10028300F0708871228570F0A871E52A23541E113D
+:100293008385F0708871228570F0A871E52AC423E4
+:1002A300541E118385F0708871228570F0A871E562
+:1002B3002B23541E1183A6F008E878E8302D02782A
+:1002C300D0F6C2AF202B05B22DD2AF22D22ED2AFA1
+:1002D30022E52DB4F00DE52C7009752AFF752BFF6F
+:1002E300514F22E52CA26813F52AA26913252AF59A
+:1002F3002AE52D03C2E7F52B03C2E7352BF52B5176
+:100303004F22202B0132C0D0C0E075D01118E8B5C0
+:100313006C3320850BD285856D8A08D0E0D0D0322E
+:1003230075A400D285C22B758E0C7589AAC2A9C289
+:1003330089C28B758A00758C00758200D2A8D2AAF7
+:10034300D0E0D0D032B285868AD0E0D0D032C0D0CF
+:10035300C0E0C2CF053BE5606009E55F6005755FFE
+:1003630000801A755F01E53160021531E54C6005C7
+:10037300754D008008054DE54D7002154DD0E0D058
+:10038300D032C2AF53E67F7592FA7593FFC2105312
+:10039300917FD2AF327582007527007528007529C9
+:1003A30000780075F000E0C0E0C395F0D0F0C3958D
+:1003B300644002D2430582E0C0E0C395F0D0F0C3AD
+:1003C30095644002D2420582E0C0E0C395F0D0F0CC
+:1003D300C395644002D2410582E0C0E0C395F0D0EA
+:1003E300F0C395644002D2400582E0C0E0C395F0BB
+:1003F300D0F0C395644002D23F0582E0C0E0C395CC
+:10040300F0D0F0C395644002D23E0582E0C0E0C361
+:1004130095F0D0F0C395644002D23D0582E0C0E080
+:10042300C395F0D0F0C395644002D23C0582E0C08E
+:10043300E0C395F0D0F0C395644002D23B0582E05F
+:10044300C0E0C395F0D0F0C395644002D23A058270
+:10045300E0C0E0C395F0D0F0C395644002D2390503
+:1004630082E0C0E0C395F0D0F0C395644002D23877
+:100473000582E0C0E0C395F0D0F0C395644002D29A
+:100483004B0582E0C0E0C395F0D0F0C39564400211
+:10049300D24A0582E0C0E0C395F0D0F0C395644032
+:1004A30002D2490582E0C0E0C395F0D0F0C3956461
+:1004B3004002D2480582E527C46527652830270115
+:1004C300F46529540F22900000D2A8D2AA0530E582
+:1004D3003070021530C3E5309432400A754C00D0B9
+:1004E300F0D0E0D0D03275310AD0F0D0E0D0D032A5
+:1004F300C2AFC2A8C28EC0D0C0E0C0F075D010C2D7
+:10050300CAA8CCA9CDD2CAD2AF758A00C3E8956672
+:10051300F8E99567F9E970AEE8956840A9956950DF
+:10052300A5E582B410A07198709C85276A85286B15
+:10053300D22C20270E900000D2A8D2AAD0F0D0E06F
+:10054300D0D032302E04B22DC22E756CD0302D0394
+:10055300756CE878D0302D0278E8E6C28D758A365E
+:10056300758E097589A2D22BD2A9D28575A420755F
+:10057300D011F8D0F0D0E0D0D032048322007EBD79
+:10058300FCFCA23692E2A23792E3048322FDFC7CB8
+:100593003CFE7E3E1E7F3F1F0FA23692E2A23792A1
+:1005A300E3048322820803008120010080200000ED
+:1005B300C22CC2AFA96AE56AC39460FAE56BD2AFF5
+:1005C3009400FB50177B007A00E96010C313500CB2
+:1005D300B54A04054B8005F54A754B0020310AC224
+:1005E300AF75E90075EA00D2AF302A27C3EA94D089
+:1005F300F8EB9407F9400BE8FAE9FB202509D2252B
+:10060300C10A302502C225202902B225C3EA33FAE2
+:10061300EB33FBEB2B3B3BF9C3EB13FBEA13FAEA9C
+:1006230029FAEB3400FB30E3047AFF7BFFE5235424
+:1006330003601D201A1AE55B33F9EB7007C3E99ACF
+:100643004002E9FAE5375403B17D2AFAEB3400FBA3
+:10065300C3EA33C4540FF8EB33C454F048F87005BD
+:10066300EA60027801E53060021530D220884CE55B
+:100673004C6003754D00AC59C3E559955A4002AC23
+:100683005AC3EC954C5008EC75F008A4FAABF08A09
+:10069300523037155352078BF07900EAA2F013A2B8
+:1006A300F113A2F213F4F88027303617535207EBF5
+:1006B300131392D5A2E7F45401F9EA13A2D513F464
+:1006C300F8800D535203EB13F45403F9EA13F4F8CF
+:1006D300C3E8941FFAE99400FB50047A007B00C33B
+:1006E300E89578F554E99579F555500675540075F4
+:1006F3005500E54C54FE70067554FF857A55C3E5E5
+:1007030054941FF556E5559400F557500975560056
+:1007130075570075541F301A54C215C3E8954EF52A
+:10072300F0E9954F504943F00F55F0F5F07400B1DF
+:100733008545F0B4FF1D7401B18545F0B4FF19740C
+:1007430002B18545F0B4FF157403B18545F0B4FFDC
+:1007530011802A857458800D8575588008857658D0
+:100763008003857758E54C54E06002C214800E5331
+:10077300F0F045F055736005755802D215884E891F
+:100783004F8A508B51203009755801203103120CC8
+:10079300F675310A22C0E0E58AF0058285658BD0C3
+:1007A300E032C2AA85658BD28EC2CA85CC6685CD5E
+:1007B30067D2CA3253E6EF3279018014790380108D
+:1007C300790A800C791E80087964800479C88000D6
+:1007D3007817E4D5E0FDE8C0E0E9C0E0302C02B1D1
+:1007E300B3D0E0F9D0E0F8D8E9D9E52278FF2018B2
+:1007F3002879A1E76023E52D601F74FF852DF08420
+:10080300853AF030190375F005A4F8C5F060027855
+:10081300FFC3E8955B5002A85B885A22C3E52C947A
+:10082300A0E52D9400E55A5003148001046002F5FD
+:100833005A2275E89022055CC3E55C940840533066
+:10084300EDF4A8BDA9BE755C00E9FA79A0E76041A3
+:10085300EA7006E55D6018800CC3E8955D600EE5FF
+:100863005D5005600A1480070460FA8002E55DF5B7
+:100873005DC39563401B7559C0C3940440137559F8
+:1008830080C39404400B755940C3940440037559C5
+:100893000022E5592410500274FFF559227432796D
+:1008A300A687F0A4C5F0A2F733F55B22752C00757B
+:1008B3002DF012118622C2AFC2CAA8CCA9CDAA3B81
+:1008C30030CF010AD2CAD2AFC3EA13FAE913F9E867
+:1008D30013F8AB3CAC3D883C893DC3E89BF8E99CED
+:1008E30020180A547FF9301C0221D68040AD3E8A7D
+:1008F3003EF9EA9D547FFA600678FF79FF8013AED4
+:100903003FAF408B3F8C40A83CA93DC3E89EF8E92C
+:100913009FF9C3E52D13FBE52C13FAE82AF52CE91F
+:100923003BF52D5006752CFF752DFF804DAA2CAB82
+:100933002DAC2CAD2D7E047F02C3EB9404400D1E21
+:100943001FC3EB940840052019021E1FC3ED13FDBE
+:10095300EC13FCDEF7C3EA9CFAEB9DFBEF6009C3E3
+:10096300E913F9E813F8DFF7EA28FAEB39FB8A2CE5
+:100973008B2D50067BFF8B2C8B2DC3EB94025002E7
+:10098300D21C3018047F03801C7892E6FFC3E5383D
+:10099300948240110FC3E53894A040010FC3EF9434
+:1009A3000640027F057E02E52DC4540FF9E52DC4F0
+:1009B30054F0F8E52CC4540F28F8C3E89EFAE994E0
+:1009C30000FB400AC3EA9401EB94004001227A0140
+:1009D300E4FB22AA2CAB2DEBC4FEEAC4540F4EFC5D
+:1009E300C3EA9CFAEB9400FBC3E813C313F8EA28A9
+:1009F300FAEB3400FB8A2C8B2DC3EB94024002C22A
+:100A03001C7802EBC4FE7B00EAC4540F4EFAC3EA1F
+:100A130098FA4005C3940150027A017892E6FF22C6
+:100A23003010024123854692854793D21043E680D6
+:100A330022C3E49AF8E49BF9C3E833F8E933F920D5
+:100A43001C6FE8FAE9FBD3E913FDE813FC88468938
+:100A530047C3EF94036037EF20E00CE82CF8E93D3F
+:100A6300F9ECFAEDFB8013E828F8E939F9C3E82437
+:100A730001F8E93400F97AFF7BFFC3EF9403400EDA
+:100A83008A488B49884289438C448D4580578848DE
+:100A930089498A428B438C448D453018127548F03E
+:100AA3007549FF7544F07545FF7546F07547FF803E
+:100AB30034E8FAD3E813FC8846C3EF94036020EFCD
+:100AC30020E007E82CF8ECFA41D4E8282401F87A6E
+:100AD300FFC3EF940340088A4888428C4480068809
+:100AE300488A428C4430100241E8753502D21043E3
+:100AF300E680E5235403602DA82CA92DC3E913F93F
+:100B0300E813F8301804E92440F9C2AF53E67F75BF
+:100B13009100C3E498F594E499F595759104D21086
+:100B230043E680D2AF22D211754100752000301BFD
+:100B3300037520406147D211754100752040301B79
+:100B43000375200078017901201C28E523540360F4
+:100B530002C2117914E52DC313700104F8C3941470
+:100B630040027814301804781B791BC3E833F8C3A8
+:100B7300E933F9201010E541600C301803D5350432
+:100B8300D21261FA51F00541E59B5440B520026150
+:100B9300E430180908C3E899400118617620110A66
+:100BA30008C3E899400261476176C21153E67F7535
+:100BB3009100301C117594007595F0759104D21055
+:100BC30043E680020B47E52D7EFFC3334009C33361
+:100BD3004005C3334001FEC3E49E759400F595615F
+:100BE300BEC3E5349401500261473011026147D816
+:100BF3000261F86176C212C2AF53E67F7591008538
+:100C03004894854995759104854292854393D21002
+:100C130043E680D2AFE5235403600720190205346D
+:100C2300801030120D201B0A201107158115810237
+:100C330017D72278003011027801E53875F007A440
+:100C4300F9E5F028F5F0E9A2F013A2F113A2F213EB
+:100C5300F538C394785003753878C3E5389539402F
+:100C630009C290C292C29475D53F9180301002811F
+:100C73006F854492854593D21043E68022D235E5B1
+:100C83002D54FC7002C235302C071205B3203501F8
+:100C93002230271E202B1BD572191202AD75720547
+:100CA30030350F1202D412026F12028812029A75A3
+:100CB300720122E572B404041202D422B4030412B2
+:100CC300026F22B402041202882212029A22E5530E
+:100CD300455230370512181C800B30360512181C8C
+:100CE300800312180BF5535353FEA2E0E54E92E036
+:100CF300F54E2291D1303717C2AF75D800854EFB20
+:100D0300854EFC8550E98550EA75D840D2AF22C2A2
+:100D1300AF75D800854EFB854FFC8550E98551EAB8
+:100D230075D840D2AF2230144730371975D80085B3
+:100D330050F975FA00854EFB854EFC8550E98550C8
+:100D4300EA75D8402275D8008550F98551FA53F7D2
+:100D53007F854EFB854FFC8550E98551EA43F7803B
+:100D6300854EFB854FFC8550E98551EA75D84022B5
+:100D730030371975D8008556F975FA008554FB8507
+:100D83004EFC8556E98550EA75D8402275D8008512
+:100D930056F98557FA53F77F8554FB8555FC8556DD
+:100DA300E98557EA43F780854EFB854FFC8550E97B
+:100DB3008551EA75D84022201549C2333037197559
+:100DC300D80075F90075FA0075FB1E75FC0075E90E
+:100DD3000075EA0075D8402275D80075F90075FAD8
+:100DE3000053F77F75FB1E75FC0075E90075EA007B
+:100DF30043F78075FB0075FC0075E90075EA007523
+:100E0300D84022D23320344730371975D80075F9CA
+:100E1300FF75FA0075FBFF75FCFF75E90075EA00C5
+:100E230075D8402275D80075F9FF857AFA53F77F94
+:100E330075FBFF857AFC75E90075EA0043F7807559
+:100E4300FBFF857AFC75E90075EA0075D84022300E
+:100E5300371975D80075F9E175FA0075FBFF75FC54
+:100E6300FF75E9E175EAE175D8402275D80075F997
+:100E7300E1857AFA53F77F75FBFF857AFC75E9E123
+:100E8300857AEA43F78075FBFF857AFC75E9E1858E
+:100E93007AEA75D84022D213D558040558C2139163
+:100EA300D1201304B1298002B1BA22E558B4010755
+:100EB300B233303302B12C22202816C2AFC293C200
+:100EC3009575D50FD291D2AF303202D1AE759F2036
+:100ED30022C2AFC291C29375D53CD295D2AF303204
+:100EE30002D1AE759F2022202816C2AFC293C295AD
+:100EF30075D533D291D2AF303102D199759F30225B
+:100F0300C2AFC291C29375D533D295D2AF303102FD
+:100F1300D199759F1022202816C2AFC291C2937532
+:100F2300D533D295D2AF303202D1AE759F1022C2E3
+:100F3300AFC293C29575D533D291D2AF303202D1BD
+:100F4300AE759F3022202816C2AFC291C29375D5C9
+:100F53003CD295D2AF303102D199759F2022C2AFD6
+:100F6300C293C29575D50FD291D2AF303102D199C8
+:100F7300759F2022202816C2AFC291C29575D53C19
+:100F8300D293D2AF303202D1AE759F3022C2AFC2FC
+:100F930091C29575D50FD293D2AF303202D1AE75CF
+:100FA3009F1022202816C2AFC291C29575D50FD2C9
+:100FB30093D2AF303102D199759F1022C2AFC29143
+:100FC300C29575D53CD293D2AF303102D199759F7A
+:100FD30030227A147B7880127A107B8C800C7A0D05
+:100FE3007BB480067A0B7BC88000E55ED5E00122E6
+:100FF3007902E4C293D5E0FDD292D5E0FDC292D549
+:10100300E0FDD293D5E0FDE920E002D29030E0028A
+:10101300D294E55ED5E0FDE920E002C29030E00223
+:10102300C2947496D5E0FDD9C9EAF8D5E0FDD8FBA2
+:10103300DBB8C29322C290C292C294C291C293C23D
+:101043009575D53F22788076FF0876FF0876FF08EE
+:1010530076FF0876FF0876FF0876090876FF08769C
+:1010630001788C76010876FF0876FF0876FF08760C
+:10107300FF0876FF0876030876FF0876FF0876FFF9
+:101083000876250876D0087628087650087604086E
+:1010930076FF0876020876FF08767A0876FF0876E8
+:1010A300070876010876FF0876FF087600087600C1
+:1010B300227888E6C39403D22A5002C22AC228E6C1
+:1010C30030E102D228A22892297886E614C2AFC062
+:1010D30083C08290007693D082D083D2AF78A6F675
+:1010E3007886E6F53AC394025003753A02789CE693
+:1010F3007539FFB402037539A0B4030375398278D7
+:10110300A0E6F8600674282409D8FCF56311382298
+:1011130078A5E6F954037000E9540C7000E95430E3
+:101123007000E954C070002275658075642075688D
+:101133006C756914756DFF756EEC756FDD227565E1
+:10114300007564407568D9756928756DEC756ECB4B
+:10115300756FAB22741E208515C0D0C0E0740AD50C
+:10116300E0FDD0E0D0D02085EBD5E0EDC322C0D0A8
+:10117300C0E0740AD5E0FDD0E0D0D03085D6D5E00C
+:10118300EDD3223027211202D412026F12028812E9
+:10119300029A1202AD1202D412026F1202881202D4
+:1011A3009A1202AD75720522A23692E2A23792E339
+:1011B30004832200020383000181C10080C0E07523
+:1011C30061007562007597DE7597AD7581AF43FF5A
+:1011D3008075EF0675A90011387580FF75F1F075FC
+:1011E300A4007580FF75D4FF75900075F23F75A557
+:1011F3003F75900075D53F75A60175E34075E20212
+:101203001138E47800F6D8FD114812185D789886F5
+:101213005E7897E6A2E59236A2E69237303702C2AD
+:1012230036A2E7232354FC92E7B4800EC231C232C4
+:10123300757800757900C23041C4A2E79232D23189
+:1012430030370B23F578437801757900800C23330D
+:10125300F578437803335401F579789686F0D230E4
+:10126300C3E5F094325044C3E5F09428502FC3E50E
+:10127300F0941E5015C3E5F0941450007574027574
+:101283007501757601757701802DB40702C230753B
+:101293007402757502757601757701801A7574038A
+:1012A300757502757602757701800C757404757512
+:1012B30003757602757701A2E29234540331ABF5DC
+:1012C30073C2AF1207CF120FD51207C7120FDB126B
+:1012D30007C7120FE11207C73113C2AF756100755B
+:1012E300620078FA79FA308516D9FBD8F775A91315
+:1012F30075A71075C10075A700756000021C007505
+:10130300A71075C13075A70075A90375600111B4E5
+:101313007898865E113875C80475910475D8407540
+:10132300D908740012059CF5F7759E037401120524
+:101333009CF553740212059CF57A75580175560095
+:101343007557007554FF7555FF75960275DA42752A
+:10135300DB42759B80759D0075D10C75BCB975BB5F
+:101363001075E88075B201C22C1207BB753700C235
+:10137300201207CFD2243157922775E4D5302703A3
+:1013830075E45DC22BC22CC22DC22E757205758801
+:1013930051758E0C7589AA758C00758D00758E0C30
+:1013A30075E68075B80375A82831411208AFD2AF2E
+:1013B30075300A1207CB754A00754B00C3E53094AC
+:1013C3000A4017312B75300A1207CB754A00754B4B
+:1013D30000C3E530940A400241DD1207BF2020021A
+:1013E30041DDC2AF120FD5120FD5120FD5D2AF12F6
+:1013F30007CF7561007562001207CBE54C60028070
+:10140300F1C2AF120FE7120FE7120FE7D2AF1207C5
+:10141300CF1208AFE4F532F5330532E532F4703E0E
+:101423000533789AE67819146012783214600D78CF
+:101433007D14600878FA146003753300C3E53398AC
+:10144300401C11381207BB15337532007899865E3C
+:10145300C2AF120FE7D2AF7898865E1207CB12079E
+:10146300C3E531700241DD302C031205B3C3E54CF3
+:101473009401500AC3E54A9401501602141C120742
+:10148300CBE53170030212DD754A00754B0002167D
+:1014930042C3E54A94017016C2AF11387899865E4B
+:1014A300120FD57898865ED2AF1207CBC134C3E54D
+:1014B3004A94027016C2AF11387899865E120FDB18
+:1014C3007898865ED2AF1207CBC134C3E54A940342
+:1014D3007016C2AF11387899865E120FE17898863C
+:1014E3005ED2AF1207CBC134C3E54A94047016C26F
+:1014F300AF11387899865E120FE77898865ED2AF7F
+:101503001207CBC134C3E54A94057016C2AF113834
+:101513007899865E120FE77898865ED2AF1207CB72
+:10152300C134C3E54A94077019C3E54B94065002CE
+:10153300816A7401302A0274037888F6C228C229AA
+:10154300C134C3E54A94087019C3E54B94065002AD
+:10155300816A7402302A0274047888F6D228D22968
+:10156300C134C3E54A94097017C3E54B940650028E
+:10157300816A302A0BC37888E69402F6C22AC13402
+:10158300C3E54A940A7016C3E54B94065002816A78
+:10159300202A0A7888E62402F6D22AC134C3E54A0F
+:1015A3009414702DC3E54B94065002816A7400C2F3
+:1015B300AFC083C082901A0B93D082D083D2AF780E
+:1015C30088F613C228C2294002D2284002D22980B9
+:1015D30060C3E54A94157040C3E54B9406405874C4
+:1015E30000C2AFC083C082901A0B93D082D083D243
+:1015F300AFF8B801027402B802027401B8030274AE
+:1016030004B8040274037888F613C228C22940027E
+:10161300D2284002D2298019C3E54A940C7012756E
+:1016230061A57562F1C3E54B9406400B1218A4D271
+:10163300AF754A00754B00756100756200816A11D0
+:1016430038755C00752200752300753800755C08D9
+:101653001207BB12083530EDFDA8BDA9BEE9700124
+:10166300F8885D120839755C08C2AF1208A0855B63
+:1016730059855B5AD2AF302A07C228302502D228B7
+:10168300D218753400302C031205B3120F77120FE2
+:10169300A61208AF1208B9120A23120A341208AFAD
+:1016A3001208B9120A23120A341208AF120B3912A4
+:1016B3000C36120EBB1208B9120A23120A34120B8B
+:1016C30029201C031207EF301C0312081F120C36CB
+:1016D300120EEA1208B9120A23120A34120B391233
+:1016E3000C36120F191208B9120A23120A34120BFC
+:1016F30029120C36120F481208B9120A23120A349F
+:10170300120B39120C36120F771208B9120A231270
+:101713000A34120835120B29120C36120FA61208BE
+:10172300391208B9120A23120A34301827855B5973
+:1017330079187A0CC3E53499400EC218D2198A3647
+:10174300855B59855B5A800CC3E54C9401400302C9
+:1017530016AFE1DF301921201B1EE536147008C2D5
+:1017630019D21AD214C1AFF536202A07C3E54C9417
+:101773000140030216AF8064753700202A0F78FA00
+:1017830079A4E760027803C3E54D98504FE53160D3
+:101793004B302A17202805202507800F302502808B
+:1017A3000A201B07D21B855B59C1F178F0301B055A
+:1017B300855B597820C3E52D9850030216AF301B83
+:1017C3001BC21BC228302502D228D21975361285B6
+:1017D3005B59C1AF0537E54C60028003753700C222
+:1017E300AF1138752300D2AF1207CB113875220021
+:1017F30078A4E66006D291D293D295C3E5379404D8
+:1018030040030212DD0214148322010203050C08B3
+:101813000409050C080D09050C83220102030405C4
+:1018230006070938302820181008110938302820F5
+:1018330018101911093830282018211911093830C6
+:101843002820292119110938302831292119110992
+:1018530038303931292119110938901A0D7820119E
+:10186300DDE520B4550AA311DDE520B4AA028013F7
+:101873007561A57562F112104811A4756100756256
+:1018830000801D901A0378807B0A11DDA308DBFA20
+:10189300901A0F788C7B1A11DDA308DBFA901A29B2
+:1018A30022C2AF312011FD901A00741011E2A3740B
+:1018B3000911E2A3742111E2901A0378807B0A11C3
+:1018C300E1A308DBFA901A0F788C7B1A11E1A308C5
+:1018D300DBFA31333111901A2922E493F622E643DD
+:1018E3008F01538FFDFFC3E583941C400122EF85D5
+:1018F30061B78562B7F0538FFE22438F02438F0196
+:101903008561B78562B7901A0DF0538FFC22901A48
+:101913000D745511E2901A0E74AA11E2227A3079ED
+:10192300D07820901A4011DDE520F709A3DAF722D9
+:101933007A3079D0901A40E711E209A3DAF9227CD0
+:10194300051207CFDCFB22C2AF120FD5120FDB1239
+:101953000FE1120FE71207C3120FD5120FDB120F9D
+:10196300E1120FE71207C3120FD5120FDB120FE1BB
+:10197300120FE7D2AF22C2AF120FE7120FE1120F1D
+:10198300DB120FD51207C3120FE7120FE1120FDBA1
+:10199300120FD51207C3120FE7120FE1120FDB125A
+:0519A3000FD5D2AF22B8
+:1019FD000211C2100921FFFFFFFFFFFF09FF01FFC9
+:101A0D0055AA01FFFFFFFFFF03FFFFFF25D0285061
+:0D1A1D0004FF02FF7AFF0701FFFF0000FF3A
+:101A400023415F485F3330232020202020202020A6
+:101A500023424C48454C492445464D3842323123B7
+:101A60002020202020202020202020202020202076
+:101C0000C2AFC2D37597DE7597AD7581C075A90057
+:101C100043FF80752438752503B1DB7529A5752A26
+:101C2000F143F12053A4DFD28575D4FF75E3407DE5
+:101C3000FA780379E67C10752200752300901DE880
+:101C40007B0675249875250130850281482085081A
+:101C5000D8FBD9F9DCF781BA308508D8FBD9F9DC93
+:101C6000F781BAB19BE493A3C39A6004DDC381BA40
+:101C7000DBDBB18D600281317B08E493FAA3B16EA6
+:101C8000DBF87A30B16E752200752300B18DEAFC65
+:101C9000EBFDC3ED94FE400EB18D8A278B28ED300D
+:101CA000E0048A828B83B18D7AC270D8C3ED94FE32
+:101CB000601950CEBD0029EC600F7520007521FF22
+:101CC000752900752A000200008100A827A92808AC
+:101CD00009D804D90281DDB191EAF281D10D81A642
+:101CE000C3ED94035051EDA2E092007AC5C3E582A2
+:101CF0009400E583941C508C200010438F02438F86
+:101D0000018529B7852AB7F0300027A827A9280818
+:101D100009438F01538FFDD804D902A132C3E58353
+:101D2000941C4003A3A117E28529B7852AB7F0A325
+:101D3000A117538FFC81827AC1BD030CE493A3FAEF
+:101D4000B152DCF8B14A81828184AA22AB23B16E00
+:101D5000EBFAEA6222752608C3E52313F523E52290
+:101D600013F52250066323A0632201D526EAB1DBD6
+:101D7000B1DB75260AEAF4200102D285300102C2E5
+:101D800085B1DBC3134002C201D526EB22B191EA33
+:101D9000FB208502A191308502A196752608AE240C
+:101DA000AF25C3EF13FFEE13FEB1DFB1DBC3EA13C0
+:101DB0003085024480FA30E703632201C3E5231330
+:101DC000F523E52213F52250066323A0632201D5F3
+:101DD00026D9E52265236523F52222AE24AF250E00
+:101DE0000FDEFEDFFCD20122424C48656C693437BD
+:061DF0003164E8B20601B7
+:00000001FF
diff --git a/dshot2.cpp b/dshot2.cpp
new file mode 100644
index 0000000..6c0a890
--- /dev/null
+++ b/dshot2.cpp
@@ -0,0 +1,446 @@
+#include "stm32g4xx.h"
+
+#include "gpio.h"
+
+#include "dshot2.h"
+
+#define DSHOT_FREQ_SCALE 10'000'000
+
+#define DSHOT_FRAME_DATA 16 
+#define DSHOT_FRAME_SIZE 20
+
+#define DSHOT_COUNT 4
+
+#define DSHOT_TIM   2
+
+#define DSHOT_TIM2  0
+#define DSHOT_TIM1  1
+
+#define DSHOT_CCR  2
+#define DSHOT_CCR1 0
+#define DSHOT_CCR2 1
+#define DSHOT_CCR3 0
+#define DSHOT_CCR4 1
+
+static bool DSHOT_Bidirect;
+
+static unsigned short DSHOT_Shift, DSHOT_PeriodTX, DSHOT_PeriodRX, DSHOT_Bit_0, DSHOT_Bit_1;
+
+static unsigned short DSHOT_TX_Buff[DSHOT_TIM][DSHOT_FRAME_SIZE*DSHOT_CCR]; // TIM | CCR1 + CCR2
+
+#define DSHOT_RX_SAMP 4
+#define DSHOT_RX_SIZE 32
+
+static unsigned short DSHOT_RX_Buff[DSHOT_RX_SIZE*DSHOT_RX_SAMP];
+static GPIO_TypeDef* DSHOT_RX_Port;
+static unsigned long DSHOT_RX_Mode, DSHOT_TX_Mode;
+static unsigned short DSHOT_RX_Index[DSHOT_COUNT];
+
+// Таблица декодирования GCR (DShot RPM)
+static const unsigned char GCR_DECODE_TABLE[32]
+{
+  [0x19] = 0x0, // 11001b -> 0
+  [0x1B] = 0x1, // 11011b -> 1
+  [0x12] = 0x2, // 10010b -> 2
+  [0x13] = 0x3, // 10011b -> 3
+  [0x1D] = 0x4, // 11101b -> 4
+  [0x15] = 0x5, // 10101b -> 5
+  [0x16] = 0x6, // 10110b -> 6
+  [0x17] = 0x7, // 10111b -> 7
+  [0x1A] = 0x8, // 11010b -> 8
+  [0x09] = 0x9, // 01001b -> 9
+  [0x0A] = 0xA, // 01010b -> A
+  [0x0B] = 0xB, // 01011b -> B
+  [0x1E] = 0xC, // 11110b -> C
+  [0x0D] = 0xD, // 01101b -> D
+  [0x0E] = 0xE, // 01110b -> E
+  [0x0F] = 0xF, // 01111b -> F
+  
+  // Остальные значения 0xFF (ошибки)
+  [0x00] = 0xFF, [0x01] = 0xFF, [0x02] = 0xFF, [0x03] = 0xFF,
+  [0x04] = 0xFF, [0x05] = 0xFF, [0x06] = 0xFF, [0x07] = 0xFF,
+  [0x08] = 0xFF, [0x0C] = 0xFF, [0x10] = 0xFF, [0x11] = 0xFF,
+  [0x14] = 0xFF, [0x18] = 0xFF, [0x1C] = 0xFF, [0x1F] = 0xFF
+};
+
+inline unsigned char decodeGCR(unsigned char gcr_val)
+{
+  return GCR_DECODE_TABLE[gcr_val & 0x1F]; // gcr_val должен быть от 0 до 31 (5 бит)
+}
+//------------------------------------------------------------------------------
+
+static unsigned long CalcERPM(unsigned long code)
+{
+  code = code ^ (code>>1);
+  
+  const unsigned long mask = 0x1F; 
+
+  unsigned long gcr1 = (code >> 0)  & mask;
+  unsigned long gcr2 = (code >> 5)  & mask;
+  unsigned long gcr3 = (code >> 10) & mask;
+  unsigned long gcr4 = (code >> 15) & mask;
+  
+  gcr1=decodeGCR(gcr1);
+  gcr2=decodeGCR(gcr2);
+  gcr3=decodeGCR(gcr3);
+  gcr4=decodeGCR(gcr4);
+  
+  unsigned long crc = (~(gcr2 ^ gcr3 ^ gcr4)) & 0x0F;
+
+  bool valid = crc==gcr1;
+  
+  if (!valid || gcr1==0xFF || gcr2==0xFF || gcr3==0xFF || gcr4==0xFF) return -1;
+  
+  unsigned short data = (gcr4 << 8) | (gcr3 << 4) | gcr2;
+  
+  unsigned long e = (data >> 9) & 0x07; // Старшие 3 бита
+  unsigned long m = data & 0x1FF; 
+  
+  unsigned long p = m<<e;
+  
+  if (p==0) return -1;
+  
+  return 1'000'000/p;
+}
+//------------------------------------------------------------------------------
+
+static void Begin_ModeRX()
+{
+  TIM6->ARR  = 2300; // 20us
+  TIM6->EGR  = TIM_EGR_UG;
+  TIM6->SR   = 0;
+  TIM6->DIER = TIM_DIER_UIE;
+  TIM6->CR1 = TIM_CR1_CEN;
+}
+//------------------------------------------------------------------------------
+
+extern "C" void DMA2_Channel1_IRQHandler()
+{
+  unsigned long sr = DMA2->ISR;
+  if(sr & DMA_ISR_TCIF1)
+  {
+    DMA2->IFCR = DMA_IFCR_CTCIF1;
+    
+    Begin_ModeRX();
+  }
+}
+//------------------------------------------------------------------------------
+
+extern "C" void TIM6_DAC_IRQHandler()
+{
+  DSHOT_RX_Port->MODER &= DSHOT_RX_Mode;
+  TIM6->ARR  = DSHOT_PeriodRX;
+  TIM6->EGR  = TIM_EGR_UG;
+  TIM6->DIER = TIM_DIER_UDE;
+}
+//------------------------------------------------------------------------------
+
+     
+static void Begin_ModeTX()
+{
+  DSHOT_RX_Port->MODER |= DSHOT_TX_Mode;
+}
+//------------------------------------------------------------------------------
+
+extern "C" void DMA2_Channel3_IRQHandler()
+{
+  unsigned long sr = DMA2->ISR;
+  if(sr & DMA_ISR_TCIF3)
+  {
+    DMA2->IFCR = DMA_IFCR_CTCIF3;
+    TIM6->DIER = 0;
+    
+    Begin_ModeTX();
+  }
+}
+//------------------------------------------------------------------------------
+
+bool DSHOT_Mode(unsigned long Freq, bool Bidirect)
+{
+  if(Freq<150'000) Freq=150'000;
+  
+  DSHOT_PeriodTX=SystemCoreClock/Freq;
+  DSHOT_PeriodRX=(DSHOT_PeriodTX-DSHOT_PeriodTX/4)/DSHOT_RX_SAMP;
+  
+  DSHOT_Bit_0 = DSHOT_PeriodTX*379/1024; // 37%
+  DSHOT_Bit_1 = DSHOT_PeriodTX*767/1024; // 75%
+  
+  DSHOT_Bidirect=Bidirect;
+}
+//------------------------------------------------------------------------------
+
+// Режим приема
+static bool Init_ModeRX()
+{
+  if(RCC->APB1ENR1 & RCC_APB1ENR1_TIM6EN) return false;
+  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
+  
+  if(DMAMUX1_Channel8->CCR & DMAMUX_CxCR_DMAREQ_ID_Msk) return false;
+  DMAMUX1_Channel8->CCR = (8 << DMAMUX_CxCR_DMAREQ_ID_Pos); // TIM6_UP
+  
+  DMA2_Channel3->CCR  = 0;
+  DMA2_Channel3->CPAR = (unsigned long)&GPIOA->IDR;
+  DMA2_Channel3->CMAR = (unsigned long)DSHOT_RX_Buff;
+  DMA2_Channel3->CCR  = DMA_CCR_MINC | DMA_CCR_MSIZE_0 | DMA_CCR_PSIZE_0 | DMA_CCR_TCIE;
+  
+  NVIC_SetPriority(DMA2_Channel3_IRQn, 0);
+  NVIC_EnableIRQ(DMA2_Channel3_IRQn);
+  
+  NVIC_SetPriority(TIM6_DAC_IRQn, 0);
+  NVIC_EnableIRQ(TIM6_DAC_IRQn);
+  
+  return true;
+}
+//------------------------------------------------------------------------------
+     
+// Режим передачи
+static void Init_TIM2_TX()
+{
+  TIM2->CR1 = 0;
+  TIM2->SMCR = (6 << TIM_SMCR_SMS_Pos); // Slaver
+  TIM2->PSC = 0;
+  TIM2->ARR = DSHOT_PeriodTX;
+  TIM2->CCMR1 = (6 << TIM_CCMR1_OC1M_Pos) | TIM_CCMR1_OC1PE | (6 << TIM_CCMR1_OC2M_Pos) | TIM_CCMR1_OC2PE;
+  TIM2->CCR1=TIM2->CCR2=0;
+  TIM2->EGR = TIM_EGR_UG;
+  if(DSHOT_Bidirect) TIM2->CCER = TIM_CCER_CC1E | TIM_CCER_CC1P | TIM_CCER_CC2E | TIM_CCER_CC2P;
+  else               TIM2->CCER = TIM_CCER_CC1E | TIM_CCER_CC2E;
+  TIM2->DIER = TIM_DIER_UDE;
+  TIM2->DCR = (0x0D << TIM_DCR_DBA_Pos) | (1 << TIM_DCR_DBL_Pos); // Burst CCR1 & CCR2
+  
+  TIM2->BDTR = TIM_BDTR_MOE;
+  
+  DMAMUX1_Channel6->CCR = (60 << DMAMUX_CxCR_DMAREQ_ID_Pos); // TIM2_UP
+  
+  // TX DMA TIM2
+  DMA2_Channel1->CPAR = (unsigned long)&TIM2->DMAR;
+  DMA2_Channel1->CMAR = (unsigned long)DSHOT_TX_Buff[DSHOT_TIM2];
+  DMA2_Channel1->CCR  = DMA_CCR_DIR | DMA_CCR_MINC |  DMA_CCR_MSIZE_0 | DMA_CCR_PSIZE_1;
+  
+  if(DSHOT_Bidirect) DMA2_Channel1->CCR |= DMA_CCR_TCIE;
+  
+  NVIC_SetPriority(DMA2_Channel1_IRQn, 0);
+  
+  NVIC_EnableIRQ(DMA2_Channel1_IRQn);
+}
+//------------------------------------------------------------------------------
+     
+static void Init_TIM1_TX()
+{
+  TIM1->CR1 = 0;
+  TIM1->CR2 |= (0x01 << TIM_CR2_MMS_Pos); // Master генерирует TRGO при включении (CEN=1)
+  TIM1->PSC = 0;
+  TIM1->ARR = DSHOT_PeriodTX;
+  TIM1->CCMR2 = (6 << TIM_CCMR2_OC3M_Pos) | TIM_CCMR2_OC3PE | (6 << TIM_CCMR2_OC4M_Pos) | TIM_CCMR2_OC4PE;
+  TIM1->CCR3=TIM1->CCR4=0;
+  TIM1->EGR = TIM_EGR_UG;
+  if(DSHOT_Bidirect) TIM1->CCER = TIM_CCER_CC3E | TIM_CCER_CC3P | TIM_CCER_CC4E | TIM_CCER_CC4P;
+  else               TIM1->CCER = TIM_CCER_CC3E | TIM_CCER_CC4E;
+  TIM1->DIER = TIM_DIER_UDE;
+  TIM1->DCR = (0x0F << TIM_DCR_DBA_Pos) | (1 << TIM_DCR_DBL_Pos); // Burst CCR3 & CCR4
+  
+  TIM1->BDTR = TIM_BDTR_MOE;
+ 
+  DMAMUX1_Channel7->CCR = (46 << DMAMUX_CxCR_DMAREQ_ID_Pos); // TIM1_UP
+
+  // TX DMA TIM1
+  DMA2_Channel2->CPAR = (unsigned long)&TIM1->DMAR;
+  DMA2_Channel2->CMAR = (unsigned long)DSHOT_TX_Buff[DSHOT_TIM1];
+  DMA2_Channel2->CCR |= DMA_CCR_DIR | DMA_CCR_MINC |  DMA_CCR_MSIZE_0 | DMA_CCR_PSIZE_0;
+}
+//------------------------------------------------------------------------------
+     
+bool DSHOT_Init(unsigned long Freq, bool Bidirect)
+{
+  if(RCC->APB2ENR & RCC_APB2ENR_TIM1EN)   return false;
+  if(RCC->APB1ENR1 & RCC_APB1ENR1_TIM2EN) return false;
+  
+  RCC->APB2ENR  |= RCC_APB2ENR_TIM1EN;
+  RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
+  
+  RCC->AHB1ENR  |= RCC_AHB1ENR_DMA2EN | RCC_AHB1ENR_DMAMUX1EN;
+  
+  RCC->AHB2ENR  |= RCC_AHB2ENR_GPIOAEN;
+  
+  GPIO_InitPin(GPIO_PIN_0  | GPIO_PORT_A | GPIO_ALTER | GPIO_PULLUP | GPIO_AF1);  // TIM2_CH1
+  GPIO_InitPin(GPIO_PIN_1  | GPIO_PORT_A | GPIO_ALTER | GPIO_PULLUP | GPIO_AF1);  // TIM2_CH2
+  GPIO_InitPin(GPIO_PIN_10 | GPIO_PORT_A | GPIO_ALTER | GPIO_PULLUP | GPIO_AF6);  // TIM1_CH3
+  GPIO_InitPin(GPIO_PIN_11 | GPIO_PORT_A | GPIO_ALTER | GPIO_PULLUP | GPIO_AF11); // TIM1_CH4
+  
+  DSHOT_RX_Port=GPIOA;
+  DSHOT_TX_Mode=GPIO_MODER_MODE0_1 | GPIO_MODER_MODE1_1 | GPIO_MODER_MODE10_1 | GPIO_MODER_MODE11_1;
+  DSHOT_RX_Mode=~(GPIO_MODER_MODE0 | GPIO_MODER_MODE1 | GPIO_MODER_MODE10 | GPIO_MODER_MODE11);
+  
+  DSHOT_RX_Index[0]=1<<GPIO_PIN_0;
+  DSHOT_RX_Index[1]=1<<GPIO_PIN_1;
+  DSHOT_RX_Index[2]=1<<GPIO_PIN_10;
+  DSHOT_RX_Index[3]=1<<GPIO_PIN_11;
+  
+  DSHOT_Mode(Freq, Bidirect);
+  
+  for (int a = 0; a < DSHOT_TIM; a++) // Zero end in buffer
+    for (int b = DSHOT_FRAME_DATA*DSHOT_CCR; b < DSHOT_FRAME_SIZE*DSHOT_CCR; b++)
+      DSHOT_TX_Buff[a][b] = 0;
+
+  if(Bidirect) Init_ModeRX();
+  
+  Init_TIM2_TX();
+  Init_TIM1_TX();
+  
+  return true;
+}
+//------------------------------------------------------------------------------
+
+static void ds_packet1(unsigned short throttle, bool tele, unsigned short* buff)
+{
+  unsigned short packet = (throttle << 5) | ((tele) << 4); 
+  unsigned short checksum;
+  
+  if(DSHOT_Bidirect) checksum = ~(packet >> 4 ^ packet >> 8 ^ packet >> 12) & 0x0F;
+  else               checksum =  (packet >> 4 ^ packet >> 8 ^ packet >> 12) & 0x0F;
+  
+  packet |= checksum;
+  
+  for (int a = 0; a < DSHOT_FRAME_DATA; a++) buff[a] = (packet & (0x8000 >> a)) ? DSHOT_Bit_1 : DSHOT_Bit_0;
+}
+//------------------------------------------------------------------------------
+     
+static void ds_packet2(unsigned short throttle, bool tele, unsigned short* buff, unsigned long shift)
+{
+  unsigned short packet = (throttle << 5) | ((tele) << 4); 
+  unsigned short checksum;
+  
+  if(DSHOT_Bidirect) checksum = ~(packet >> 4 ^ packet >> 8 ^ packet >> 12) & 0x0F;
+  else               checksum =  (packet >> 4 ^ packet >> 8 ^ packet >> 12) & 0x0F;
+  
+  packet |= checksum;
+  
+  for (int a = 0, b = shift; a < DSHOT_FRAME_DATA; a++, b+=2) buff[b] = (packet & (0x8000 >> a)) ? DSHOT_Bit_1 : DSHOT_Bit_0;
+}
+//------------------------------------------------------------------------------
+     
+bool DSHOT_SetCommand(unsigned short Command[4], bool Forced)
+{
+  if(!Command) return false;
+  
+  if(!Forced)
+  {
+    if(DMA2_Channel1->CNDTR || DMA2_Channel2->CNDTR) return false;
+    if(DSHOT_Bidirect && DMA2_Channel3->CNDTR) return false;
+  }
+  
+  TIM2->CR1 = TIM1->CR1 = TIM6->CR1 = 0;
+  TIM2->DIER = TIM1->DIER = TIM6->DIER = 0;
+  
+  if(Forced) Begin_ModeTX();
+  
+  ds_packet2(Command[0] & 0x0FFF, Command[0] >> 15, DSHOT_TX_Buff[DSHOT_TIM2], DSHOT_CCR1);
+  ds_packet2(Command[1] & 0x0FFF, Command[1] >> 15, DSHOT_TX_Buff[DSHOT_TIM2], DSHOT_CCR2);
+  
+  ds_packet2(Command[2] & 0x0FFF, Command[2] >> 15, DSHOT_TX_Buff[DSHOT_TIM1], DSHOT_CCR3);
+  ds_packet2(Command[3] & 0x0FFF, Command[3] >> 15, DSHOT_TX_Buff[DSHOT_TIM1], DSHOT_CCR4);
+  
+  DMA2_Channel1->CCR  &= ~DMA_CCR_EN;
+  DMA2_Channel1->CNDTR =  DSHOT_FRAME_SIZE*DSHOT_CCR;
+  DMA2_Channel1->CCR  |=  DMA_CCR_EN;
+  
+  DMA2_Channel2->CCR  &= ~DMA_CCR_EN;
+  DMA2_Channel2->CNDTR =  DSHOT_FRAME_SIZE*DSHOT_CCR;
+  DMA2_Channel2->CCR  |=  DMA_CCR_EN;
+  
+  TIM2->DIER = TIM1->DIER = TIM_DIER_UDE;
+  
+  if(DSHOT_Bidirect)
+  {
+    DMA2_Channel3->CCR  &= ~DMA_CCR_EN;
+    DMA2_Channel3->CNDTR =  DSHOT_RX_SIZE*DSHOT_RX_SAMP;
+    DMA2_Channel3->CCR  |=  DMA_CCR_EN;
+  }
+  
+  TIM1->CR1 = TIM_CR1_CEN; // TIM1(master) & TIM2(slaver)
+  
+  return true;
+}
+//------------------------------------------------------------------------------
+
+bool DSHOT_GetERPM(unsigned short eRPM[4], bool Error[4])
+{
+  if(!eRPM) return false;
+  
+  struct
+  {
+    bool prev=true;
+    unsigned long len=0;
+    unsigned long code=0;
+    unsigned long count=0;
+  }data[DSHOT_COUNT];
+  
+  for(int a=1; a<DSHOT_RX_SIZE*DSHOT_RX_SAMP-1; a++)
+  {
+    unsigned short* buf = DSHOT_RX_Buff;
+    unsigned short filt = (buf[a-1] & buf[a]) | (buf[a] & buf[a+1]) | (buf[a-1] & buf[a+1]);
+    
+    
+    for(int b=0; b<DSHOT_COUNT; b++)
+    {
+      bool bit = filt & DSHOT_RX_Index[b];
+      bool prv = data[b].prev;
+      
+      if(prv!=bit)
+      {
+        unsigned long cnt = data[b].len ? data[b].count : 0;
+        
+        if(cnt>=DSHOT_RX_SAMP*3-2) 
+        {
+          data[b].len+=3;
+          data[b].code<<=3;
+          if(prv) data[b].code|=0x07;
+        }
+        else if(cnt>=DSHOT_RX_SAMP*2-2) 
+        {
+          data[b].len+=2;
+          data[b].code<<=2;
+          if(prv) data[b].code|=0x03;
+        }
+        else
+        {
+          data[b].len+=1;
+          data[b].code<<=1;
+          if(prv) data[b].code|=0x01;
+        }
+
+        data[b].prev=bit;
+        data[b].count=0;
+      }
+      else
+      {
+        data[b].count++;
+      }
+    }
+    
+  }
+  
+  for(int a=0; a<4; a++)
+  {
+    if(data[a].len==21) 
+    {
+      data[a].code<<=1;
+      data[a].code|=1;
+    }
+    else if(data[a].len==20) 
+    {
+      data[a].code<<=2;
+      data[a].code|=3;
+    }
+     
+    short erpm = CalcERPM(data[a].code);
+    if(Error) 
+    {
+      Error[a]=erpm==-1;
+      if(Error[a] && !a) 
+        int z=0;
+    }
+    eRPM[a] = erpm;
+  }
+}
+//------------------------------------------------------------------------------
diff --git a/dshot2.h b/dshot2.h
new file mode 100644
index 0000000..5fcbe07
--- /dev/null
+++ b/dshot2.h
@@ -0,0 +1,9 @@
+#include "stm32g4xx.h"
+
+bool DSHOT_Init(unsigned long Freq, bool Bidirect);
+
+bool DSHOT_Mode(unsigned long Freq, bool Bidirect);
+
+bool DSHOT_SetCommand(unsigned short Command[4], bool Forced); // tele - 0x8000 | data - 0x0FFF
+
+bool DSHOT_GetERPM(unsigned short eRPM[4], bool Error[4]);
diff --git a/dsp_manager.c b/dsp_manager.c
index 5f172d6..62b6395 100644
--- a/dsp_manager.c
+++ b/dsp_manager.c
@@ -8,38 +8,33 @@ static float32_t fft_output[FFT_SIZE] __attribute__((section(".sram2")));
 static float32_t magnitudes[FFT_SIZE / 2] __attribute__((section(".sram2")));
 static float32_t hann_window[FFT_SIZE] __attribute__((section(".sram2")));
 
-// Коэффициенты биквадратного фильтра
-float32_t b[3] = {1.0f, -2.0f, 1.0f}; // Примерные значения
-float32_t a[3] = {1.0f, -1.8f, 0.81f};
-
-// Буфер состояния фильтра
-float32_t x[3] = {0};
-float32_t y[3] = {0};
-
-// Структура БПФ из библиотеки
+// FFT handler
 static arm_rfft_fast_instance_f32 fft_handler;
 
-// Определение переменных
-uint8_t dsp_buffer_ready = 0;
-uint16_t sample_count = 0;
-volatile uint16_t dsp_notch_freqs[3] = {0, 0, 0};
+// Публичные переменные (см. dsp_manager.h)
+uint8_t fft_ready = 0;
+uint16_t fft_index = 0;
+volatile uint16_t notch_report_hz[3] = {0, 0, 0};
 
-// Переменные для сглаживания частоты (чтобы не прыгала)
-static float32_t filtered_freqs[3] = {0.0f, 0.0f, 0.0f};
-const float32_t SMOOTH_ALPHA = 0.2f; // Коэффициент плавности (0.1 - очень медленно, 0.9 - мгновенно)
-const float32_t FAST_TRACK_ALPHA = 0.55f;
-const float32_t PEAK_THRESHOLD = 2200.0f;
-const float32_t FAST_TRACK_DELTA_HZ = 25.0f;
-const float32_t TRACK_WINDOW_HZ = 35.0f;
-static uint8_t freq_seen_frames[3] = {0, 0, 0};
-static uint8_t freq_missing_frames[3] = {0, 0, 0};
-const uint8_t REQUIRED_STABLE_FRAMES = 2;
-const uint8_t DISABLE_AFTER_MISSING_FRAMES = 5;
+// Внутренние состояния и телеметрия
+static float32_t notch_freq_smoothed[3] = {0.0f, 0.0f, 0.0f};
+volatile float32_t motor_erpm_avg = 0.0f;
+volatile float32_t motor_mech_rpm = 0.0f;
+volatile float32_t motor_elec_hz = 0.0f;
+
+void DSP_SetMotorErpm(const uint16_t eRPM[4], uint8_t pole_pairs) {
+    // ERPM используем только как debug-метрику, не как источник notch-частот
+    float base_hz = (float)eRPM[3] / 60.0f;
+
+    motor_erpm_avg = (float)eRPM[3];
+    motor_elec_hz = base_hz;
+    motor_mech_rpm = (float)eRPM[3] / (float)pole_pairs;
+}
 
 void DSP_Init(void) {
     // Инициализируем структуру БПФ
     arm_rfft_fast_init_f32(&fft_handler, FFT_SIZE);
-    
+
     // Генерируем окно Ханна (делается один раз)
     const float32_t sin_delta = 0.0246374509f;
     const float32_t cos_delta = 0.9996964600f;
@@ -57,13 +52,13 @@ void DSP_Init(void) {
 }
 
 void DSP_AddSample(float32_t sample) {
-    if (dsp_buffer_ready) return; // Ждем, пока обработают прошлую пачку
+    if (fft_ready) return; // Ждем, пока обработают прошлую пачку
 
-    fft_input[sample_count++] = sample;
+    fft_input[fft_index++] = sample;
 
-    if (sample_count >= FFT_SIZE) {
-        sample_count = 0;
-        dsp_buffer_ready = 1; // Сигнализируем в main
+    if (fft_index >= FFT_SIZE) {
+        fft_index = 0;
+        fft_ready = 1; // Сигнализируем в main
     }
 }
 
@@ -77,137 +72,59 @@ void DSP_Process(void) {
     // 3. Амплитуды
     arm_cmplx_mag_f32(fft_output, magnitudes, FFT_SIZE / 2);
 
-    // 4. Поиск 3-х режекторных частот.
-    // Активные фильтры трекаем локально вокруг уже захваченной частоты,
-    // а глобальный поиск используем только для первичного захвата.
-    float32_t current_iteration_freqs[3] = {0.0f, 0.0f, 0.0f};
+    // Ищем один базовый пик в спектре и строим гармоники от него.
+    // Это убирает прыжки второго и третьего notch'ей между соседними пиками.
     const uint32_t start_bin = (uint32_t)((50.0f * FFT_SIZE) / DSP_SAMPLE_RATE_HZ);
-    const float32_t freq_per_bin = DSP_SAMPLE_RATE_HZ / FFT_SIZE;
-    const uint32_t track_window_bins = (uint32_t)(TRACK_WINDOW_HZ / freq_per_bin);
 
-    for (int k = 0; k < 3; k++) {
-        float32_t max_val = 0.0f;
-        int32_t max_idx = -1;
+    float32_t base_peak_freq = 0.0f;
+    float32_t max_val = 0.0f;
+    int32_t max_idx = -1;
 
-        uint32_t search_start_bin = start_bin;
-        uint32_t search_end_bin = (FFT_SIZE / 2);
-
-        if (filtered_freqs[k] > 1.0f) {
-            int32_t center_bin = (int32_t)(filtered_freqs[k] / freq_per_bin);
-
-            if (center_bin < (int32_t)track_window_bins) {
-                search_start_bin = start_bin;
-            } else {
-                search_start_bin = (uint32_t)(center_bin - (int32_t)track_window_bins);
-            }
-
-            search_end_bin = (uint32_t)(center_bin + (int32_t)track_window_bins);
-            if (search_end_bin > (FFT_SIZE / 2)) {
-                search_end_bin = (FFT_SIZE / 2);
-            }
-        }
-
-        for (uint32_t i = search_start_bin; i < search_end_bin; i++) {
-            float32_t freq_hz = ((float32_t)i * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
-
-            if (filtered_freqs[k] <= 1.0f) {
-                uint8_t too_close_to_locked = 0;
-                for (int locked = 0; locked < k; locked++) {
-                    if (filtered_freqs[locked] > 1.0f && fabsf(freq_hz - filtered_freqs[locked]) < 40.0f) {
-                        too_close_to_locked = 1;
-                        break;
-                    }
-                }
-
-                if (too_close_to_locked) {
-                    continue;
-                }
-            }
-
-            uint8_t too_close = 0;
-            for (int j = 0; j < k; j++) {
-                if (current_iteration_freqs[j] > 1.0f && fabsf(freq_hz - current_iteration_freqs[j]) < 40.0f) { // Разнос 40Гц
-                    too_close = 1;
-                    break;
-                }
-            }
-
-            if (!too_close && magnitudes[i] > max_val) {
-                max_val = magnitudes[i];
-                max_idx = i;
-            }
-        }
-
-        // Если нашли пик выше порога
-        if (max_idx != -1 && max_val > PEAK_THRESHOLD) {
-            current_iteration_freqs[k] = ((float32_t)max_idx * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
-        } else if (filtered_freqs[k] > 1.0f) {
-            // Если активный notch не нашел точку в окне, удерживаем текущую частоту,
-            // а не переводим его в гонку за соседним пиком.
-            current_iteration_freqs[k] = filtered_freqs[k];
+    for (uint32_t i = start_bin; i < (FFT_SIZE / 2); i++) {
+        if (magnitudes[i] > max_val) {
+            max_val = magnitudes[i];
+            max_idx = (int32_t)i;
         }
     }
 
-    // Применяем задержку подтверждения и сглаживание к частотам
-    for (int k = 0; k < 3; k++) {
-        if (current_iteration_freqs[k] > 1.0f) {
-            freq_missing_frames[k] = 0;
+    if (max_idx >= 0) {
+        base_peak_freq = ((float32_t)max_idx * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
+    }
 
-            if (freq_seen_frames[k] < 255) {
-                freq_seen_frames[k]++;
-            }
-
-            if (filtered_freqs[k] < 1.0f) {
-                if (freq_seen_frames[k] >= REQUIRED_STABLE_FRAMES) {
-                    filtered_freqs[k] = current_iteration_freqs[k];
-                }
-            } else {
-                float32_t alpha = SMOOTH_ALPHA;
-
-                if (fabsf(current_iteration_freqs[k] - filtered_freqs[k]) > FAST_TRACK_DELTA_HZ) {
-                    alpha = FAST_TRACK_ALPHA;
-                }
-
-                filtered_freqs[k] = (alpha * current_iteration_freqs[k]) +
-                                    ((1.0f - alpha) * filtered_freqs[k]);
-            }
+    // Лёгкое сглаживание только для базовой частоты
+    if (base_peak_freq > 1.0f) {
+        if (notch_freq_smoothed[0] < 1.0f) {
+            notch_freq_smoothed[0] = base_peak_freq;
         } else {
-            freq_seen_frames[k] = 0;
+            const float32_t alpha = 0.25f;
+            notch_freq_smoothed[0] = (alpha * base_peak_freq) + ((1.0f - alpha) * notch_freq_smoothed[0]);
+        }
 
-            if (freq_missing_frames[k] < 255) {
-                freq_missing_frames[k]++;
-            }
+        // Гармоники считаем строго от базы
+        notch_freq_smoothed[1] = notch_freq_smoothed[0] * 2.0f;
+        notch_freq_smoothed[2] = notch_freq_smoothed[0] * 3.0f;
+    }
 
-            if (freq_missing_frames[k] >= DISABLE_AFTER_MISSING_FRAMES) {
-                filtered_freqs[k] = 0.0f;
-            }
+    // Не даём гармоникам уйти за предел Найквиста
+    const float32_t nyquist = (DSP_SAMPLE_RATE_HZ * 0.5f) - 1.0f;
+    for (int k = 0; k < 3; k++) {
+        if (notch_freq_smoothed[k] > nyquist) {
+            notch_freq_smoothed[k] = 0.0f;
         }
     }
 
     // 5. ПЕРЕНАСТРОЙКА ФИЛЬТРОВ
-    float32_t Notch_Q = 2.5f; // Чуть шире яма, чтобы лучше удерживать пик и ловить дрейф
+    float32_t Notch_Q = 2.5f;
 
-    // Вызываем инициализацию только если частота > 0
-    biquad_init_notch(&notch1, filtered_freqs[0], Notch_Q, 1000.0f);
-    biquad_init_notch(&notch2, filtered_freqs[1], Notch_Q, 1000.0f);
-    biquad_init_notch(&notch3, filtered_freqs[2], Notch_Q, 1000.0f);
+    biquad_init_notch(&notch1, notch_freq_smoothed[0], Notch_Q, DSP_SAMPLE_RATE_HZ);
+    biquad_init_notch(&notch2, notch_freq_smoothed[1], Notch_Q, DSP_SAMPLE_RATE_HZ);
+    biquad_init_notch(&notch3, notch_freq_smoothed[2], Notch_Q, DSP_SAMPLE_RATE_HZ);
 
     // В телеметрию
-    dsp_notch_freqs[0] = (uint16_t)filtered_freqs[0];
-    dsp_notch_freqs[1] = (uint16_t)filtered_freqs[1];
-    dsp_notch_freqs[2] = (uint16_t)filtered_freqs[2];
+    notch_report_hz[0] = (uint16_t)notch_freq_smoothed[0];
+    notch_report_hz[1] = (uint16_t)notch_freq_smoothed[1];
+    notch_report_hz[2] = (uint16_t)notch_freq_smoothed[2];
 
-    dsp_buffer_ready = 0; 
+    // Готово
+    fft_ready = 0;
 }
-
-// Реализация функции Biquad_Filter
-float32_t Biquad_Filter(float32_t input) {
-    float32_t output = b[0] * input + b[1] * x[1] + b[2] * x[2] - a[1] * y[1] - a[2] * y[2];
-
-    x[2] = x[1];
-    x[1] = input;
-    y[2] = y[1];
-    y[1] = output;
-
-    return output;
-}
\ No newline at end of file
diff --git a/dsp_manager.h b/dsp_manager.h
index af86986..428b349 100644
--- a/dsp_manager.h
+++ b/dsp_manager.h
@@ -2,9 +2,9 @@
 #define DSP_MANAGER_H
 
 #include "arm_math.h"
+#include <stdint.h>
 
-// Размер окна Фурье (степень двойки)
-// Уменьшен для экономии памяти
+// Размер окна Фурье (степень двойки) - Уменьшен для экономии памяти
 #define FFT_SIZE 256
 #define DSP_SAMPLE_RATE_HZ 1000.0f
 
@@ -12,13 +12,16 @@
 void DSP_Init(void);
 void DSP_AddSample(float32_t sample); // Добавить одну точку в "копилку"
 void DSP_Process(void);              // Запустить расчет (когда накопили FFT_SIZE)
+void DSP_SetMotorErpm(const uint16_t eRPM[4], uint8_t pole_pairs);
 
-// Объявление переменных
-extern uint8_t dsp_buffer_ready;
-extern uint16_t sample_count;
-extern volatile uint16_t dsp_notch_freqs[3];
+// Debug values for Live Watch (переименованы для ясности)
+extern volatile float32_t motor_erpm_avg;
+extern volatile float32_t motor_mech_rpm;
+extern volatile float32_t motor_elec_hz;
 
-// Добавление объявления переменной и прототипа функции
-float32_t Biquad_Filter(float32_t input);
+// Объявление переменных (ясные имена)
+extern uint8_t fft_ready;            // флаг: готовность FFT-пакета
+extern uint16_t fft_index;           // индекс накопления сэмплов
+extern volatile uint16_t notch_report_hz[3]; // частоты, отправляемые в телеметрию
 
 #endif
\ No newline at end of file
diff --git a/fft_az.ewp b/fft_az.ewp
index 971eac6..70ae42e 100644
--- a/fft_az.ewp
+++ b/fft_az.ewp
@@ -399,7 +399,7 @@
                 </option>
                 <option>
                     <name>IccLang</name>
-                    <state>0</state>
+                    <state>1</state>
                 </option>
                 <option>
                     <name>IccCDialect</name>
@@ -2236,12 +2236,24 @@
     <file>
         <name>$EW_DIR$\arm\CMSIS\DSP\Include\arm_math.h</name>
     </file>
+    <file>
+        <name>$PROJ_DIR$\dshot2.cpp</name>
+    </file>
+    <file>
+        <name>$PROJ_DIR$\dshot2.h</name>
+    </file>
     <file>
         <name>$PROJ_DIR$\dsp_manager.c</name>
     </file>
     <file>
         <name>$PROJ_DIR$\dsp_manager.h</name>
     </file>
+    <file>
+        <name>$PROJ_DIR$\gpio.cpp</name>
+    </file>
+    <file>
+        <name>$PROJ_DIR$\gpio.h</name>
+    </file>
     <file>
         <name>$PROJ_DIR$\imu.c</name>
     </file>
@@ -2252,10 +2264,10 @@
         <name>$PROJ_DIR$\main.c</name>
     </file>
     <file>
-        <name>$PROJ_DIR$\..\www\motors.c</name>
+        <name>$PROJ_DIR$\motors.c</name>
     </file>
     <file>
-        <name>$PROJ_DIR$\..\www\motors.h</name>
+        <name>$PROJ_DIR$\motors.h</name>
     </file>
     <file>
         <name>$PROJ_DIR$\startup_stm32g431xx.s</name>
@@ -2268,6 +2280,274 @@
     </file>
     <file>
         <name>$PROJ_DIR$\system_stm32g4xx.c</name>
+        <configuration>
+            <name>Debug</name>
+            <settings>
+                <name>ICCARM</name>
+                <data>
+                    <version>40</version>
+                    <wantNonLocal>0</wantNonLocal>
+                    <debug>1</debug>
+                    <option>
+                        <name>CCOptimizationNoSizeConstraints</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCDefines</name>
+                        <state>STM32G431xx</state>
+                        <state>ARM_MATH_CM4</state>
+                    </option>
+                    <option>
+                        <name>CCPreprocFile</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCPreprocComments</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCPreprocLine</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCListCFile</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCListCMnemonics</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCListCMessages</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCListAssFile</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCListAssSource</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCEnableRemarks</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCDiagSuppress</name>
+                        <state></state>
+                    </option>
+                    <option>
+                        <name>CCDiagRemark</name>
+                        <state></state>
+                    </option>
+                    <option>
+                        <name>CCDiagWarning</name>
+                        <state></state>
+                    </option>
+                    <option>
+                        <name>CCDiagError</name>
+                        <state></state>
+                    </option>
+                    <option>
+                        <name>CCObjPrefix</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>CCAllowList</name>
+                        <version>1</version>
+                        <state>11111110</state>
+                    </option>
+                    <option>
+                        <name>CCDebugInfo</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>IEndianMode</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>IProcessor</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>IExtraOptionsCheck</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>IExtraOptions</name>
+                        <state></state>
+                    </option>
+                    <option>
+                        <name>CCLangConformance</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCSignedPlainChar</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>CCRequirePrototypes</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCDiagWarnAreErr</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCCompilerRuntimeInfo</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>IFpuProcessor</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>OutputFile</name>
+                        <state>$FILE_BNAME$.o</state>
+                    </option>
+                    <option>
+                        <name>CCLibConfigHeader</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>PreInclude</name>
+                        <state></state>
+                    </option>
+                    <option>
+                        <name>CCIncludePath2</name>
+                        <state>$PROJ_DIR$</state>
+                        <state>$TOOLKIT_DIR$\CMSIS\DSP\Include</state>
+                    </option>
+                    <option>
+                        <name>CCStdIncCheck</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCCodeSection</name>
+                        <state>.text</state>
+                    </option>
+                    <option>
+                        <name>IProcessorMode2</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>CCOptLevel</name>
+                        <state>3</state>
+                    </option>
+                    <option>
+                        <name>CCOptStrategy</name>
+                        <version>0</version>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>CCOptLevelSlave</name>
+                        <state>3</state>
+                    </option>
+                    <option>
+                        <name>CCPosIndRopi</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCPosIndRwpi</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCPosIndNoDynInit</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>IccLang</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>IccCDialect</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>IccAllowVLA</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>IccStaticDestr</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>IccCppInlineSemantics</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>IccCmsis</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>IccFloatSemantics</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCNoLiteralPool</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCOptStrategySlave</name>
+                        <version>0</version>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>CCEncSource</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCEncOutput</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCEncOutputBom</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>CCEncInput</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>IccExceptions2</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>IccRTTI2</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCStackProtection</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCPointerAutentiction</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCBranchTargetIdentification</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCPosRadRwpi</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCPosSharedSlave</name>
+                        <state>0</state>
+                    </option>
+                    <option>
+                        <name>CCUseIarExtensions</name>
+                        <state>1</state>
+                    </option>
+                    <option>
+                        <name>CCUseGnuExtensions</name>
+                        <state>0</state>
+                    </option>
+                </data>
+            </settings>
+        </configuration>
     </file>
     <file>
         <name>$PROJ_DIR$\system_stm32g4xx.h</name>
diff --git a/fft_az.ewt b/fft_az.ewt
index b68fb8c..3530940 100644
--- a/fft_az.ewt
+++ b/fft_az.ewt
@@ -3424,12 +3424,24 @@
     <file>
         <name>$EW_DIR$\arm\CMSIS\DSP\Include\arm_math.h</name>
     </file>
+    <file>
+        <name>$PROJ_DIR$\dshot2.cpp</name>
+    </file>
+    <file>
+        <name>$PROJ_DIR$\dshot2.h</name>
+    </file>
     <file>
         <name>$PROJ_DIR$\dsp_manager.c</name>
     </file>
     <file>
         <name>$PROJ_DIR$\dsp_manager.h</name>
     </file>
+    <file>
+        <name>$PROJ_DIR$\gpio.cpp</name>
+    </file>
+    <file>
+        <name>$PROJ_DIR$\gpio.h</name>
+    </file>
     <file>
         <name>$PROJ_DIR$\imu.c</name>
     </file>
@@ -3440,10 +3452,10 @@
         <name>$PROJ_DIR$\main.c</name>
     </file>
     <file>
-        <name>$PROJ_DIR$\..\www\motors.c</name>
+        <name>$PROJ_DIR$\motors.c</name>
     </file>
     <file>
-        <name>$PROJ_DIR$\..\www\motors.h</name>
+        <name>$PROJ_DIR$\motors.h</name>
     </file>
     <file>
         <name>$PROJ_DIR$\startup_stm32g431xx.s</name>
diff --git a/gpio.cpp b/gpio.cpp
new file mode 100644
index 0000000..d90b657
--- /dev/null
+++ b/gpio.cpp
@@ -0,0 +1,37 @@
+#include "stm32g4xx.h"
+
+#include "gpio.h"
+
+void GPIO_InitPin(unsigned long I_Pin)
+{
+  unsigned long port = (I_Pin & 0x000000F0UL) >> 4;
+  GPIO_TypeDef* gpio = (GPIO_TypeDef*)(((unsigned char*)GPIOA) + (port * 0x0400));
+  unsigned long rcc = 1UL << port;
+  unsigned long pin = I_Pin & 0x0000000FUL;
+  unsigned long af = (I_Pin & 0x0F000000UL) >> 24;
+  unsigned long pupd = (I_Pin & 0x00F00000UL) >> 20;
+  unsigned long ospeed = (I_Pin & 0x000F0000UL) >> 16;
+  unsigned long mode = (I_Pin & 0x0000F000UL) >> 12;
+  unsigned long otype = (I_Pin & 0x00000100UL) >> 8;
+  unsigned long set = (I_Pin & 0x00000200UL) >> 9;
+  //---
+  if (!(RCC->AHB2ENR & rcc)) RCC->AHB2ENR |= rcc;
+  //---
+  gpio->AFR[pin >> 3] &= ~(0x0000000FUL << ((pin & 0x07) * 4));
+  gpio->AFR[pin >> 3] |= af << ((pin & 0x07) * 4);
+  //---
+  gpio->OSPEEDR &= ~(0x00000003UL << (pin * 2));
+  gpio->OSPEEDR |= ospeed << (pin * 2);
+  //---
+  gpio->OTYPER &= ~(0x00000001UL << pin);
+  gpio->OTYPER |= otype << pin;
+  //---
+  gpio->PUPDR &= ~(0x00000003UL << (pin * 2));
+  gpio->PUPDR |= pupd << (pin * 2);
+  //---
+  gpio->BSRR = 1 << (set ? pin : pin+16);
+  //---
+  gpio->MODER &= ~(0x00000003UL << (pin * 2));
+  gpio->MODER |= mode << (pin * 2);
+}
+//------------------------------------------------------------------------------
diff --git a/gpio.h b/gpio.h
new file mode 100644
index 0000000..90da667
--- /dev/null
+++ b/gpio.h
@@ -0,0 +1,69 @@
+#pragma once
+
+#define GPIO_PIN_0  0x00000000UL
+#define GPIO_PIN_1  0x00000001UL
+#define GPIO_PIN_2  0x00000002UL
+#define GPIO_PIN_3  0x00000003UL
+#define GPIO_PIN_4  0x00000004UL
+#define GPIO_PIN_5  0x00000005UL
+#define GPIO_PIN_6  0x00000006UL
+#define GPIO_PIN_7  0x00000007UL
+#define GPIO_PIN_8  0x00000008UL
+#define GPIO_PIN_9  0x00000009UL
+#define GPIO_PIN_10 0x0000000AUL
+#define GPIO_PIN_11 0x0000000BUL
+#define GPIO_PIN_12 0x0000000CUL
+#define GPIO_PIN_13 0x0000000DUL
+#define GPIO_PIN_14 0x0000000EUL
+#define GPIO_PIN_15 0x0000000FUL
+
+#define GPIO_PORT_A 0x00000000UL
+#define GPIO_PORT_B 0x00000010UL
+#define GPIO_PORT_C 0x00000020UL
+#define GPIO_PORT_D 0x00000030UL
+#define GPIO_PORT_E 0x00000040UL
+#define GPIO_PORT_F 0x00000070UL
+#define GPIO_PORT_G 0x00000080UL
+#define GPIO_PORT_H 0x00000090UL
+#define GPIO_PORT_I 0x000000A0UL
+#define GPIO_PORT_J 0x000000B0UL
+#define GPIO_PORT_K 0x000000C0UL
+
+#define GPIO_PUSHPULL  0x00000000UL
+#define GPIO_OPENDRAIN 0x00000100UL
+
+#define GPIO_RESET 0x00000000UL
+#define GPIO_SET   0x00000200UL
+
+#define GPIO_INPUT  0x00000000UL
+#define GPIO_OUTPUT 0x00001000UL
+#define GPIO_ALTER  0x00002000UL
+#define GPIO_ANALOG 0x00003000UL
+
+#define GPIO_OSPEED_LOW    0x00000000UL
+#define GPIO_OSPEED_MEDIUM 0x00010000UL
+#define GPIO_OSPEED_FAST   0x00020000UL
+#define GPIO_OSPEED_HIGH   0x00030000UL
+
+#define GPIO_NOPUPD   0x00000000UL
+#define GPIO_PULLUP   0x00100000UL
+#define GPIO_PULLDOWN 0x00200000UL
+
+#define GPIO_AF0  0x00000000UL
+#define GPIO_AF1  0x01000000UL
+#define GPIO_AF2  0x02000000UL
+#define GPIO_AF3  0x03000000UL
+#define GPIO_AF4  0x04000000UL
+#define GPIO_AF5  0x05000000UL
+#define GPIO_AF6  0x06000000UL
+#define GPIO_AF7  0x07000000UL
+#define GPIO_AF8  0x08000000UL
+#define GPIO_AF9  0x09000000UL
+#define GPIO_AF10 0x0A000000UL
+#define GPIO_AF11 0x0B000000UL
+#define GPIO_AF12 0x0C000000UL
+#define GPIO_AF13 0x0D000000UL
+#define GPIO_AF14 0x0E000000UL
+#define GPIO_AF15 0x0F000000UL
+
+void GPIO_InitPin(unsigned long I_Pin);
diff --git a/imu.c b/imu.c
index 876f333..d737d5b 100644
--- a/imu.c
+++ b/imu.c
@@ -91,7 +91,7 @@ void IMU_Init(void) {
     IMU_WriteReg(0x07, 0x00);
     
     IMU_SetBank(2);
-    IMU_WriteReg(0x01, 0x01); // Bypass (отключаем встроенный фильтр для анализа)
+    IMU_WriteReg(0x01, 0x01); // Bypass (отключили встроенный фильтр для анализа)
     IMU_SetBank(0);
 
     // Начальная инициализация ( на 0 Гц dsp_manager сам их включит)
@@ -117,8 +117,7 @@ void IMU_ReadRawData(void) {
     raw_gx = (int16_t)(buf[6] << 8 | buf[7]);
     float x = (float)raw_gx - gyro_bias_x;
 
-    // Последовательно применяем 3 режекторных фильтра
-    // dsp_manager будет менять их коэффициенты в фоновом режиме
+    // Последовательно применяем 3 режекторных фильтра dsp_manager будет менять их коэффициенты в фоновом режиме
     x = biquad_apply(&notch1, x);
     x = biquad_apply(&notch2, x);
     x = biquad_apply(&notch3, x);
diff --git a/main.c b/main.c
index ac97795..fdcc25a 100644
--- a/main.c
+++ b/main.c
@@ -2,14 +2,15 @@
 #include "imu.h"
 #include "motors.h"
 #include "dsp_manager.h"
+#include "dshot2.h"
 
 // 1. ПЕРЕМЕННЫЕ
 volatile uint8_t imu_flag = 0;
 
-volatile uint32_t m1_speed = 900;
-volatile uint32_t m2_speed = 900;
-volatile uint32_t m3_speed = 900;
-volatile uint32_t m4_speed = 900;
+uint32_t m1_speed = 0;
+uint32_t m2_speed = 0;
+uint32_t m3_speed = 0;
+uint32_t m4_speed = 0;
 
 #pragma pack(push, 1)
 typedef struct {
@@ -22,10 +23,10 @@ typedef struct {
 } Telemetry_t;
 #pragma pack(pop)
 
-// 2. ПРОТОТИПЫ (Чтобы компилятор не ругался)
+// 2. ПРОТОТИПЫ
 void SystemClock_Config_160MHz(void);
 void UART2_Init_921600(void);
-void TIM6_Init_1000Hz(void);
+void TIM7_Init_1000Hz(void);
 void UART_SendPacket(Telemetry_t *p);
 
 // 3. ОСНОВНОЙ ЦИКЛ
@@ -42,7 +43,7 @@ int main(void) {
     
     DSP_Init(); // Инициализация нашего анализатора Фурье
     
-    TIM6_Init_1000Hz();
+    TIM7_Init_1000Hz();
     __enable_irq();
 
     Telemetry_t pkt;
@@ -51,31 +52,41 @@ int main(void) {
     while (1) {
         if (imu_flag) {
               imu_flag = 0;
+              
+              static unsigned short dshot_erpm[4];
+              DSHOT_GetERPM(dshot_erpm, 0);
+              DSP_SetMotorErpm(dshot_erpm, 7);
 
               IMU_ReadRawData(); 
+              //Motors_PollTelemetry();
 
               //сохраняем во временную переменную
               int16_t gx_val = raw_gx; 
               DSP_AddSample((float32_t)gx_val);
 
-              if (dsp_buffer_ready) {
+              if (fft_ready) {
                   DSP_Process(); 
               }
 
               float32_t filt_gx_value = filt_gx;
               pkt.gx = gx_val;
               pkt.filt_gx = (int16_t)filt_gx_value;
-              pkt.notch1_hz = dsp_notch_freqs[0];
-              pkt.notch2_hz = dsp_notch_freqs[1];
-              pkt.notch3_hz = dsp_notch_freqs[2];
+              pkt.notch1_hz = notch_report_hz[0];
+              pkt.notch2_hz = notch_report_hz[1];
+              pkt.notch3_hz = notch_report_hz[2];
               UART_SendPacket(&pkt);
 
-              Set_Motor_Individual(m1_speed, m2_speed, m3_speed, m4_speed);
+              
+              uint32_t m1 = m1_speed;
+              uint32_t m2 = m2_speed;
+              uint32_t m3 = m3_speed;
+              uint32_t m4 = m4_speed;
+              Set_Motor_Individual(m1, m2, m3, m4);
           }
         }
     }
 
-// 4. РЕАЛИЗАЦИЯ ФУНКЦИЙ (Тут был провал - их не хватало!)
+// 4. РЕАЛИЗАЦИЯ ФУНКЦИЙ
 
 void SystemClock_Config_160MHz(void) {
     RCC->CR |= RCC_CR_HSION;
@@ -111,18 +122,18 @@ void UART_SendPacket(Telemetry_t *p) {
     }
 }
 
-void TIM6_Init_1000Hz(void) {
-    RCC->APB1ENR1 |= RCC_APB1ENR1_TIM6EN;
-    TIM6->PSC = 16000 - 1; 
-    TIM6->ARR = 10 - 1;
-    TIM6->DIER |= TIM_DIER_UIE;
-    NVIC_EnableIRQ(TIM6_DAC_IRQn);
-    TIM6->CR1 |= TIM_CR1_CEN;
+void TIM7_Init_1000Hz(void) {
+    RCC->APB1ENR1 |= RCC_APB1ENR1_TIM7EN;
+    TIM7->PSC = 16000 - 1; 
+    TIM7->ARR = 10 - 1;
+    TIM7->DIER |= TIM_DIER_UIE;
+    NVIC_EnableIRQ(TIM7_DAC_IRQn);
+    TIM7->CR1 |= TIM_CR1_CEN;
 }
 
-void TIM6_DAC_IRQHandler(void) {
-    if (TIM6->SR & TIM_SR_UIF) {
-        TIM6->SR = 0;
+extern "C" void TIM7_DAC_IRQHandler(void) {
+    if (TIM7->SR & TIM_SR_UIF) {
+        TIM7->SR = 0;
         imu_flag = 1;
     }
 }
\ No newline at end of file
diff --git a/motors.c b/motors.c
index a8c5ba8..6ee4707 100644
--- a/motors.c
+++ b/motors.c
@@ -1,73 +1,46 @@
 #include "stm32g4xx.h"
 #include "motors.h"
-#include "pid.h"
-#include "mixer.h"
+//#include "pid.h"
+//#include "mixer.h"
+#include "dshot2.h"
 
 void Motors_Init(void) {
-
-    RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
-    RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
-    RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
-
-    // PA0, PA1 -> TIM2 CH1/CH2
-    GPIOA->MODER &= ~(GPIO_MODER_MODE0 | GPIO_MODER_MODE1);
-    GPIOA->MODER |=  (GPIO_MODER_MODE0_1 | GPIO_MODER_MODE1_1);
-    GPIOA->AFR[0] &= ~((0xF << 0) | (0xF << 4));
-    GPIOA->AFR[0] |=  ((1 << 0) | (1 << 4));
-
-    // PA10, PA11 -> TIM1 CH3/CH4
-    GPIOA->MODER &= ~(GPIO_MODER_MODE10 | GPIO_MODER_MODE11);
-    GPIOA->MODER |=  (GPIO_MODER_MODE10_1 | GPIO_MODER_MODE11_1);
-    GPIOA->AFR[1] &= ~((0xF << 8) | (0xF << 12));
-    GPIOA->AFR[1] |=  ((6 << 8) | (11 << 12));
-
-    // TIM2 — 100 Гц
-    TIM2->PSC = 16 - 1;
-    TIM2->ARR = 10000 - 1;
-    TIM2->CCMR1 = 0x6060;
-    TIM2->CCER  = 0x11;
-    TIM2->CR1   = TIM_CR1_CEN;
-
-    // TIM1 — 100 Гц
-    TIM1->PSC = 16 - 1;
-    TIM1->ARR = 10000 - 1;
-    TIM1->CCMR2 = 0x6060;
-    TIM1->CCER  = 0x1100;
-    TIM1->BDTR |= TIM_BDTR_MOE;
-    TIM1->CR1   = TIM_CR1_CEN;
+DSHOT_Init(300'000, true);
+    
 }
 
 void Set_Motors(int val) {
-    TIM2->CCR1 = val; // M1
-    TIM2->CCR2 = val; // M2
-    TIM1->CCR3 = val; // M3
-    TIM1->CCR4 = val; // M4
+  unsigned short cmd[4]={val,val,val,val};
+    DSHOT_SetCommand(cmd, true);
 }
 
 void Set_Motor_Individual(int m1, int m2, int m3, int m4) {
-    TIM2->CCR1 = m1;
-    TIM2->CCR2 = m2;
-    TIM1->CCR3 = m3;
-    TIM1->CCR4 = m4;
+    unsigned short cmd[4]={m1,m2,m3,m4};
+    DSHOT_SetCommand(cmd, true);
 }
 
 void Motors_StartupDelay(void) {
-    Set_Motors(900);
+    unsigned short cmd[4]={0,0,0,0};
+    DSHOT_SetCommand(cmd, true);
     for (volatile int i = 0; i < 6000000; i++) __NOP();
 }
 
 void Motors_Stop(void) {
-    Set_Motors(900);
+    unsigned short cmd[4]={0,0,0,0};
+    DSHOT_SetCommand(cmd, true);
 }
 
 void Motors_Idle(void) {
-    Set_Motors(1050);
+    unsigned short cmd[4]={150,150,150,150};
+    DSHOT_SetCommand(cmd, true);
 }
 
 void Motors_Arm(void) {
-    Set_Motors(1050);
+    unsigned short cmd[4]={150,150,150,150};
+    DSHOT_SetCommand(cmd, true);
 }
 
 void Motors_Disarm(void) {
-    Set_Motors(900);
+    unsigned short cmd[4]={0,0,0,0};
+    DSHOT_SetCommand(cmd, true);
 }