質問編集履歴
1
追記
title
CHANGED
|
File without changes
|
body
CHANGED
|
@@ -110,4 +110,65 @@
|
|
|
110
110
|
上記プログラムは関係なく**フィルタを適用した上での角度算出ができるプログラム**
|
|
111
111
|
をお伺いしたいです．
|
|
112
112
|
|
|
113
|
-
知識不足な部分が多いかと思いますがよろしくお願いします．
|
|
113
|
+
知識不足な部分が多いかと思いますがよろしくお願いします．
|
|
114
|
+
|
|
115
|
+
### 追記
|
|
116
|
+
DMPを利用した角度を算出するプログラムを見つけることができたのですが，絶対角度で算出することができません．
|
|
117
|
+
こちらのコードから絶対角度を算出する方法があれば教えていただきたいです．
|
|
118
|
+
（加速度，ジャイロ，地磁気の値を取り出せれば回転行列によって求められるのですが，，，）
|
|
119
|
+
以下にそのコードを添付いたします．（必要ないものはコメントアウトしています）
|
|
120
|
+
[参照サイト](http://kousukeno.seesaa.net/article/445610208.html)
|
|
121
|
+
```Arduino
|
|
122
|
+
#include "freeram.h"
|
|
123
|
+
|
|
124
|
+
#include "mpu.h"
|
|
125
|
+
#include "I2Cdev.h"
|
|
126
|
+
|
|
127
|
+
int ret;
|
|
128
|
+
void setup() {
|
|
129
|
+
Fastwire::setup(400,0);
|
|
130
|
+
Serial.begin(9600);
|
|
131
|
+
ret = mympu_open(200);
|
|
132
|
+
//Serial.print("MPU init: "); Serial.println(ret);
|
|
133
|
+
//Serial.print("Free mem: "); Serial.println(freeRam());
|
|
134
|
+
|
|
135
|
+
}
|
|
136
|
+
|
|
137
|
+
unsigned int c = 0; //cumulative number of successful MPU/DMP reads
|
|
138
|
+
unsigned int np = 0; //cumulative number of MPU/DMP reads that brought no packet back
|
|
139
|
+
unsigned int err_c = 0; //cumulative number of MPU/DMP reads that brought corrupted packet
|
|
140
|
+
unsigned int err_o = 0; //cumulative number of MPU/DMP reads that had overflow bit set
|
|
141
|
+
|
|
142
|
+
void loop() {
|
|
143
|
+
ret = mympu_update();
|
|
144
|
+
|
|
145
|
+
switch (ret) {
|
|
146
|
+
case 0: c++; break;
|
|
147
|
+
case 1: np++; return;
|
|
148
|
+
case 2: err_o++; return;
|
|
149
|
+
case 3: err_c++; return;
|
|
150
|
+
default:
|
|
151
|
+
Serial.print("READ ERROR! ");
|
|
152
|
+
Serial.println(ret);
|
|
153
|
+
return;
|
|
154
|
+
}
|
|
155
|
+
|
|
156
|
+
if (!(c%25)) {
|
|
157
|
+
// Serial.print(np); Serial.print(" "); Serial.print(err_c); Serial.print(" "); Serial.print(err_o);
|
|
158
|
+
//Serial.print(" Yaw: ");
|
|
159
|
+
Serial.print(mympu.ypr[0]);
|
|
160
|
+
Serial.print(',');
|
|
161
|
+
//Serial.print(" Pitch: ");
|
|
162
|
+
Serial.print(mympu.ypr[1]);
|
|
163
|
+
Serial.print(',');
|
|
164
|
+
//Serial.print(" Roll: ");
|
|
165
|
+
Serial.println(mympu.ypr[2]);
|
|
166
|
+
|
|
167
|
+
|
|
168
|
+
//Serial.print("\tgy: "); Serial.print(mympu.gyro[0]);
|
|
169
|
+
//Serial.print(" gp: "); Serial.print(mympu.gyro[1]);
|
|
170
|
+
//Serial.print(" gr: "); Serial.println(mympu.gyro[2]);
|
|
171
|
+
}
|
|
172
|
+
}
|
|
173
|
+
|
|
174
|
+
```
|