Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 vadyschka01
|
1ff5db84a1 | ||
|
vadyschka01
|
babc5a24e3 |
@@ -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
|
||||
+446
@@ -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;
|
||||
}
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
@@ -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]);
|
||||
+53
-130
@@ -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,65 @@ 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};
|
||||
// Ищем 3 сильнейших пика в спектре и на них строим notch'и.
|
||||
// Телеметрия ERPM здесь не участвует, чтобы не уводить фильтры в неверные Гц.
|
||||
float32_t peak_freqs[3] = {0.0f, 0.0f, 0.0f};
|
||||
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;
|
||||
|
||||
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++) {
|
||||
for (uint32_t i = start_bin; i < (FFT_SIZE / 2); 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Гц
|
||||
if (peak_freqs[j] > 1.0f && fabsf(freq_hz - peak_freqs[j]) < 40.0f) {
|
||||
too_close = 1;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (too_close) {
|
||||
continue;
|
||||
}
|
||||
|
||||
if (!too_close && magnitudes[i] > max_val) {
|
||||
if (magnitudes[i] > max_val) {
|
||||
max_val = magnitudes[i];
|
||||
max_idx = i;
|
||||
max_idx = (int32_t)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];
|
||||
if (max_idx >= 0) {
|
||||
peak_freqs[k] = ((float32_t)max_idx * DSP_SAMPLE_RATE_HZ) / FFT_SIZE;
|
||||
}
|
||||
}
|
||||
|
||||
// Применяем задержку подтверждения и сглаживание к частотам
|
||||
// Лёгкое сглаживание только чтобы частоты не дрожали между соседними бинами
|
||||
for (int k = 0; k < 3; k++) {
|
||||
if (current_iteration_freqs[k] > 1.0f) {
|
||||
freq_missing_frames[k] = 0;
|
||||
|
||||
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];
|
||||
}
|
||||
if (peak_freqs[k] > 1.0f) {
|
||||
if (notch_freq_smoothed[k] < 1.0f) {
|
||||
notch_freq_smoothed[k] = peak_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]);
|
||||
}
|
||||
} else {
|
||||
freq_seen_frames[k] = 0;
|
||||
|
||||
if (freq_missing_frames[k] < 255) {
|
||||
freq_missing_frames[k]++;
|
||||
}
|
||||
|
||||
if (freq_missing_frames[k] >= DISABLE_AFTER_MISSING_FRAMES) {
|
||||
filtered_freqs[k] = 0.0f;
|
||||
const float32_t alpha = 0.25f;
|
||||
notch_freq_smoothed[k] = (alpha * peak_freqs[k]) + ((1.0f - alpha) * notch_freq_smoothed[k]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 5. ПЕРЕНАСТРОЙКА ФИЛЬТРОВ
|
||||
float32_t Notch_Q = 2.5f; // Чуть шире яма, чтобы лучше удерживать пик и ловить дрейф
|
||||
float32_t Notch_Q = 2.5f;
|
||||
|
||||
// Вызываем инициализацию только если частота > 0
|
||||
biquad_init_notch(¬ch1, filtered_freqs[0], Notch_Q, 1000.0f);
|
||||
biquad_init_notch(¬ch2, filtered_freqs[1], Notch_Q, 1000.0f);
|
||||
biquad_init_notch(¬ch3, filtered_freqs[2], Notch_Q, 1000.0f);
|
||||
biquad_init_notch(¬ch1, notch_freq_smoothed[0], Notch_Q, DSP_SAMPLE_RATE_HZ);
|
||||
biquad_init_notch(¬ch2, notch_freq_smoothed[1], Notch_Q, DSP_SAMPLE_RATE_HZ);
|
||||
biquad_init_notch(¬ch3, 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;
|
||||
}
|
||||
+11
-8
@@ -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
|
||||
+283
-3
@@ -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>
|
||||
|
||||
+14
-2
@@ -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>
|
||||
|
||||
@@ -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);
|
||||
}
|
||||
//------------------------------------------------------------------------------
|
||||
@@ -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);
|
||||
@@ -17,34 +17,34 @@ float biquad_apply(biquad_t *f, float x) {
|
||||
return out;
|
||||
}
|
||||
|
||||
static float fast_sin_approx(float x) {
|
||||
float x2 = x * x;
|
||||
return x * (1.0f - (x2 * 0.16666667f) + (x2 * x2 * 0.0083333338f));
|
||||
}
|
||||
|
||||
static float fast_cos_approx(float x) {
|
||||
float x2 = x * x;
|
||||
return 1.0f - (x2 * 0.5f) + (x2 * x2 * 0.041666668f) - (x2 * x2 * x2 * 0.0013888889f);
|
||||
}
|
||||
|
||||
void biquad_init_notch(biquad_t *f, float center_freq, float Q, float fs) {
|
||||
if (center_freq < 1.0f) {
|
||||
// Защита: при слишком низкой/высокой частоте уводим фильтр в bypass.
|
||||
if (center_freq < 1.0f || center_freq > (0.45f * fs) || Q <= 0.05f) {
|
||||
f->b0 = 1.0f; f->b1 = 0; f->b2 = 0;
|
||||
f->a1 = 0; f->a2 = 0;
|
||||
f->d1 = 0; f->d2 = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
float w0 = 2.0f * 3.14159265f * center_freq / fs;
|
||||
float alpha = fast_sin_approx(w0) / (2.0f * Q);
|
||||
float cosw0 = fast_cos_approx(w0);
|
||||
|
||||
float alpha = sinf(w0) / (2.0f * Q);
|
||||
float cosw0 = cosf(w0);
|
||||
|
||||
float inv_a0 = 1.0f / (1.0f + alpha); // Считаем один раз инверсию
|
||||
|
||||
|
||||
f->b0 = inv_a0;
|
||||
f->b1 = -2.0f * cosw0 * inv_a0;
|
||||
f->b2 = inv_a0;
|
||||
f->a1 = -2.0f * cosw0 * inv_a0;
|
||||
f->a2 = (1.0f - alpha) * inv_a0;
|
||||
|
||||
// Защита от NaN/Inf, чтобы не обнулялся выход при некорректной перенастройке.
|
||||
if (!isfinite(f->b0) || !isfinite(f->b1) || !isfinite(f->b2) ||
|
||||
!isfinite(f->a1) || !isfinite(f->a2)) {
|
||||
f->b0 = 1.0f; f->b1 = 0; f->b2 = 0;
|
||||
f->a1 = 0; f->a2 = 0;
|
||||
f->d1 = 0; f->d2 = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void I2C1_Init(void) {
|
||||
@@ -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(¬ch1, x);
|
||||
x = biquad_apply(¬ch2, x);
|
||||
x = biquad_apply(¬ch3, x);
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user