編集履歴

質問編集履歴

計算式のみの質問に変更しました。

2019/11/19 09:14

投稿

スコア0

test CHANGED Viewed

	@@ -1 +1 @@
1	- ~~Madgwickフィルタ~~を用いて~~の方位角(yaw角)の~~傾斜補正が~~上手く~~いかない~~(Arduinon nano)~~
1	+ オイラー角を用いて傾斜補正が行いたい

test CHANGED Viewed

@@ -1,186 +1,16 @@
 ### 前提・実現したいこと
-Madgwickフィルタから算出したroll角とpitch角を用いて地磁気センサ(MAG3110)の傾斜補正を行いたいと考えています。
+オイラー角を用いて傾斜補正を行い、地磁気センサから方位角を算出したいと考えています。
 ### 発生している問題
-水平の状態では傾斜補正前も傾斜補正後も同じ値が出ると思うのですが、一致しません。
-また、傾斜補正後の方の値が安定しません。
-また、傾斜補正後ですが、傾けると値が変わってしまいます。
-### 該当のソースコード
-見にくいプログラムですみません。。。
+算出が上手くいきません。
-houiが傾斜補正前の方位角で、houi2が傾斜補正後の値です。
-```
-#include <MadgwickAHRS.h>
-#include<Wire.h>
-#include<math.h>
-Madgwick MadgwickFilter;
-#define MPU_addr 0x68
-#define Addr 0x0E
-int16_t AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ;
-void setup(){
+### 計算式(コード)
-  Wire.beginTransmission(Addr);
-  Wire.write(0x11);
-  Wire.write(0x80);
-  Wire.endTransmission();
-  delay(15);
-  Wire.beginTransmission(Addr);
-  Wire.write(0x10);
-  Wire.write(0x01);
-  Wire.endTransmission();
-  Wire.begin();
-  Wire.beginTransmission(MPU_addr);
-  Wire.write(0x6B);
-  Wire.write(0);
-  Wire.endTransmission();
-  Serial.begin(9600);
-  MadgwickFilter.begin(100);
-}
-void loop(){
-//加速度、角速度をセンサから取得～表示
-  Wire.beginTransmission(MPU_addr);
-  Wire.write(0x3B);
-  Wire.endTransmission();
-  Wire.requestFrom(MPU_addr,14);
-  AcX=Wire.read()<<8|Wire.read();
-  AcY=Wire.read()<<8|Wire.read();
-  AcZ=Wire.read()<<8|Wire.read();
-  Tmp=Wire.read()<<8|Wire.read();
-  GyX=Wire.read()<<8|Wire.read();
-  GyY=Wire.read()<<8|Wire.read();
-  GyZ=Wire.read()<<8|Wire.read();
-  float acc_x = AcX / 16384.0;  //FS_SEL_0 16,384 LSB / g
-  float acc_y = AcY / 16384.0;
-  float acc_z = AcZ / 16384.0;
-  acc_x -= 0.045;
-  acc_y -= -0.010;
-  acc_z -= -0.175;
-  float gyro_x=GyX/131.0;
-  float gyro_y=GyY/131.0;
-  float gyro_z=GyZ/131.0;
-  gyro_x -= -1.56466;
-  gyro_y -= 1.258743;
-  gyro_z -= -0.4928;
-  MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x, acc_y, acc_z);
-  float roll  = MadgwickFilter.getRoll()-4.0;
-  float pitch = MadgwickFilter.getPitch();
-  float yaw   = MadgwickFilter.getYaw();
-  //Serial.print(roll);  Serial.print(",");
-  //Serial.print(pitch);  Serial.println("");
-  float magx,magy,magz;
-  magx=read_x()+1497;
-  magy=read_y()-2145.5;
-  magz=read_z()-607;
-  double houi,houi2,X,Y;
   X=(cos(pitch)*magx)+(sin(pitch)*sin(roll)*magy)+(sin(pitch)*cos(roll)*magz);
@@ -188,130 +18,28 @@
-  houi=-atan2(magy,magx)*(180/PI);
+  houi=-atan2(magy,magx)*(180/PI);//傾斜補正前
-  houi2=-atan2(Y,X)*(180/PI);
+  houi2=-atan2(Y,X)*(180/PI);//傾斜補正後
-  Serial.print(houi);Serial.print(",");
+roll：ロール角
+pitch：ピッチ角
-  Serial.print(houi2);Serial.println("");
+magx：地磁気センサのx軸のセンサー値
+magy：地磁気センサのy軸のセンサー値
+magz：地磁気センサのz軸のセンサー値
-  delay(10);
+###補足
-}
+傾斜補正前の方位角の算出はうまくいっています。
-int mag_read_register(int reg)
-{
-  int reg_val;
-  Wire.beginTransmission(Addr); // transmit to device 0x0E
-  Wire.write(reg);              // x MSB reg
-  Wire.endTransmission();       // stop transmitting
-  delayMicroseconds(2); //needs at least 1.3us free time between start and stop
-  Wire.requestFrom(Addr, 1); // request 1 byte
-  while(Wire.available())    // slave may write less than requested
-  {
-    reg_val = Wire.read(); // read the byte
-  }
-  return reg_val;
-}
-int mag_read_value(int msb_reg, int lsb_reg)
-{
-  int val_low, val_high;  //define the MSB and LSB
-  val_high = mag_read_register(msb_reg);
-  delayMicroseconds(2); //needs at least 1.3us free time between start and stop
-  val_low = mag_read_register(lsb_reg);
-  int out = (val_low|(val_high << 8)); //concatenate the MSB and LSB
-  return out;
-}
-int read_x(void)
-{
-  return mag_read_value(0x01, 0x02);
-}
-int read_y(void)
-{
-  return mag_read_value(0x03, 0x04);
-}
-int read_z(void)
-{
-  return mag_read_value(0x05, 0x06);
-}
-```
-```ここに言語名を入力
-arduino言語
-```
-### 補足情報（FW/ツールのバージョンなど）
-センサは以下のものを使用しています。
-6軸センサ：https://www.switch-science.com/catalog/1208/
-地磁気センサ：https://www.switch-science.com/catalog/1964/
-・6軸センサ(MPU6050)と地磁気センサのオフセットは完了しています。
+地磁気センサのオフセットは完了しています。