{"id":466346,"date":"2025-07-07T21:00:48","date_gmt":"2025-07-07T21:00:48","guid":{"rendered":"http:\/\/savepearlharbor.com\/?p=466346"},"modified":"-0001-11-30T00:00:00","modified_gmt":"-0001-11-29T21:00:00","slug":"","status":"publish","type":"post","link":"https:\/\/savepearlharbor.com\/?p=466346","title":{"rendered":"<span>\u041f\u0440\u043e\u0433\u0440\u0430\u043c\u043c\u0438\u0440\u0443\u0435\u043c \u043a\u0432\u0430\u0434\u0440\u043e\u043a\u043e\u043f\u0442\u0435\u0440 \u043d\u0430 STM32<\/span>"},"content":{"rendered":"<div><!--[--><!--]--><\/div>\n<div id=\"post-content-body\">\n<div>\n<div class=\"article-formatted-body article-formatted-body article-formatted-body_version-2\">\n<div xmlns=\"http:\/\/www.w3.org\/1999\/xhtml\">\n<p>\u0412 \u044d\u0442\u043e\u0439 \u0441\u0442\u0430\u0442\u044c\u0435 \u0440\u0430\u0441\u0441\u043c\u043e\u0442\u0440\u0438\u043c \u0441\u0438\u0441\u0442\u0435\u043c\u0443 \u0441\u0442\u0430\u0431\u0438\u043b\u0438\u0437\u0430\u0446\u0438\u0438 \u043a\u0432\u0430\u0434\u0440\u043e\u043a\u043e\u043f\u0442\u0435\u0440\u0430.\u00a0 \u0421\u0442\u0430\u0442\u044c\u044f \u0432\u0434\u043e\u0445\u043d\u043e\u0432\u043b\u0435\u043d\u0430 <a href=\"https:\/\/habr.com\/ru\/articles\/227425\/\" rel=\"noopener noreferrer nofollow\">\u041f\u0440\u043e\u0433\u0440\u0430\u043c\u043c\u0438\u0440\u0443\u0435\u043c \u043a\u0432\u0430\u0434\u0440\u043e\u043a\u043e\u043f\u0442\u0435\u0440 \u043d\u0430 Arduino (\u0447\u0430\u0441\u0442\u044c 1)<\/a> . \u041f\u0440\u0438\u0441\u0442\u0443\u043f\u0438\u043c. \u041d\u0430\u0447\u043d\u0435\u043c \u0441 \u043d\u0430\u0441\u0442\u0440\u043e\u0439\u043a\u0438 \u0438\u043d\u0442\u0435\u0440\u0444\u0435\u0439\u0441\u043e\u0432 \u0438 \u0442\u0430\u0439\u043c\u0435\u0440\u043e\u0432 \u043c\u043a. \u0414\u0430\u043b\u0435\u0435 \u043a\u043e\u0440\u043e\u0442\u043a\u043e\u0435 \u043e\u043f\u0438\u0441\u0430\u043d\u0438\u0435 \u0444\u0443\u043d\u043a\u0446\u0438\u0439 \u043a\u043e\u0434\u0430 \u0438 \u0432\u044b\u0432\u043e\u0434.\u041f\u043e\u043b\u043d\u044b\u0439 \u043a\u043e\u0434 \u043f\u0440\u043e\u0435\u043a\u0442\u0430 \u0441\u043d\u0430\u0431\u0436\u0435\u043d \u043a\u043e\u043c\u043c\u0435\u043d\u0442\u0430\u0440\u0438\u044f\u043c\u0438 \u043a\u043e\u0442\u043e\u0440\u044b\u0435 \u043e\u0444\u043e\u0440\u043c\u043b\u0435\u043d\u044b \u043f\u043e\u0434 \u0441\u0442\u0438\u043b\u044c \u043a\u043e\u043c\u043c\u0435\u043d\u0442\u0430\u0440\u0438\u0435\u0432 \u0432 HAL. <\/p>\n<p>\u041d\u0430\u0441\u0442\u0440\u043e\u0438\u043c \u0447\u0435\u0442\u044b\u0440\u0435 \u043a\u0430\u043d\u0430\u043b\u0430 \u0443 TIM1 \u0438 \u0432\u043a\u043b\u044e\u0447\u0438\u043c DMA \u043e\u043d \u0431\u0443\u0434\u0435\u0442 \u0443\u043f\u0440\u0430\u0432\u043b\u044f\u0442\u044c ESC \u043f\u043e Multishot. Multishot \u2014 \u044d\u0442\u043e \u043f\u0440\u043e\u0442\u043e\u043a\u043e\u043b \u043f\u0435\u0440\u0435\u0434\u0430\u0447\u0438 \u0441\u0438\u0433\u043d\u0430\u043b\u043e\u0432 \u043e\u0442 \u043f\u043e\u043b\u0451\u0442\u043d\u043e\u0433\u043e \u043a\u043e\u043d\u0442\u0440\u043e\u043b\u043b\u0435\u0440\u0430\u00a0 \u043a \u0440\u0435\u0433\u0443\u043b\u044f\u0442\u043e\u0440\u0443 \u0441\u043a\u043e\u0440\u043e\u0441\u0442\u0438 ESC. \u0428\u0438\u0440\u0438\u043d\u0430 \u0438\u043c\u043f\u0443\u043b\u044c\u0441\u043e\u0432 5 &#8212; 25 \u043c\u043a\u0441.\u00a0\u00a0ESC \u0438\u0437\u043c\u0435\u0440\u044f\u0435\u0442 \u0434\u043b\u0438\u0442\u0435\u043b\u044c\u043d\u043e\u0441\u0442\u044c \u0438\u043c\u043f\u0443\u043b\u044c\u0441\u0430 \u0438 \u043f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u0443\u0435\u0442 \u0435\u0451 \u0432 \u0441\u043a\u043e\u0440\u043e\u0441\u0442\u044c \u0432\u0440\u0430\u0449\u0435\u043d\u0438\u044f \u0434\u0432\u0438\u0433\u0430\u0442\u0435\u043b\u044f. \u0427\u0435\u043c \u0434\u043b\u0438\u043d\u043d\u0435\u0435 \u0438\u043c\u043f\u0443\u043b\u044c\u0441, \u0442\u0435\u043c \u0432\u044b\u0448\u0435 \u043e\u0431\u043e\u0440\u043e\u0442\u044b. <\/p>\n<pre><code class=\"cpp\">\u0421\u0438\u0433\u043d\u0430\u043b Multishot :   \u041c\u0438\u043d: |\u203e\u203e\u203e|_______| (5 \u043c\u043a\u0441)   50%:  |\u203e\u203e\u203e\u203e\u203e\u203e\u203e|____| (15 \u043c\u043a\u0441)   \u041c\u0430\u043a\u0441:  |\u203e\u203e\u203e\u203e\u203e\u203e\u203e\u203e\u203e\u203e\u203e\u203e| (25 \u043c\u043a\u0441)  <\/code><\/pre>\n<figure class=\"full-width\"><img decoding=\"async\" src=\"https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/611\/ab5\/7d3\/611ab57d386e683c4a64d34c8c0a30f0.jpg\" alt=\"1\" title=\"1\" width=\"1267\" height=\"608\" sizes=\"auto, (max-width: 780px) 100vw, 50vw\" srcset=\"https:\/\/habrastorage.org\/r\/w780\/getpro\/habr\/upload_files\/611\/ab5\/7d3\/611ab57d386e683c4a64d34c8c0a30f0.jpg 780w,&#10;       https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/611\/ab5\/7d3\/611ab57d386e683c4a64d34c8c0a30f0.jpg 781w\" loading=\"lazy\" decode=\"async\"\/><\/p>\n<div><figcaption>1<\/figcaption><\/div>\n<\/figure>\n<figure class=\"full-width\"><img decoding=\"async\" src=\"https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/a61\/238\/ca2\/a61238ca2647e26b2acfb4483dbc84b6.jpg\" alt=\"2\" title=\"2\" width=\"1292\" height=\"638\" sizes=\"auto, (max-width: 780px) 100vw, 50vw\" srcset=\"https:\/\/habrastorage.org\/r\/w780\/getpro\/habr\/upload_files\/a61\/238\/ca2\/a61238ca2647e26b2acfb4483dbc84b6.jpg 780w,&#10;       https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/a61\/238\/ca2\/a61238ca2647e26b2acfb4483dbc84b6.jpg 781w\" loading=\"lazy\" decode=\"async\"\/><\/p>\n<div><figcaption>2<\/figcaption><\/div>\n<\/figure>\n<p>\u041c\u0438\u043a\u0440\u043e\u043a\u043e\u043d\u0442\u0440\u043e\u043b\u043b\u0435\u0440 \u043d\u0430\u0441\u0442\u0440\u043e\u0435\u043d \u043d\u0430 100MHz \u043e\u0434\u0438\u043d \u0442\u0438\u043a \u0442\u0430\u0439\u043c\u0435\u0440\u0430.<code>T = 1\/100 \u041c\u0413\u0446 = 0.01 \u043c\u043a\u0441 = 10 \u043d\u0441<\/code>  \u0412\u0440\u0435\u043c\u044f \u043f\u0435\u0440\u0435\u043f\u043e\u043b\u043d\u0435\u043d\u0438\u044f <code>T_overflow = 3000 \u00d7 10 \u043d\u0441 = 30 \u043c\u043a\u0441<\/code> \u0412 \u043a\u043e\u0434\u0435 \u0443\u043a\u0430\u0436\u0435\u043c \u043c\u0438\u043d\u0438\u043c\u0430\u043b\u044c\u043d\u043e\u0435 \u043a\u043e\u043b\u0438\u0447\u0435\u0441\u0442\u0432\u043e \u0442\u0438\u043a\u043e\u0432 500 \u044d\u0442\u043e \u0431\u0443\u0434\u0435\u0442 5\u043c\u043a\u0441 \u0438 \u043c\u0430\u043a\u0441\u0438\u043c\u0430\u043b\u044c\u043d\u043e\u0435 2500 \u044d\u0442\u043e \u0431\u0443\u0434\u0435\u0442 25\u043c\u043a\u0441. \u0414\u0430\u043b\u0435\u0435 \u043d\u0430\u0441\u0442\u0440\u043e\u0438\u043c TIM11 \u043f\u0440\u0435\u0434\u0434\u0435\u043b\u0438\u0442\u0435\u043b\u044c (Prescaler) \u0442\u0430\u0439\u043c\u0435\u0440\u0430 99 -1 \u043f\u0435\u0440\u0438\u043e\u0434 (Period) 999 &#8212; 1.  \u0422\u0430\u043a\u0442\u043e\u0432\u0430\u044f \u0447\u0430\u0441\u0442\u043e\u0442\u0430 100 \u041c\u0413\u0446.  \u041f\u043e\u0441\u043b\u0435 \u043f\u0440\u0435\u0434\u0434\u0435\u043b\u0438\u0442\u0435\u043b\u044f <code>100 \u041c\u0413\u0446 \/ 100 = 1 \u041c\u0413\u0446 <\/code>\u0434\u043b\u0438\u0442\u0435\u043b\u044c\u043d\u043e\u0441\u0442\u044c \u043e\u0434\u043d\u043e\u0433\u043e \u0442\u0438\u043a\u0430 <code>1 \/ 1 \u041c\u0413\u0446 = 1 \u043c\u043a\u0441<\/code>  \u043f\u0435\u0440\u0438\u043e\u0434 \u043f\u0435\u0440\u0435\u043f\u043e\u043b\u043d\u0435\u043d\u0438\u044f  <code>1000 \u00d7 1 \u043c\u043a\u0441 = 1000 \u043c\u043a\u0441 = 1 \u043c\u0441<\/code> \u0447\u0430\u0441\u0442\u043e\u0442\u0430 \u043f\u0440\u0435\u0440\u044b\u0432\u0430\u043d\u0438\u0439 <code>1 \/ 1 \u043c\u0441 = 1000 \u0413\u0446 (1 \u043a\u0413\u0446). <\/code> <\/p>\n<figure class=\"full-width\"><img decoding=\"async\" src=\"https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/979\/f1b\/32c\/979f1b32c913aa5d9a357784f9698f99.jpg\" alt=\"3\" title=\"3\" width=\"1381\" height=\"619\" sizes=\"auto, (max-width: 780px) 100vw, 50vw\" srcset=\"https:\/\/habrastorage.org\/r\/w780\/getpro\/habr\/upload_files\/979\/f1b\/32c\/979f1b32c913aa5d9a357784f9698f99.jpg 780w,&#10;       https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/979\/f1b\/32c\/979f1b32c913aa5d9a357784f9698f99.jpg 781w\" loading=\"lazy\" decode=\"async\"\/><\/p>\n<div><figcaption>3<\/figcaption><\/div>\n<\/figure>\n<p>\u0422\u0430\u0439\u043c\u0435\u0440 TIM11 \u0431\u0443\u0434\u0435\u0442 \u0433\u0435\u043d\u0435\u0440\u0438\u0440\u043e\u0432\u0430\u0442\u044c \u043f\u0440\u0435\u0440\u044b\u0432\u0430\u043d\u0438\u044f \u0441 \u0447\u0430\u0441\u0442\u043e\u0442\u043e\u0439 1 \u043a\u0413\u0446, \u0438 \u0432 \u043e\u0431\u0440\u0430\u0431\u043e\u0442\u0447\u0438\u043a\u0435 \u043f\u0440\u0435\u0440\u044b\u0432\u0430\u043d\u0438\u044f (\u0444\u0443\u043d\u043a\u0446\u0438\u0438 \u043e\u0431\u0440\u0430\u0442\u043d\u043e\u0433\u043e \u0432\u044b\u0437\u043e\u0432\u0430) \u0431\u0443\u0434\u0435\u0442 \u0432\u044b\u0437\u044b\u0432\u0430\u0442\u044c\u0441\u044f \u043e\u0441\u043d\u043e\u0432\u043d\u043e\u0439 \u0446\u0438\u043a\u043b \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f: <code>run_control_loop():<\/code><\/p>\n<pre><code class=\"cpp\">void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {     if (htim == &amp;htim11) {         run_control_loop(); \/\/ \u0412\u044b\u0437\u043e\u0432 \u0432\u0441\u0435\u0439 \u043b\u043e\u0433\u0438\u043a\u0438 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f     } }<\/code><\/pre>\n<p>\u0412 \u043a\u0430\u0447\u0435\u0441\u0442\u0432\u0435 IMU \u0438\u0441\u043f\u043e\u043b\u044c\u0437\u0443\u0435\u0442\u0441\u044f BMX055 \u043f\u043e\u0434\u043a\u043b\u044e\u0447\u0435\u043d\u043d\u044b\u0439 \u043f\u043e I2C \u0438\u043d\u0442\u0435\u0440\u0444\u0435\u0439\u0441 \u043d\u0430\u0441\u0442\u0440\u043e\u0435\u043d \u043d\u0430 \u0431\u044b\u0441\u0442\u0440\u044b\u0439 \u0440\u0435\u0436\u0438\u043c.<\/p>\n<figure class=\"full-width\"><img decoding=\"async\" src=\"https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/0f7\/f0f\/f9e\/0f7f0ff9e689009a0ef2b6417e840d2c.jpg\" alt=\"4\" title=\"4\" width=\"1381\" height=\"619\" sizes=\"auto, (max-width: 780px) 100vw, 50vw\" srcset=\"https:\/\/habrastorage.org\/r\/w780\/getpro\/habr\/upload_files\/0f7\/f0f\/f9e\/0f7f0ff9e689009a0ef2b6417e840d2c.jpg 780w,&#10;       https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/0f7\/f0f\/f9e\/0f7f0ff9e689009a0ef2b6417e840d2c.jpg 781w\" loading=\"lazy\" decode=\"async\"\/><\/p>\n<div><figcaption>4<\/figcaption><\/div>\n<\/figure>\n<figure class=\"full-width\"><img decoding=\"async\" src=\"https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/f8a\/91b\/cf0\/f8a91bcf0f2e632c61040370507b71e4.jpg\" alt=\"5\" title=\"5\" width=\"856\" height=\"786\" sizes=\"auto, (max-width: 780px) 100vw, 50vw\" srcset=\"https:\/\/habrastorage.org\/r\/w780\/getpro\/habr\/upload_files\/f8a\/91b\/cf0\/f8a91bcf0f2e632c61040370507b71e4.jpg 780w,&#10;       https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/f8a\/91b\/cf0\/f8a91bcf0f2e632c61040370507b71e4.jpg 781w\" loading=\"lazy\" decode=\"async\"\/><\/p>\n<div><figcaption>5<\/figcaption><\/div>\n<\/figure>\n<p>\u0412 \u043a\u0430\u0447\u0435\u0441\u0442\u0432\u0435 \u0440\u0430\u0434\u0438\u043e\u043c\u043e\u0434\u0443\u043b\u044f  E220-400T22D \u043f\u043e\u0434\u043a\u043b\u044e\u0447\u0435\u043d\u043d\u044b\u0439 \u043f\u043e UART6 \u0434\u043b\u044f \u043a\u043e\u0442\u043e\u0440\u043e\u0433\u043e \u0432\u043a\u043b\u044e\u0447\u0435\u043d\u043e DMA.<\/p>\n<figure class=\"full-width\"><img decoding=\"async\" src=\"https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/205\/dc6\/f4d\/205dc6f4d37d42c4de67986949eae79c.jpg\" alt=\"6\" title=\"6\" width=\"1225\" height=\"605\" sizes=\"auto, (max-width: 780px) 100vw, 50vw\" srcset=\"https:\/\/habrastorage.org\/r\/w780\/getpro\/habr\/upload_files\/205\/dc6\/f4d\/205dc6f4d37d42c4de67986949eae79c.jpg 780w,&#10;       https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/205\/dc6\/f4d\/205dc6f4d37d42c4de67986949eae79c.jpg 781w\" loading=\"lazy\" decode=\"async\"\/><\/p>\n<div><figcaption>6<\/figcaption><\/div>\n<\/figure>\n<figure class=\"full-width\"><img decoding=\"async\" src=\"https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/244\/0f5\/cae\/2440f5cae0290d9929d5e49dddf23be9.jpg\" alt=\"7\" title=\"7\" width=\"577\" height=\"265\" sizes=\"auto, (max-width: 780px) 100vw, 50vw\" srcset=\"https:\/\/habrastorage.org\/r\/w780\/getpro\/habr\/upload_files\/244\/0f5\/cae\/2440f5cae0290d9929d5e49dddf23be9.jpg 780w,&#10;       https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/244\/0f5\/cae\/2440f5cae0290d9929d5e49dddf23be9.jpg 781w\" loading=\"lazy\" decode=\"async\"\/><\/p>\n<div><figcaption>7<\/figcaption><\/div>\n<\/figure>\n<p>    \u041d\u0430\u0441\u0442\u0440\u043e\u0439\u043a\u0438 E220-400T22D \u043f\u0440\u043e\u0438\u0437\u0432\u043e\u0434\u0438\u043b\u0438\u0441\u044c \u0438\u0437 \u043f\u0440\u043e\u0433\u0440\u0430\u043c\u043c\u044b \u043e\u0442 EBYTE.<\/p>\n<figure class=\"full-width\"><img decoding=\"async\" src=\"https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/f1f\/1a3\/7e6\/f1f1a37e6f5387470fb6152e731211b4.jpg\" alt=\"8\" title=\"8\" width=\"684\" height=\"549\" sizes=\"auto, (max-width: 780px) 100vw, 50vw\" srcset=\"https:\/\/habrastorage.org\/r\/w780\/getpro\/habr\/upload_files\/f1f\/1a3\/7e6\/f1f1a37e6f5387470fb6152e731211b4.jpg 780w,&#10;       https:\/\/habrastorage.org\/r\/w1560\/getpro\/habr\/upload_files\/f1f\/1a3\/7e6\/f1f1a37e6f5387470fb6152e731211b4.jpg 781w\" loading=\"lazy\" decode=\"async\"\/><\/p>\n<div><figcaption>8<\/figcaption><\/div>\n<\/figure>\n<p>\u0412\u0441\u0435 \u043d\u0430\u0441\u0442\u0440\u043e\u0439\u043a\u0438 \u043f\u043e \u0443\u043c\u043e\u043b\u0447\u0430\u043d\u0438\u044e. \u041c\u043e\u0434\u0443\u043b\u044c \u0437\u0430\u0440\u0430\u0431\u043e\u0442\u0430\u043b \u0441\u0440\u0430\u0437\u0443 \u043a\u0430\u043a \u0442\u043e\u043b\u044c\u043a\u043e \u0431\u044b\u043b \u043f\u043e\u0434\u043a\u043b\u044e\u0447\u0435\u043d.<\/p>\n<h3>\u0418\u043d\u0438\u0446\u0438\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u044f \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u0432 \u0438 \u0430\u043b\u0433\u043e\u0440\u0438\u0442\u043c\u043e\u0432 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f.<\/h3>\n<p>\u041f\u043e\u0441\u043b\u0435 \u0438\u043d\u0438\u0446\u0438\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u0438 IMU (BMX055) \u0432 \u043a\u043e\u0434\u0435 \u0432\u044b\u043f\u043e\u043b\u043d\u044f\u0435\u0442\u0441\u044f \u043d\u0430\u0441\u0442\u0440\u043e\u0439\u043a\u0430 \u043d\u0435\u0441\u043a\u043e\u043b\u044c\u043a\u0438\u0445 \u0442\u0438\u043f\u043e\u0432 \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u0432 \u0438 \u041f\u0418\u0414-\u0440\u0435\u0433\u0443\u043b\u044f\u0442\u043e\u0440\u043e\u0432: <\/p>\n<h4>1. \u0424\u0438\u043b\u044c\u0442\u0440\u044b \u043d\u0438\u0437\u043a\u0438\u0445 \u0447\u0430\u0441\u0442\u043e\u0442 (LPF)<\/h4>\n<p>\u0418\u0441\u043f\u043e\u043b\u044c\u0437\u0443\u0435\u0442\u0441\u044f \u043a\u0430\u0441\u043a\u0430\u0434\u043d\u0430\u044f \u0441\u0442\u0440\u0443\u043a\u0442\u0443\u0440\u0430 \u0431\u0438\u043a\u0432\u0430\u0434\u0440\u0430\u0442\u043d\u044b\u0445 \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u0432 (Biquad) \u0438\u0437 \u0431\u0438\u0431\u043b\u0438\u043e\u0442\u0435\u043a\u0438 CMSIS-DSP:<\/p>\n<pre><code class=\"cpp\">arm_biquad_cascade_df2T_init_f32(&amp;lpf_Gx, NUM_STAGES, Coeffs_lpf, filterState_lpf_Gx); arm_biquad_cascade_df2T_init_f32(&amp;lpf_Gy, NUM_STAGES, Coeffs_lpf, filterState_lpf_Gy); arm_biquad_cascade_df2T_init_f32(&amp;lpf_Gz, NUM_STAGES, Coeffs_lpf, filterState_lpf_Gz);<\/code><\/pre>\n<p>\u0412\u0441\u0435\u0433\u043e \u0441\u043e\u0437\u0434\u0430\u0435\u0442\u0441\u044f 9 \u044d\u043a\u0437\u0435\u043c\u043f\u043b\u044f\u0440\u043e\u0432 \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u0432:<\/p>\n<ul>\n<li>\n<p>3 \u0434\u043b\u044f \u043e\u0441\u0435\u0439 \u0433\u0438\u0440\u043e\u0441\u043a\u043e\u043f\u0430 (Gx, Gy, Gz)<\/p>\n<\/li>\n<li>\n<p>3 \u0434\u043b\u044f \u043e\u0441\u0435\u0439 \u0430\u043a\u0441\u0435\u043b\u0435\u0440\u043e\u043c\u0435\u0442\u0440\u0430 (Ax, Ay, Az)<\/p>\n<\/li>\n<li>\n<p>3 \u0434\u043b\u044f \u043e\u0448\u0438\u0431\u043e\u043a \u043f\u043e\u043b\u043e\u0436\u0435\u043d\u0438\u044f (Errorx, Errory, Errorz)<\/p>\n<\/li>\n<\/ul>\n<h4>2. \u0424\u0438\u043b\u044c\u0442\u0440\u044b \u041a\u0430\u043b\u043c\u0430\u043d\u0430<\/h4>\n<p>\u0418\u043d\u0438\u0446\u0438\u0430\u043b\u0438\u0437\u0438\u0440\u0443\u044e\u0442\u0441\u044f \u043e\u0442\u0434\u0435\u043b\u044c\u043d\u043e \u0434\u043b\u044f \u043a\u0430\u0436\u0434\u043e\u0433\u043e \u043a\u0430\u043d\u0430\u043b\u0430:<\/p>\n<pre><code class=\"cpp\">kalman_filter_init(&amp;gyro_filter_x, GYRO_PROCESS_NOISE, GYRO_MEASUREMENT_NOISE, GYRO_ESTIMATION_ERROR, GYRO_INITIAL_VALUE); kalman_filter_init(&amp;gyro_filter_y, GYRO_PROCESS_NOISE, GYRO_MEASUREMENT_NOISE, GYRO_ESTIMATION_ERROR, GYRO_INITIAL_VALUE); kalman_filter_init(&amp;gyro_filter_z, GYRO_PROCESS_NOISE, GYRO_MEASUREMENT_NOISE, GYRO_ESTIMATION_ERROR, GYRO_INITIAL_VALUE);<\/code><\/pre>\n<h4>3. \u0420\u0435\u0436\u0435\u043a\u0442\u043e\u0440\u043d\u044b\u0435 \u0444\u0438\u043b\u044c\u0442\u0440\u044b (Notch)<\/h4>\n<p>\u041d\u0430\u0441\u0442\u0440\u043e\u0435\u043d\u044b \u0434\u043b\u044f \u043f\u043e\u0434\u0430\u0432\u043b\u0435\u043d\u0438\u044f \u043a\u043e\u043d\u043a\u0440\u0435\u0442\u043d\u044b\u0445 \u0447\u0430\u0441\u0442\u043e\u0442\u043d\u044b\u0445 \u0441\u043e\u0441\u0442\u0430\u0432\u043b\u044f\u044e\u0449\u0438\u0445:<\/p>\n<pre><code class=\"cpp\">init_notch_filter(&amp;notch_ax, NOTCH_CENTER_FREQ, NOTCH_WIDTH, NOTCH_SAMPLE_RATE); init_notch_filter(&amp;notch_ay, NOTCH_CENTER_FREQ, NOTCH_WIDTH, NOTCH_SAMPLE_RATE); init_notch_filter(&amp;notch_az, NOTCH_CENTER_FREQ, NOTCH_WIDTH, NOTCH_SAMPLE_RATE);<\/code><\/pre>\n<h4>4. \u0424\u0438\u043b\u044c\u0442\u0440\u044b \u0432\u044b\u0441\u043e\u043a\u0438\u0445 \u0447\u0430\u0441\u0442\u043e\u0442 (HPF)<\/h4>\n<p>\u0418\u0441\u043f\u043e\u043b\u044c\u0437\u0443\u044e\u0442\u0441\u044f \u0434\u043b\u044f \u0443\u0441\u0442\u0440\u0430\u043d\u0435\u043d\u0438\u044f \u043f\u043e\u0441\u0442\u043e\u044f\u043d\u043d\u043e\u0439 \u0441\u043e\u0441\u0442\u0430\u0432\u043b\u044f\u044e\u0449\u0435\u0439:<\/p>\n<pre><code class=\"cpp\">HPF_Init(&amp;hpf_x, HPF_ALPHA, Gx[0]); HPF_Init(&amp;hpf_y, HPF_ALPHA, Gy[0]); HPF_Init(&amp;hpf_z, HPF_ALPHA, Gz[0]);<\/code><\/pre>\n<h4>5. \u041f\u0418\u0414-\u0440\u0435\u0433\u0443\u043b\u044f\u0442\u043e\u0440\u044b<\/h4>\n<p>\u0418\u043d\u0438\u0446\u0438\u0430\u043b\u0438\u0437\u0438\u0440\u0443\u044e\u0442\u0441\u044f \u0441 \u0437\u0430\u0434\u0430\u043d\u043d\u044b\u043c\u0438 \u043a\u043e\u044d\u0444\u0444\u0438\u0446\u0438\u0435\u043d\u0442\u0430\u043c\u0438 \u0434\u043b\u044f \u043a\u0430\u0436\u0434\u043e\u0433\u043e \u043a\u0430\u043d\u0430\u043b\u0430 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f:<\/p>\n<pre><code class=\"cpp\">PID_Init(&amp;pitch_pid, PITCH_PID_KP, PITCH_PID_KI, PITCH_PID_KD, ALPHA, ALPHA_DERIVATIVE, INTEGRAL_LIMIT, SCALE_FACTOR); PID_Init(&amp;roll_pid, ROLL_PID_KP, ROLL_PID_KI, ROLL_PID_KD, ALPHA, ALPHA_DERIVATIVE, INTEGRAL_LIMIT, SCALE_FACTOR); PID_Init(&amp;yaw_pid, YAW_PID_KP, YAW_PID_KI, YAW_PID_KD, ALPHA, ALPHA_DERIVATIVE, INTEGRAL_LIMIT, SCALE_FACTOR);<\/code><\/pre>\n<h3>\u041e\u0441\u043d\u043e\u0432\u043d\u043e\u0439 \u0446\u0438\u043a\u043b \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f<\/h3>\n<p>\u041f\u043e\u0441\u043b\u0435 \u0437\u0430\u0432\u0435\u0440\u0448\u0435\u043d\u0438\u044f \u0438\u043d\u0438\u0446\u0438\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u0438 \u043f\u0435\u0440\u0435\u0445\u043e\u0434\u0438\u043c \u0432 \u043e\u0441\u043d\u043e\u0432\u043d\u043e\u0439 \u0446\u0438\u043a\u043b, \u0433\u0434\u0435 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u0435 \u043e\u0441\u0443\u0449\u0435\u0441\u0442\u0432\u043b\u044f\u0435\u0442\u0441\u044f \u0447\u0435\u0440\u0435\u0437 \u043f\u0440\u0435\u0440\u044b\u0432\u0430\u043d\u0438\u0435 \u0442\u0430\u0439\u043c\u0435\u0440\u0430 TIM11 \u0441 \u0447\u0430\u0441\u0442\u043e\u0442\u043e\u0439 1 \u043a\u0413\u0446. \u0412 \u043e\u0431\u0440\u0430\u0431\u043e\u0442\u0447\u0438\u043a\u0435 \u043f\u0440\u0435\u0440\u044b\u0432\u0430\u043d\u0438\u044f \u0432\u044b\u0437\u044b\u0432\u0430\u0435\u0442\u0441\u044f \u0444\u0443\u043d\u043a\u0446\u0438\u044f <code>run_control_loop()<\/code>: <\/p>\n<h4>1. \u041f\u0440\u043e\u0432\u0435\u0440\u043a\u0430 \u0441\u0432\u044f\u0437\u0438 \u0441 \u043f\u0443\u043b\u044c\u0442\u043e\u043c \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f<\/h4>\n<p>check_connection_loss();<\/p>\n<pre><code class=\"cpp\">void check_connection_loss(void) {     uint32_t current_time = HAL_GetTick();     if ((current_time - last_received_time) &gt; CONNECTION_TIMEOUT_MS) {         connection_lost = true;         \/\/ \u0421\u0431\u0440\u043e\u0441 \u0443\u043f\u0440\u0430\u0432\u043b\u044f\u044e\u0449\u0438\u0445 \u0441\u0438\u0433\u043d\u0430\u043b\u043e\u0432         joystick_x = 0;         joystick_y = 0;         right_left = 0;         potentiometer_value = 0;         button = 0;         \/\/ \u0423\u0441\u0442\u0430\u043d\u043e\u0432\u043a\u0430 \u043c\u0438\u043d\u0438\u043c\u0430\u043b\u044c\u043d\u043e\u0439 \u0442\u044f\u0433\u0438 \u043d\u0430 \u043c\u043e\u0442\u043e\u0440\u0430\u0445         Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_1);         Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_2);         Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_3);         Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_4);     } }<\/code><\/pre>\n<p>\u041f\u043e\u043b\u0443\u0447\u0430\u0435\u0442 \u0442\u0435\u043a\u0443\u0449\u0435\u0435 \u0432\u0440\u0435\u043c\u044f \u0441\u0438\u0441\u0442\u0435\u043c\u043d\u043e\u0433\u043e \u0442\u0430\u0439\u043c\u0435\u0440\u0430. \u041f\u0440\u043e\u0432\u0435\u0440\u044f\u0435\u0442, \u043f\u0440\u0435\u0432\u044b\u0448\u0430\u0435\u0442 \u043b\u0438 \u0432\u0440\u0435\u043c\u044f \u0441 \u043f\u043e\u0441\u043b\u0435\u0434\u043d\u0435\u0433\u043e \u0443\u0441\u043f\u0435\u0448\u043d\u043e\u0433\u043e \u043f\u0440\u0438\u0435\u043c\u0430 \u0434\u0430\u043d\u043d\u044b\u0445 \u0437\u0430\u0434\u0430\u043d\u043d\u044b\u0439 \u0442\u0430\u0439\u043c\u0430\u0443\u0442.  \u041f\u0440\u0438 \u043f\u043e\u0442\u0435\u0440\u0435 \u0441\u0432\u044f\u0437\u0438 \u0443\u0441\u0442\u0430\u043d\u0430\u0432\u043b\u0438\u0432\u0430\u0435\u0442 \u0444\u043b\u0430\u0433 <code>connection_lost,<\/code> \u0441\u0431\u0440\u0430\u0441\u044b\u0432\u0430\u0435\u0442 \u0432\u0441\u0435 \u0443\u043f\u0440\u0430\u0432\u043b\u044f\u044e\u0449\u0438\u0435 \u0441\u0438\u0433\u043d\u0430\u043b\u044b \u0432 \u043d\u043e\u043b\u044c,  \u0443\u0441\u0442\u0430\u043d\u0430\u0432\u043b\u0438\u0432\u0430\u0435\u0442 \u043c\u0438\u043d\u0438\u043c\u0430\u043b\u044c\u043d\u044b\u0435 \u0438\u043c\u043f\u0443\u043b\u044c\u0441\u044b \u043d\u0430 \u0432\u0441\u0435\u0445 \u043c\u043e\u0442\u043e\u0440\u0430\u0445.<\/p>\n<h4>2. \u041f\u0440\u0438\u0435\u043c \u0441\u0438\u0433\u043d\u0430\u043b\u0430 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f<\/h4>\n<pre><code class=\"cpp\">void receive_and_parse_data(int data_size) {     calculated_checksum = 0;          \/\/ \u041f\u0440\u043e\u0432\u0435\u0440\u043a\u0430 \u043c\u0438\u043d\u0438\u043c\u0430\u043b\u044c\u043d\u043e\u0433\u043e \u0440\u0430\u0437\u043c\u0435\u0440\u0430 \u043f\u0430\u043a\u0435\u0442\u0430     if (data_size &lt; 4) {         return; \/\/ \u041d\u0435\u0434\u043e\u0441\u0442\u0430\u0442\u043e\u0447\u043d\u043e \u0434\u0430\u043d\u043d\u044b\u0445 \u0434\u043b\u044f \u0440\u0430\u0437\u0431\u043e\u0440\u0430     }          \/\/ \u0412\u044b\u0447\u0438\u0441\u043b\u0435\u043d\u0438\u0435 \u043a\u043e\u043d\u0442\u0440\u043e\u043b\u044c\u043d\u043e\u0439 \u0441\u0443\u043c\u043c\u044b     for (uint8_t i = 0; i &lt; data_size - 2; i++) {         calculated_checksum += buffer_message[i];     }          \/\/ \u041f\u043e\u043b\u0443\u0447\u0435\u043d\u0438\u0435 \u043a\u043e\u043d\u0442\u0440\u043e\u043b\u044c\u043d\u043e\u0439 \u0441\u0443\u043c\u043c\u044b \u0438\u0437 \u043f\u0430\u043a\u0435\u0442\u0430     received_checksum = (buffer_message[data_size - 2] &lt;&lt; 8) | buffer_message[data_size - 1];          \/\/ \u041f\u0440\u043e\u0432\u0435\u0440\u043a\u0430 \u043a\u043e\u043d\u0442\u0440\u043e\u043b\u044c\u043d\u043e\u0439 \u0441\u0443\u043c\u043c\u044b     if (received_checksum == calculated_checksum) {         \/\/ \u041e\u0431\u043d\u043e\u0432\u043b\u0435\u043d\u0438\u0435 \u0432\u0440\u0435\u043c\u0435\u043d\u0438 \u043f\u043e\u0441\u043b\u0435\u0434\u043d\u0435\u0433\u043e \u0443\u0441\u043f\u0435\u0448\u043d\u043e\u0433\u043e \u043f\u043e\u043b\u0443\u0447\u0435\u043d\u0438\u044f \u0434\u0430\u043d\u043d\u044b\u0445         last_received_time = HAL_GetTick();         connection_lost = false;                  \/\/ \u0420\u0430\u0437\u0431\u043e\u0440 \u0434\u0430\u043d\u043d\u044b\u0445 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f:         joystick_x = (int16_t)((buffer_message[0] &lt;&lt; 8) | buffer_message[1];         joystick_y = (int16_t)((buffer_message[2] &lt;&lt; 8) | buffer_message[3];         potentiometer_value = (buffer_message[4] &lt;&lt; 8) | buffer_message[5];         right_left = (int16_t)((buffer_message[6] &lt;&lt; 8) | buffer_message[7];         button = (buffer_message[8] &lt;&lt; 8) | buffer_message[9];     } }<\/code><\/pre>\n<p>  \u041f\u0430\u043a\u0435\u0442 \u0434\u0430\u043d\u043d\u044b\u0445 \u0438\u043c\u0435\u0435\u0442 \u0441\u043b\u0435\u0434\u0443\u044e\u0449\u0443\u044e \u0441\u0442\u0440\u0443\u043a\u0442\u0443\u0440\u0443 (12 \u0431\u0430\u0439\u0442):<\/p>\n<div>\n<div class=\"table\">\n<table>\n<tbody>\n<tr>\n<th>\n<p align=\"left\">\u041f\u043e\u0437\u0438\u0446\u0438\u044f<\/p>\n<\/th>\n<th>\n<p align=\"left\">\u0414\u0430\u043d\u043d\u044b\u0435<\/p>\n<\/th>\n<th>\n<p align=\"left\">\u0420\u0430\u0437\u043c\u0435\u0440<\/p>\n<\/th>\n<th>\n<p align=\"left\">\u041e\u043f\u0438\u0441\u0430\u043d\u0438\u0435<\/p>\n<\/th>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">0-1<\/p>\n<\/td>\n<td>\n<p align=\"left\">joystick_x<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u041f\u043e\u043b\u043e\u0436\u0435\u043d\u0438\u0435 \u0434\u0436\u043e\u0439\u0441\u0442\u0438\u043a\u0430 \u043f\u043e X<\/p>\n<\/td>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">2-3<\/p>\n<\/td>\n<td>\n<p align=\"left\">joystick_y<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u041f\u043e\u043b\u043e\u0436\u0435\u043d\u0438\u0435 \u0434\u0436\u043e\u0439\u0441\u0442\u0438\u043a\u0430 \u043f\u043e Y<\/p>\n<\/td>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">4-5<\/p>\n<\/td>\n<td>\n<p align=\"left\">potentiometer<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u0417\u043d\u0430\u0447\u0435\u043d\u0438\u0435 \u043f\u043e\u0442\u0435\u043d\u0446\u0438\u043e\u043c\u0435\u0442\u0440\u0430<\/p>\n<\/td>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">6-7<\/p>\n<\/td>\n<td>\n<p align=\"left\">right_left<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u0423\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u0435 \u0440\u044b\u0441\u043a\u0430\u043d\u0438\u0435\u043c<\/p>\n<\/td>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">8-9<\/p>\n<\/td>\n<td>\n<p align=\"left\">button<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u0421\u043e\u0441\u0442\u043e\u044f\u043d\u0438\u0435 \u043a\u043d\u043e\u043f\u043a\u0438<\/p>\n<\/td>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">10-11<\/p>\n<\/td>\n<td>\n<p align=\"left\">checksum<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u041a\u043e\u043d\u0442\u0440\u043e\u043b\u044c\u043d\u0430\u044f \u0441\u0443\u043c\u043c\u0430<\/p>\n<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<\/div>\n<\/div>\n<h4>3. \u041f\u043e\u043b\u0443\u0447\u0435\u043d\u0438\u0435 \u0438 \u043e\u0431\u0440\u0430\u0431\u043e\u0442\u043a\u0430 \u0434\u0430\u043d\u043d\u044b\u0445 \u0441 IMU<\/h4>\n<p>\u0424\u0443\u043d\u043a\u0446\u0438\u044f get_filter_data_accel();<\/p>\n<pre><code class=\"cpp\">void get_filter_data_accel() {     BMX055_Read_Accel(&amp;hi2c1, &amp;BMX055);          Ax[0] = round_to(BMX055.Ax);     Ax[0] = Ax[0] - offset_Ax;     Ax[0] = apply_notch_filter(&amp;notch_ax, Ax[0]);     arm_biquad_cascade_df2T_f32(&amp;lpf_Ax, Ax, filter_Ax, BLOCK_SIZE);     filter_Ax[0] = kalman_filter_update(&amp;accel_filter_x, filter_Ax[0]);          \/\/ \u0410\u043d\u0430\u043b\u043e\u0433\u0438\u0447\u043d\u043e \u0434\u043b\u044f Ay \u0438 Az     ... }<\/code><\/pre>\n<p>\u042d\u0442\u0430\u043f\u044b \u043e\u0431\u0440\u0430\u0431\u043e\u0442\u043a\u0438:<\/p>\n<ol>\n<li>\n<p>\u0427\u0442\u0435\u043d\u0438\u0435 \u0441\u044b\u0440\u044b\u0445 \u0434\u0430\u043d\u043d\u044b\u0445 \u0441 \u0430\u043a\u0441\u0435\u043b\u0435\u0440\u043e\u043c\u0435\u0442\u0440\u0430 \u0447\u0435\u0440\u0435\u0437 I2C<\/p>\n<\/li>\n<li>\n<p>\u041e\u043a\u0440\u0443\u0433\u043b\u0435\u043d\u0438\u0435 \u0437\u043d\u0430\u0447\u0435\u043d\u0438\u0439 \u0434\u043e 2 \u0437\u043d\u0430\u043a\u043e\u0432 \u043f\u043e\u0441\u043b\u0435 \u0437\u0430\u043f\u044f\u0442\u043e\u0439<\/p>\n<\/li>\n<li>\n<p>\u0412\u044b\u0447\u0438\u0442\u0430\u043d\u0438\u0435 \u043a\u0430\u043b\u0438\u0431\u0440\u043e\u0432\u043e\u0447\u043d\u043e\u0433\u043e \u0441\u043c\u0435\u0449\u0435\u043d\u0438\u044f<\/p>\n<\/li>\n<li>\n<p>\u041f\u043e\u0434\u0430\u0432\u043b\u0435\u043d\u0438\u0435 \u0443\u0437\u043a\u043e\u043f\u043e\u043b\u043e\u0441\u043d\u044b\u0445 \u043f\u043e\u043c\u0435\u0445 \u0440\u0435\u0436\u0435\u043a\u0442\u043e\u0440\u043d\u044b\u043c \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u043c<\/p>\n<\/li>\n<li>\n<p>\u0424\u0438\u043b\u044c\u0442\u0440\u0430\u0446\u0438\u044f \u043d\u0438\u0437\u043a\u0438\u0445 \u0447\u0430\u0441\u0442\u043e\u0442 \u0431\u0438\u043a\u0432\u0430\u0434\u0440\u0430\u0442\u043d\u044b\u043c \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u043c<\/p>\n<\/li>\n<li>\n<p>\u041e\u043a\u043e\u043d\u0447\u0430\u0442\u0435\u043b\u044c\u043d\u0430\u044f \u0444\u0438\u043b\u044c\u0442\u0440\u0430\u0446\u0438\u044f \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u043c \u041a\u0430\u043b\u043c\u0430\u043d\u0430<\/p>\n<\/li>\n<\/ol>\n<p>\u0424\u0443\u043d\u043a\u0446\u0438\u044f get_filter_data_gyro();<\/p>\n<pre><code class=\"cpp\">void get_filter_data_gyro() {     BMX055_Read_Gyro(&amp;hi2c1, &amp;BMX055);          Gx[0] = round_to(BMX055.Gx);     Gx[0] = Gx[0] - bias_Gx;     Gx[0] = apply_notch_filter(&amp;notch_gx, Gx[0]);     Gx[0] = HPF_Update(&amp;hpf_x, Gx[0]);     arm_biquad_cascade_df2T_f32(&amp;lpf_Gx, Gx, filter_Gx, BLOCK_SIZE);     filter_Gx[0] = kalman_filter_update(&amp;gyro_filter_x, filter_Gx[0]);          \/\/ \u0410\u043d\u0430\u043b\u043e\u0433\u0438\u0447\u043d\u043e \u0434\u043b\u044f Gy \u0438 Gz     ... }<\/code><\/pre>\n<ol>\n<li>\n<p>\u0414\u043e\u043f\u043e\u043b\u043d\u0438\u0442\u0435\u043b\u044c\u043d\u044b\u0439 \u044d\u0442\u0430\u043f &#8212; \u0444\u0438\u043b\u044c\u0442\u0440 \u0432\u044b\u0441\u043e\u043a\u0438\u0445 \u0447\u0430\u0441\u0442\u043e\u0442 \u0434\u043b\u044f \u0443\u0441\u0442\u0440\u0430\u043d\u0435\u043d\u0438\u044f \u0434\u0440\u0435\u0439\u0444\u0430<\/p>\n<\/li>\n<li>\n<p>\u0418\u0441\u043f\u043e\u043b\u044c\u0437\u0443\u044e\u0442\u0441\u044f \u043e\u0442\u0434\u0435\u043b\u044c\u043d\u044b\u0435 \u043a\u0430\u043b\u0438\u0431\u0440\u043e\u0432\u043e\u0447\u043d\u044b\u0435 \u043a\u043e\u044d\u0444\u0444\u0438\u0446\u0438\u0435\u043d\u0442\u044b (<code>bias_Gx<\/code> \u0432\u043c\u0435\u0441\u0442\u043e <code>offset_Ax<\/code>)<\/p>\n<\/li>\n<\/ol>\n<h4>4. \u0420\u0430\u0441\u0447\u0435\u0442 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438<\/h4>\n<p>\u0424\u0443\u043d\u043a\u0446\u0438\u044f get_angle_mahony(); <\/p>\n<pre><code class=\"cpp\">void get_angle_mahony(void) {     \/\/ \u041f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u043e\u0432\u0430\u043d\u0438\u0435 \u0435\u0434\u0438\u043d\u0438\u0446 \u0438\u0437\u043c\u0435\u0440\u0435\u043d\u0438\u044f     float filterAx = filter_Ax[0]*9.81; \/\/ \u043c\/\u0441\u00b2     float filterGx = filter_Gx[0]*DEG_TO_RAD; \/\/ \u0440\u0430\u0434\/\u0441          \/\/ \u041e\u0431\u043d\u043e\u0432\u043b\u0435\u043d\u0438\u0435 \u0430\u043b\u0433\u043e\u0440\u0438\u0442\u043c\u0430 Mahony     MahonyAHRSupdateIMU(filterAx, filterAy, filterAz,                         filterGx, filterGy, filterGz,                         CONTROL_LOOP_DT);          \/\/ \u041f\u043e\u043b\u0443\u0447\u0435\u043d\u0438\u0435 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438     Quat_actual[0] = (*(getQ()));     ...          \/\/ \u041a\u043e\u043c\u043f\u043b\u0435\u043c\u0435\u043d\u0442\u0430\u0440\u043d\u0430\u044f \u0444\u0438\u043b\u044c\u0442\u0440\u0430\u0446\u0438\u044f \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430     float alpha = 0.5;     float dot = Quat_previous[0]*Quat_actual[0] + ...;     if (dot &lt; 0) {         \/\/ \u0418\u043d\u0432\u0435\u0440\u0442\u0438\u0440\u043e\u0432\u0430\u043d\u0438\u0435 \u043f\u0440\u0438 \u043e\u0442\u0440\u0438\u0446\u0430\u0442\u0435\u043b\u044c\u043d\u043e\u043c \u0441\u043a\u0430\u043b\u044f\u0440\u043d\u043e\u043c \u043f\u0440\u043e\u0438\u0437\u0432\u0435\u0434\u0435\u043d\u0438\u0438         Quat_actual[0] = alpha * (-Quat_actual[0]) + ...;         ...     }          \/\/ \u041d\u043e\u0440\u043c\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u044f \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430     normalizeQuaternion(Quat_actual);          \/\/ \u041f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u043e\u0432\u0430\u043d\u0438\u0435 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430 \u0432 \u0443\u0433\u043b\u044b \u042d\u0439\u043b\u0435\u0440\u0430     float q0 = Quat_actual[0];     ...     pitch = asinf(2.0f * (q1q3 - q0q2)) * RAD_TO_DEG;     roll = atan2f(2.0f * (q0q1 + q2q3), q0q0 - q1q1 - q2q2 + q3q3) * RAD_TO_DEG;     yaw = atan2f(2.0f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3) * RAD_TO_DEG;          \/\/ \u0424\u0438\u043b\u044c\u0442\u0440\u0430\u0446\u0438\u044f \u0443\u0433\u043b\u043e\u0432     roll = apply_notch_filter(&amp;notch_roll, roll);     ... }<\/code><\/pre>\n<h3>\u041e\u043f\u0438\u0441\u0430\u043d\u0438\u0435 \u0440\u0430\u0441\u0447\u0435\u0442\u0430 \u043e\u0448\u0438\u0431\u043a\u0438 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438.<\/h3>\n<p>\u0420\u0430\u0441\u0447\u0435\u0442 \u043e\u0448\u0438\u0431\u043a\u0438 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438 \u043c\u0435\u0436\u0434\u0443 \u0442\u0435\u043a\u0443\u0449\u0438\u043c \u043f\u043e\u043b\u043e\u0436\u0435\u043d\u0438\u0435\u043c \u043a\u0432\u0430\u0434\u0440\u043e\u043a\u043e\u043f\u0442\u0435\u0440\u0430 \u0438 \u0446\u0435\u043b\u0435\u0432\u044b\u043c \u043f\u043e\u043b\u043e\u0436\u0435\u043d\u0438\u0435\u043c \u0437\u0430\u0434\u0430\u043d\u043d\u044b\u043c \u0441 \u043f\u0443\u043b\u044c\u0442\u0430 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f \u0432\u044b\u043f\u043e\u043b\u043d\u044f\u0435\u0442\u0441\u044f \u0441 \u0438\u0441\u043f\u043e\u043b\u044c\u0437\u043e\u0432\u0430\u043d\u0438\u0435\u043c \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u043e\u0432 \u0438 \u0432\u043a\u043b\u044e\u0447\u0430\u0435\u0442 \u043d\u0435\u0441\u043a\u043e\u043b\u044c\u043a\u043e \u044d\u0442\u0430\u043f\u043e\u0432. <\/p>\n<h4>\u041f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u043e\u0432\u0430\u043d\u0438\u0435 \u043a\u043e\u043c\u0430\u043d\u0434 \u0441 \u043f\u0443\u043b\u044c\u0442\u0430 \u0432 \u0446\u0435\u043b\u0435\u0432\u043e\u0439 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d<\/h4>\n<p> \u0424\u0443\u043d\u043a\u0446\u0438\u044f <code>eulerToQuaternion()<\/code> \u043f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u0443\u0435\u0442 \u0441\u0438\u0433\u043d\u0430\u043b\u044b \u0434\u0436\u043e\u0439\u0441\u0442\u0438\u043a\u0430 (\u0432 \u0434\u0438\u0430\u043f\u0430\u0437\u043e\u043d\u0435 -100..100) \u0432 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d \u0446\u0435\u043b\u0435\u0432\u043e\u0439 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438.<\/p>\n<pre><code class=\"cpp\">void eulerToQuaternion(int joystick_x, int joystick_y, int right_left, float q[4]) {     \/\/ \u041d\u043e\u0440\u043c\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u044f \u0432\u0445\u043e\u0434\u043d\u044b\u0445 \u0437\u043d\u0430\u0447\u0435\u043d\u0438\u0439 \u043a \u0434\u0438\u0430\u043f\u0430\u0437\u043e\u043d\u0443 [-1, 1] \u0438 \u043f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u043e\u0432\u0430\u043d\u0438\u0435 \u0432 \u0440\u0430\u0434\u0438\u0430\u043d\u044b     float roll = (joystick_y \/ 100.0f) * M_PI;   \/\/ \u041e\u0441\u044c X (\u043a\u0440\u0435\u043d)     float pitch = (joystick_x \/ 100.0f) * M_PI;  \/\/ \u041e\u0441\u044c Y (\u0442\u0430\u043d\u0433\u0430\u0436)     float yaw = (right_left \/ 100.0f) * M_PI;    \/\/ \u041e\u0441\u044c Z (\u0440\u044b\u0441\u043a\u0430\u043d\u0438\u0435)      \/\/ \u0412\u044b\u0447\u0438\u0441\u043b\u0435\u043d\u0438\u0435 \u043f\u043e\u043b\u043e\u0432\u0438\u043d\u043d\u044b\u0445 \u0443\u0433\u043b\u043e\u0432 \u0434\u043b\u044f \u043e\u043f\u0442\u0438\u043c\u0438\u0437\u0430\u0446\u0438\u0438     float cy = cos(yaw * 0.5f);     float sy = sin(yaw * 0.5f);     float cp = cos(pitch * 0.5f);     float sp = sin(pitch * 0.5f);     float cr = cos(roll * 0.5f);     float sr = sin(roll * 0.5f);      \/\/ \u0424\u043e\u0440\u043c\u0438\u0440\u043e\u0432\u0430\u043d\u0438\u0435 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430     q[0] = cr * cp * cy + sr * sp * sy; \/\/ \u0412\u0435\u0449\u0435\u0441\u0442\u0432\u0435\u043d\u043d\u0430\u044f \u0447\u0430\u0441\u0442\u044c (w)     q[1] = sr * cp * cy - cr * sp * sy; \/\/ \u041c\u043d\u0438\u043c\u0430\u044f \u0447\u0430\u0441\u0442\u044c (x)     q[2] = cr * sp * cy + sr * cp * sy; \/\/ \u041c\u043d\u0438\u043c\u0430\u044f \u0447\u0430\u0441\u0442\u044c (y)     q[3] = cr * cp * sy - sr * sp * cy; \/\/ \u041c\u043d\u0438\u043c\u0430\u044f \u0447\u0430\u0441\u0442\u044c (z)      normalizeQuaternion(q); \/\/ \u041d\u043e\u0440\u043c\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u044f \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430 }<\/code><\/pre>\n<p><strong>\u0420\u0430\u0441\u0447\u0435\u0442 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430 \u043e\u0448\u0438\u0431\u043a\u0438<\/strong><\/p>\n<p>\u0424\u0443\u043d\u043a\u0446\u0438\u044f \u043e\u0431\u044a\u0435\u0434\u0438\u043d\u044f\u044e\u0449\u0430\u044f \u043e\u043f\u0435\u0440\u0430\u0446\u0438\u0438 \u0440\u0430\u0441\u0447\u0435\u0442\u0430 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430 \u043e\u0448\u0438\u0431\u043a\u0438.<\/p>\n<pre><code class=\"cpp\">void quat_error(float q_actual[4], float q_actual_inv[4],                 float q_target[4], float q_error[4]) {     \/\/ \u0428\u0430\u0433 1: \u0412\u044b\u0447\u0438\u0441\u043b\u0435\u043d\u0438\u0435 \u043e\u0431\u0440\u0430\u0442\u043d\u043e\u0433\u043e \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430 \u0442\u0435\u043a\u0443\u0449\u0435\u0439 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438     quat_inverse(q_actual, q_actual_inv);          \/\/ \u0428\u0430\u0433 2: \u0423\u043c\u043d\u043e\u0436\u0435\u043d\u0438\u0435 \u0446\u0435\u043b\u0435\u0432\u043e\u0433\u043e \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430 \u043d\u0430 \u043e\u0431\u0440\u0430\u0442\u043d\u044b\u0439 \u0442\u0435\u043a\u0443\u0449\u0438\u0439     \/\/ q_error = q_target \u2297 q_actual^-1     quat_multiply(q_target, q_actual_inv, q_error);          \/\/ \u0428\u0430\u0433 3: \u041d\u043e\u0440\u043c\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u044f \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430 \u043e\u0448\u0438\u0431\u043a\u0438     normalizeQuaternion(q_error); }<\/code><\/pre>\n<p>\u0414\u043b\u044f \u0440\u0430\u0441\u0447\u0435\u0442\u0430 \u043e\u0448\u0438\u0431\u043a\u0438 \u043d\u0435\u043e\u0431\u0445\u043e\u0434\u0438\u043c\u043e \u043d\u0430\u0439\u0442\u0438 \u043e\u0431\u0440\u0430\u0442\u043d\u044b\u0439 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d \u0442\u0435\u043a\u0443\u0449\u0435\u0439 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438.<\/p>\n<pre><code class=\"cpp\">void quat_inverse(float quat[4], float quat_inv[4]) {     quat_inv[0] = quat[0];   \/\/ w' = w     quat_inv[1] = -quat[1];  \/\/ x' = -x     quat_inv[2] = -quat[2];  \/\/ y' = -y     quat_inv[3] = -quat[3];  \/\/ z' = -z     \/\/ \u041d\u043e\u0440\u043c\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u044f \u043d\u0435 \u0442\u0440\u0435\u0431\u0443\u0435\u0442\u0441\u044f, \u0435\u0441\u043b\u0438 \u0432\u0445\u043e\u0434\u043d\u043e\u0439 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d \u043d\u043e\u0440\u043c\u0430\u043b\u0438\u0437\u043e\u0432\u0430\u043d }<\/code><\/pre>\n<p>  \u041e\u0441\u043d\u043e\u0432\u043d\u0430\u044f \u043e\u043f\u0435\u0440\u0430\u0446\u0438\u044f \u0434\u043b\u044f \u0440\u0430\u0441\u0447\u0435\u0442\u0430 \u043e\u0448\u0438\u0431\u043a\u0438 &#8212; \u0443\u043c\u043d\u043e\u0436\u0435\u043d\u0438\u0435 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u043e\u0432.<\/p>\n<pre><code class=\"cpp\">void quat_multiply(float q1[4], float q2[4], float result[4]) {     result[0] = q1[0]*q2[0] - q1[1]*q2[1] - q1[2]*q2[2] - q1[3]*q2[3]; \/\/ w     result[1] = q1[0]*q2[1] + q1[1]*q2[0] + q1[2]*q2[3] - q1[3]*q2[2]; \/\/ x     result[2] = q1[0]*q2[2] - q1[1]*q2[3] + q1[2]*q2[0] + q1[3]*q2[1]; \/\/ y     result[3] = q1[0]*q2[3] + q1[1]*q2[2] - q1[2]*q2[1] + q1[3]*q2[0]; \/\/ z }<\/code><\/pre>\n<p> \u0412 \u0444\u043e\u0440\u043c\u0438\u0440\u043e\u0432\u0430\u043d\u0438\u0438 \u0443\u043f\u0440\u0430\u0432\u043b\u044f\u044e\u0449\u0438\u0445 \u0441\u0438\u0433\u043d\u0430\u043b\u043e\u0432 \u0438\u0441\u043f\u043e\u043b\u044c\u0437\u0443\u0435\u0442\u0441\u044f \u0432\u0435\u043a\u0442\u043e\u0440\u043d\u0430\u044f \u0447\u0430\u0441\u0442\u044c \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430.<\/p>\n<pre><code class=\"cpp\">double error_pitch = Quat_error[2] * (-1); \/\/ \u041e\u0441\u044c Y double error_roll = Quat_error[1];         \/\/ \u041e\u0441\u044c X double error_yaw = Quat_error[3];         \/\/ \u041e\u0441\u044c Z  forse_pitch = PID_Compute(&amp;pitch_pid, error_pitch, TRIM_PITCH_ERROR); forse_roll = PID_Compute(&amp;roll_pid, error_roll, TRIM_ROLL_ERROR); forse_yaw = PID_Compute(&amp;yaw_pid, error_yaw, TRIM_YAW_ERROR);<\/code><\/pre>\n<p>\u0420\u0430\u0441\u0447\u0435\u0442 \u0442\u044f\u0433\u0438 \u0434\u043b\u044f \u043c\u043e\u0442\u043e\u0440\u043e\u0432.<\/p>\n<pre><code class=\"cpp\">if(button == 1 &amp;&amp; potentiometer_value &gt; 0) {     \/\/ \u041f\u0435\u0440\u0435\u0434\u043d\u0438\u0439 \u043b\u0435\u0432\u044b\u0439 \u043c\u043e\u0442\u043e\u0440     float pid_correction_1 = forse_roll + forse_pitch + forse_yaw;     pid_correction_1 = constrain_float(pid_correction_1, -max_pid_correction_mshot, max_pid_correction_mshot);     int total_power_1 = throttle_mshot + pid_correction_1 + TRIM_FRONT_LEFT;     total_power_1 = constrain(total_power_1, MIN_PULSE_WIDTH, MAX_PULSE_WIDTH);          \/\/ \u041e\u0441\u0442\u0430\u043b\u044c\u043d\u044b\u0435 \u043c\u043e\u0442\u043e\u0440\u044b (\u0430\u043d\u0430\u043b\u043e\u0433\u0438\u0447\u043d\u043e \u0441 \u0440\u0430\u0437\u043d\u044b\u043c\u0438 \u0437\u043d\u0430\u043a\u0430\u043c\u0438 \u043a\u043e\u0440\u0440\u0435\u043a\u0446\u0438\u0438)     ...          \/\/ \u0423\u0441\u0442\u0430\u043d\u043e\u0432\u043a\u0430 \u0442\u044f\u0433\u0438     Multishot_SetThrottle(total_power_1, TIM_CHANNEL_1);     ... } else {     \/\/ \u0420\u0435\u0436\u0438\u043c \u0431\u0435\u0437\u043e\u043f\u0430\u0441\u043d\u043e\u0441\u0442\u0438     Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_1);     ... }<\/code><\/pre>\n<h3>\u0412\u044b\u0432\u043e\u0434<\/h3>\n<p>\u0426\u0435\u043b\u044c\u044e \u043d\u0430\u043f\u0438\u0441\u0430\u043d\u0438\u044f \u0431\u044b\u043b \u0438\u043d\u0442\u0435\u0440\u0435\u0441 \u0438 \u0436\u0435\u043b\u0430\u043d\u0438\u0435 \u0441\u043e\u0437\u0434\u0430\u0442\u044c \u043c\u0438\u043d\u0438\u043c\u0430\u043b\u044c\u043d\u043e \u043d\u0435\u043e\u0431\u0445\u043e\u0434\u0438\u043c\u044b\u0439 \u0434\u043b\u044f \u0440\u0430\u0431\u043e\u0442\u044b \u043a\u0432\u0430\u0434\u0440\u043e\u043a\u043e\u043f\u0442\u0435\u0440\u0430 \u043a\u043e\u0434. \u0418 \u044f \u043d\u0435 \u0434\u043e\u0432\u043e\u043b\u0435\u043d \u0440\u0435\u0437\u0443\u043b\u044c\u0442\u0430\u0442\u043e\u043c. \u0418\u0442\u043e\u0433\u043e\u0432\u044b\u0439 \u0432\u0430\u0440\u0438\u0430\u043d\u0442 \u043b\u0435\u0442\u0430\u0435\u0442 \u043d\u0430 \u0442\u0432\u0435\u0440\u0434\u0443\u044e 2 \u0438\u0437 5. \u0414\u043e\u0431\u0438\u0442\u044c\u0441\u044f \u0445\u043e\u0442\u044f \u0431\u044b \u0432\u043c\u0435\u043d\u044f\u0435\u043c\u043e\u0433\u043e \u0437\u0430\u0432\u0438\u0441\u0430\u043d\u0438\u044f \u043d\u0430 \u043c\u0435\u0441\u0442\u0435 \u043d\u0435 \u043f\u043e\u043b\u0443\u0447\u0438\u043b\u043e\u0441\u044c. \u041f\u0440\u0438\u0447\u0438\u043d\u0430 \u0441\u043a\u043e\u0440\u0435\u0435 \u0432\u0441\u0435\u0433\u043e \u0432 \u043d\u0435\u043f\u0440\u0430\u0432\u0438\u043b\u044c\u043d\u043e\u0439 \u0440\u0430\u0431\u043e\u0442\u0435 \u0441 \u0434\u0430\u043d\u043d\u044b\u043c\u0438 IMU. \u0410 \u0438\u043c\u0435\u043d\u043d\u043e \u0432 \u043d\u0435\u0432\u0435\u0440\u043d\u043e\u0439 \u043d\u0430\u0441\u0442\u0440\u043e\u0439\u043a\u0435 \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u0432, \u043d\u0430\u043f\u0440\u0438\u043c\u0435\u0440 \u043a\u0430\u043a \u0442\u043e\u043b\u044c\u043a\u043e \u044f \u0441\u043e\u0437\u0434\u0430\u0432\u0430\u043b \u0444\u0438\u043b\u044c\u0442\u0440 arm_biquad_cascade_df2T_init_f32 \u0432\u044b\u0448\u0435 2 \u043f\u043e\u0440\u044f\u0434\u043a\u0430 \u043e\u043d \u0442\u0443\u0442 \u0436\u0435 \u043f\u0435\u0440\u0435\u0441\u0442\u0430\u0432\u0430\u043b \u0440\u0430\u0431\u043e\u0442\u0430\u0442\u044c. \u041d\u0435\u0432\u0435\u0440\u043d\u044b\u0439 \u043f\u043e\u0434\u0431\u043e\u0440 \u0447\u0430\u0441\u0442\u043e\u0442 \u0434\u0430\u043d\u043d\u044b\u0445 \u0438 \u0447\u0430\u0441\u0442\u043e\u0442 \u0441\u0440\u0435\u0437\u0430. \u041d\u0435\u0434\u043e\u0441\u0442\u0440\u043e\u0438\u043b \u041f\u0418\u0414 \u0440\u0435\u0433\u0443\u043b\u044f\u0442\u043e\u0440 \u043d\u0443 \u0438 \u0432\u043e\u0437\u043c\u043e\u0436\u043d\u043e \u0434\u0435\u043b\u043e \u0432 ESC \u0438 \u043c\u043e\u0442\u043e\u0440\u0430\u0445 \u0438\u0431\u043e \u0431\u0440\u0430\u043b \u0438\u0445 \u043e\u0434\u043d\u0438\u043c \u043a\u043e\u043c\u043f\u043b\u0435\u043a\u0442\u043e\u043c \u0441\u0430\u043c\u044b\u0435 \u0434\u0435\u0448\u0435\u0432\u044b\u0435.<\/p>\n<p>\u041a\u043e\u0434 \u043d\u0430 GitHub.<\/p>\n<div class=\"embed_link\">\n<div class=\"embed__thumb\" style=\"background-image: url(&quot;https:\/\/opengraph.githubassets.com\/33c851597c49fbeb96a0ceab00d14f5ad234a9c61d636134ab0583cdb8f0f098\/DGray3232\/Controller&quot;);\"><\/div>\n<div class=\"embed__caption\">\n<div class=\"embed__caption-title\"><span>GitHub &#8212; DGray3232\/Controller<\/span><\/div>\n<p><a href=\"https:\/\/github.com\/DGray3232\/Controller\" target=\"_blank\" rel=\"noopener noreferrer nofollow\" class=\"embed__caption-host\">github.com<\/a><\/div>\n<\/div>\n<p><strong>\u0418\u0441\u0442\u043e\u0447\u043d\u0438\u043a\u0438.<\/strong><\/p>\n<div class=\"embed_link\">\n<div class=\"embed__thumb\" style=\"background-image: url(&quot;https:\/\/opengraph.githubassets.com\/709e4f539b2f7718536189adabba1325eeb8ff290c0293fd2ae7e36a6a81a492\/xioTechnologies\/Fusion&quot;);\"><\/div>\n<div class=\"embed__caption\">\n<div class=\"embed__caption-title\"><span>GitHub &#8212; xioTechnologies\/Fusion<\/span><\/div>\n<p><a href=\"https:\/\/github.com\/xioTechnologies\/Fusion\" target=\"_blank\" rel=\"noopener noreferrer nofollow\" class=\"embed__caption-host\">github.com<\/a><\/div>\n<\/div>\n<div class=\"embed_link\">\n<div class=\"embed__thumb\" style=\"background-image: url(&quot;https:\/\/opengraph.githubassets.com\/8ca4233443118b7a019c69697cd50be4d5d2ec1663b219adf28434292e52bd39\/dccharacter\/AHRS&quot;);\"><\/div>\n<div class=\"embed__caption\">\n<div class=\"embed__caption-title\"><span>GitHub &#8212; dccharacter\/AHRS<\/span><\/div>\n<p><a href=\"https:\/\/github.com\/dccharacter\/AHRS\" target=\"_blank\" rel=\"noopener noreferrer nofollow\" class=\"embed__caption-host\">github.com<\/a><\/div>\n<\/div>\n<div class=\"embed_link\">\n<div class=\"embed__thumb\" style=\"background-image: url(&quot;https:\/\/opengraph.githubassets.com\/4edfb31f87dc196447e0d98b304d5436d1a3f814bb8482cdb0d4f54c599b1aab\/tbrec3\/stm32-adaptive-notch&quot;);\"><\/div>\n<div class=\"embed__caption\">\n<div class=\"embed__caption-title\"><span>GitHub &#8212; tbrec3\/stm32-adaptive-notch: A real-time implementation of an adaptive notch filter using DMA and interrupts on an STM32G4 board<\/span><\/div>\n<p><a href=\"https:\/\/github.com\/tbrec3\/stm32-adaptive-notch\" target=\"_blank\" rel=\"noopener noreferrer nofollow\" class=\"embed__caption-host\">github.com<\/a><\/div>\n<\/div>\n<div class=\"embed_link\">\n<div class=\"embed__thumb\" style=\"background-image: url(&quot;https:\/\/opengraph.githubassets.com\/6bc89d4dffba68c178a2246664205489424172840d63e0d050276e4eb607c88e\/thanapathkmu\/KalmanFilter_STM32&quot;);\"><\/div>\n<div class=\"embed__caption\">\n<div class=\"embed__caption-title\"><span>GitHub &#8212; thanapathkmu\/KalmanFilter_STM32: Kalman Filter for ADC signal on STM32_F4xx<\/span><\/div>\n<p><a href=\"https:\/\/github.com\/thanapathkmu\/KalmanFilter_STM32\" target=\"_blank\" rel=\"noopener noreferrer nofollow\" class=\"embed__caption-host\">github.com<\/a><\/div>\n<\/div>\n<div class=\"embed_link\">\n<div class=\"embed__thumb\" style=\"background-image: url(&quot;undefined&quot;);\"><\/div>\n<div class=\"embed__caption\">\n<div class=\"embed__caption-title\"><span>Configuring DSP libraries on STM32CubeIDE<\/span><\/div>\n<p><a href=\"https:\/\/community.st.com\/t5\/stm32-mcus\/configuring-dsp-libraries-on-stm32cubeide\/ta-p\/49637\" target=\"_blank\" rel=\"noopener noreferrer nofollow\" class=\"embed__caption-host\">community.st.com<\/a><\/div>\n<\/div>\n<\/div>\n<\/div>\n<\/div>\n<p><!----><!----><\/div>\n<p><!----><!----><br \/> \u0441\u0441\u044b\u043b\u043a\u0430 \u043d\u0430 \u043e\u0440\u0438\u0433\u0438\u043d\u0430\u043b \u0441\u0442\u0430\u0442\u044c\u0438 <a href=\"https:\/\/habr.com\/ru\/articles\/925562\/\"> https:\/\/habr.com\/ru\/articles\/925562\/<\/a><\/p>\n","protected":false},"excerpt":{"rendered":"<div><!--[--><!--]--><\/div>\n<div id=\"post-content-body\">\n<div>\n<div class=\"article-formatted-body article-formatted-body article-formatted-body_version-2\">\n<div xmlns=\"http:\/\/www.w3.org\/1999\/xhtml\">\n<p>\u0412 \u044d\u0442\u043e\u0439 \u0441\u0442\u0430\u0442\u044c\u0435 \u0440\u0430\u0441\u0441\u043c\u043e\u0442\u0440\u0438\u043c \u0441\u0438\u0441\u0442\u0435\u043c\u0443 \u0441\u0442\u0430\u0431\u0438\u043b\u0438\u0437\u0430\u0446\u0438\u0438 \u043a\u0432\u0430\u0434\u0440\u043e\u043a\u043e\u043f\u0442\u0435\u0440\u0430.\u00a0 \u0421\u0442\u0430\u0442\u044c\u044f \u0432\u0434\u043e\u0445\u043d\u043e\u0432\u043b\u0435\u043d\u0430 <a href=\"https:\/\/habr.com\/ru\/articles\/227425\/\" rel=\"noopener noreferrer nofollow\">\u041f\u0440\u043e\u0433\u0440\u0430\u043c\u043c\u0438\u0440\u0443\u0435\u043c \u043a\u0432\u0430\u0434\u0440\u043e\u043a\u043e\u043f\u0442\u0435\u0440 \u043d\u0430 Arduino (\u0447\u0430\u0441\u0442\u044c 1)<\/a> . \u041f\u0440\u0438\u0441\u0442\u0443\u043f\u0438\u043c. \u041d\u0430\u0447\u043d\u0435\u043c \u0441 \u043d\u0430\u0441\u0442\u0440\u043e\u0439\u043a\u0438 \u0438\u043d\u0442\u0435\u0440\u0444\u0435\u0439\u0441\u043e\u0432 \u0438 \u0442\u0430\u0439\u043c\u0435\u0440\u043e\u0432 \u043c\u043a. \u0414\u0430\u043b\u0435\u0435 \u043a\u043e\u0440\u043e\u0442\u043a\u043e\u0435 \u043e\u043f\u0438\u0441\u0430\u043d\u0438\u0435 \u0444\u0443\u043d\u043a\u0446\u0438\u0439 \u043a\u043e\u0434\u0430 \u0438 \u0432\u044b\u0432\u043e\u0434.\u041f\u043e\u043b\u043d\u044b\u0439 \u043a\u043e\u0434 \u043f\u0440\u043e\u0435\u043a\u0442\u0430 \u0441\u043d\u0430\u0431\u0436\u0435\u043d \u043a\u043e\u043c\u043c\u0435\u043d\u0442\u0430\u0440\u0438\u044f\u043c\u0438 \u043a\u043e\u0442\u043e\u0440\u044b\u0435 \u043e\u0444\u043e\u0440\u043c\u043b\u0435\u043d\u044b \u043f\u043e\u0434 \u0441\u0442\u0438\u043b\u044c \u043a\u043e\u043c\u043c\u0435\u043d\u0442\u0430\u0440\u0438\u0435\u0432 \u0432 HAL. <\/p>\n<p>\u041d\u0430\u0441\u0442\u0440\u043e\u0438\u043c \u0447\u0435\u0442\u044b\u0440\u0435 \u043a\u0430\u043d\u0430\u043b\u0430 \u0443 TIM1 \u0438 \u0432\u043a\u043b\u044e\u0447\u0438\u043c DMA \u043e\u043d \u0431\u0443\u0434\u0435\u0442 \u0443\u043f\u0440\u0430\u0432\u043b\u044f\u0442\u044c ESC \u043f\u043e Multishot. Multishot \u2014 \u044d\u0442\u043e \u043f\u0440\u043e\u0442\u043e\u043a\u043e\u043b \u043f\u0435\u0440\u0435\u0434\u0430\u0447\u0438 \u0441\u0438\u0433\u043d\u0430\u043b\u043e\u0432 \u043e\u0442 \u043f\u043e\u043b\u0451\u0442\u043d\u043e\u0433\u043e \u043a\u043e\u043d\u0442\u0440\u043e\u043b\u043b\u0435\u0440\u0430\u00a0 \u043a \u0440\u0435\u0433\u0443\u043b\u044f\u0442\u043e\u0440\u0443 \u0441\u043a\u043e\u0440\u043e\u0441\u0442\u0438 ESC. \u0428\u0438\u0440\u0438\u043d\u0430 \u0438\u043c\u043f\u0443\u043b\u044c\u0441\u043e\u0432 5 &#8212; 25 \u043c\u043a\u0441.\u00a0\u00a0ESC \u0438\u0437\u043c\u0435\u0440\u044f\u0435\u0442 \u0434\u043b\u0438\u0442\u0435\u043b\u044c\u043d\u043e\u0441\u0442\u044c \u0438\u043c\u043f\u0443\u043b\u044c\u0441\u0430 \u0438 \u043f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u0443\u0435\u0442 \u0435\u0451 \u0432 \u0441\u043a\u043e\u0440\u043e\u0441\u0442\u044c \u0432\u0440\u0430\u0449\u0435\u043d\u0438\u044f \u0434\u0432\u0438\u0433\u0430\u0442\u0435\u043b\u044f. \u0427\u0435\u043c \u0434\u043b\u0438\u043d\u043d\u0435\u0435 \u0438\u043c\u043f\u0443\u043b\u044c\u0441, \u0442\u0435\u043c \u0432\u044b\u0448\u0435 \u043e\u0431\u043e\u0440\u043e\u0442\u044b. <\/p>\n<pre><code class=\"cpp\">\u0421\u0438\u0433\u043d\u0430\u043b Multishot :   \u041c\u0438\u043d: |\u203e\u203e\u203e|_______| (5 \u043c\u043a\u0441)   50%:  |\u203e\u203e\u203e\u203e\u203e\u203e\u203e|____| (15 \u043c\u043a\u0441)   \u041c\u0430\u043a\u0441:  |\u203e\u203e\u203e\u203e\u203e\u203e\u203e\u203e\u203e\u203e\u203e\u203e| (25 \u043c\u043a\u0441)  <\/code><\/pre>\n<figure class=\"full-width\">\n<div><figcaption>1<\/figcaption><\/div>\n<\/figure>\n<figure class=\"full-width\">\n<div><figcaption>2<\/figcaption><\/div>\n<\/figure>\n<p>\u041c\u0438\u043a\u0440\u043e\u043a\u043e\u043d\u0442\u0440\u043e\u043b\u043b\u0435\u0440 \u043d\u0430\u0441\u0442\u0440\u043e\u0435\u043d \u043d\u0430 100MHz \u043e\u0434\u0438\u043d \u0442\u0438\u043a \u0442\u0430\u0439\u043c\u0435\u0440\u0430.<code>T = 1\/100 \u041c\u0413\u0446 = 0.01 \u043c\u043a\u0441 = 10 \u043d\u0441<\/code>  \u0412\u0440\u0435\u043c\u044f \u043f\u0435\u0440\u0435\u043f\u043e\u043b\u043d\u0435\u043d\u0438\u044f <code>T_overflow = 3000 \u00d7 10 \u043d\u0441 = 30 \u043c\u043a\u0441<\/code> \u0412 \u043a\u043e\u0434\u0435 \u0443\u043a\u0430\u0436\u0435\u043c \u043c\u0438\u043d\u0438\u043c\u0430\u043b\u044c\u043d\u043e\u0435 \u043a\u043e\u043b\u0438\u0447\u0435\u0441\u0442\u0432\u043e \u0442\u0438\u043a\u043e\u0432 500 \u044d\u0442\u043e \u0431\u0443\u0434\u0435\u0442 5\u043c\u043a\u0441 \u0438 \u043c\u0430\u043a\u0441\u0438\u043c\u0430\u043b\u044c\u043d\u043e\u0435 2500 \u044d\u0442\u043e \u0431\u0443\u0434\u0435\u0442 25\u043c\u043a\u0441. \u0414\u0430\u043b\u0435\u0435 \u043d\u0430\u0441\u0442\u0440\u043e\u0438\u043c TIM11 \u043f\u0440\u0435\u0434\u0434\u0435\u043b\u0438\u0442\u0435\u043b\u044c (Prescaler) \u0442\u0430\u0439\u043c\u0435\u0440\u0430 99 -1 \u043f\u0435\u0440\u0438\u043e\u0434 (Period) 999 &#8212; 1.  \u0422\u0430\u043a\u0442\u043e\u0432\u0430\u044f \u0447\u0430\u0441\u0442\u043e\u0442\u0430 100 \u041c\u0413\u0446.  \u041f\u043e\u0441\u043b\u0435 \u043f\u0440\u0435\u0434\u0434\u0435\u043b\u0438\u0442\u0435\u043b\u044f <code>100 \u041c\u0413\u0446 \/ 100 = 1 \u041c\u0413\u0446 <\/code>\u0434\u043b\u0438\u0442\u0435\u043b\u044c\u043d\u043e\u0441\u0442\u044c \u043e\u0434\u043d\u043e\u0433\u043e \u0442\u0438\u043a\u0430 <code>1 \/ 1 \u041c\u0413\u0446 = 1 \u043c\u043a\u0441<\/code>  \u043f\u0435\u0440\u0438\u043e\u0434 \u043f\u0435\u0440\u0435\u043f\u043e\u043b\u043d\u0435\u043d\u0438\u044f  <code>1000 \u00d7 1 \u043c\u043a\u0441 = 1000 \u043c\u043a\u0441 = 1 \u043c\u0441<\/code> \u0447\u0430\u0441\u0442\u043e\u0442\u0430 \u043f\u0440\u0435\u0440\u044b\u0432\u0430\u043d\u0438\u0439 <code>1 \/ 1 \u043c\u0441 = 1000 \u0413\u0446 (1 \u043a\u0413\u0446). <\/code> <\/p>\n<figure class=\"full-width\">\n<div><figcaption>3<\/figcaption><\/div>\n<\/figure>\n<p>\u0422\u0430\u0439\u043c\u0435\u0440 TIM11 \u0431\u0443\u0434\u0435\u0442 \u0433\u0435\u043d\u0435\u0440\u0438\u0440\u043e\u0432\u0430\u0442\u044c \u043f\u0440\u0435\u0440\u044b\u0432\u0430\u043d\u0438\u044f \u0441 \u0447\u0430\u0441\u0442\u043e\u0442\u043e\u0439 1 \u043a\u0413\u0446, \u0438 \u0432 \u043e\u0431\u0440\u0430\u0431\u043e\u0442\u0447\u0438\u043a\u0435 \u043f\u0440\u0435\u0440\u044b\u0432\u0430\u043d\u0438\u044f (\u0444\u0443\u043d\u043a\u0446\u0438\u0438 \u043e\u0431\u0440\u0430\u0442\u043d\u043e\u0433\u043e \u0432\u044b\u0437\u043e\u0432\u0430) \u0431\u0443\u0434\u0435\u0442 \u0432\u044b\u0437\u044b\u0432\u0430\u0442\u044c\u0441\u044f \u043e\u0441\u043d\u043e\u0432\u043d\u043e\u0439 \u0446\u0438\u043a\u043b \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f: <code>run_control_loop():<\/code><\/p>\n<pre><code class=\"cpp\">void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {     if (htim == &amp;htim11) {         run_control_loop(); \/\/ \u0412\u044b\u0437\u043e\u0432 \u0432\u0441\u0435\u0439 \u043b\u043e\u0433\u0438\u043a\u0438 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f     } }<\/code><\/pre>\n<p>\u0412 \u043a\u0430\u0447\u0435\u0441\u0442\u0432\u0435 IMU \u0438\u0441\u043f\u043e\u043b\u044c\u0437\u0443\u0435\u0442\u0441\u044f BMX055 \u043f\u043e\u0434\u043a\u043b\u044e\u0447\u0435\u043d\u043d\u044b\u0439 \u043f\u043e I2C \u0438\u043d\u0442\u0435\u0440\u0444\u0435\u0439\u0441 \u043d\u0430\u0441\u0442\u0440\u043e\u0435\u043d \u043d\u0430 \u0431\u044b\u0441\u0442\u0440\u044b\u0439 \u0440\u0435\u0436\u0438\u043c.<\/p>\n<figure class=\"full-width\">\n<div><figcaption>4<\/figcaption><\/div>\n<\/figure>\n<figure class=\"full-width\">\n<div><figcaption>5<\/figcaption><\/div>\n<\/figure>\n<p>\u0412 \u043a\u0430\u0447\u0435\u0441\u0442\u0432\u0435 \u0440\u0430\u0434\u0438\u043e\u043c\u043e\u0434\u0443\u043b\u044f  E220-400T22D \u043f\u043e\u0434\u043a\u043b\u044e\u0447\u0435\u043d\u043d\u044b\u0439 \u043f\u043e UART6 \u0434\u043b\u044f \u043a\u043e\u0442\u043e\u0440\u043e\u0433\u043e \u0432\u043a\u043b\u044e\u0447\u0435\u043d\u043e DMA.<\/p>\n<figure class=\"full-width\">\n<div><figcaption>6<\/figcaption><\/div>\n<\/figure>\n<figure class=\"full-width\">\n<div><figcaption>7<\/figcaption><\/div>\n<\/figure>\n<p>    \u041d\u0430\u0441\u0442\u0440\u043e\u0439\u043a\u0438 E220-400T22D \u043f\u0440\u043e\u0438\u0437\u0432\u043e\u0434\u0438\u043b\u0438\u0441\u044c \u0438\u0437 \u043f\u0440\u043e\u0433\u0440\u0430\u043c\u043c\u044b \u043e\u0442 EBYTE.<\/p>\n<figure class=\"full-width\">\n<div><figcaption>8<\/figcaption><\/div>\n<\/figure>\n<p>\u0412\u0441\u0435 \u043d\u0430\u0441\u0442\u0440\u043e\u0439\u043a\u0438 \u043f\u043e \u0443\u043c\u043e\u043b\u0447\u0430\u043d\u0438\u044e. \u041c\u043e\u0434\u0443\u043b\u044c \u0437\u0430\u0440\u0430\u0431\u043e\u0442\u0430\u043b \u0441\u0440\u0430\u0437\u0443 \u043a\u0430\u043a \u0442\u043e\u043b\u044c\u043a\u043e \u0431\u044b\u043b \u043f\u043e\u0434\u043a\u043b\u044e\u0447\u0435\u043d.<\/p>\n<h3>\u0418\u043d\u0438\u0446\u0438\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u044f \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u0432 \u0438 \u0430\u043b\u0433\u043e\u0440\u0438\u0442\u043c\u043e\u0432 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f.<\/h3>\n<p>\u041f\u043e\u0441\u043b\u0435 \u0438\u043d\u0438\u0446\u0438\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u0438 IMU (BMX055) \u0432 \u043a\u043e\u0434\u0435 \u0432\u044b\u043f\u043e\u043b\u043d\u044f\u0435\u0442\u0441\u044f \u043d\u0430\u0441\u0442\u0440\u043e\u0439\u043a\u0430 \u043d\u0435\u0441\u043a\u043e\u043b\u044c\u043a\u0438\u0445 \u0442\u0438\u043f\u043e\u0432 \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u0432 \u0438 \u041f\u0418\u0414-\u0440\u0435\u0433\u0443\u043b\u044f\u0442\u043e\u0440\u043e\u0432: <\/p>\n<h4>1. \u0424\u0438\u043b\u044c\u0442\u0440\u044b \u043d\u0438\u0437\u043a\u0438\u0445 \u0447\u0430\u0441\u0442\u043e\u0442 (LPF)<\/h4>\n<p>\u0418\u0441\u043f\u043e\u043b\u044c\u0437\u0443\u0435\u0442\u0441\u044f \u043a\u0430\u0441\u043a\u0430\u0434\u043d\u0430\u044f \u0441\u0442\u0440\u0443\u043a\u0442\u0443\u0440\u0430 \u0431\u0438\u043a\u0432\u0430\u0434\u0440\u0430\u0442\u043d\u044b\u0445 \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u0432 (Biquad) \u0438\u0437 \u0431\u0438\u0431\u043b\u0438\u043e\u0442\u0435\u043a\u0438 CMSIS-DSP:<\/p>\n<pre><code class=\"cpp\">arm_biquad_cascade_df2T_init_f32(&amp;lpf_Gx, NUM_STAGES, Coeffs_lpf, filterState_lpf_Gx); arm_biquad_cascade_df2T_init_f32(&amp;lpf_Gy, NUM_STAGES, Coeffs_lpf, filterState_lpf_Gy); arm_biquad_cascade_df2T_init_f32(&amp;lpf_Gz, NUM_STAGES, Coeffs_lpf, filterState_lpf_Gz);<\/code><\/pre>\n<p>\u0412\u0441\u0435\u0433\u043e \u0441\u043e\u0437\u0434\u0430\u0435\u0442\u0441\u044f 9 \u044d\u043a\u0437\u0435\u043c\u043f\u043b\u044f\u0440\u043e\u0432 \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u0432:<\/p>\n<ul>\n<li>\n<p>3 \u0434\u043b\u044f \u043e\u0441\u0435\u0439 \u0433\u0438\u0440\u043e\u0441\u043a\u043e\u043f\u0430 (Gx, Gy, Gz)<\/p>\n<\/li>\n<li>\n<p>3 \u0434\u043b\u044f \u043e\u0441\u0435\u0439 \u0430\u043a\u0441\u0435\u043b\u0435\u0440\u043e\u043c\u0435\u0442\u0440\u0430 (Ax, Ay, Az)<\/p>\n<\/li>\n<li>\n<p>3 \u0434\u043b\u044f \u043e\u0448\u0438\u0431\u043e\u043a \u043f\u043e\u043b\u043e\u0436\u0435\u043d\u0438\u044f (Errorx, Errory, Errorz)<\/p>\n<\/li>\n<\/ul>\n<h4>2. \u0424\u0438\u043b\u044c\u0442\u0440\u044b \u041a\u0430\u043b\u043c\u0430\u043d\u0430<\/h4>\n<p>\u0418\u043d\u0438\u0446\u0438\u0430\u043b\u0438\u0437\u0438\u0440\u0443\u044e\u0442\u0441\u044f \u043e\u0442\u0434\u0435\u043b\u044c\u043d\u043e \u0434\u043b\u044f \u043a\u0430\u0436\u0434\u043e\u0433\u043e \u043a\u0430\u043d\u0430\u043b\u0430:<\/p>\n<pre><code class=\"cpp\">kalman_filter_init(&amp;gyro_filter_x, GYRO_PROCESS_NOISE, GYRO_MEASUREMENT_NOISE, GYRO_ESTIMATION_ERROR, GYRO_INITIAL_VALUE); kalman_filter_init(&amp;gyro_filter_y, GYRO_PROCESS_NOISE, GYRO_MEASUREMENT_NOISE, GYRO_ESTIMATION_ERROR, GYRO_INITIAL_VALUE); kalman_filter_init(&amp;gyro_filter_z, GYRO_PROCESS_NOISE, GYRO_MEASUREMENT_NOISE, GYRO_ESTIMATION_ERROR, GYRO_INITIAL_VALUE);<\/code><\/pre>\n<h4>3. \u0420\u0435\u0436\u0435\u043a\u0442\u043e\u0440\u043d\u044b\u0435 \u0444\u0438\u043b\u044c\u0442\u0440\u044b (Notch)<\/h4>\n<p>\u041d\u0430\u0441\u0442\u0440\u043e\u0435\u043d\u044b \u0434\u043b\u044f \u043f\u043e\u0434\u0430\u0432\u043b\u0435\u043d\u0438\u044f \u043a\u043e\u043d\u043a\u0440\u0435\u0442\u043d\u044b\u0445 \u0447\u0430\u0441\u0442\u043e\u0442\u043d\u044b\u0445 \u0441\u043e\u0441\u0442\u0430\u0432\u043b\u044f\u044e\u0449\u0438\u0445:<\/p>\n<pre><code class=\"cpp\">init_notch_filter(&amp;notch_ax, NOTCH_CENTER_FREQ, NOTCH_WIDTH, NOTCH_SAMPLE_RATE); init_notch_filter(&amp;notch_ay, NOTCH_CENTER_FREQ, NOTCH_WIDTH, NOTCH_SAMPLE_RATE); init_notch_filter(&amp;notch_az, NOTCH_CENTER_FREQ, NOTCH_WIDTH, NOTCH_SAMPLE_RATE);<\/code><\/pre>\n<h4>4. \u0424\u0438\u043b\u044c\u0442\u0440\u044b \u0432\u044b\u0441\u043e\u043a\u0438\u0445 \u0447\u0430\u0441\u0442\u043e\u0442 (HPF)<\/h4>\n<p>\u0418\u0441\u043f\u043e\u043b\u044c\u0437\u0443\u044e\u0442\u0441\u044f \u0434\u043b\u044f \u0443\u0441\u0442\u0440\u0430\u043d\u0435\u043d\u0438\u044f \u043f\u043e\u0441\u0442\u043e\u044f\u043d\u043d\u043e\u0439 \u0441\u043e\u0441\u0442\u0430\u0432\u043b\u044f\u044e\u0449\u0435\u0439:<\/p>\n<pre><code class=\"cpp\">HPF_Init(&amp;hpf_x, HPF_ALPHA, Gx[0]); HPF_Init(&amp;hpf_y, HPF_ALPHA, Gy[0]); HPF_Init(&amp;hpf_z, HPF_ALPHA, Gz[0]);<\/code><\/pre>\n<h4>5. \u041f\u0418\u0414-\u0440\u0435\u0433\u0443\u043b\u044f\u0442\u043e\u0440\u044b<\/h4>\n<p>\u0418\u043d\u0438\u0446\u0438\u0430\u043b\u0438\u0437\u0438\u0440\u0443\u044e\u0442\u0441\u044f \u0441 \u0437\u0430\u0434\u0430\u043d\u043d\u044b\u043c\u0438 \u043a\u043e\u044d\u0444\u0444\u0438\u0446\u0438\u0435\u043d\u0442\u0430\u043c\u0438 \u0434\u043b\u044f \u043a\u0430\u0436\u0434\u043e\u0433\u043e \u043a\u0430\u043d\u0430\u043b\u0430 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f:<\/p>\n<pre><code class=\"cpp\">PID_Init(&amp;pitch_pid, PITCH_PID_KP, PITCH_PID_KI, PITCH_PID_KD, ALPHA, ALPHA_DERIVATIVE, INTEGRAL_LIMIT, SCALE_FACTOR); PID_Init(&amp;roll_pid, ROLL_PID_KP, ROLL_PID_KI, ROLL_PID_KD, ALPHA, ALPHA_DERIVATIVE, INTEGRAL_LIMIT, SCALE_FACTOR); PID_Init(&amp;yaw_pid, YAW_PID_KP, YAW_PID_KI, YAW_PID_KD, ALPHA, ALPHA_DERIVATIVE, INTEGRAL_LIMIT, SCALE_FACTOR);<\/code><\/pre>\n<h3>\u041e\u0441\u043d\u043e\u0432\u043d\u043e\u0439 \u0446\u0438\u043a\u043b \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f<\/h3>\n<p>\u041f\u043e\u0441\u043b\u0435 \u0437\u0430\u0432\u0435\u0440\u0448\u0435\u043d\u0438\u044f \u0438\u043d\u0438\u0446\u0438\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u0438 \u043f\u0435\u0440\u0435\u0445\u043e\u0434\u0438\u043c \u0432 \u043e\u0441\u043d\u043e\u0432\u043d\u043e\u0439 \u0446\u0438\u043a\u043b, \u0433\u0434\u0435 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u0435 \u043e\u0441\u0443\u0449\u0435\u0441\u0442\u0432\u043b\u044f\u0435\u0442\u0441\u044f \u0447\u0435\u0440\u0435\u0437 \u043f\u0440\u0435\u0440\u044b\u0432\u0430\u043d\u0438\u0435 \u0442\u0430\u0439\u043c\u0435\u0440\u0430 TIM11 \u0441 \u0447\u0430\u0441\u0442\u043e\u0442\u043e\u0439 1 \u043a\u0413\u0446. \u0412 \u043e\u0431\u0440\u0430\u0431\u043e\u0442\u0447\u0438\u043a\u0435 \u043f\u0440\u0435\u0440\u044b\u0432\u0430\u043d\u0438\u044f \u0432\u044b\u0437\u044b\u0432\u0430\u0435\u0442\u0441\u044f \u0444\u0443\u043d\u043a\u0446\u0438\u044f <code>run_control_loop()<\/code>: <\/p>\n<h4>1. \u041f\u0440\u043e\u0432\u0435\u0440\u043a\u0430 \u0441\u0432\u044f\u0437\u0438 \u0441 \u043f\u0443\u043b\u044c\u0442\u043e\u043c \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f<\/h4>\n<p>check_connection_loss();<\/p>\n<pre><code class=\"cpp\">void check_connection_loss(void) {     uint32_t current_time = HAL_GetTick();     if ((current_time - last_received_time) &gt; CONNECTION_TIMEOUT_MS) {         connection_lost = true;         \/\/ \u0421\u0431\u0440\u043e\u0441 \u0443\u043f\u0440\u0430\u0432\u043b\u044f\u044e\u0449\u0438\u0445 \u0441\u0438\u0433\u043d\u0430\u043b\u043e\u0432         joystick_x = 0;         joystick_y = 0;         right_left = 0;         potentiometer_value = 0;         button = 0;         \/\/ \u0423\u0441\u0442\u0430\u043d\u043e\u0432\u043a\u0430 \u043c\u0438\u043d\u0438\u043c\u0430\u043b\u044c\u043d\u043e\u0439 \u0442\u044f\u0433\u0438 \u043d\u0430 \u043c\u043e\u0442\u043e\u0440\u0430\u0445         Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_1);         Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_2);         Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_3);         Multishot_SetThrottle(MIN_PULSE_WIDTH, TIM_CHANNEL_4);     } }<\/code><\/pre>\n<p>\u041f\u043e\u043b\u0443\u0447\u0430\u0435\u0442 \u0442\u0435\u043a\u0443\u0449\u0435\u0435 \u0432\u0440\u0435\u043c\u044f \u0441\u0438\u0441\u0442\u0435\u043c\u043d\u043e\u0433\u043e \u0442\u0430\u0439\u043c\u0435\u0440\u0430. \u041f\u0440\u043e\u0432\u0435\u0440\u044f\u0435\u0442, \u043f\u0440\u0435\u0432\u044b\u0448\u0430\u0435\u0442 \u043b\u0438 \u0432\u0440\u0435\u043c\u044f \u0441 \u043f\u043e\u0441\u043b\u0435\u0434\u043d\u0435\u0433\u043e \u0443\u0441\u043f\u0435\u0448\u043d\u043e\u0433\u043e \u043f\u0440\u0438\u0435\u043c\u0430 \u0434\u0430\u043d\u043d\u044b\u0445 \u0437\u0430\u0434\u0430\u043d\u043d\u044b\u0439 \u0442\u0430\u0439\u043c\u0430\u0443\u0442.  \u041f\u0440\u0438 \u043f\u043e\u0442\u0435\u0440\u0435 \u0441\u0432\u044f\u0437\u0438 \u0443\u0441\u0442\u0430\u043d\u0430\u0432\u043b\u0438\u0432\u0430\u0435\u0442 \u0444\u043b\u0430\u0433 <code>connection_lost,<\/code> \u0441\u0431\u0440\u0430\u0441\u044b\u0432\u0430\u0435\u0442 \u0432\u0441\u0435 \u0443\u043f\u0440\u0430\u0432\u043b\u044f\u044e\u0449\u0438\u0435 \u0441\u0438\u0433\u043d\u0430\u043b\u044b \u0432 \u043d\u043e\u043b\u044c,  \u0443\u0441\u0442\u0430\u043d\u0430\u0432\u043b\u0438\u0432\u0430\u0435\u0442 \u043c\u0438\u043d\u0438\u043c\u0430\u043b\u044c\u043d\u044b\u0435 \u0438\u043c\u043f\u0443\u043b\u044c\u0441\u044b \u043d\u0430 \u0432\u0441\u0435\u0445 \u043c\u043e\u0442\u043e\u0440\u0430\u0445.<\/p>\n<h4>2. \u041f\u0440\u0438\u0435\u043c \u0441\u0438\u0433\u043d\u0430\u043b\u0430 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f<\/h4>\n<pre><code class=\"cpp\">void receive_and_parse_data(int data_size) {     calculated_checksum = 0;          \/\/ \u041f\u0440\u043e\u0432\u0435\u0440\u043a\u0430 \u043c\u0438\u043d\u0438\u043c\u0430\u043b\u044c\u043d\u043e\u0433\u043e \u0440\u0430\u0437\u043c\u0435\u0440\u0430 \u043f\u0430\u043a\u0435\u0442\u0430     if (data_size &lt; 4) {         return; \/\/ \u041d\u0435\u0434\u043e\u0441\u0442\u0430\u0442\u043e\u0447\u043d\u043e \u0434\u0430\u043d\u043d\u044b\u0445 \u0434\u043b\u044f \u0440\u0430\u0437\u0431\u043e\u0440\u0430     }          \/\/ \u0412\u044b\u0447\u0438\u0441\u043b\u0435\u043d\u0438\u0435 \u043a\u043e\u043d\u0442\u0440\u043e\u043b\u044c\u043d\u043e\u0439 \u0441\u0443\u043c\u043c\u044b     for (uint8_t i = 0; i &lt; data_size - 2; i++) {         calculated_checksum += buffer_message[i];     }          \/\/ \u041f\u043e\u043b\u0443\u0447\u0435\u043d\u0438\u0435 \u043a\u043e\u043d\u0442\u0440\u043e\u043b\u044c\u043d\u043e\u0439 \u0441\u0443\u043c\u043c\u044b \u0438\u0437 \u043f\u0430\u043a\u0435\u0442\u0430     received_checksum = (buffer_message[data_size - 2] &lt;&lt; 8) | buffer_message[data_size - 1];          \/\/ \u041f\u0440\u043e\u0432\u0435\u0440\u043a\u0430 \u043a\u043e\u043d\u0442\u0440\u043e\u043b\u044c\u043d\u043e\u0439 \u0441\u0443\u043c\u043c\u044b     if (received_checksum == calculated_checksum) {         \/\/ \u041e\u0431\u043d\u043e\u0432\u043b\u0435\u043d\u0438\u0435 \u0432\u0440\u0435\u043c\u0435\u043d\u0438 \u043f\u043e\u0441\u043b\u0435\u0434\u043d\u0435\u0433\u043e \u0443\u0441\u043f\u0435\u0448\u043d\u043e\u0433\u043e \u043f\u043e\u043b\u0443\u0447\u0435\u043d\u0438\u044f \u0434\u0430\u043d\u043d\u044b\u0445         last_received_time = HAL_GetTick();         connection_lost = false;                  \/\/ \u0420\u0430\u0437\u0431\u043e\u0440 \u0434\u0430\u043d\u043d\u044b\u0445 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f:         joystick_x = (int16_t)((buffer_message[0] &lt;&lt; 8) | buffer_message[1];         joystick_y = (int16_t)((buffer_message[2] &lt;&lt; 8) | buffer_message[3];         potentiometer_value = (buffer_message[4] &lt;&lt; 8) | buffer_message[5];         right_left = (int16_t)((buffer_message[6] &lt;&lt; 8) | buffer_message[7];         button = (buffer_message[8] &lt;&lt; 8) | buffer_message[9];     } }<\/code><\/pre>\n<p>  \u041f\u0430\u043a\u0435\u0442 \u0434\u0430\u043d\u043d\u044b\u0445 \u0438\u043c\u0435\u0435\u0442 \u0441\u043b\u0435\u0434\u0443\u044e\u0449\u0443\u044e \u0441\u0442\u0440\u0443\u043a\u0442\u0443\u0440\u0443 (12 \u0431\u0430\u0439\u0442):<\/p>\n<div>\n<div class=\"table\">\n<table>\n<tbody>\n<tr>\n<th>\n<p align=\"left\">\u041f\u043e\u0437\u0438\u0446\u0438\u044f<\/p>\n<\/th>\n<th>\n<p align=\"left\">\u0414\u0430\u043d\u043d\u044b\u0435<\/p>\n<\/th>\n<th>\n<p align=\"left\">\u0420\u0430\u0437\u043c\u0435\u0440<\/p>\n<\/th>\n<th>\n<p align=\"left\">\u041e\u043f\u0438\u0441\u0430\u043d\u0438\u0435<\/p>\n<\/th>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">0-1<\/p>\n<\/td>\n<td>\n<p align=\"left\">joystick_x<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u041f\u043e\u043b\u043e\u0436\u0435\u043d\u0438\u0435 \u0434\u0436\u043e\u0439\u0441\u0442\u0438\u043a\u0430 \u043f\u043e X<\/p>\n<\/td>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">2-3<\/p>\n<\/td>\n<td>\n<p align=\"left\">joystick_y<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u041f\u043e\u043b\u043e\u0436\u0435\u043d\u0438\u0435 \u0434\u0436\u043e\u0439\u0441\u0442\u0438\u043a\u0430 \u043f\u043e Y<\/p>\n<\/td>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">4-5<\/p>\n<\/td>\n<td>\n<p align=\"left\">potentiometer<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u0417\u043d\u0430\u0447\u0435\u043d\u0438\u0435 \u043f\u043e\u0442\u0435\u043d\u0446\u0438\u043e\u043c\u0435\u0442\u0440\u0430<\/p>\n<\/td>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">6-7<\/p>\n<\/td>\n<td>\n<p align=\"left\">right_left<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u0423\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u0435 \u0440\u044b\u0441\u043a\u0430\u043d\u0438\u0435\u043c<\/p>\n<\/td>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">8-9<\/p>\n<\/td>\n<td>\n<p align=\"left\">button<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u0421\u043e\u0441\u0442\u043e\u044f\u043d\u0438\u0435 \u043a\u043d\u043e\u043f\u043a\u0438<\/p>\n<\/td>\n<\/tr>\n<tr>\n<td>\n<p align=\"left\">10-11<\/p>\n<\/td>\n<td>\n<p align=\"left\">checksum<\/p>\n<\/td>\n<td>\n<p align=\"left\">2 \u0431\u0430\u0439\u0442\u0430<\/p>\n<\/td>\n<td>\n<p align=\"left\">\u041a\u043e\u043d\u0442\u0440\u043e\u043b\u044c\u043d\u0430\u044f \u0441\u0443\u043c\u043c\u0430<\/p>\n<\/td>\n<\/tr>\n<\/tbody>\n<\/table>\n<\/div>\n<\/div>\n<h4>3. \u041f\u043e\u043b\u0443\u0447\u0435\u043d\u0438\u0435 \u0438 \u043e\u0431\u0440\u0430\u0431\u043e\u0442\u043a\u0430 \u0434\u0430\u043d\u043d\u044b\u0445 \u0441 IMU<\/h4>\n<p>\u0424\u0443\u043d\u043a\u0446\u0438\u044f get_filter_data_accel();<\/p>\n<pre><code class=\"cpp\">void get_filter_data_accel() {     BMX055_Read_Accel(&amp;hi2c1, &amp;BMX055);          Ax[0] = round_to(BMX055.Ax);     Ax[0] = Ax[0] - offset_Ax;     Ax[0] = apply_notch_filter(&amp;notch_ax, Ax[0]);     arm_biquad_cascade_df2T_f32(&amp;lpf_Ax, Ax, filter_Ax, BLOCK_SIZE);     filter_Ax[0] = kalman_filter_update(&amp;accel_filter_x, filter_Ax[0]);          \/\/ \u0410\u043d\u0430\u043b\u043e\u0433\u0438\u0447\u043d\u043e \u0434\u043b\u044f Ay \u0438 Az     ... }<\/code><\/pre>\n<p>\u042d\u0442\u0430\u043f\u044b \u043e\u0431\u0440\u0430\u0431\u043e\u0442\u043a\u0438:<\/p>\n<ol>\n<li>\n<p>\u0427\u0442\u0435\u043d\u0438\u0435 \u0441\u044b\u0440\u044b\u0445 \u0434\u0430\u043d\u043d\u044b\u0445 \u0441 \u0430\u043a\u0441\u0435\u043b\u0435\u0440\u043e\u043c\u0435\u0442\u0440\u0430 \u0447\u0435\u0440\u0435\u0437 I2C<\/p>\n<\/li>\n<li>\n<p>\u041e\u043a\u0440\u0443\u0433\u043b\u0435\u043d\u0438\u0435 \u0437\u043d\u0430\u0447\u0435\u043d\u0438\u0439 \u0434\u043e 2 \u0437\u043d\u0430\u043a\u043e\u0432 \u043f\u043e\u0441\u043b\u0435 \u0437\u0430\u043f\u044f\u0442\u043e\u0439<\/p>\n<\/li>\n<li>\n<p>\u0412\u044b\u0447\u0438\u0442\u0430\u043d\u0438\u0435 \u043a\u0430\u043b\u0438\u0431\u0440\u043e\u0432\u043e\u0447\u043d\u043e\u0433\u043e \u0441\u043c\u0435\u0449\u0435\u043d\u0438\u044f<\/p>\n<\/li>\n<li>\n<p>\u041f\u043e\u0434\u0430\u0432\u043b\u0435\u043d\u0438\u0435 \u0443\u0437\u043a\u043e\u043f\u043e\u043b\u043e\u0441\u043d\u044b\u0445 \u043f\u043e\u043c\u0435\u0445 \u0440\u0435\u0436\u0435\u043a\u0442\u043e\u0440\u043d\u044b\u043c \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u043c<\/p>\n<\/li>\n<li>\n<p>\u0424\u0438\u043b\u044c\u0442\u0440\u0430\u0446\u0438\u044f \u043d\u0438\u0437\u043a\u0438\u0445 \u0447\u0430\u0441\u0442\u043e\u0442 \u0431\u0438\u043a\u0432\u0430\u0434\u0440\u0430\u0442\u043d\u044b\u043c \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u043c<\/p>\n<\/li>\n<li>\n<p>\u041e\u043a\u043e\u043d\u0447\u0430\u0442\u0435\u043b\u044c\u043d\u0430\u044f \u0444\u0438\u043b\u044c\u0442\u0440\u0430\u0446\u0438\u044f \u0444\u0438\u043b\u044c\u0442\u0440\u043e\u043c \u041a\u0430\u043b\u043c\u0430\u043d\u0430<\/p>\n<\/li>\n<\/ol>\n<p>\u0424\u0443\u043d\u043a\u0446\u0438\u044f get_filter_data_gyro();<\/p>\n<pre><code class=\"cpp\">void get_filter_data_gyro() {     BMX055_Read_Gyro(&amp;hi2c1, &amp;BMX055);          Gx[0] = round_to(BMX055.Gx);     Gx[0] = Gx[0] - bias_Gx;     Gx[0] = apply_notch_filter(&amp;notch_gx, Gx[0]);     Gx[0] = HPF_Update(&amp;hpf_x, Gx[0]);     arm_biquad_cascade_df2T_f32(&amp;lpf_Gx, Gx, filter_Gx, BLOCK_SIZE);     filter_Gx[0] = kalman_filter_update(&amp;gyro_filter_x, filter_Gx[0]);          \/\/ \u0410\u043d\u0430\u043b\u043e\u0433\u0438\u0447\u043d\u043e \u0434\u043b\u044f Gy \u0438 Gz     ... }<\/code><\/pre>\n<ol>\n<li>\n<p>\u0414\u043e\u043f\u043e\u043b\u043d\u0438\u0442\u0435\u043b\u044c\u043d\u044b\u0439 \u044d\u0442\u0430\u043f &#8212; \u0444\u0438\u043b\u044c\u0442\u0440 \u0432\u044b\u0441\u043e\u043a\u0438\u0445 \u0447\u0430\u0441\u0442\u043e\u0442 \u0434\u043b\u044f \u0443\u0441\u0442\u0440\u0430\u043d\u0435\u043d\u0438\u044f \u0434\u0440\u0435\u0439\u0444\u0430<\/p>\n<\/li>\n<li>\n<p>\u0418\u0441\u043f\u043e\u043b\u044c\u0437\u0443\u044e\u0442\u0441\u044f \u043e\u0442\u0434\u0435\u043b\u044c\u043d\u044b\u0435 \u043a\u0430\u043b\u0438\u0431\u0440\u043e\u0432\u043e\u0447\u043d\u044b\u0435 \u043a\u043e\u044d\u0444\u0444\u0438\u0446\u0438\u0435\u043d\u0442\u044b (<code>bias_Gx<\/code> \u0432\u043c\u0435\u0441\u0442\u043e <code>offset_Ax<\/code>)<\/p>\n<\/li>\n<\/ol>\n<h4>4. \u0420\u0430\u0441\u0447\u0435\u0442 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438<\/h4>\n<p>\u0424\u0443\u043d\u043a\u0446\u0438\u044f get_angle_mahony(); <\/p>\n<pre><code class=\"cpp\">void get_angle_mahony(void) {     \/\/ \u041f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u043e\u0432\u0430\u043d\u0438\u0435 \u0435\u0434\u0438\u043d\u0438\u0446 \u0438\u0437\u043c\u0435\u0440\u0435\u043d\u0438\u044f     float filterAx = filter_Ax[0]*9.81; \/\/ \u043c\/\u0441\u00b2     float filterGx = filter_Gx[0]*DEG_TO_RAD; \/\/ \u0440\u0430\u0434\/\u0441          \/\/ \u041e\u0431\u043d\u043e\u0432\u043b\u0435\u043d\u0438\u0435 \u0430\u043b\u0433\u043e\u0440\u0438\u0442\u043c\u0430 Mahony     MahonyAHRSupdateIMU(filterAx, filterAy, filterAz,                         filterGx, filterGy, filterGz,                         CONTROL_LOOP_DT);          \/\/ \u041f\u043e\u043b\u0443\u0447\u0435\u043d\u0438\u0435 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438     Quat_actual[0] = (*(getQ()));     ...          \/\/ \u041a\u043e\u043c\u043f\u043b\u0435\u043c\u0435\u043d\u0442\u0430\u0440\u043d\u0430\u044f \u0444\u0438\u043b\u044c\u0442\u0440\u0430\u0446\u0438\u044f \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430     float alpha = 0.5;     float dot = Quat_previous[0]*Quat_actual[0] + ...;     if (dot &lt; 0) {         \/\/ \u0418\u043d\u0432\u0435\u0440\u0442\u0438\u0440\u043e\u0432\u0430\u043d\u0438\u0435 \u043f\u0440\u0438 \u043e\u0442\u0440\u0438\u0446\u0430\u0442\u0435\u043b\u044c\u043d\u043e\u043c \u0441\u043a\u0430\u043b\u044f\u0440\u043d\u043e\u043c \u043f\u0440\u043e\u0438\u0437\u0432\u0435\u0434\u0435\u043d\u0438\u0438         Quat_actual[0] = alpha * (-Quat_actual[0]) + ...;         ...     }          \/\/ \u041d\u043e\u0440\u043c\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u044f \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430     normalizeQuaternion(Quat_actual);          \/\/ \u041f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u043e\u0432\u0430\u043d\u0438\u0435 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u0430 \u0432 \u0443\u0433\u043b\u044b \u042d\u0439\u043b\u0435\u0440\u0430     float q0 = Quat_actual[0];     ...     pitch = asinf(2.0f * (q1q3 - q0q2)) * RAD_TO_DEG;     roll = atan2f(2.0f * (q0q1 + q2q3), q0q0 - q1q1 - q2q2 + q3q3) * RAD_TO_DEG;     yaw = atan2f(2.0f * (q1q2 + q0q3), q0q0 + q1q1 - q2q2 - q3q3) * RAD_TO_DEG;          \/\/ \u0424\u0438\u043b\u044c\u0442\u0440\u0430\u0446\u0438\u044f \u0443\u0433\u043b\u043e\u0432     roll = apply_notch_filter(&amp;notch_roll, roll);     ... }<\/code><\/pre>\n<h3>\u041e\u043f\u0438\u0441\u0430\u043d\u0438\u0435 \u0440\u0430\u0441\u0447\u0435\u0442\u0430 \u043e\u0448\u0438\u0431\u043a\u0438 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438.<\/h3>\n<p>\u0420\u0430\u0441\u0447\u0435\u0442 \u043e\u0448\u0438\u0431\u043a\u0438 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438 \u043c\u0435\u0436\u0434\u0443 \u0442\u0435\u043a\u0443\u0449\u0438\u043c \u043f\u043e\u043b\u043e\u0436\u0435\u043d\u0438\u0435\u043c \u043a\u0432\u0430\u0434\u0440\u043e\u043a\u043e\u043f\u0442\u0435\u0440\u0430 \u0438 \u0446\u0435\u043b\u0435\u0432\u044b\u043c \u043f\u043e\u043b\u043e\u0436\u0435\u043d\u0438\u0435\u043c \u0437\u0430\u0434\u0430\u043d\u043d\u044b\u043c \u0441 \u043f\u0443\u043b\u044c\u0442\u0430 \u0443\u043f\u0440\u0430\u0432\u043b\u0435\u043d\u0438\u044f \u0432\u044b\u043f\u043e\u043b\u043d\u044f\u0435\u0442\u0441\u044f \u0441 \u0438\u0441\u043f\u043e\u043b\u044c\u0437\u043e\u0432\u0430\u043d\u0438\u0435\u043c \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d\u043e\u0432 \u0438 \u0432\u043a\u043b\u044e\u0447\u0430\u0435\u0442 \u043d\u0435\u0441\u043a\u043e\u043b\u044c\u043a\u043e \u044d\u0442\u0430\u043f\u043e\u0432. <\/p>\n<h4>\u041f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u043e\u0432\u0430\u043d\u0438\u0435 \u043a\u043e\u043c\u0430\u043d\u0434 \u0441 \u043f\u0443\u043b\u044c\u0442\u0430 \u0432 \u0446\u0435\u043b\u0435\u0432\u043e\u0439 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d<\/h4>\n<p> \u0424\u0443\u043d\u043a\u0446\u0438\u044f <code>eulerToQuaternion()<\/code> \u043f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u0443\u0435\u0442 \u0441\u0438\u0433\u043d\u0430\u043b\u044b \u0434\u0436\u043e\u0439\u0441\u0442\u0438\u043a\u0430 (\u0432 \u0434\u0438\u0430\u043f\u0430\u0437\u043e\u043d\u0435 -100..100) \u0432 \u043a\u0432\u0430\u0442\u0435\u0440\u043d\u0438\u043e\u043d \u0446\u0435\u043b\u0435\u0432\u043e\u0439 \u043e\u0440\u0438\u0435\u043d\u0442\u0430\u0446\u0438\u0438.<\/p>\n<pre><code class=\"cpp\">void eulerToQuaternion(int joystick_x, int joystick_y, int right_left, float q[4]) {     \/\/ \u041d\u043e\u0440\u043c\u0430\u043b\u0438\u0437\u0430\u0446\u0438\u044f \u0432\u0445\u043e\u0434\u043d\u044b\u0445 \u0437\u043d\u0430\u0447\u0435\u043d\u0438\u0439 \u043a \u0434\u0438\u0430\u043f\u0430\u0437\u043e\u043d\u0443 [-1, 1] \u0438 \u043f\u0440\u0435\u043e\u0431\u0440\u0430\u0437\u043e\u0432\u0430\u043d\u0438\u0435 \u0432 \u0440\u0430\u0434\u0438\u0430\u043d\u044b     float roll = (joystick_y \/ 100.0f) * M_PI;   \/\/ \u041e\u0441\u044c <\/code><\/pre>\n<\/div>\n<\/div>\n<\/div>\n<\/div>\n","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"footnotes":""},"categories":[],"tags":[],"class_list":["post-466346","post","type-post","status-publish","format-standard","hentry"],"_links":{"self":[{"href":"https:\/\/savepearlharbor.com\/index.php?rest_route=\/wp\/v2\/posts\/466346","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/savepearlharbor.com\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/savepearlharbor.com\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/savepearlharbor.com\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/savepearlharbor.com\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=466346"}],"version-history":[{"count":0,"href":"https:\/\/savepearlharbor.com\/index.php?rest_route=\/wp\/v2\/posts\/466346\/revisions"}],"wp:attachment":[{"href":"https:\/\/savepearlharbor.com\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=466346"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/savepearlharbor.com\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=466346"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/savepearlharbor.com\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=466346"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}