{"id":2324,"date":"2026-02-16T14:18:52","date_gmt":"2026-02-16T05:18:52","guid":{"rendered":"https:\/\/rfsec.ddns.net\/db\/?p=2324"},"modified":"2026-02-18T08:53:46","modified_gmt":"2026-02-17T23:53:46","slug":"%e3%82%b3%e3%83%b3%e3%83%91%e3%82%b9","status":"publish","type":"post","link":"https:\/\/rfsec.ddns.net\/db\/?p=2324","title":{"rendered":"\u30b3\u30f3\u30d1\u30b9\uff08ICM20948\uff09"},"content":{"rendered":"<div class=\"wp-block-ub-content-toggle wp-block-ub-content-toggle-block\" id=\"ub-content-toggle-block-343bf833-f196-4431-a4a5-686c0d6d47e0\" data-mobilecollapse=\"false\" data-desktopcollapse=\"true\" data-preventcollapse=\"false\" data-showonlyone=\"false\">\n<div class=\"wp-block-ub-content-toggle-accordion\" style=\"border-color: #f1f1f1;\" id=\"ub-content-toggle-panel-block-\">\n\t\t\t<div class=\"wp-block-ub-content-toggle-accordion-title-wrap\" style=\"background-color: #f1f1f1;\" aria-controls=\"ub-content-toggle-panel-0-343bf833-f196-4431-a4a5-686c0d6d47e0\" tabindex=\"0\">\n\t\t\t<p class=\"wp-block-ub-content-toggle-accordion-title ub-content-toggle-title-343bf833-f196-4431-a4a5-686c0d6d47e0\" style=\"color: #000000; \">\u6e29\u5ea6\u88dc\u6b63<\/p>\n\t\t\t<div class=\"wp-block-ub-content-toggle-accordion-toggle-wrap right\" style=\"color: #000000;\"><span class=\"wp-block-ub-content-toggle-accordion-state-indicator wp-block-ub-chevron-down\"><\/span><\/div>\n\t\t<\/div>\n\t\t\t<div role=\"region\" aria-expanded=\"false\" class=\"wp-block-ub-content-toggle-accordion-content-wrap ub-hide\" id=\"ub-content-toggle-panel-0-343bf833-f196-4431-a4a5-686c0d6d47e0\">\n\n<p><\/p>\n\n\n\n<div class=\"hcb_wrap\"><pre class=\"prism line-numbers lang-plain\"><code>\/*\n * ICM-20948 \u6e29\u5ea6\u88dc\u6b63\u30ac\u30a4\u30c9\n *\/\n\n\/\/ =========================================\n\/\/ \u6e29\u5ea6\u88dc\u6b63\u306e\u4ed5\u7d44\u307f\n\/\/ =========================================\n\nICM-20948\u306b\u306f\u6e29\u5ea6\u30bb\u30f3\u30b5\u30fc\u304c\u5185\u8535\u3055\u308c\u3066\u304a\u308a\u3001\n\u30bb\u30f3\u30b5\u30fc\u30c7\u30fc\u30bf\u306f\u6e29\u5ea6\u306b\u3088\u3063\u3066\u5909\u5316\u3057\u307e\u3059\u3002\n\n\u5f71\u97ff\u3092\u53d7\u3051\u308b\u30bb\u30f3\u30b5\u30fc:\n1. \u30b8\u30e3\u30a4\u30ed\u30b9\u30b3\u30fc\u30d7 - \u6e29\u5ea6\u30c9\u30ea\u30d5\u30c8\u304c\u6700\u3082\u5927\u304d\u3044\n2. \u78c1\u6c17\u30bb\u30f3\u30b5\u30fc - \u6e29\u5ea6\u306b\u3088\u308b\u611f\u5ea6\u5909\u5316\n3. \u52a0\u901f\u5ea6\u30bb\u30f3\u30b5\u30fc - \u6e29\u5ea6\u306b\u3088\u308b\u611f\u5ea6\u5909\u5316\uff08\u5c0f\u3055\u3044\uff09\n\n\n\/\/ =========================================\n\/\/ \u5b9f\u88c5\u3055\u308c\u305f\u6e29\u5ea6\u88dc\u6b63\n\/\/ =========================================\n\n1. \u6e29\u5ea6\u8aad\u307f\u53d6\u308a\n   temperature = myImu.temp();  \/\/ \u00b0C\n\n2. \u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\u6642\u306e\u6e29\u5ea6\u3092\u8a18\u9332\n   temperatureOffset = temperature;\n\n3. \u6e29\u5ea6\u5dee\u3092\u8a08\u7b97\n   tempDiff = temperature - temperatureOffset;\n\n4. \u30bb\u30f3\u30b5\u30fc\u30c7\u30fc\u30bf\u3092\u88dc\u6b63\n   - \u30b8\u30e3\u30a4\u30ed: gx -= gyroTempCoeff * tempDiff;\n   - \u78c1\u6c17: mx -= magTempCoeffX * tempDiff;\n\n\n\/\/ =========================================\n\/\/ \u6e29\u5ea6\u4fc2\u6570\u306e\u8abf\u6574\n\/\/ =========================================\n\n\u30b3\u30fc\u30c9\u5185\u306e\u6e29\u5ea6\u4fc2\u6570:\n\nconst float gyroTempCoeff = 0.01;  \/\/ \u30b8\u30e3\u30a4\u30ed: 1\u00b0C\u3042\u305f\u308a0.01 deg\/s\nconst float magTempCoeffX = 0.05;  \/\/ \u78c1\u6c17X: 1\u00b0C\u3042\u305f\u308a0.05 \u00b5T\nconst float magTempCoeffY = 0.05;  \/\/ \u78c1\u6c17Y: 1\u00b0C\u3042\u305f\u308a0.05 \u00b5T\nconst float magTempCoeffZ = 0.05;  \/\/ \u78c1\u6c17Z: 1\u00b0C\u3042\u305f\u308a0.05 \u00b5T\n\n\u3053\u308c\u3089\u306e\u5024\u306f\u500b\u4f53\u5dee\u304c\u3042\u308b\u305f\u3081\u3001\u8abf\u6574\u304c\u5fc5\u8981\u306a\u5834\u5408\u304c\u3042\u308a\u307e\u3059\u3002\n\n\n\/\/ =========================================\n\/\/ \u6e29\u5ea6\u4fc2\u6570\u306e\u6e2c\u5b9a\u65b9\u6cd5\n\/\/ =========================================\n\n\u3010\u65b9\u6cd51\u3011\u7c21\u6613\u6e2c\u5b9a\n\n1. \u30bb\u30f3\u30b5\u30fc\u3092\u5ba4\u6e29\u306b\u653e\u7f6e\uff0820-25\u00b0C\uff09\n2. \u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\u5b9f\u884c\n3. \u30bb\u30f3\u30b5\u30fc\u3092\u51b7\u8535\u5eab\u306b\u5165\u308c\u308b\uff085-10\u00b0C\uff09\n4. 10\u5206\u5f85\u3063\u3066\u5b89\u5b9a\u3055\u305b\u308b\n5. \u65b9\u4f4d\u306e\u305a\u308c\u3092\u6e2c\u5b9a\n6. \u6e29\u5ea6\u5dee\u3067\u5272\u308b\n\n\u4f8b: \n- \u6e29\u5ea6\u5dee: 25\u00b0C \u2192 10\u00b0C = 15\u00b0C\n- \u65b9\u4f4d\u305a\u308c: 3\u00b0\n- \u4fc2\u6570 = 3\u00b0 \/ 15\u00b0C = 0.2\u00b0 per \u00b0C\n\n\u3010\u65b9\u6cd52\u3011\u7cbe\u5bc6\u6e2c\u5b9a\n\n1. \u6052\u6e29\u69fd\u3067\u8907\u6570\u306e\u6e29\u5ea6\u70b9\u3067\u6e2c\u5b9a\n2. \u6e29\u5ea6 vs \u30c9\u30ea\u30d5\u30c8\u306e\u30b0\u30e9\u30d5\u3092\u4f5c\u6210\n3. \u7dda\u5f62\u8fd1\u4f3c\u3057\u3066\u4fc2\u6570\u3092\u8a08\u7b97\n\n\n\/\/ =========================================\n\/\/ \u6e29\u5ea6\u8868\u793a\n\/\/ =========================================\n\n\u6e29\u5ea6\u306f\u4ee5\u4e0b\u306e\u753b\u9762\u3067\u8868\u793a\u3055\u308c\u307e\u3059:\n\n1. I2C\u30b9\u30ad\u30e3\u30f3\u753b\u9762\n   - \u53f3\u4e0a\u306b \"Temp: XX.XC\" \u3068\u8868\u793a\n   - \u30ea\u30a2\u30eb\u30bf\u30a4\u30e0\u66f4\u65b0\n\n2. \u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\u5b8c\u4e86\u753b\u9762\n   - \u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\u6642\u306e\u6e29\u5ea6\u3092\u8a18\u9332\n   - \"Temp: XX.XC\" \u3068\u8868\u793a\n\n3. COMPASS\u753b\u9762\uff08\u5b9f\u88c5\u6e08\u307f\uff09\n   - \u53f3\u4e0a\u306b\u5c0f\u3055\u304f\u6e29\u5ea6\u8868\u793a\n\n\n\/\/ =========================================\n\/\/ \u6e29\u5ea6\u88dc\u6b63\u306e\u52b9\u679c\n\/\/ =========================================\n\n\u6e29\u5ea6\u88dc\u6b63\u306a\u3057:\n- 10\u00b0C\u6e29\u5ea6\u5909\u5316 \u2192 \u65b9\u4f4d\u305a\u308c 2-5\u00b0\n- \u30b8\u30e3\u30a4\u30ed\u30c9\u30ea\u30d5\u30c8\u5897\u52a0\n\n\u6e29\u5ea6\u88dc\u6b63\u3042\u308a:\n- 10\u00b0C\u6e29\u5ea6\u5909\u5316 \u2192 \u65b9\u4f4d\u305a\u308c 0.5-1\u00b0\n- \u30b8\u30e3\u30a4\u30ed\u30c9\u30ea\u30d5\u30c8\u6700\u5c0f\u5316\n\n\n\/\/ =========================================\n\/\/ \u6e29\u5ea6\u88dc\u6b63\u3092\u7121\u52b9\u306b\u3059\u308b\n\/\/ =========================================\n\n\u6e29\u5ea6\u88dc\u6b63\u304c\u4e0d\u8981\u306a\u5834\u5408\u306f\u3001\u4fc2\u6570\u3092\u30bc\u30ed\u306b\u3057\u307e\u3059:\n\nconst float gyroTempCoeff = 0.0;   \/\/ \u7121\u52b9\nconst float magTempCoeffX = 0.0;   \/\/ \u7121\u52b9\nconst float magTempCoeffY = 0.0;   \/\/ \u7121\u52b9\nconst float magTempCoeffZ = 0.0;   \/\/ \u7121\u52b9\n\n\n\/\/ =========================================\n\/\/ ICM-20948\u306e\u6e29\u5ea6\u7279\u6027\n\/\/ =========================================\n\n\u30b8\u30e3\u30a4\u30ed\u30b9\u30b3\u30fc\u30d7:\n- \u6e29\u5ea6\u4fc2\u6570: \u7d04 0.01-0.05 deg\/s per \u00b0C\n- \u7dda\u5f62\u6027: \u826f\u597d\n- \u6700\u3082\u6e29\u5ea6\u306e\u5f71\u97ff\u3092\u53d7\u3051\u308b\n\n\u78c1\u6c17\u30bb\u30f3\u30b5\u30fc (AK09916):\n- \u6e29\u5ea6\u4fc2\u6570: \u7d04 0.05-0.2 \u00b5T per \u00b0C\n- \u3084\u3084\u975e\u7dda\u5f62\n- \u500b\u4f53\u5dee\u304c\u5927\u304d\u3044\n\n\u52a0\u901f\u5ea6\u30bb\u30f3\u30b5\u30fc:\n- \u6e29\u5ea6\u4fc2\u6570: \u7d04 0.001 g per \u00b0C\n- \u5f71\u97ff\u306f\u5c0f\u3055\u3044\n- \u901a\u5e38\u306f\u88dc\u6b63\u4e0d\u8981\n\n\n\/\/ =========================================\n\/\/ \u52d5\u4f5c\u6e29\u5ea6\u7bc4\u56f2\n\/\/ =========================================\n\nICM-20948\u306e\u4ed5\u69d8:\n- \u52d5\u4f5c\u6e29\u5ea6\u7bc4\u56f2: -40\u00b0C to +85\u00b0C\n- \u63a8\u5968\u6e29\u5ea6\u7bc4\u56f2: 0\u00b0C to +70\u00b0C\n\n\u6700\u9069\u306a\u7cbe\u5ea6\u304c\u5f97\u3089\u308c\u308b\u6e29\u5ea6:\n- 20\u00b0C to 30\u00b0C\n\n\n\/\/ =========================================\n\/\/ \u6e29\u5ea6\u30c9\u30ea\u30d5\u30c8\u306e\u6700\u5c0f\u5316\n\/\/ =========================================\n\n1. \u30a6\u30a9\u30fc\u30e0\u30a2\u30c3\u30d7\n   - \u96fb\u6e90\u6295\u5165\u5f8c5-10\u5206\u5f85\u3064\n   - \u30bb\u30f3\u30b5\u30fc\u304c\u5b89\u5b9a\u3059\u308b\u307e\u3067\u5f85\u3064\n\n2. \u65ad\u71b1\n   - \u30bb\u30f3\u30b5\u30fc\u3092\u65ad\u71b1\u6750\u3067\u8986\u3046\n   - \u6025\u6fc0\u306a\u6e29\u5ea6\u5909\u5316\u3092\u907f\u3051\u308b\n\n3. \u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\n   - \u4f7f\u7528\u6e29\u5ea6\u3067\u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\n   - \u6e29\u5ea6\u304c\u5909\u308f\u3063\u305f\u3089\u518d\u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\n\n\n\/\/ =========================================\n\/\/ \u30c8\u30e9\u30d6\u30eb\u30b7\u30e5\u30fc\u30c6\u30a3\u30f3\u30b0\n\/\/ =========================================\n\n\u3010\u554f\u984c1\u3011\u6e29\u5ea6\u88dc\u6b63\u5f8c\u3082\u65b9\u4f4d\u304c\u305a\u308c\u308b\n\u2192 \u6e29\u5ea6\u4fc2\u6570\u304c\u4e0d\u6b63\u78ba\n\u2192 \u5b9f\u6e2c\u3057\u3066\u8abf\u6574\n\n\u3010\u554f\u984c2\u3011\u6e29\u5ea6\u304c\u8868\u793a\u3055\u308c\u306a\u3044\n\u2192 myImu.temp() \u304c\u52d5\u4f5c\u3057\u3066\u3044\u306a\u3044\n\u2192 \u30bb\u30f3\u30b5\u30fc\u521d\u671f\u5316\u3092\u78ba\u8a8d\n\n\u3010\u554f\u984c3\u3011\u6e29\u5ea6\u304c\u7570\u5e38\u306b\u9ad8\u3044\/\u4f4e\u3044\n\u2192 \u30bb\u30f3\u30b5\u30fc\u306e\u81ea\u5df1\u767a\u71b1\n\u2192 \u5468\u56f2\u306e\u71b1\u6e90\u3092\u78ba\u8a8d\n\n\u3010\u554f\u984c4\u3011\u6e29\u5ea6\u88dc\u6b63\u5f8c\u306b\u4e0d\u5b89\u5b9a\n\u2192 \u6e29\u5ea6\u4fc2\u6570\u304c\u5927\u304d\u3059\u304e\u308b\n\u2192 \u4fc2\u6570\u3092\u5c0f\u3055\u304f\u3057\u3066\u8abf\u6574\n\n\n\/\/ =========================================\n\/\/ \u53c2\u8003\u60c5\u5831\n\/\/ =========================================\n\nICM-20948\u30c7\u30fc\u30bf\u30b7\u30fc\u30c8:\n<blockquote class=\"wp-embedded-content\" data-secret=\"rAHvNLWFCk\"><a href=\"https:\/\/invensense.tdk.com\/products\/motion-tracking\/9-axis\/icm-20948\/\">ICM-20948<\/a><\/blockquote><iframe loading=\"lazy\" class=\"wp-embedded-content\" sandbox=\"allow-scripts\" security=\"restricted\" style=\"position: absolute; visibility: hidden;\" title=\"\u201cICM-20948\u201d \u2014 TDK InvenSense\" src=\"https:\/\/invensense.tdk.com\/products\/motion-tracking\/9-axis\/icm-20948\/embed\/#?secret=cHLaYYt34B#?secret=rAHvNLWFCk\" data-secret=\"rAHvNLWFCk\" width=\"600\" height=\"338\" frameborder=\"0\" marginwidth=\"0\" marginheight=\"0\" scrolling=\"no\"><\/iframe>\n\n\u6e29\u5ea6\u88dc\u6b63\u306e\u7406\u8ad6:\n- Temperature compensation for MEMS gyroscopes\n- Thermal drift correction in magnetometers\n\n*\/<\/code><\/pre><\/div>\n\n<\/div>\n\t\t<\/div>\n<\/div>\n\n<div class=\"wp-block-ub-content-toggle wp-block-ub-content-toggle-block\" id=\"ub-content-toggle-block-62b4bb4b-0a07-4116-ab54-4a87df0d5d84\" data-mobilecollapse=\"false\" data-desktopcollapse=\"true\" data-preventcollapse=\"false\" data-showonlyone=\"false\">\n<div class=\"wp-block-ub-content-toggle-accordion\" style=\"border-color: #f1f1f1;\" id=\"ub-content-toggle-panel-block-\">\n\t\t\t<div class=\"wp-block-ub-content-toggle-accordion-title-wrap\" style=\"background-color: #f1f1f1;\" aria-controls=\"ub-content-toggle-panel-0-62b4bb4b-0a07-4116-ab54-4a87df0d5d84\" tabindex=\"0\">\n\t\t\t<p class=\"wp-block-ub-content-toggle-accordion-title ub-content-toggle-title-62b4bb4b-0a07-4116-ab54-4a87df0d5d84\" style=\"color: #000000; \">\u30c9\u30ea\u30d5\u30c8\u88dc\u6b63\u30d1\u30e9\u30e1\u30fc\u30bf\u8abf\u6574\u30ac\u30a4\u30c9<\/p>\n\t\t\t<div class=\"wp-block-ub-content-toggle-accordion-toggle-wrap right\" style=\"color: #000000;\"><span class=\"wp-block-ub-content-toggle-accordion-state-indicator wp-block-ub-chevron-down\"><\/span><\/div>\n\t\t<\/div>\n\t\t\t<div role=\"region\" aria-expanded=\"false\" class=\"wp-block-ub-content-toggle-accordion-content-wrap ub-hide\" id=\"ub-content-toggle-panel-0-62b4bb4b-0a07-4116-ab54-4a87df0d5d84\">\n\n<p><\/p>\n\n\n\n<div class=\"hcb_wrap\"><pre class=\"prism line-numbers lang-plain\"><code>\/*\n * \u30c9\u30ea\u30d5\u30c8\u88dc\u6b63\u30d1\u30e9\u30e1\u30fc\u30bf\u8abf\u6574\u30ac\u30a4\u30c9\n * \n * \u30bb\u30f3\u30b5\u30fc\u3092\u56fa\u5b9a\u3057\u3066\u3044\u3066\u3082\u5024\u304c\u5909\u5316\u3059\u308b\u5834\u5408\u306e\u5bfe\u51e6\u6cd5\n *\/\n\n\/\/ =========================================\n\/\/ \u30c9\u30ea\u30d5\u30c8\u306e\u539f\u56e0\n\/\/ =========================================\n\n1. \u30b8\u30e3\u30a4\u30ed\u30bb\u30f3\u30b5\u30fc\u306e\u30d0\u30a4\u30a2\u30b9\u8aa4\u5dee\n   - \u7a4d\u5206\u3059\u308b\u3053\u3068\u3067\u8aa4\u5dee\u304c\u7d2f\u7a4d\n   - \u6642\u9593\u3068\u3068\u3082\u306b\u305a\u308c\u304c\u5927\u304d\u304f\u306a\u308b\n\n2. \u74b0\u5883\u8981\u56e0\n   - \u6e29\u5ea6\u5909\u5316\n   - \u632f\u52d5\n   - \u96fb\u78c1\u30ce\u30a4\u30ba\n\n3. \u30bb\u30f3\u30b5\u30fc\u306e\u54c1\u8cea\n   - \u4f4e\u4fa1\u683c\u30bb\u30f3\u30b5\u30fc\u306f\u30c9\u30ea\u30d5\u30c8\u304c\u5927\u304d\u3044\n\n\n\/\/ =========================================\n\/\/ \u8abf\u6574\u53ef\u80fd\u306a\u30d1\u30e9\u30e1\u30fc\u30bf (3\u3064)\n\/\/ =========================================\n\n1. ALPHA_ACCEL (\u52a0\u901f\u5ea6\u8a08\u306e\u4fe1\u983c\u5ea6)\n   \u73fe\u5728\u5024: 0.05\n   \u7bc4\u56f2: 0.01 - 0.1\n   \n   \u5c0f\u3055\u3044\u5024 (0.01-0.03):\n   \u2713 \u53cd\u5fdc\u304c\u901f\u3044\n   \u2717 \u30c9\u30ea\u30d5\u30c8\u3057\u3084\u3059\u3044\n   \n   \u5927\u304d\u3044\u5024 (0.05-0.1):\n   \u2713 \u5b89\u5b9a\u3001\u30c9\u30ea\u30d5\u30c8\u5c11\u306a\u3044\n   \u2717 \u53cd\u5fdc\u304c\u9045\u3044\n\n   \u63a8\u5968\u8a2d\u5b9a:\n   - \u624b\u6301\u3061\u4f7f\u7528: 0.02-0.03\n   - \u56fa\u5b9a\u4f7f\u7528: 0.05-0.08\n\n\n2. ALPHA_MAG (\u78c1\u6c17\u30bb\u30f3\u30b5\u30fc\u306e\u4fe1\u983c\u5ea6)\n   \u73fe\u5728\u5024: 0.1\n   \u7bc4\u56f2: 0.05 - 0.3\n   \n   \u5c0f\u3055\u3044\u5024 (0.05-0.1):\n   \u2713 \u6ed1\u3089\u304b\u306a\u52d5\u304d\n   \u2717 \u65b9\u4f4d\u304c\u3075\u3089\u3064\u304f\n   \n   \u5927\u304d\u3044\u5024 (0.15-0.3):\n   \u2713 \u65b9\u4f4d\u304c\u5b89\u5b9a\n   \u2717 \u30ab\u30af\u30ab\u30af\u3059\u308b\n\n   \u63a8\u5968\u8a2d\u5b9a:\n   - \u96fb\u78c1\u30ce\u30a4\u30ba\u304c\u591a\u3044: 0.05-0.1\n   - \u9759\u304b\u306a\u74b0\u5883: 0.15-0.2\n\n\n3. GYRO_DRIFT_THRESHOLD (\u30c9\u30ea\u30d5\u30c8\u88dc\u6b63\u306e\u95be\u5024)\n   \u73fe\u5728\u5024: 2.0 deg\/s\n   \u7bc4\u56f2: 0.5 - 5.0\n   \n   \u5c0f\u3055\u3044\u5024 (0.5-1.0):\n   \u2713 \u5c0f\u3055\u306a\u52d5\u304d\u3067\u3082\u53cd\u5fdc\n   \u2717 \u9759\u6b62\u5224\u5b9a\u304c\u53b3\u3057\u3044\n   \n   \u5927\u304d\u3044\u5024 (2.0-5.0):\n   \u2713 \u9759\u6b62\u4e2d\u306f\u5f37\u529b\u306b\u88dc\u6b63\n   \u2717 \u9045\u3044\u52d5\u304d\u3067\u8ffd\u5f93\u304c\u60aa\u3044\n\n   \u63a8\u5968\u8a2d\u5b9a:\n   - \u5b8c\u5168\u306b\u56fa\u5b9a: 3.0-5.0\n   - \u624b\u6301\u3061: 1.0-2.0\n\n\n\/\/ =========================================\n\/\/ \u30b7\u30fc\u30f3\u5225\u63a8\u5968\u8a2d\u5b9a\n\/\/ =========================================\n\n\u3010\u30b7\u30fc\u30f31\u3011\u5b8c\u5168\u306b\u56fa\u5b9a\u3001\u5b89\u5b9a\u6027\u6700\u512a\u5148\nconst float ALPHA_ACCEL = 0.08;\nconst float ALPHA_MAG = 0.2;\nconst float GYRO_DRIFT_THRESHOLD = 5.0;\n\u2192 \u6700\u3082\u5b89\u5b9a\u3001\u30c9\u30ea\u30d5\u30c8\u307b\u307c\u30bc\u30ed\n\n\u3010\u30b7\u30fc\u30f32\u3011\u56fa\u5b9a\u3060\u304c\u53cd\u5fdc\u3082\u91cd\u8996\nconst float ALPHA_ACCEL = 0.05;\nconst float ALPHA_MAG = 0.15;\nconst float GYRO_DRIFT_THRESHOLD = 3.0;\n\u2192 \u30d0\u30e9\u30f3\u30b9\u578b\uff08\u73fe\u5728\u306e\u8a2d\u5b9a\u306b\u8fd1\u3044\uff09\n\n\u3010\u30b7\u30fc\u30f33\u3011\u624b\u6301\u3061\u3001\u53cd\u5fdc\u901f\u5ea6\u91cd\u8996\nconst float ALPHA_ACCEL = 0.02;\nconst float ALPHA_MAG = 0.08;\nconst float GYRO_DRIFT_THRESHOLD = 1.0;\n\u2192 \u7d20\u65e9\u304f\u53cd\u5fdc\u3001\u82e5\u5e72\u30c9\u30ea\u30d5\u30c8\u3042\u308a\n\n\u3010\u30b7\u30fc\u30f34\u3011\u8eca\u8f09\u3001\u632f\u52d5\u304c\u591a\u3044\nconst float ALPHA_ACCEL = 0.1;\nconst float ALPHA_MAG = 0.25;\nconst float GYRO_DRIFT_THRESHOLD = 2.0;\n\u2192 \u632f\u52d5\u306b\u5f37\u3044\n\n\n\/\/ =========================================\n\/\/ \u3055\u3089\u306a\u308b\u30c9\u30ea\u30d5\u30c8\u5bfe\u7b56\n\/\/ =========================================\n\n1. \u30ed\u30fc\u30d1\u30b9\u30d5\u30a3\u30eb\u30bf\u306e\u8ffd\u52a0\n   \u30bb\u30f3\u30b5\u30fc\u5024\u306b\u79fb\u52d5\u5e73\u5747\u3092\u9069\u7528\n\n2. \u30c7\u30c3\u30c9\u30be\u30fc\u30f3\u306e\u8a2d\u5b9a\n   \u5c0f\u3055\u306a\u5909\u5316\u306f\u7121\u8996\u3059\u308b\n\n3. \u6e29\u5ea6\u88dc\u6b63\n   ICM-20948\u306e\u6e29\u5ea6\u30bb\u30f3\u30b5\u30fc\u3092\u4f7f\u7528\n\n4. \u5b9a\u671f\u7684\u306a\u30ea\u30bb\u30c3\u30c8\n   \u9759\u6b62\u3092\u691c\u51fa\u3057\u305f\u3089\u59ff\u52e2\u3092\u30ea\u30bb\u30c3\u30c8\n\n\n\/\/ =========================================\n\/\/ \u30b3\u30fc\u30c9\u5185\u306e\u4fee\u6b63\u7b87\u6240\n\/\/ =========================================\n\n\u30d5\u30a1\u30a4\u30eb: compass_display.ino\n\n\u3010\u30d1\u30e9\u30e1\u30fc\u30bf\u5b9a\u7fa9\u3011(\u7d0430\u884c\u76ee)\nconst float ALPHA_ACCEL = 0.05;\nconst float ALPHA_MAG = 0.1;\nconst float GYRO_DRIFT_THRESHOLD = 2.0;\n\n\u3010\u9759\u6b62\u6642\u306e\u88dc\u6b63\u5f37\u5ea6\u3011(\u7d04220\u884c\u76ee)\nfloat driftAlpha = 0.2;  \/\/ 0.1-0.3 \u3067\u8abf\u6574\n\n\u3010\u30d8\u30c7\u30a3\u30f3\u30b0\u306e\u88dc\u6b63\u5f37\u5ea6\u3011(\u7d04265\u884c\u76ee)\nfloat yawDriftAlpha = 0.1;  \/\/ 0.05-0.2 \u3067\u8abf\u6574\n\n\n\/\/ =========================================\n\/\/ \u8abf\u6574\u624b\u9806\n\/\/ =========================================\n\n\u30b9\u30c6\u30c3\u30d71: \u30bb\u30f3\u30b5\u30fc\u3092\u5b8c\u5168\u306b\u56fa\u5b9a\n\u30b9\u30c6\u30c3\u30d72: Serial Monitor\u3067\u5024\u306e\u5909\u5316\u3092\u89b3\u5bdf\n\u30b9\u30c6\u30c3\u30d73: \u30c9\u30ea\u30d5\u30c8\u304c\u5927\u304d\u3044\u8ef8\u3092\u7279\u5b9a\n  - Roll \u304c\u30c9\u30ea\u30d5\u30c8 \u2192 ALPHA_ACCEL \u3092\u5897\u3084\u3059\n  - Heading \u304c\u30c9\u30ea\u30d5\u30c8 \u2192 ALPHA_MAG \u3092\u5897\u3084\u3059\n\u30b9\u30c6\u30c3\u30d74: \u30d1\u30e9\u30e1\u30fc\u30bf\u3092\u5909\u66f4\u3057\u3066\u30a2\u30c3\u30d7\u30ed\u30fc\u30c9\n\u30b9\u30c6\u30c3\u30d75: 5\u5206\u9593\u89b3\u5bdf\u3057\u3066\u518d\u8abf\u6574\n\u30b9\u30c6\u30c3\u30d76: \u6e80\u8db3\u3044\u304f\u8a2d\u5b9a\u3092\u8a18\u9332\n\n\n\/\/ =========================================\n\/\/ \u30c7\u30c3\u30c9\u30be\u30fc\u30f3\u306e\u8ffd\u52a0 (\u30aa\u30d7\u30b7\u30e7\u30f3)\n\/\/ =========================================\n\ndisplayCompass()\u95a2\u6570\u5185\u306b\u8ffd\u52a0:\n\n  \/\/ \u5fae\u5c0f\u5909\u5316\u3092\u7121\u8996\n  float rollChange = abs(roll - prevRoll);\n  float pitchChange = abs(pitch - prevPitch);\n  \n  if (rollChange &lt; 0.3) roll = prevRoll;   \/\/ 0.3\u5ea6\u4ee5\u4e0b\u306f\u7121\u8996\n  if (pitchChange &lt; 0.3) pitch = prevPitch;\n\n\n\/\/ =========================================\n\/\/ \u79fb\u52d5\u5e73\u5747\u30d5\u30a3\u30eb\u30bf\u306e\u8ffd\u52a0 (\u30aa\u30d7\u30b7\u30e7\u30f3)\n\/\/ =========================================\n\n\u30b0\u30ed\u30fc\u30d0\u30eb\u5909\u6570\u306b\u8ffd\u52a0:\nfloat rollHistory[5] = {0};\nint rollIndex = 0;\n\nsensorFusion()\u306e\u6700\u5f8c\u306b\u8ffd\u52a0:\n  \/\/ \u79fb\u52d5\u5e73\u5747\n  rollHistory[rollIndex] = fusedRoll;\n  rollIndex = (rollIndex + 1) % 5;\n  \n  float rollSum = 0;\n  for(int i = 0; i &lt; 5; i++) {\n    rollSum += rollHistory[i];\n  }\n  roll = rollSum \/ 5.0;\n\n\n\/\/ =========================================\n\/\/ \u30c8\u30e9\u30d6\u30eb\u30b7\u30e5\u30fc\u30c6\u30a3\u30f3\u30b0\n\/\/ =========================================\n\n\u3010\u554f\u984c1\u3011Roll\/Pitch\u304c\u5e38\u306b\u5909\u5316\u3059\u308b\n\u2192 ALPHA_ACCEL \u3092 0.08-0.1 \u306b\u5897\u3084\u3059\n\u2192 GYRO_DRIFT_THRESHOLD \u3092 3.0-5.0 \u306b\u5897\u3084\u3059\n\n\u3010\u554f\u984c2\u3011Heading\u304c\u3075\u3089\u3064\u304f\n\u2192 ALPHA_MAG \u3092 0.15-0.25 \u306b\u5897\u3084\u3059\n\u2192 \u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\u3092\u3084\u308a\u76f4\u3059\n\n\u3010\u554f\u984c3\u3011\u5168\u4f53\u7684\u306b\u4e0d\u5b89\u5b9a\n\u2192 \u5468\u56f2\u306e\u78c1\u77f3\u3084\u91d1\u5c5e\u3092\u9060\u3056\u3051\u308b\n\u2192 \u96fb\u6e90\u30b1\u30fc\u30d6\u30eb\u304b\u3089\u96e2\u3059\n\u2192 ICM-20948\u306e\u56fa\u5b9a\u3092\u78ba\u8a8d\n\n\u3010\u554f\u984c4\u3011\u8a2d\u5b9a\u3092\u5909\u3048\u3066\u3082\u6539\u5584\u3057\u306a\u3044\n\u2192 \u30bb\u30f3\u30b5\u30fc\u81ea\u4f53\u306e\u554f\u984c\u306e\u53ef\u80fd\u6027\n\u2192 \u5225\u306eICM-20948\u3067\u8a66\u3059\n\u2192 \u6e29\u5ea6\u304c\u5b89\u5b9a\u3059\u308b\u307e\u3067\u5f85\u3064(5-10\u5206)\n\n*\/<\/code><\/pre><\/div>\n\n<\/div>\n\t\t<\/div>\n<\/div>\n\n\n<p><\/p>\n\n\n<div class=\"wp-block-ub-content-toggle wp-block-ub-content-toggle-block\" id=\"ub-content-toggle-block-07d3f450-0920-4d59-b512-b5b041365aaf\" data-mobilecollapse=\"false\" data-desktopcollapse=\"true\" data-preventcollapse=\"false\" data-showonlyone=\"false\">\n<div class=\"wp-block-ub-content-toggle-accordion\" style=\"border-color: #f1f1f1;\" id=\"ub-content-toggle-panel-block-\">\n\t\t\t<div class=\"wp-block-ub-content-toggle-accordion-title-wrap\" style=\"background-color: #f1f1f1;\" aria-controls=\"ub-content-toggle-panel-0-07d3f450-0920-4d59-b512-b5b041365aaf\" tabindex=\"0\">\n\t\t\t<p class=\"wp-block-ub-content-toggle-accordion-title ub-content-toggle-title-07d3f450-0920-4d59-b512-b5b041365aaf\" style=\"color: #000000; \">Madgwick AHRS<\/p>\n\t\t\t<div class=\"wp-block-ub-content-toggle-accordion-toggle-wrap right\" style=\"color: #000000;\"><span class=\"wp-block-ub-content-toggle-accordion-state-indicator wp-block-ub-chevron-down\"><\/span><\/div>\n\t\t<\/div>\n\t\t\t<div role=\"region\" aria-expanded=\"false\" class=\"wp-block-ub-content-toggle-accordion-content-wrap ub-hide\" id=\"ub-content-toggle-panel-0-07d3f450-0920-4d59-b512-b5b041365aaf\">\n\n<p><\/p>\n\n\n\n<div class=\"hcb_wrap\"><pre class=\"prism line-numbers lang-plain\"><code>\/*\n * Madgwick AHRS\u30a2\u30eb\u30b4\u30ea\u30ba\u30e0\u5b9f\u88c5\u30ac\u30a4\u30c9\n * \n * \u5f93\u6765\u306eComplementary Filter\u3068\u306e\u6bd4\u8f03\n *\/\n\n\/\/ =========================================\n\/\/ Madgwick AHRS\u3068\u306f\uff1f\n\/\/ =========================================\n\nMadgwick AHRS\u306f\u3001Sebastian Madgwick\u304c\u958b\u767a\u3057\u305f\n\u9ad8\u5ea6\u306a\u59ff\u52e2\u63a8\u5b9a\u30a2\u30eb\u30b4\u30ea\u30ba\u30e0\u3067\u3059\u3002\n\n\u7279\u5fb4:\n- \u30af\u30a9\u30fc\u30bf\u30cb\u30aa\u30f3\u30d9\u30fc\u30b9\uff08\u30b8\u30f3\u30d0\u30eb\u30ed\u30c3\u30af\u56de\u907f\uff09\n- 9\u8ef8\u30bb\u30f3\u30b5\u30fc\u878d\u5408\uff08\u52a0\u901f\u5ea6+\u30b8\u30e3\u30a4\u30ed+\u78c1\u6c17\uff09\n- \u8a08\u7b97\u52b9\u7387\u304c\u826f\u3044\uff08\u7d44\u307f\u8fbc\u307f\u306b\u6700\u9069\uff09\n- \u30c9\u30ea\u30d5\u30c8\u88dc\u6b63\u304c\u512a\u79c0\n\n\n\/\/ =========================================\n\/\/ \u5f93\u6765\u65b9\u5f0f\u3068\u306e\u6bd4\u8f03\n\/\/ =========================================\n\n\u3010Complementary Filter\u3011\uff08\u5f93\u6765\u306e\u30b3\u30fc\u30c9\uff09\n\u2713 \u30b7\u30f3\u30d7\u30eb\n\u2713 \u8efd\u91cf\n\u2717 \u30b8\u30f3\u30d0\u30eb\u30ed\u30c3\u30af\u767a\u751f\n\u2717 \u9577\u6642\u9593\u3067\u30c9\u30ea\u30d5\u30c8\n\u2717 \u78c1\u6c17\u30bb\u30f3\u30b5\u30fc\u306e\u6271\u3044\u304c\u5358\u7d14\n\n\u3010Madgwick AHRS\u3011\uff08\u65b0\u5b9f\u88c5\uff09\n\u2713 \u30b8\u30f3\u30d0\u30eb\u30ed\u30c3\u30af\u306a\u3057\n\u2713 \u9577\u6642\u9593\u5b89\u5b9a\n\u2713 \u9ad8\u7cbe\u5ea6\u306a\u78c1\u6c17\u88dc\u6b63\n\u2713 \u696d\u754c\u6a19\u6e96\u30a2\u30eb\u30b4\u30ea\u30ba\u30e0\n\u2717 \u3084\u3084\u8907\u96d1\n\u2717 \u30e9\u30a4\u30d6\u30e9\u30ea\u304c\u5fc5\u8981\n\n\n\/\/ =========================================\n\/\/ \u5fc5\u8981\u306a\u30e9\u30a4\u30d6\u30e9\u30ea\n\/\/ =========================================\n\nArduino IDE\u3067\u30a4\u30f3\u30b9\u30c8\u30fc\u30eb:\n\n1. Madgwick\n   \u30e9\u30a4\u30d6\u30e9\u30ea\u30de\u30cd\u30fc\u30b8\u30e3\u3067\u691c\u7d22: \"Madgwick\"\n   \u4f5c\u8005: Arduino\n   \u30d0\u30fc\u30b8\u30e7\u30f3: \u6700\u65b0\u7248\n\n\u30a4\u30f3\u30b9\u30c8\u30fc\u30eb\u624b\u9806:\n  Arduino IDE \u2192 \u30c4\u30fc\u30eb \u2192 \u30e9\u30a4\u30d6\u30e9\u30ea\u3092\u7ba1\u7406\n  \u2192 \"Madgwick\" \u3067\u691c\u7d22 \u2192 \u30a4\u30f3\u30b9\u30c8\u30fc\u30eb\n\n\n\/\/ =========================================\n\/\/ \u4e3b\u306a\u5909\u66f4\u70b9\n\/\/ =========================================\n\n1. \u30bb\u30f3\u30b5\u30fc\u30d5\u30e5\u30fc\u30b8\u30e7\u30f3\u30a2\u30eb\u30b4\u30ea\u30ba\u30e0\n   Complementary Filter \u2192 Madgwick AHRS\n\n2. \u59ff\u52e2\u8868\u73fe\n   \u30aa\u30a4\u30e9\u30fc\u89d2 \u2192 \u30af\u30a9\u30fc\u30bf\u30cb\u30aa\u30f3 \u2192 \u30aa\u30a4\u30e9\u30fc\u89d2\n\n3. \u30b8\u30e3\u30a4\u30ed\u30c7\u30fc\u30bf\n   deg\/s \u2192 rad\/s \u306b\u5909\u63db\n\n4. \u30bb\u30f3\u30b5\u30fc\u30ec\u30f3\u30b8\n   \u00b12g\/\u00b1250dps \u2192 \u00b116g\/\u00b12000dps (\u5e83\u7bc4\u56f2)\n\n5. \u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\n   Hard Iron\u88dc\u6b63\u306e\u307f \u2192 Hard + Soft Iron\u88dc\u6b63\n\n\n\/\/ =========================================\n\/\/ \u30d1\u30e9\u30e1\u30fc\u30bf\u8abf\u6574\n\/\/ =========================================\n\n\u30101. Sample Frequency\uff08\u30b5\u30f3\u30d7\u30eb\u5468\u6ce2\u6570\uff09\u3011\n\u73fe\u5728: 50Hz\n\u63a8\u5968\u7bc4\u56f2: 50-200Hz\n\n\u9ad8\u3044\u5024:\n\u2713 \u3088\u308a\u6b63\u78ba\n\u2717 CPU\u8ca0\u8377\u5897\n\n\u4f4e\u3044\u5024:\n\u2713 \u8efd\u3044\n\u2717 \u7cbe\u5ea6\u4f4e\u4e0b\n\n\u8a2d\u5b9a:\n  const float sampleFreq = 50.0f;\n\n\n\u30102. Beta\uff08\u30d5\u30a3\u30eb\u30bf\u30b2\u30a4\u30f3\uff09\u3011\n\u73fe\u5728: 0.033 (\u30c7\u30d5\u30a9\u30eb\u30c8)\n\u63a8\u5968\u7bc4\u56f2: 0.01-0.5\n\n\u5c0f\u3055\u3044\u5024 (0.01-0.05):\n\u2713 \u6ed1\u3089\u304b\n\u2713 \u30ce\u30a4\u30ba\u306b\u5f37\u3044\n\u2717 \u53ce\u675f\u304c\u9045\u3044\n\n\u5927\u304d\u3044\u5024 (0.1-0.5):\n\u2713 \u901f\u3044\u5fdc\u7b54\n\u2713 \u901f\u3044\u53ce\u675f\n\u2717 \u30ce\u30a4\u30b8\u30fc\n\n\u8a2d\u5b9a:\n  filter.setBeta(0.1f);  \/\/ setup()\u5185\n\n\n\/\/ =========================================\n\/\/ \u30b7\u30fc\u30f3\u5225\u63a8\u5968\u8a2d\u5b9a\n\/\/ =========================================\n\n\u3010\u30b7\u30fc\u30f31\u3011\u56fa\u5b9a\u8a2d\u7f6e\u3001\u5b89\u5b9a\u6027\u6700\u512a\u5148\nconst float sampleFreq = 50.0f;\nfilter.setBeta(0.033f);  \/\/ \u30c7\u30d5\u30a9\u30eb\u30c8\n\u2192 \u6700\u3082\u5b89\u5b9a\u3001\u30c9\u30ea\u30d5\u30c8\u307b\u307c\u30bc\u30ed\n\n\u3010\u30b7\u30fc\u30f32\u3011\u624b\u6301\u3061\u3001\u30d0\u30e9\u30f3\u30b9\u91cd\u8996\nconst float sampleFreq = 100.0f;\nfilter.setBeta(0.05f);\n\u2192 \u826f\u597d\u306a\u30d0\u30e9\u30f3\u30b9\n\n\u3010\u30b7\u30fc\u30f33\u3011\u9ad8\u901f\u52d5\u4f5c\u3001\u53cd\u5fdc\u901f\u5ea6\u512a\u5148\nconst float sampleFreq = 200.0f;\nfilter.setBeta(0.2f);\n\u2192 \u7d20\u65e9\u304f\u8ffd\u5f93\n\n\u3010\u30b7\u30fc\u30f34\u3011\u30c9\u30ed\u30fc\u30f3\u30fb\u30ed\u30dc\u30c3\u30c8\nconst float sampleFreq = 200.0f;\nfilter.setBeta(0.1f);\n\u2192 \u9ad8\u7cbe\u5ea6\u5236\u5fa1\n\n\n\/\/ =========================================\n\/\/ Madgwick vs Mahony\n\/\/ =========================================\n\nMadgwick\u306e\u4ee3\u308f\u308a\u306bMahony AHRS\u3082\u4f7f\u7528\u53ef\u80fd:\n\n#include &lt;MahonyAHRS.h&gt;\nMahony filter;\n\n\u7279\u5fb4:\n- Madgwick\u3088\u308a\u8efd\u91cf\n- \u3084\u3084\u7cbe\u5ea6\u306f\u52a3\u308b\n- \u30d1\u30e9\u30e1\u30fc\u30bf: Kp, Ki\n\n\u9078\u629e\u57fa\u6e96:\n- \u9ad8\u7cbe\u5ea6\u5fc5\u8981 \u2192 Madgwick\n- \u8efd\u91cf\u512a\u5148 \u2192 Mahony\n\n\n\/\/ =========================================\n\/\/ \u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\n\/\/ =========================================\n\nMadgwick\u7248\u3067\u306f\u4ee5\u4e0b\u3092\u5b9f\u884c:\n\n1. Hard Iron\u88dc\u6b63\uff08\u5f93\u6765\u901a\u308a\uff09\n   magOffsetX\/Y\/Z\n\n2. Soft Iron\u88dc\u6b63\uff08\u65b0\u6a5f\u80fd\uff09\n   magScaleX\/Y\/Z\n   \u2192 \u78c1\u5834\u306e\u6b6a\u307f\u3092\u88dc\u6b63\n\n\u5b9f\u884c\u65b9\u6cd5:\n  \u30dc\u30bf\u30f3\u9577\u62bc\u30572\u79d2 \u2192 8\u306e\u5b57\u306b\u52d5\u304b\u3059\n\n\n\/\/ =========================================\n\/\/ \u30c8\u30e9\u30d6\u30eb\u30b7\u30e5\u30fc\u30c6\u30a3\u30f3\u30b0\n\/\/ =========================================\n\n\u3010\u554f\u984c1\u3011\u30b3\u30f3\u30d1\u30a4\u30eb\u30a8\u30e9\u30fc: Madgwick.h not found\n\u2192 Madgwick\u30e9\u30a4\u30d6\u30e9\u30ea\u3092\u30a4\u30f3\u30b9\u30c8\u30fc\u30eb\n\n\u3010\u554f\u984c2\u3011\u65b9\u4f4d\u304c\u304a\u304b\u3057\u3044\n\u2192 \u78c1\u6c17\u30bb\u30f3\u30b5\u30fc\u306e\u30ad\u30e3\u30ea\u30d6\u30ec\u30fc\u30b7\u30e7\u30f3\u5b9f\u884c\n\u2192 my = -my; \u306e\u884c\u3092\u78ba\u8a8d\n\n\u3010\u554f\u984c3\u3011\u8d77\u52d5\u6642\u306b\u59ff\u52e2\u304c\u5b89\u5b9a\u3057\u306a\u3044\n\u2192 \u6570\u79d2\u5f85\u3064\uff08Madgwick\u306e\u53ce\u675f\u5f85\u3061\uff09\n\u2192 beta\u3092\u5927\u304d\u304f\u3059\u308b\uff08\u4f8b: 0.1\uff09\n\n\u3010\u554f\u984c4\u3011Roll\/Pitch\u304c90\u5ea6\u3067\u304a\u304b\u3057\u304f\u306a\u308b\n\u2192 \u3053\u308c\u306f\u30b8\u30f3\u30d0\u30eb\u30ed\u30c3\u30af\n\u2192 Madgwick\u7248\u3067\u306f\u767a\u751f\u3057\u306a\u3044\u306f\u305a\n\u2192 \u30bb\u30f3\u30b5\u30fc\u5411\u304d\u3092\u78ba\u8a8d\n\n\u3010\u554f\u984c5\u3011\u5f93\u6765\u7248\u3088\u308a\u9045\u3044\n\u2192 sampleFreq\u3092\u4e0b\u3052\u308b\uff08\u4f8b: 25Hz\uff09\n\u2192 delay(20)\u3092\u8abf\u6574\n\n\n\/\/ =========================================\n\/\/ Madgwick\u30a2\u30eb\u30b4\u30ea\u30ba\u30e0\u306e\u539f\u7406\n\/\/ =========================================\n\n1. \u4e88\u6e2c\u30b9\u30c6\u30c3\u30d7\n   \u30b8\u30e3\u30a4\u30ed\u30c7\u30fc\u30bf\u3067\u59ff\u52e2\u3092\u7a4d\u5206\n\n2. \u88dc\u6b63\u30b9\u30c6\u30c3\u30d7\n   \u52a0\u901f\u5ea6\u30fb\u78c1\u6c17\u30bb\u30f3\u30b5\u30fc\u3067\u8aa4\u5dee\u3092\u4fee\u6b63\n\n3. \u30af\u30a9\u30fc\u30bf\u30cb\u30aa\u30f3\u66f4\u65b0\n   \u65b0\u3057\u3044\u59ff\u52e2\u3092\u8a08\u7b97\n\n4. \u6b63\u898f\u5316\n   \u30af\u30a9\u30fc\u30bf\u30cb\u30aa\u30f3\u3092\u6b63\u898f\u5316\n\n\u8a73\u7d30:\n<blockquote class=\"wp-embedded-content\" data-secret=\"6FFQKwT3rS\"><a href=\"https:\/\/x-io.co.uk\/open-source-imu-and-ahrs-algorithms\/\">Open source IMU and AHRS algorithms<\/a><\/blockquote><iframe loading=\"lazy\" class=\"wp-embedded-content\" sandbox=\"allow-scripts\" security=\"restricted\" style=\"position: absolute; visibility: hidden;\" title=\"\u201cOpen source IMU and AHRS algorithms\u201d \u2014 x-io Technologies\" src=\"https:\/\/x-io.co.uk\/open-source-imu-and-ahrs-algorithms\/embed\/#?secret=tyfUL1dxCp#?secret=6FFQKwT3rS\" data-secret=\"6FFQKwT3rS\" width=\"600\" height=\"338\" frameborder=\"0\" marginwidth=\"0\" marginheight=\"0\" scrolling=\"no\"><\/iframe>\n\n\n\/\/ =========================================\n\/\/ \u30d1\u30d5\u30a9\u30fc\u30de\u30f3\u30b9\u6bd4\u8f03\n\/\/ =========================================\n\n                  Complementary | Madgwick\n-----------------------------------------------\nCPU\u4f7f\u7528\u7387           \u4f4e           \u3084\u3084\u9ad8\n\u30e1\u30e2\u30ea\u4f7f\u7528          \u5c11           \u3084\u3084\u591a\n\u7cbe\u5ea6                \u4e2d           \u9ad8\n\u30c9\u30ea\u30d5\u30c8\u8010\u6027        \u4e2d           \u9ad8\n\u30b8\u30f3\u30d0\u30eb\u30ed\u30c3\u30af      \u3042\u308a         \u306a\u3057\n\u78c1\u6c17\u88dc\u6b63            \u7c21\u6613         \u9ad8\u5ea6\n\u53ce\u675f\u6642\u9593            \u901f\u3044         \u3084\u3084\u9045\u3044\n\u7528\u9014                \u4e00\u822c         \u9ad8\u7cbe\u5ea6\n\n\n\/\/ =========================================\n\/\/ \u4f7f\u7528\u4f8b\n\/\/ =========================================\n\n\/\/ setup()\u5185\nfilter.begin(50);        \/\/ 50Hz\nfilter.setBeta(0.1f);    \/\/ \u9ad8\u901f\u5fdc\u7b54\n\n\/\/ loop()\u5185\nfilter.update(gx, gy, gz, ax, ay, az, mx, my, mz);\nfloat roll = filter.getRoll();\nfloat pitch = filter.getPitch();\nfloat yaw = filter.getYaw();\n\n\n\/\/ =========================================\n\/\/ \u9ad8\u5ea6\u306a\u4f7f\u3044\u65b9\n\/\/ =========================================\n\n\u30101\u3011\u30af\u30a9\u30fc\u30bf\u30cb\u30aa\u30f3\u3092\u76f4\u63a5\u53d6\u5f97\nfloat q0, q1, q2, q3;\nfilter.getQuaternion(&amp;q0, &amp;q1, &amp;q2, &amp;q3);\n\n\u30102\u3011\u91cd\u529b\u30d9\u30af\u30c8\u30eb\u53d6\u5f97\n\/\/ \u52a0\u901f\u5ea6\u304b\u3089\u91cd\u529b\u3092\u9664\u53bb\nfloat grav[3];\nfilter.getGravity(grav);\n\n\u30103\u3011\u7dda\u5f62\u52a0\u901f\u5ea6\u8a08\u7b97\nfloat linearAx = ax - grav[0];\nfloat linearAy = ay - grav[1];\nfloat linearAz = az - grav[2];\n\n\n\/\/ =========================================\n\/\/ \u307e\u3068\u3081\n\/\/ =========================================\n\nMadgwick AHRS\u3092\u4f7f\u7528\u3059\u308b\u3053\u3068\u3067:\n\u2713 \u30b8\u30f3\u30d0\u30eb\u30ed\u30c3\u30af\u306a\u3057\n\u2713 \u9577\u6642\u9593\u5b89\u5b9a\n\u2713 \u9ad8\u7cbe\u5ea6\u306a\u59ff\u52e2\u63a8\u5b9a\n\u2713 \u30d7\u30ed\u30d5\u30a7\u30c3\u30b7\u30e7\u30ca\u30eb\u306a\u5b9f\u88c5\n\n\u5f93\u6765\u306eComplementary Filter\u3088\u308a\u512a\u308c\u3066\u3044\u307e\u3059\u304c\u3001\n\u30e9\u30a4\u30d6\u30e9\u30ea\u306e\u30a4\u30f3\u30b9\u30c8\u30fc\u30eb\u304c\u5fc5\u8981\u3067\u3059\u3002\n\n*\/<\/code><\/pre><\/div>\n\n<\/div>\n\t\t<\/div>\n<\/div>\n\n<div class=\"wp-block-ub-content-toggle wp-block-ub-content-toggle-block\" id=\"ub-content-toggle-block-6f1f4e55-7b50-4df0-95a7-1d1b0b4849e2\" data-mobilecollapse=\"false\" data-desktopcollapse=\"true\" data-preventcollapse=\"false\" data-showonlyone=\"false\">\n<div class=\"wp-block-ub-content-toggle-accordion\" style=\"border-color: #f1f1f1;\" id=\"ub-content-toggle-panel-block-\">\n\t\t\t<div class=\"wp-block-ub-content-toggle-accordion-title-wrap\" style=\"background-color: #f1f1f1;\" aria-controls=\"ub-content-toggle-panel-0-6f1f4e55-7b50-4df0-95a7-1d1b0b4849e2\" tabindex=\"0\">\n\t\t\t<p class=\"wp-block-ub-content-toggle-accordion-title ub-content-toggle-title-6f1f4e55-7b50-4df0-95a7-1d1b0b4849e2\" style=\"color: #000000; \">C++\u30b3\u30fc\u30c9<\/p>\n\t\t\t<div class=\"wp-block-ub-content-toggle-accordion-toggle-wrap right\" style=\"color: #000000;\"><span class=\"wp-block-ub-content-toggle-accordion-state-indicator wp-block-ub-chevron-down\"><\/span><\/div>\n\t\t<\/div>\n\t\t\t<div role=\"region\" aria-expanded=\"false\" class=\"wp-block-ub-content-toggle-accordion-content-wrap ub-hide\" id=\"ub-content-toggle-panel-0-6f1f4e55-7b50-4df0-95a7-1d1b0b4849e2\">\n\n<p><\/p>\n\n\n\n<div class=\"hcb_wrap\"><pre class=\"prism line-numbers lang-cpp\" data-lang=\"C++\"><code>#include &lt;Wire.h&gt;\n#include &lt;SPI.h&gt;\n#include &lt;Adafruit_GFX.h&gt;\n#include &lt;Adafruit_ST7789.h&gt;\n#include &lt;ICM_20948.h&gt;\n#include &lt;MadgwickAHRS.h&gt;\n#include &lt;math.h&gt;\n\n#define CODE_VERSION \"v7.11.0-Madgwick\"\n\n\/\/ --- Sensor Orientation Configuration ---\n#define SENSOR_INVERTED false  \/\/ true = \u88cf\u8fd4\u3057, false = \u901a\u5e38\n\n\/\/ --- Pin Settings ---\n#define ENC_A_PIN  27\n#define ENC_B_PIN  25\n#define ENC_SW_PIN 32\n#define SDA_PIN 21\n#define SCL_PIN 22\n\n\/\/ --- ST7789 SPI Settings ---\n#define TFT_CS    5\n#define TFT_DC    26\n#define TFT_RST   -1 \n\nAdafruit_ST7789 display = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_RST);\nICM_20948_I2C myImu;\n\n\/\/ --- Madgwick Filter ---\nMadgwick filter;\nconst float sampleFreq = 50.0f;  \/\/ 50Hz update rate\n\n\/\/ --- Compass Variables ---\nfloat heading = 0.0;\nfloat roll = 0.0;\nfloat pitch = 0.0;\nfloat headingOffset = 0.0;\nfloat rollOffset = 0.0;\nfloat pitchOffset = 0.0;\n\n\/\/ --- Timing ---\nunsigned long lastUpdateTime = 0;\nfloat dt = 0.0;\n\n\/\/ --- Magnetometer Calibration ---\nfloat magOffsetX = 0.0;\nfloat magOffsetY = 0.0;\nfloat magOffsetZ = 0.0;\n\nfloat magMinX = 0.0, magMaxX = 0.0;\nfloat magMinY = 0.0, magMaxY = 0.0;\nfloat magMinZ = 0.0, magMaxZ = 0.0;\n\n\/\/ Magnetometer scale (hard\/soft iron correction)\nfloat magScaleX = 1.0;\nfloat magScaleY = 1.0;\nfloat magScaleZ = 1.0;\n\n\/\/ --- Button Variables ---\nbool buttonPressed = false;\nunsigned long buttonPressTime = 0;\nunsigned long longPressDuration = 2000;\nbool isCalibrating = false;\nunsigned long calibrationStartTime = 0;\nunsigned long calibrationDuration = 15000;\n\n\/\/ --- Previous values for differential update ---\nfloat prevHeading = -999.0;\nfloat prevRoll = -999.0;\nfloat prevPitch = -999.0;\n\n\/\/ --- Color Definitions ---\n#define COLOR_BG       0x0000\n#define COLOR_TEXT     0xFFFF\n#define COLOR_HEADING  0x07FF\n#define COLOR_ROLL     0xF81F\n#define COLOR_PITCH    0xFFE0\n#define COLOR_COMPASS  0x07E0\n#define COLOR_NEEDLE   0xF800\n\nvoid setup() {\n  Serial.begin(115200);\n  delay(100);\n  \n  Serial.println(\"Compass Display \" CODE_VERSION);\n  Serial.println(\"Using Madgwick AHRS Algorithm\");\n  \n  \/\/ --- Button Init ---\n  pinMode(ENC_SW_PIN, INPUT_PULLUP);\n  \n  \/\/ --- I2C Init ---\n  Wire.begin(SDA_PIN, SCL_PIN);\n  Wire.setClock(400000);\n  \n  \/\/ --- Display Init ---\n  display.init(240, 320);\n  display.setRotation(0);\n  display.fillScreen(COLOR_BG);\n  \n  display.setTextColor(COLOR_TEXT);\n  display.setTextSize(2);\n  display.setCursor(20, 140);\n  display.print(\"Initializing...\");\n  \n  \/\/ --- ICM-20948 Init ---\n  bool initialized = false;\n  while (!initialized) {\n    myImu.begin(Wire, 0);  \/\/ AD0 = 0\n    \n    if (myImu.status == ICM_20948_Stat_Ok) {\n      Serial.println(\"ICM-20948 initialized successfully\");\n      initialized = true;\n    } else {\n      Serial.print(\"ICM-20948 init failed. Status: \");\n      Serial.println(myImu.status);\n      delay(500);\n    }\n  }\n  \n  \/\/ Configure ICM-20948 - Use RAW sensor data\n  ICM_20948_fss_t myFSS;\n  myFSS.a = gpm16;   \/\/ Accelerometer: \u00b116g (high range for Madgwick)\n  myFSS.g = dps2000; \/\/ Gyroscope: \u00b12000 dps (high range)\n  myImu.setFullScale(ICM_20948_Internal_Acc | ICM_20948_Internal_Gyr, myFSS);\n  \n  \/\/ Enable magnetometer\n  myImu.startupMagnetometer();\n  \n  \/\/ --- Initialize Madgwick Filter ---\n  filter.begin(sampleFreq);\n  \n  \/\/ Madgwick filter gain (beta)\n  \/\/ \u5c0f\u3055\u3044\u5024(0.01-0.1): \u3088\u308a\u6ed1\u3089\u304b\u3001\u9045\u3044\u53ce\u675f\n  \/\/ \u5927\u304d\u3044\u5024(0.1-0.5): \u901f\u3044\u53ce\u675f\u3001\u30ce\u30a4\u30b8\u30fc\n  \/\/ \u63a8\u5968: 0.033 (\u30c7\u30d5\u30a9\u30eb\u30c8) \u307e\u305f\u306f 0.1 (\u9ad8\u901f\u5fdc\u7b54)\n  \/\/ filter.setBeta(0.1f);  \/\/ \u30b3\u30e1\u30f3\u30c8\u3092\u5916\u3057\u3066\u8abf\u6574\u53ef\u80fd\n  \n  delay(1000);\n  display.fillScreen(COLOR_BG);\n  \n  lastUpdateTime = micros();\n  \n  Serial.println(\"Madgwick AHRS ready!\");\n  Serial.print(\"Sample frequency: \");\n  Serial.print(sampleFreq);\n  Serial.println(\" Hz\");\n}\n\nvoid loop() {\n  handleButton();\n  \n  if (isCalibrating) {\n    performCalibration();\n    return;\n  }\n  \n  if (myImu.dataReady()) {\n    myImu.getAGMT();\n    \n    \/\/ Calculate delta time\n    unsigned long currentTime = micros();\n    dt = (currentTime - lastUpdateTime) \/ 1000000.0f;\n    lastUpdateTime = currentTime;\n    \n    \/\/ Update orientation using Madgwick\n    updateMadgwick();\n    \n    \/\/ Display compass\n    displayCompass();\n    \n    \/\/ Debug output\n    Serial.print(\"H:\");\n    Serial.print(heading, 1);\n    Serial.print(\"\u00b0 R:\");\n    Serial.print(roll, 1);\n    Serial.print(\"\u00b0 P:\");\n    Serial.print(pitch, 1);\n    Serial.println(\"\u00b0\");\n  }\n  \n  delay(20);  \/\/ ~50Hz\n}\n\nvoid updateMadgwick() {\n  \/\/ --- Get RAW sensor data ---\n  float ax = myImu.accX();  \/\/ g\n  float ay = myImu.accY();\n  float az = myImu.accZ();\n  \n  float gx = myImu.gyrX();  \/\/ deg\/s\n  float gy = myImu.gyrY();\n  float gz = myImu.gyrZ();\n  \n  float mx = myImu.magX();  \/\/ \u00b5T\n  float my = myImu.magY();\n  float mz = myImu.magZ();\n  \n  \/\/ --- Apply sensor orientation correction ---\n  #if SENSOR_INVERTED\n    ax = -ax;\n    az = -az;\n    gx = -gx;\n    gz = -gz;\n    mx = -mx;\n    mz = -mz;\n  #endif\n  \n  \/\/ --- Apply magnetometer calibration ---\n  mx = (mx - magOffsetX) * magScaleX;\n  my = (my - magOffsetY) * magScaleY;\n  mz = (mz - magOffsetZ) * magScaleZ;\n  \n  \/\/ 180\u5ea6\u88dc\u6b63\u304c\u5fc5\u8981\u306a\u5834\u5408\n  my = -my;\n  \n  \/\/ --- Convert gyro from deg\/s to rad\/s for Madgwick ---\n  gx = gx * DEG_TO_RAD;\n  gy = gy * DEG_TO_RAD;\n  gz = gz * DEG_TO_RAD;\n  \n  \/\/ --- Update Madgwick filter ---\n  \/\/ Madgwick expects: gx, gy, gz (rad\/s), ax, ay, az (any unit), mx, my, mz (any unit)\n  filter.update(gx, gy, gz, ax, ay, az, mx, my, mz);\n  \n  \/\/ --- Get orientation from quaternion ---\n  \/\/ Roll, Pitch, Yaw in degrees\n  roll = filter.getRoll() - rollOffset;\n  pitch = filter.getPitch() - pitchOffset;\n  float yaw = filter.getYaw();\n  \n  \/\/ Convert yaw to heading (0-360)\n  heading = yaw - headingOffset;\n  if (heading &lt; 0) heading += 360.0f;\n  if (heading &gt;= 360.0f) heading -= 360.0f;\n}\n\nvoid displayCompass() {\n  float headingDiff = abs(heading - prevHeading);\n  float rollDiff = abs(roll - prevRoll);\n  float pitchDiff = abs(pitch - prevPitch);\n  \n  bool fullRedraw = (prevHeading &lt; -900 || headingDiff &gt; 180 || headingDiff &gt; 5 || rollDiff &gt; 2 || pitchDiff &gt; 2);\n  \n  if (fullRedraw) {\n    display.fillScreen(COLOR_BG);\n    drawCompassBackground();\n  }\n  \n  drawCompassNeedle();\n  drawHeadingInfo();\n  drawAttitudeBars();\n  \n  prevHeading = heading;\n  prevRoll = roll;\n  prevPitch = pitch;\n}\n\nvoid drawCompassBackground() {\n  int centerX = 120;\n  int centerY = 160;\n  int radius = 100;\n  \n  display.drawCircle(centerX, centerY, radius, COLOR_COMPASS);\n  display.drawCircle(centerX, centerY, radius - 1, COLOR_COMPASS);\n  display.drawCircle(centerX, centerY, radius - 2, COLOR_COMPASS);\n  display.drawCircle(centerX, centerY, radius - 15, COLOR_TEXT);\n  \n  for (int i = 0; i &lt; 360; i += 30) {\n    float angle = (i - 90) * M_PI \/ 180.0;\n    int x1 = centerX + (radius - 10) * cos(angle);\n    int y1 = centerY + (radius - 10) * sin(angle);\n    int x2 = centerX + (radius - 3) * cos(angle);\n    int y2 = centerY + (radius - 3) * sin(angle);\n    \n    if (i % 90 == 0) {\n      display.drawLine(x1, y1, x2, y2, COLOR_TEXT);\n      display.drawLine(x1 + 1, y1, x2 + 1, y2, COLOR_TEXT);\n    } else {\n      x1 = centerX + (radius - 6) * cos(angle);\n      display.drawLine(x1, y1, x2, y2, COLOR_TEXT);\n    }\n  }\n  \n  display.setTextSize(3);\n  display.setTextColor(COLOR_TEXT);\n  \n  display.setCursor(centerX - 12, centerY - radius + 20);\n  display.print(\"N\");\n  display.setCursor(centerX + radius - 35, centerY - 12);\n  display.print(\"E\");\n  display.setCursor(centerX - 10, centerY + radius - 45);\n  display.print(\"S\");\n  display.setCursor(centerX - radius + 15, centerY - 12);\n  display.print(\"W\");\n}\n\nvoid drawCompassNeedle() {\n  int centerX = 120;\n  int centerY = 160;\n  int radius = 100;\n  \n  display.fillCircle(centerX, centerY, radius - 17, COLOR_BG);\n  \n  float headingRad = (heading - 90) * M_PI \/ 180.0;\n  \n  int needleLength = radius - 20;\n  int needleX = centerX + needleLength * cos(headingRad);\n  int needleY = centerY + needleLength * sin(headingRad);\n  \n  for (int i = -2; i &lt;= 2; i++) {\n    float perpAngle = headingRad + M_PI \/ 2;\n    int offsetX = i * cos(perpAngle);\n    int offsetY = i * sin(perpAngle);\n    display.drawLine(centerX + offsetX, centerY + offsetY, needleX + offsetX, needleY + offsetY, COLOR_NEEDLE);\n  }\n  \n  float arrowAngle1 = headingRad - 2.8;\n  float arrowAngle2 = headingRad + 2.8;\n  int arrowX1 = needleX - 15 * cos(arrowAngle1);\n  int arrowY1 = needleY - 15 * sin(arrowAngle1);\n  int arrowX2 = needleX - 15 * cos(arrowAngle2);\n  int arrowY2 = needleY - 15 * sin(arrowAngle2);\n  \n  display.fillTriangle(needleX, needleY, arrowX1, arrowY1, arrowX2, arrowY2, COLOR_NEEDLE);\n  \n  int southLength = radius - 30;\n  float southAngle = headingRad + M_PI;\n  int southX = centerX + southLength * cos(southAngle);\n  int southY = centerY + southLength * sin(southAngle);\n  display.drawLine(centerX, centerY, southX, southY, COLOR_TEXT);\n  display.drawLine(centerX + 1, centerY, southX + 1, southY, COLOR_TEXT);\n  \n  display.fillCircle(centerX, centerY, 6, COLOR_NEEDLE);\n  display.drawCircle(centerX, centerY, 7, COLOR_TEXT);\n}\n\nvoid drawHeadingInfo() {\n  display.fillRect(0, 0, 240, 70, COLOR_BG);\n  \n  display.setTextSize(3);\n  display.setTextColor(COLOR_HEADING);\n  display.setCursor(95, 10);\n  display.print(getDirectionString(heading));\n  \n  display.setTextSize(2);\n  display.setCursor(80, 40);\n  display.print((int)heading);\n  display.print(\"deg\");\n}\n\nvoid drawAttitudeBars() {\n  display.fillRect(0, 295, 240, 25, COLOR_BG);\n  \n  display.setTextSize(1);\n  \n  display.setCursor(10, 300);\n  display.setTextColor(COLOR_ROLL);\n  display.print(\"R:\");\n  display.print((int)roll);\n  display.print(\"deg\");\n  \n  int rollBarX = 80;\n  int rollBarY = 300;\n  int rollBarWidth = 140;\n  int rollPos = map(constrain(roll, -90, 90), -90, 90, 0, rollBarWidth);\n  display.drawRect(rollBarX, rollBarY, rollBarWidth, 8, COLOR_TEXT);\n  display.fillRect(rollBarX + rollBarWidth\/2 - 1, rollBarY, 2, 8, COLOR_TEXT);\n  display.fillRect(rollBarX + rollPos - 2, rollBarY + 1, 4, 6, COLOR_ROLL);\n  \n  display.setCursor(10, 312);\n  display.setTextColor(COLOR_PITCH);\n  display.print(\"P:\");\n  display.print((int)pitch);\n  display.print(\"deg\");\n  \n  int pitchBarX = 80;\n  int pitchBarY = 312;\n  int pitchBarWidth = 140;\n  int pitchPos = map(constrain(pitch, -90, 90), -90, 90, 0, pitchBarWidth);\n  display.drawRect(pitchBarX, pitchBarY, pitchBarWidth, 8, COLOR_TEXT);\n  display.fillRect(pitchBarX + pitchBarWidth\/2 - 1, pitchBarY, 2, 8, COLOR_TEXT);\n  display.fillRect(pitchBarX + pitchPos - 2, pitchBarY + 1, 4, 6, COLOR_PITCH);\n}\n\nString getDirectionString(float hdg) {\n  if (hdg &gt;= 337.5 || hdg &lt; 22.5) return \"N\";\n  else if (hdg &gt;= 22.5 &amp;&amp; hdg &lt; 67.5) return \"NE\";\n  else if (hdg &gt;= 67.5 &amp;&amp; hdg &lt; 112.5) return \"E\";\n  else if (hdg &gt;= 112.5 &amp;&amp; hdg &lt; 157.5) return \"SE\";\n  else if (hdg &gt;= 157.5 &amp;&amp; hdg &lt; 202.5) return \"S\";\n  else if (hdg &gt;= 202.5 &amp;&amp; hdg &lt; 247.5) return \"SW\";\n  else if (hdg &gt;= 247.5 &amp;&amp; hdg &lt; 292.5) return \"W\";\n  else if (hdg &gt;= 292.5 &amp;&amp; hdg &lt; 337.5) return \"NW\";\n  return \"?\";\n}\n\nvoid handleButton() {\n  bool currentState = digitalRead(ENC_SW_PIN) == LOW;\n  \n  if (currentState &amp;&amp; !buttonPressed) {\n    buttonPressed = true;\n    buttonPressTime = millis();\n  } \n  else if (!currentState &amp;&amp; buttonPressed) {\n    unsigned long pressDuration = millis() - buttonPressTime;\n    \n    if (pressDuration &gt;= longPressDuration) {\n      startCalibration();\n    } else {\n      zeroHeading();\n    }\n    \n    buttonPressed = false;\n  }\n}\n\nvoid zeroHeading() {\n  \/\/ Get current orientation from Madgwick\n  headingOffset = filter.getYaw();\n  rollOffset = filter.getRoll();\n  pitchOffset = filter.getPitch();\n  \n  Serial.print(\"Zeroed! H:\");\n  Serial.print(headingOffset);\n  Serial.print(\"\u00b0 R:\");\n  Serial.print(rollOffset);\n  Serial.print(\"\u00b0 P:\");\n  Serial.print(pitchOffset);\n  Serial.println(\"\u00b0\");\n  \n  display.fillRect(60, 150, 120, 40, COLOR_BG);\n  display.setTextSize(2);\n  display.setTextColor(COLOR_HEADING);\n  display.setCursor(70, 160);\n  display.print(\"ZEROED!\");\n  delay(1000);\n  prevHeading = -999.0;\n}\n\nvoid startCalibration() {\n  isCalibrating = true;\n  calibrationStartTime = millis();\n  \n  magMinX = magMaxX = myImu.magX();\n  magMinY = magMaxY = myImu.magY();\n  magMinZ = magMaxZ = myImu.magZ();\n  \n  Serial.println(\"Magnetometer calibration started!\");\n  Serial.println(\"Move sensor in figure-8 pattern...\");\n  \n  display.fillScreen(COLOR_BG);\n  display.setTextSize(2);\n  display.setTextColor(COLOR_HEADING);\n  display.setCursor(20, 100);\n  display.print(\"CALIBRATING\");\n  display.setTextSize(1);\n  display.setCursor(20, 140);\n  display.print(\"Move sensor in\");\n  display.setCursor(20, 160);\n  display.print(\"figure-8 pattern\");\n}\n\nvoid performCalibration() {\n  myImu.getAGMT();\n  \n  float mx = myImu.magX();\n  float my = myImu.magY();\n  float mz = myImu.magZ();\n  \n  if (mx &lt; magMinX) magMinX = mx;\n  if (mx &gt; magMaxX) magMaxX = mx;\n  if (my &lt; magMinY) magMinY = my;\n  if (my &gt; magMaxY) magMaxY = my;\n  if (mz &lt; magMinZ) magMinZ = mz;\n  if (mz &gt; magMaxZ) magMaxZ = mz;\n  \n  unsigned long elapsed = millis() - calibrationStartTime;\n  unsigned long remaining = calibrationDuration - elapsed;\n  \n  if (elapsed % 500 &lt; 100) {\n    display.fillRect(20, 200, 200, 60, COLOR_BG);\n    display.setTextSize(2);\n    display.setTextColor(COLOR_TEXT);\n    display.setCursor(40, 210);\n    display.print(\"Time: \");\n    display.print(remaining \/ 1000);\n    display.print(\"s\");\n    \n    int barWidth = 200;\n    int progress = (elapsed * barWidth) \/ calibrationDuration;\n    display.drawRect(20, 240, barWidth, 20, COLOR_TEXT);\n    display.fillRect(22, 242, progress - 4, 16, COLOR_HEADING);\n  }\n  \n  if (elapsed &gt;= calibrationDuration) {\n    finishCalibration();\n  }\n  \n  delay(10);\n}\n\nvoid finishCalibration() {\n  \/\/ Hard iron correction (offset)\n  magOffsetX = (magMaxX + magMinX) \/ 2.0f;\n  magOffsetY = (magMaxY + magMinY) \/ 2.0f;\n  magOffsetZ = (magMaxZ + magMinZ) \/ 2.0f;\n  \n  \/\/ Soft iron correction (scale)\n  float rangeX = magMaxX - magMinX;\n  float rangeY = magMaxY - magMinY;\n  float rangeZ = magMaxZ - magMinZ;\n  float avgRange = (rangeX + rangeY + rangeZ) \/ 3.0f;\n  \n  magScaleX = avgRange \/ rangeX;\n  magScaleY = avgRange \/ rangeY;\n  magScaleZ = avgRange \/ rangeZ;\n  \n  isCalibrating = false;\n  \n  Serial.println(\"Magnetometer calibration complete!\");\n  Serial.println(\"Hard Iron Offsets:\");\n  Serial.print(\"  X: \"); Serial.println(magOffsetX);\n  Serial.print(\"  Y: \"); Serial.println(magOffsetY);\n  Serial.print(\"  Z: \"); Serial.println(magOffsetZ);\n  Serial.println(\"Soft Iron Scales:\");\n  Serial.print(\"  X: \"); Serial.println(magScaleX);\n  Serial.print(\"  Y: \"); Serial.println(magScaleY);\n  Serial.print(\"  Z: \"); Serial.println(magScaleZ);\n  \n  display.fillScreen(COLOR_BG);\n  display.setTextSize(2);\n  display.setTextColor(COLOR_HEADING);\n  display.setCursor(20, 100);\n  display.print(\"COMPLETE!\");\n  \n  display.setTextSize(1);\n  display.setTextColor(COLOR_TEXT);\n  display.setCursor(20, 140);\n  display.print(\"Offsets:\");\n  display.setCursor(20, 160);\n  display.print(\"X:\"); display.print(magOffsetX, 1);\n  display.setCursor(20, 175);\n  display.print(\"Y:\"); display.print(magOffsetY, 1);\n  display.setCursor(20, 190);\n  display.print(\"Z:\"); display.print(magOffsetZ, 1);\n  \n  delay(3000);\n  display.fillScreen(COLOR_BG);\n  prevHeading = -999.0;\n}\n<\/code><\/pre><\/div>\n\n<\/div>\n\t\t<\/div>\n<\/div>","protected":false},"excerpt":{"rendered":"","protected":false},"author":1,"featured_media":0,"comment_status":"open","ping_status":"open","sticky":false,"template":"","format":"standard","meta":{"_jetpack_memberships_contains_paid_content":false,"footnotes":""},"categories":[34,6],"tags":[],"class_list":["post-2324","post","type-post","status-publish","format-standard","hentry","category-34","category-make"],"featured_image_src":null,"author_info":{"display_name":"mars","author_link":"https:\/\/rfsec.ddns.net\/db\/?author=1"},"jetpack_featured_media_url":"","jetpack_sharing_enabled":true,"_links":{"self":[{"href":"https:\/\/rfsec.ddns.net\/db\/index.php?rest_route=\/wp\/v2\/posts\/2324","targetHints":{"allow":["GET"]}}],"collection":[{"href":"https:\/\/rfsec.ddns.net\/db\/index.php?rest_route=\/wp\/v2\/posts"}],"about":[{"href":"https:\/\/rfsec.ddns.net\/db\/index.php?rest_route=\/wp\/v2\/types\/post"}],"author":[{"embeddable":true,"href":"https:\/\/rfsec.ddns.net\/db\/index.php?rest_route=\/wp\/v2\/users\/1"}],"replies":[{"embeddable":true,"href":"https:\/\/rfsec.ddns.net\/db\/index.php?rest_route=%2Fwp%2Fv2%2Fcomments&post=2324"}],"version-history":[{"count":3,"href":"https:\/\/rfsec.ddns.net\/db\/index.php?rest_route=\/wp\/v2\/posts\/2324\/revisions"}],"predecessor-version":[{"id":2328,"href":"https:\/\/rfsec.ddns.net\/db\/index.php?rest_route=\/wp\/v2\/posts\/2324\/revisions\/2328"}],"wp:attachment":[{"href":"https:\/\/rfsec.ddns.net\/db\/index.php?rest_route=%2Fwp%2Fv2%2Fmedia&parent=2324"}],"wp:term":[{"taxonomy":"category","embeddable":true,"href":"https:\/\/rfsec.ddns.net\/db\/index.php?rest_route=%2Fwp%2Fv2%2Fcategories&post=2324"},{"taxonomy":"post_tag","embeddable":true,"href":"https:\/\/rfsec.ddns.net\/db\/index.php?rest_route=%2Fwp%2Fv2%2Ftags&post=2324"}],"curies":[{"name":"wp","href":"https:\/\/api.w.org\/{rel}","templated":true}]}}