aitendoの商品で面白いものを見つけたので、Processingの勉強がてらに3D表示を実験。
購入したのはマルチ機能センサー[[ATD-M4S]]で、海外ではGY-80という名前で売られています。
9軸センサーとGR-SAKURAを組み合わせて、傾きを算出できれば、自作マルチコプターの平衡システムとして利用できそうです。
しくみとせつめいは下記
今回のシステムの流れは下記のとおり
- GR-SAKURAでGY-80を初期化
- 値を読んでシリアル出力
- PC側(Processing)で値を受け取り、計算
- グラフ化してついでに３Dの箱も回転させる
ちなみに、Processing側で計算させなくても、そのままGR-SAKURA側でAHRSの計算をさせることは十分可能です。
しかし、KpとKiがどれだけ作用するかを簡単に調べたかったのでProcessing側で計算させています。
尚、GY-80の内部のセンサーは、
ADXL345(加速度)、L3G4200D(ジャイロ)、HMC5883L(コンパス)、BMP085(気圧)ですが
今回は気圧センサーは使いません。(10軸も可能だけど、今は必要ないし・・・)
使用したコードとか
ここまで頑張って作ったコードを無料でなんの謝礼もない外にばらまくのは何か時間を無駄にしたみたいで嫌ですが
GPL感染してるからしかたないか。。。(GY-80ライブラリは既にがじぇるねで公開してるけど)
コピペ学生諸君は感謝してね星ﾐ
GR-SAKURA
メインプログラム
#include
#include "GY80.h"
GY80 se(0x0F);
char msg[255];
bool sa;
float cxMax, cxMin, cyMax, cyMin, czMax, czMin;
float cxCenter, cyCenter, czCenter;
void setup(){
pinMode(PIN_LED0,OUTPUT);
pinMode(PIN_LED1,OUTPUT);
pinMode(PIN_LED2,OUTPUT);
pinMode(PIN_SW,INPUT);
Serial.begin(9600);
sa = true;
cxMax = 0.0;
cxMin = 0.0;
cyMax = 0.0;
cyMin = 0.0;
czMax = 0.0;
czMin = 0.0;
cxCenter = 0.0;
cyCenter = 0.0;
czCenter = 0.0;
digitalWrite(PIN_LED0, HIGH);
digitalWrite(PIN_LED1, HIGH);
digitalWrite(PIN_LED2, HIGH);
while(digitalRead(PIN_SW) == 1);
delay(200);
digitalWrite(PIN_LED2, LOW);
se.init();
delay(200);
se.setAcceleroOffsetAuto(50);
digitalWrite(PIN_LED1, LOW);
}
void loop(){
if(digitalRead(PIN_SW) == 0){
if(!sa){
digitalWrite(PIN_LED0, HIGH);
Serial.begin(9600);
sa = true;
delay(200);
}else{
digitalWrite(PIN_LED0, LOW);
Serial.end();
sa = false;
delay(200);
}
}
if(!sa) return;
se.getGyroAxis(false);
se.getAcceleroAxis();
se.getCompassAxis();
float mx = se.c.x;
float my = se.c.y;
float mz = se.c.z;
if((mx > cxMax) && mx < 0.6) cxMax = mx;
if((mx < cxMin) && mx > -0.6) cxMin = mx;
if((my > cyMax) && my < 0.6) cyMax = my;
if((my < cyMin) && my > -0.6) cyMin = my;
if((mz > czMax) && mz < 0.6) czMax = mz;
if((mz < czMin) && mz > -0.6) czMin = mz;
cxCenter = (cxMax + cxMin) * 0.5;
cyCenter = (cyMax + cyMin) * 0.5;
czCenter = (czMax + czMin) * 0.5;
sprintf(
msg,
"%f,%f,%f:%f,%f,%f:%f,%f,%f",
se.g.x, se.g.y, se.g.z,
se.ac.x, se.ac.y, se.ac.z,
(mx - cxCenter), (my - cyCenter), (mz - czCenter)
);
Serial.println(msg);
delay(50);
}
GY80.h
#include
#include
#ifndef GY80_LIB
#define GY80_LIB
// [使用環境に合わせて書き換えてください]============================
// I2C(TwoWire) 使用チャンネル
// GR-SAKURA、GR-KAEDEで使えるものはWire, Wire1, Wire2, Wire3, Wire4, Wire5, Wire6, Wire7
// GR-KURUMIはWireのみ
#define IIC_CHANNEL_NAME Wire
// I2C周波数 (参考:NORMAL=100KHz, HIGHSPEED=200KHz)
#define IIC_SIGNALFREQ_HZ 200000
// 以下、変更不要 ===================================================
#define IIC_ADDR_GYRO 0x69 //L3G4200D
#define IIC_ADDR_ACCEL 0x53 //ADXL345
#define IIC_ADDR_CMPS 0x1E //HMC5883L
#define IIC_ADDR_APRESS 0x77 //BMP085
//L3G4200D(GYROSCOPE)
#define GYRO_SCALE 0.0175
#define GYRO_CTRL_REG1 0x20
#define GYRO_CTRL_REG2 0x21
#define GYRO_CTRL_REG3 0x22
#define GYRO_CTRL_REG4 0x23
#define GYRO_CTRL_REG5 0x24
#define GYRO_REFERENCE 0x25
#define GYRO_OUT_TEMP 0x26
#define GYRO_STATUS_REG 0x27
#define GYRO_OUT_X_L 0x28
#define GYRO_OUT_X_H 0x29
#define GYRO_OUT_Y_L 0x2A
#define GYRO_OUT_Y_H 0x2B
#define GYRO_OUT_Z_L 0x2C
#define GYRO_OUT_Z_H 0x2D
#define GYRO_FIFO_CTRL_REG 0x2E
#define GYRO_FIFO_SRC_REG 0x2F
#define GYRO_INT1_CFG 0x30
#define GYRO_INT1_SRC 0x31
#define GYRO_INT1_TSH_XH 0x32
#define GYRO_INT1_TSH_XL 0x33
#define GYRO_INT1_TSH_YH 0x34
#define GYRO_INT1_TSH_YL 0x35
#define GYRO_INT1_TSH_ZH 0x36
#define GYRO_INT1_TSH_ZL 0x37
#define GYRO_INT1_DURATION 0x38
//ADXL345(ACCELEROMETER)
#define ACCEL_SCALE 0.00390625
#define ACCEL_THRESH_TAP 0x1D
#define ACCEL_OFSX 0x1E
#define ACCEL_OFSY 0x1F
#define ACCEL_OFSZ 0x20
#define ACCEL_DUR 0x21
#define ACCEL_Latent 0x22
#define ACCEL_Window 0x23
#define ACCEL_THRESH_ACT 0x24
#define ACCEL_THRESH_INACT 0x25
#define ACCEL_TIME_INACT 0x26
#define ACCEL_ACT_INACT_CTL 0x27
#define ACCEL_THRESH_FF 0x28
#define ACCEL_TIME_FF 0x29
#define ACCEL_TAP_AXES 0x2A
#define ACCEL_ACT_TAP_STATUS 0x2B
#define ACCEL_BW_RATE 0x2C
#define ACCEL_POWER_CTL 0x2D
#define ACCEL_INT_ENABLE 0x2E
#define ACCEL_INT_MAP 0x2F
#define ACCEL_INT_SOURCE 0x30
#define ACCEL_DATA_FORMAT 0x31
#define ACCEL_DATAX0 0x32
#define ACCEL_DATAX1 0x33
#define ACCEL_DATAY0 0x34
#define ACCEL_DATAY1 0x35
#define ACCEL_DATAZ0 0x36
#define ACCEL_DATAZ1 0x37
#define ACCEL_FIFO_CTL 0x38
#define ACCEL_FIFO_STATUS 0x39
//HMC5883L(COMPASS)
#define CMPS_SCALE 0.00122
#define CMPS_REGA 0x00
#define CMPS_REGB 0x01
#define CMPS_MODE 0x02
#define CMPS_DATA_XMSB 0x03
#define CMPS_DATA_XLSB 0x04
#define CMPS_DATA_ZMSB 0x05
#define CMPS_DATA_ZLSB 0x06
#define CMPS_DATA_YMSB 0x07
#define CMPS_DATA_YLSB 0x08
#define CMPS_STATUS 0x09
//BMP085(ATMOSPHERIC PRESSURE)
#define APRESS_REG 0xF4
#define APRESS_TEMPERATURE 0x2E
#define APRESS_PRESSURE_ULP 0x34
#define APRESS_PRESSURE_STD 0x74
#define APRESS_PRESSURE_HR 0xB4
#define APRESS_PRESSURE_UHR 0xF4
#define APRESS_MSB 0xF6
#define APRESS_LSB 0xF7
#define APRESS_XLSB 0xF8
#define APRESS_A1_H 0xAA
#define APRESS_A1_L 0xAB
#define APRESS_A2_H 0xAC
#define APRESS_A2_L 0xAD
#define APRESS_A3_H 0xAE
#define APRESS_A3_L 0xAF
#define APRESS_A4_H 0xB0
#define APRESS_A4_L 0xB1
#define APRESS_A5_H 0xB2
#define APRESS_A5_L 0xB3
#define APRESS_A6_H 0xB4
#define APRESS_A6_L 0xB5
#define APRESS_B1_H 0xB6
#define APRESS_B1_L 0xB7
#define APRESS_B2_H 0xB8
#define APRESS_B2_L 0xB9
#define APRESS_MB_H 0xBA
#define APRESS_MB_L 0xBB
#define APRESS_MC_H 0xBC
#define APRESS_MC_L 0xBD
#define APRESS_MD_H 0xBE
#define APRESS_MD_L 0xBF
//ジャイロセンサー用構造体
struct s_gyro{
float x;
float y;
float z;
short temp;
};
//加速度センサー用構造体
struct s_accel{
float x;
float y;
float z;
float offX;
float offY;
float offZ;
};
//地磁気センサー用構造体
struct s_cmps{
float x;
float y;
float z;
};
//気圧センサー用構造体
struct s_apress{
short a1;
short a2;
short a3;
unsigned short a4;
unsigned short a5;
unsigned short a6;
short b1;
short b2;
long b5;
short mb;
short mc;
short md;
float temp;
long pressure;
};
//GY80管理クラス ====================================================
class GY80{
public:
GY80(char modeflag=0x0F);
private:
char umode;
//I2C読み取り用
unsigned char wire_read(unsigned char devAddr, unsigned char reg);
void wire_readArray(unsigned char *buff, unsigned char devAddr, unsigned char reg, unsigned char length);
//I2C書き込み用
void wire_write(unsigned char devAddr, unsigned char reg, unsigned char val);
public:
//各センサーからの値管理用構造体
s_gyro g; //ジャイロ用
s_accel ac; //加速度用
s_cmps c; //コンパス用
s_apress ap; //気圧用
//I2Cとセンサーの初期化開始
void init(bool i2cinit=true);
//L3G4200D(GYROSCOPE)
void initGyro(); //初期化用(通常はinitで勝手に呼ばれるので明示的に呼び出す必要はない)
void getGyroAll(bool isDEG=true); //全軸の角速度と温度を取得して構造体gの値を更新
void getGyroAxis(bool isDEG=true); //全軸の角速度を取得して構造体gの値を更新
void getGyroAxisXY(bool isDEG=true); //XY軸の角速度を取得して構造体gの値を更新
void getGyroAxisXZ(bool isDEG=true); //XZ軸の角速度を取得して構造体gの値を更新
void getGyroAxisYZ(bool isDEG=true); //YZ軸の角速度を取得して構造体gの値を更新
void getGyroAxisX(bool isDEG=true); //X軸の角速度を取得して構造体gの値を更新
void getGyroAxisY(bool isDEG=true); //Y軸の角速度を取得して構造体gの値を更新
void getGyroAxisZ(bool isDEG=true); //Z軸の角速度を取得して構造体gの値を更新
void getGyroTemp(); //温度を取得して構造体gの値を更新
//ADXL345(ACCELEROMETER)
void initAccelero(); //初期化用(通常はinitで勝手に呼ばれるので明示的に呼び出す必要はない)
void setAcceleroOffsetAuto(char sample_n=20); //加速度センサーの自動補正(sample_nで指定された回数の取得平均を補正値としてレジスタに書き込む)
void setAcceleroOffsetAuto(bool targetX, bool targetY, bool targetZ, char sample_n=20); //加速度センサーの自動補正(対象軸指定)
void setAcceleroOffset(float offsetX, float offsetY, float offsetZ); //補正値手動設定
void getAcceleroAll(); //全軸の加速度と補正値を取得して構造体acの値を更新
void getAcceleroAxis(); //全軸の加速度を取得して構造体acの値を更新
void getAcceleroAxisXY(); //XY軸の加速度を取得して構造体acの値を更新
void getAcceleroAxisXZ(); //XZ軸の加速度を取得して構造体acの値を更新
void getAcceleroAxisYZ(); //YZ軸の加速度を取得して構造体acの値を更新
void getAcceleroAxisX(); //X軸の加速度を取得して構造体acの値を更新
void getAcceleroAxisY(); //Y軸の加速度を取得して構造体acの値を更新
void getAcceleroAxisZ(); //Z軸の加速度を取得して構造体acの値を更新
void getAcceleroOffset(); //全軸の補正値を取得して構造体acの値を更新
//HMC5883L(COMPASS)
void initCompass(); //初期化用(通常はinitで勝手に呼ばれるので明示的に呼び出す必要はない)
void getCompassAll(); //全軸の地磁気の値を取得して構造体cの値を更新
void getCompassAxis(); //getCompassAllと同一
void getCompassAxisXY(); //XY軸の地磁気の値を取得して構造体cの値を更新
void getCompassAxisXZ(); //XZ軸の地磁気の値を取得して構造体cの値を更新
void getCompassAxisYZ(); //YZ軸の地磁気の値を取得して構造体cの値を更新
void getCompassAxisX(); //X軸の地磁気の値を取得して構造体cの値を更新
void getCompassAxisY(); //Y軸の地磁気の値を取得して構造体cの値を更新
void getCompassAxisZ(); //Z軸の地磁気の値を取得して構造体cの値を更新
//BMP085(ATMOSPHERIC PRESSURE)
void initAPressure(); //初期化用(通常はinitで勝手に呼ばれるので明示的に呼び出す必要はない)
void updateValAPressure(); //数値計算用係数を再ロード
long getAPressureTempParam(); //気温算出用の値を取得
long getAPressurePressParam(unsigned char resolution=1); //気圧算出用値を取得(resolution:精度 1-3[3が最高精度])
void getAPressureCalcTemp(); //気温算出用の値から気温を計算して構造体apの値を更新
void getAPressureCalcPress(unsigned char resolution=1); //気圧算出用の値から気圧を計算して構造体apの値を更新(resolution:精度 1-3[3が最高精度])
float calcTemp(long tempParam); //気温算出用の値から気温を計算
long calcPressure(long pressParam, unsigned char resolution=1); //気圧算出用の値から気圧を計算
float calcPa2Atm(long pressurePa_value); //気圧(パスカル)を大気圧値に変換
float calcAltitudeSample(long sealevelPa_value, long nowPa_value, float nowTemp); //海面基準気圧から現在の標高を算出
//ARHS用RAWデータ出力
void getRaw6(short &gx, short &gy, short &gz, short &ax, short &ay, short &az);
void getRaw9(short &gx, short &gy, short &gz, short &ax, short &ay, short &az, short &mx, short &my, short &mz);
};
#endif
GY80.cpp
#include "GY80.h"
// コンストラクタ ===================================================
GY80::GY80(char modeflag): umode(modeflag){
/*下位4ビットから各センサーを使用するかどうか確認
| 4bit | 3bit | 2bit | 1bit |
| 気圧 | 磁気 | 加速 | ジャイロ |
例)
全部(デフォルト) = 1111 (0x0F)
気圧,加速,ジャイロ = 1011 (0x0B)
磁気,加速 = 0110 (0x06)
気圧 = 1000 (0x08)
*/
}
// I2Cとセンサーの初期化開始 ========================================
void GY80::init(bool i2cinit){
if(i2cinit){
IIC_CHANNEL_NAME.setFrequency(IIC_SIGNALFREQ_HZ);
IIC_CHANNEL_NAME.begin();
}
if(umode & 0x01) initGyro();
if(umode & 0x02) initAccelero();
if(umode & 0x04) initCompass();
if(umode & 0x08) initAPressure();
}
// I2C読み取り ===================================
// ＠単純読み取り
unsigned char GY80::wire_read(unsigned char devAddr, unsigned char reg){
IIC_CHANNEL_NAME.beginTransmission(devAddr);
IIC_CHANNEL_NAME.write(reg);
IIC_CHANNEL_NAME.endTransmission();
IIC_CHANNEL_NAME.requestFrom(devAddr, 1U);
return IIC_CHANNEL_NAME.read();
}
// ＠複数読み取り
void GY80::wire_readArray(unsigned char *buff, unsigned char devAddr, unsigned char reg, unsigned char length){
IIC_CHANNEL_NAME.beginTransmission(devAddr);
IIC_CHANNEL_NAME.write(reg);
IIC_CHANNEL_NAME.endTransmission();
IIC_CHANNEL_NAME.requestFrom(devAddr, length);
unsigned char i = 0;
while(i 100)? 100 : (sample_n < 10)? 10 : sample_n;
float xf=0.0, yf=0.0, zf=0.0;
short x=0, y=0, z=0;
unsigned char ary[6];
for(char n=0; n<sample_n; n++){="" wire_readarray(ary,="" iic_addr_accel,="" accel_datax0,="" 6);="" x="(ary[1]" <<="" 8)="" |="" ary[0];="" y="(ary[3]" ary[2];="" z="(ary[5]" ary[4];="" xf="" +="(x" *="" accel_scale);="" yf="" zf="" delay(10);="" }="" ac.offx="-1.0" (xf="" sample_n);="" ac.offy="-1.0" (yf="" ac.offz="-1.0" (zf="" sample_n)="" 1.0;="" z軸は重力加速度の影響を受ける="" 補正値を書き込み="" setaccelerooffset(ac.offx,="" ac.offy,="" ac.offz);="" ＠自動平均補正(対象軸を指定)="" void="" gy80::setaccelerooffsetauto(bool="" targetx,="" bool="" targety,="" targetz,="" char="" sample_n){="" n個のサンプルから平均値を取得し、補正値としてレジスタに書き込む="" sample_n="(sample_n"> 100)? 100 : (sample_n < 10)? 10 : sample_n;
float xf=0.0, yf=0.0, zf=0.0;
short x=0, y=0, z=0;
unsigned char ary[6];
for(char n=0; n<sample_n; n++){="" wire_readarray(ary,="" iic_addr_accel,="" accel_datax0,="" 6);="" x="(ary[1]" <<="" 8)="" |="" ary[0];="" y="(ary[3]" ary[2];="" z="(ary[5]" ary[4];="" xf="" +="(x" *="" accel_scale);="" yf="" zf="" delay(10);="" }="" 各軸が対象であれば構造体の補正値を更新="" if(targetx)="" ac.offx="-1.0" (xf="" sample_n);="" if(targety)="" ac.offy="-1.0" (yf="" if(targetz)="" ac.offz="-1.0" (zf="" sample_n)="" 1.0;="" z軸は重力加速度の影響を受ける="" 補正値を書き込み="" setaccelerooffset(ac.offx,="" ac.offy,="" ac.offz);="" ＠数値指定補正用="" void="" gy80::setaccelerooffset(float="" offsetx,="" float="" offsety,="" offsetz){="" 0xff(-2g)から0x7f(+2g)の間で符号付き8ビットで補正="" +1.25gの補正をしたい場合、1.25="" 0.015625="80(0x50)になる" if(offsetx="" !="0.0f)" wire_write(iic_addr_accel,="" accel_ofsx,="" (char)(offsetx="" 0.015625));="" if(offsety="" accel_ofsy,="" (char)(offsety="" if(offsetz="" accel_ofsz,="" (char)(offsetz="" 構造体の中の変数も更新="" getaccelerooffset();="" 加速度センサーデータ取得="========================================" ＠全軸+補正値="" gy80::getacceleroall(){="" unsigned="" char="" ary[6];="" short="" x,="" y,="" z;="" ac.x="x" accel_scale;="" ac.y="y" ac.z="z" (signed="" char)wire_read(iic_addr_accel,="" accel_ofsx);="" accel_ofsy);="" accel_ofsz);="" ＠全軸="" gy80::getacceleroaxis(){="" ＠xy軸="" gy80::getacceleroaxisxy(){="" y;="" ＠xz軸="" gy80::getacceleroaxisxz(){="" ＠yz軸="" gy80::getacceleroaxisyz(){="" ＠x軸="" gy80::getacceleroaxisx(){="" ＠y軸="" gy80::getacceleroaxisy(){="" ＠z軸="" gy80::getacceleroaxisz(){="" ＠補正値取得="" gy80::getaccelerooffset(){="" 地磁気センサー初期化="=============================================" gy80::initcompass(){="" wire_read(iic_addr_cmps,="" cmps_rega);="" dummy(最初のほうの信号だけ妙に安定しないため)="" cmps_regb);="" cmps_mode);="" delay(50);="" wire_write(iic_addr_cmps,="" cmps_rega,="" 0x58);="" out="" 4samples="" average,="" data="" rate="" 75hz,="" normal="" measure="" cmps_regb,="" 0x40);="" gain="" 820="" (+-1.9ga="" range)="" cmps_mode,="" 0x00);="" highspeed="" i2c(3400khz)="" off,="" continuous="" measurement="" mode="" delay(300);="" 地磁気センサーデータ取得="=========================================" gy80::getcompassall(){="" iic_addr_cmps,="" cmps_data_xmsb,="" ary[1];="" ary[3];="" レジスタの並びがx,="" z,="" yであることに注意="" ary[5];="" c.x="x" cmps_scale;="" c.y="y" c.z="z" ＠全軸(getcompassallとgetcompassaxisは同一)="" gy80::getcompassaxis(){="" gy80::getcompassaxisxy(){="" gy80::getcompassaxisxz(){="" gy80::getcompassaxisyz(){="" gy80::getcompassaxisx(){="" cmps_data_xmsb)="" cmps_data_xlsb);="" gy80::getcompassaxisy(){="" cmps_data_ymsb)="" cmps_data_ylsb);="" gy80::getcompassaxisz(){="" cmps_data_zmsb)="" cmps_data_zlsb);="" 気圧センサー初期化="==============================================" gy80::initapressure(){="" delay(20);="" wire_read(iic_addr_apress,="" apress_md_h);="" apress_md_l);="" 計算用補正データの取得="" ap.a1="(wire_read(IIC_ADDR_APRESS," apress_a1_h)="" apress_a1_l);="" ap.a2="(wire_read(IIC_ADDR_APRESS," apress_a2_h)="" apress_a2_l);="" ap.a3="(wire_read(IIC_ADDR_APRESS," apress_a3_h)="" apress_a3_l);="" ap.a4="(wire_read(IIC_ADDR_APRESS," apress_a4_h)="" apress_a4_l);="" ap.a5="(wire_read(IIC_ADDR_APRESS," apress_a5_h)="" apress_a5_l);="" ap.a6="(wire_read(IIC_ADDR_APRESS," apress_a6_h)="" apress_a6_l);="" ap.b1="(wire_read(IIC_ADDR_APRESS," apress_b1_h)="" apress_b1_l);="" ap.b2="(wire_read(IIC_ADDR_APRESS," apress_b2_h)="" apress_b2_l);="" ap.mb="(wire_read(IIC_ADDR_APRESS," apress_mb_h)="" apress_mb_l);="" ap.mc="(wire_read(IIC_ADDR_APRESS," apress_mc_h)="" apress_mc_l);="" ap.md="(wire_read(IIC_ADDR_APRESS," apress_md_h)="" 計算用補正データの更新(ふつうは呼ばなくていいハズ)="==============" gy80::updatevalapressure(){="" delay(100);="" 気圧センサーデータ取得="===========================================" ＠気温計算用数値ut="" long="" gy80::getapressuretempparam(){="" wire_write(iic_addr_apress,="" apress_reg,="" apress_temperature);="" delay(5);="" return="" (wire_read(iic_addr_apress,="" apress_msb)="" apress_lsb);="" ＠気圧計算用数値up="" gy80::getapressurepressparam(unsigned="" resolution){="" resolution="(resolution"> 3)? 3 : resolution;
switch(resolution){
case 0:
wire_write(IIC_ADDR_APRESS, APRESS_REG, APRESS_PRESSURE_ULP);
delay(5);
break;
case 1:
wire_write(IIC_ADDR_APRESS, APRESS_REG, APRESS_PRESSURE_STD);
delay(8);
break;
case 2:
wire_write(IIC_ADDR_APRESS, APRESS_REG, APRESS_PRESSURE_HR);
delay(14);
break;
case 3:
wire_write(IIC_ADDR_APRESS, APRESS_REG, APRESS_PRESSURE_UHR);
delay(26);
break;
}
long retval =
(wire_read(IIC_ADDR_APRESS, APRESS_MSB) << 16) |
(wire_read(IIC_ADDR_APRESS, APRESS_LSB) << 8) |
wire_read(IIC_ADDR_APRESS, APRESS_XLSB);
return retval >> (8 - resolution);
}
// ＠気温計算用数値UTを取得して計算して値に格納(単位 C:セルシウス度)
void GY80::getAPressureCalcTemp(){
ap.temp = calcTemp(getAPressureTempParam());
}
// ＠気圧計算用数値UPを取得して計算して値に格納(単位 Pa:パスカル)
void GY80::getAPressureCalcPress(unsigned char resolution){
ap.pressure = calcPressure(getAPressurePressParam(resolution), resolution);
}
// ＠気温計算用数値UTから気温を算出(単位 C:セルシウス度)
float GY80::calcTemp(long tempParam){
//以下計算式はデータシートより
long x1 = ((tempParam - ap.a6) * ap.a5) >> 15;
ap.b5 = x1 + ((ap.mc << 11) / (x1 + ap.md));
float retval = (float)(ap.b5 + 8) / 16;
return retval / 10;
}
// ＠気圧計算用数値UPから気圧を算出(単位 Pa:パスカル)
long GY80::calcPressure(long pressParam, unsigned char resolution){
resolution = (resolution > 3)? 3 : resolution;
//以下計算式はデータシートより
long b6 = ap.b5 - 4000;
long bb6 = (b6 * b6) >> 12;
long b3 = ((((ap.a1 << 2) + (((ap.b2 * bb6) >> 11) + ((ap.a2 * b6) >> 11))) << resolution) + 2) >> 2;
unsigned long b4 = (ap.a4 * (unsigned long)(((((ap.a3 * b6 >> 13) + ((ap.b1 * bb6) >> 16)) + 2) >> 2)+32768)) >> 15;
unsigned long b7 = ((unsigned long)pressParam - b3) * (50000 >> resolution);
long p = (b7 < 0x80000000)? (b7 << 1) / b4: (b7 / b4) << 1;
long x1 = ((p >> 8) * (p >> 8) * 3038) >> 16;
long x2 = (-7357 * p) >> 16;
return p + ((x1 + x2 + 3791) >> 4);
}
// ＠気圧(Pa:パスカル)を大気圧に換算
float GY80::calcPa2Atm(long pressurePa_value){
return pressurePa_value / 101325;
}
// ＠海面気圧、現地気圧、現地気温から標高を算出
float GY80::calcAltitudeSample(long sealevelPa_value, long nowPa_value, float nowTemp){
float sea_hPa = sealevelPa_value / 100;
float now_hPa = nowPa_value / 100;
return ((pow(sea_hPa / now_hPa, 0.190222560395662) - 1) * (nowTemp + 273.15)) / 0.0065;
}
// ARHS用RAWデータ取得 ==============================================
// ＠6軸
void GY80::getRaw6(short &gx, short &gy, short &gz, short &ax, short &ay, short &az){
unsigned char ary[6];
gx = (wire_read(IIC_ADDR_GYRO, GYRO_OUT_X_H) << 8) | wire_read(IIC_ADDR_GYRO, GYRO_OUT_X_L);
gy = (wire_read(IIC_ADDR_GYRO, GYRO_OUT_Y_H) << 8) | wire_read(IIC_ADDR_GYRO, GYRO_OUT_Y_L);
gz = (wire_read(IIC_ADDR_GYRO, GYRO_OUT_Z_H) << 8) | wire_read(IIC_ADDR_GYRO, GYRO_OUT_Z_L);
wire_readArray(ary, IIC_ADDR_ACCEL, ACCEL_DATAX0, 6);
ax = (ary[1] << 8) | ary[0];
ay = (ary[3] << 8) | ary[2];
az = (ary[5] << 8) | ary[4];
}
// ＠9軸
void GY80::getRaw9(short &gx, short &gy, short &gz, short &ax, short &ay, short &az, short &mx, short &my, short &mz){
unsigned char ary[6];
gx = (wire_read(IIC_ADDR_GYRO, GYRO_OUT_X_H) << 8) | wire_read(IIC_ADDR_GYRO, GYRO_OUT_X_L);
gy = (wire_read(IIC_ADDR_GYRO, GYRO_OUT_Y_H) << 8) | wire_read(IIC_ADDR_GYRO, GYRO_OUT_Y_L);
gz = (wire_read(IIC_ADDR_GYRO, GYRO_OUT_Z_H) << 8) | wire_read(IIC_ADDR_GYRO, GYRO_OUT_Z_L);
wire_readArray(ary, IIC_ADDR_ACCEL, ACCEL_DATAX0, 6);
ax = (ary[1] << 8) | ary[0];
ay = (ary[3] << 8) | ary[2];
az = (ary[5] << 8) | ary[4];
wire_readArray(ary, IIC_ADDR_CMPS, CMPS_DATA_XMSB, 6);
mx = (ary[0] << 8) | ary[1];
my = (ary[4] << 8) | ary[5]; //レジスタの並びがX, Z, Yであることに注意
mz = (ary[2] << 8) | ary[3];
}
</sample_n;></sample_n;>
Processing
Processingでそれぞれ必要なライブラリは自分でインストールしてインポートしてね。
sketch_MahonyAHRS_PIDcheck.pde
import processing.serial.*;
import com.citumpe.ctpTools.jWMI;
import controlP5.*;
//CONST
public static final float ARHS_KP = 0.5;
public static final float ARHS_KI = 0.0;
public static final int GRAPH_WIDTH = 920;
public static final int GRAPH_HEIGHT = 380;
public static final int GRAPH_LASTIDX = GRAPH_WIDTH - 1;
Serial comp;
ControlP5 cp5;
PFont font;
Textarea noticeArea;
HashMap guimap;
M_AHRS ahrs;
PGraphics pg;
//グローバル変数(演算用)
float[] gx, gy, gz;
float[] acx, acy, acz;
float[] cx, cy, cz;
float[] roll, pitch, yaw;
public void setup(){
size(960, 720, P3D);
surface.setTitle("MahonyAHRS Kp Ki check");
font = loadFont("VL-Gothic-Regular-32.vlw");
textFont(font, 14);
guimap = new HashMap();
createUI(font, 14);
ahrs = new M_AHRS(ARHS_KP, ARHS_KI);
pg = createGraphics(960, 720, P2D);
pg.beginDraw();
pg.background(128);
pg.smooth();
pg.textFont(font, 14);
pg.stroke(255);
pg.fill(32);
pg.strokeWeight(1);
pg.rect(8, 98, 944, 482, 5, 5, 5, 5);
pg.strokeWeight(0.5);
pg.stroke(128);
pg.line(20, 120, 20, 500);
pg.line(20, 500, 940, 500);
pg.stroke(64);
pg.line(20, 310, 940, 310);
pg.fill(255);
pg.text("+180°", 20, 113);
pg.text("0°", 20, 303);
pg.text("-180°", 20, 497);
pg.fill(255, 32, 32);
pg.text("Roll (X)", 20, 520);
pg.fill(32, 255, 32);
pg.text("Pitch (Y)", 20, 540);
pg.fill(96, 96, 255);
pg.text("Yaw (Z)", 20, 560);
pg.fill(96);
pg.text("[-4]", 360, 573);
pg.text("[-3]", 480, 573);
pg.text("[-2]", 600, 573);
pg.text("[-1]", 720, 573);
pg.text("Now", 840, 573);
pg.fill(255,192,192);
pg.text("Gyro (rad/s) :", 20, 600);
pg.fill(192,255,192);
pg.text("Accelero (G/s) :", 20, 620);
pg.fill(192,192,255);
pg.text("Compass (Gauss) :", 20, 640);
pg.endDraw();
gx = new float[GRAPH_WIDTH];
gy = new float[GRAPH_WIDTH];
gz = new float[GRAPH_WIDTH];
acx = new float[GRAPH_WIDTH];
acy = new float[GRAPH_WIDTH];
acz = new float[GRAPH_WIDTH];
cx = new float[GRAPH_WIDTH];
cy = new float[GRAPH_WIDTH];
cz = new float[GRAPH_WIDTH];
roll = new float[GRAPH_WIDTH];
pitch = new float[GRAPH_WIDTH];
yaw = new float[GRAPH_WIDTH];
noticeArea = (Textarea) guimap.get("noticeTextArea");
getSerialDevices();
}
void draw(){
background(128);
image(pg, 0, 0);
//立体の表示
pushMatrix();
translate(width/2, height/2, 200);
strokeWeight(2.5);
stroke(0xCCFFFFFF);
fill(255, 255, 255, 96);
rotateX(radians(-pitch[GRAPH_LASTIDX]));
rotateY(radians(yaw[GRAPH_LASTIDX] - 50));
rotateZ(radians(-roll[GRAPH_LASTIDX]));
box(100, 20, 55);
popMatrix();
fill(255, 32, 32);
text(str(roll[GRAPH_LASTIDX-4]), 360, 520);
text(str(roll[GRAPH_LASTIDX-3]), 480, 520);
text(str(roll[GRAPH_LASTIDX-2]), 600, 520);
text(str(roll[GRAPH_LASTIDX-1]), 720, 520);
text(str(roll[GRAPH_LASTIDX]), 840, 520);
fill(32, 255, 32);
text(str(pitch[GRAPH_LASTIDX-4]), 360, 540);
text(str(pitch[GRAPH_LASTIDX-3]), 480, 540);
text(str(pitch[GRAPH_LASTIDX-2]), 600, 540);
text(str(pitch[GRAPH_LASTIDX-1]), 720, 540);
text(str(pitch[GRAPH_LASTIDX]), 840, 540);
fill(96, 96, 255);
text(str(yaw[GRAPH_LASTIDX-4]), 360, 560);
text(str(yaw[GRAPH_LASTIDX-3]), 480, 560);
text(str(yaw[GRAPH_LASTIDX-2]), 600, 560);
text(str(yaw[GRAPH_LASTIDX-1]), 720, 560);
text(str(yaw[GRAPH_LASTIDX]), 840, 560);
fill(255,192,192);
text(str(gx[GRAPH_LASTIDX]), 270, 600);
text(str(gy[GRAPH_LASTIDX]), 360, 600);
text(str(gz[GRAPH_LASTIDX]), 450, 600);
fill(192,255,192);
text(str(acx[GRAPH_LASTIDX]), 270, 620);
text(str(acy[GRAPH_LASTIDX]), 360, 620);
text(str(acz[GRAPH_LASTIDX]), 450, 620);
fill(192,192,255);
text(str(cx[GRAPH_LASTIDX]), 270, 640);
text(str(cy[GRAPH_LASTIDX]), 360, 640);
text(str(cz[GRAPH_LASTIDX]), 450, 640);
//グラフ
strokeWeight(0.5);
for(int i=0; i<graph_lastidx; i++){="" stroke(255,="" 32,="" 32);="" line(20+i,="" map(roll[i],="" -180,="" 180,="" 500,="" 120),="" 21+i,="" map(roll[i+1],="" 120));="" stroke(32,="" 255,="" map(pitch[i],="" map(pitch[i+1],="" stroke(96,="" 96,="" 255);="" map(yaw[i],="" map(yaw[i+1],="" }="" serialevent="" :="" serialにデータを受信すると実行される="" void="" serialevent(serial="" comdev){="" string="" str="comdev.readStringUntil('\n');" if(str="=" null="" ||="" str.length()="" <="" 20)="" return;="" splitserialdatastr(str);="" float="" data[]="ahrs.calc(" gx[graph_lastidx],="" gy[graph_lastidx],="" gz[graph_lastidx],="" acx[graph_lastidx],="" acy[graph_lastidx],="" acz[graph_lastidx],="" 0.0,="" 0.0="" cx[graph_lastidx]*1000,="" cy[graph_lastidx]*1000,="" cz[graph_lastidx]*1000="" );="" arraycopy(roll,="" 1,="" roll,="" 0,="" graph_lastidx);="" arraycopy(pitch,="" pitch,="" arraycopy(yaw,="" yaw,="" roll[graph_lastidx]="data[0];" pitch[graph_lastidx]="data[1];" yaw[graph_lastidx]="data[2];" ^^^^^^^^^^^^^^^^="" gui="" events="===========================================================================" [●="" 通信開始]ボタン押下時の処理="" startbutton(){="" dropdownlist="" devicedd="(DropdownList)" guimap.get("devicedropdown");="" li[];="" try{="" li="jWMI.getWMIValue(" "select="" caption="" from="" win32_serialport",="" "caption"="" ).split("\n",="" 0);="" }catch(exception="" e){="" noticearea.append("serial="" device="" error:="" 通信可能なcomポートがありません。\n");="" noticearea.scroll(1);="" if(devicedd.getlabel().indexof("(com")="" 0){="" 選択されたものは通信可能なcomポートではありません。\n");="" if(!li[int(devicedd.getvalue())].equals(devicedd.getlabel())){="" ポート番号が更新されています。リロードしてリストからデバイスを選びなおしてください。\n");="" if(devicedd.getlabel().indexof(serial.list()[int(devicedd.getvalue())])="" kp="float(((Textfield)" guimap.get("kptextfield")).gettext());="" ki="float(((Textfield)" guimap.get("kitextfield")).gettext());="" if(kp="=" float.nan="" float.nan){="" noticearea.append("number="" format="" exception:="" 比例ゲインkp="" または="" 積分ゲインki="" の入力値が数値に変換できません。\n");="" speeddd="(DropdownList)" guimap.get("speeddropdown");="" ahrs.resetkpki(kp,="" ki);="" comp="new" serial(this,="" serial.list()[int(devicedd.getvalue())],="" int(speeddd.getlabel()));="" comp.clear();="" comp.readstringuntil('\n');="" noticearea.append("[シリアル通信開始]="" \""="" +="" devicedd.getlabel()="" "\"\n");="" noticearea.append("比例ゲインkp=" + kp + " ,="" \n");="" [■="" 停止]ボタン押下時の処理="" stopbutton(){="" if(comp="=" null)="" comp.stop();="" ahrs.reset();="" noticearea.append("ahrsの蓄積をクリアしました。\n");="" noticearea.append("[シリアル通信停止]="" 通信を停止しました。\n");="" [デバイス再検出]ボタン押下時の処理="" reloadbutton(){="" getserialdevices();="" noticearea.append("[シリアルデバイス更新]="" デバイスリストを更新しました。\n");="" original="" methods="===========================================================================" シリアルデバイスの一覧をドロップダウンに読み込む="" public="" getserialdevices(){="" devicedd.close();="" items[]="{"NOTHING"};" devicedd.setitems(items);="" seriallist[]="jWMI.getWMIValue(" devicedd.setitems(seriallist);="" println(e);="" シリアルから読み取った文字を分解して変数に格納="" splitserialdatastr(string="" str){="" if(str.indexof("null")="=" 0)="" sense[]="split(str," ":");="" if(sense.length="" 3)="" gyro[]="split(sense[0]," ",");="" accel[]="split(sense[1]," compas[]="split(sense[2]," arraycopy(gx,="" gx,="" arraycopy(gy,="" gy,="" arraycopy(gz,="" gz,="" arraycopy(acx,="" acx,="" arraycopy(acy,="" acy,="" arraycopy(acz,="" acz,="" arraycopy(cx,="" cx,="" arraycopy(cy,="" cy,="" arraycopy(cz,="" cz,="" gx[graph_lastidx]="float(gyro[0]);" gy[graph_lastidx]="float(gyro[1]);" gz[graph_lastidx]="float(gyro[2]);" acx[graph_lastidx]="float(accel[0]);" acy[graph_lastidx]="float(accel[1]);" acz[graph_lastidx]="float(accel[2]);" cx[graph_lastidx]="float(compas[0]);" cy[graph_lastidx]="float(compas[1]);" cz[graph_lastidx]="float(compas[2]);" pre="">
P5_GUI.pde
//GUIコントロールの登録 =============================================
public void createUI(PFont basefont, int fontsize){
cp5 = new ControlP5(this, new ControlFont(basefont, fontsize));
guimap.put("deviceLabel", cp5.addLabel("deviceLabel"));
guimap.put("deviceDropDown", cp5.addDropdownList("deviceDropDown"));
guimap.put("speedLabel", cp5.addLabel("speedLabel"));
guimap.put("speedDropDown", cp5.addDropdownList("speedDropDown"));
guimap.put("bpsLabel", cp5.addLabel("bpsLabel"));
guimap.put("kpLabel", cp5.addLabel("kpLabel"));
guimap.put("kpTextfield", cp5.addTextfield("kpTextfield"));
guimap.put("kiLabel", cp5.addLabel("kiLabel"));
guimap.put("kiTextfield", cp5.addTextfield("kiTextfield"));
guimap.put("startButton", cp5.addButton("startButton"));
guimap.put("stopButton", cp5.addButton("stopButton"));
guimap.put("reloadButton", cp5.addButton("reloadButton"));
guimap.put("noticeTextArea", cp5.addTextarea("noticeTextArea"));
customizeUI();
}
//GUIコントロールのカスタマイズ ======================================
public void customizeUI(){
Textlabel label;
DropdownList dd;
Button button;
Textfield tf;
Textarea ta;
//"deviceLabel"
label = (Textlabel) guimap.get("deviceLabel");
label.setText("デバイス : ");
label.setPosition(8, 13);
//"deviceDropDown"
dd = (DropdownList) guimap.get("deviceDropDown");
dd.setPosition(88, 8);
dd.setSize(320, 264);
dd.setBarHeight(24);
dd.setItemHeight(24);
dd.getCaptionLabel().setText("通信デバイスを選択してください▽");
dd.getCaptionLabel().getStyle().marginTop = 3;
dd.getCaptionLabel().getStyle().marginLeft = 3;
dd.getValueLabel().getStyle().marginTop = 3;
dd.setItems(new String[]{"NOTHING"});
dd.close();
dd.bringToFront();
//"speedLabel"
label = (Textlabel) guimap.get("speedLabel");
label.setText("速度 : ");
label.setPosition(456, 13);
//"speedDropDown"
dd = (DropdownList) guimap.get("speedDropDown");
dd.setPosition(512, 8);
dd.setSize(144, 264);
dd.setBarHeight(24);
dd.setItemHeight(24);
dd.getCaptionLabel().setText("9600");
dd.getCaptionLabel().getStyle().marginTop = 3;
dd.getCaptionLabel().getStyle().marginLeft = 3;
dd.getValueLabel().getStyle().marginTop = 3;
dd.setItems(new String[]{"115200","57600","38400","28800","19200","14400","9600","4800","2400","1200"});
dd.close();
dd.bringToFront();
//"bpsLabel"
label = (Textlabel) guimap.get("bpsLabel");
label.setText("bps");
label.setPosition(656, 13);
//"startButton"
button = (Button) guimap.get("startButton");
button.setLabel("● 通信開始");
button.setPosition(8, 40);
button.setSize(128, 40);
button.setColorLabel(0xFFFF0000);
button.setColorBackground(0xFF000000);
//"stopButton"
button = (Button) guimap.get("stopButton");
button.setLabel("■ 停止");
button.setPosition(152, 40);
button.setSize(128, 40);
button.setColorLabel(0xFFFFFFFF);
button.setColorBackground(0xFF000000);
//"reloadButton"
button = (Button) guimap.get("reloadButton");
button.setLabel("デバイス再検出");
button.setPosition(296, 40);
button.setSize(128, 40);
//"kpLabel"
label = (Textlabel) guimap.get("kpLabel");
label.setText("比例ゲイン(Kp) : ");
label.setPosition(432, 43);
//"kpTextfield"
tf = (Textfield) guimap.get("kpTextfield");
tf.setText(str(ARHS_KP));
tf.setLabel("");
tf.setPosition(557, 37);
tf.setSize(55, 22);
//"kiTextfield"
label = (Textlabel) guimap.get("kiLabel");
label.setText("積分ゲイン(Ki) : ");
label.setPosition(432, 65);
//"kiTextfield"
tf = (Textfield) guimap.get("kiTextfield");
tf.setText(str(ARHS_KI));
tf.setLabel("");
tf.setPosition(557, 59);
tf.setSize(55, 22);
//"noticeTextArea"
ta = (Textarea) guimap.get("noticeTextArea");
ta.setText("Hello!\n");
ta.setPosition(8, 648);
ta.setSize(944, 64);
ta.setColor(0xFFFFFFFF);
ta.setColorBackground(0xDD000000);
ta.setScrollActive(0xFFEEEEEE);
ta.setScrollForeground(0xFFEED000);
ta.setScrollBackground(0xFF4455AA);
ta.showScrollbar();
}
AHRS.pde
/* ================================================================== 注意：GNU General Public Licence ・プログラムの実行 ・動作の改変 ・再頒布 ・改良版のリリース このプログラムは上記4点の権利を保護され、二次著作物についても 上記4点が保護されていなければなりません。二次著作物を独占的に 権利を書き換えての頒布は認められていません。 つまり、このライ センスは二次著作物に感染します。 ===================================================================== * Madgwick Filter AHRSOpen source IMU and AHRS algorithms* MahonyAHRS Madgwick's implementation of Mayhony's AHRS algorithm. See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms *参考 cosmith/ahrs-gy-80:GY80.h Adapted by Corentin Smith from https://github.com/kriswiner/GY-80 by Kris Winer https://github.com/cosmith/ahrs-gy-80/tree/master/src モータのＰＩＤ制御法 http://www.picfun.com/motor05.html PID制御 https://ja.wikipedia.org/wiki/PID%E5%88%B6%E5%BE%A1 四元数と三次元空間における回転 http://mathtrain.jp/quaternion Fast inverse square root https://en.wikipedia.org/wiki/Fast_inverse_square_root ===================================================================== 以下のコードは上記作成元のコードに加え、参考元一覧の中にある既存の 多数のライブラリから最も合うであろうものを抽出し、私のわかる範囲で 書き直し、クラスにまとめたものです。算出のための係数などは変更して おりません。算出の方法や各アルゴリズムについての詳細は作成元の説明 および論文やソースコードをご参照ください。 =================================================================== */ class M_AHRS{ //PI制御の比例ゲインと積分ゲイン private float Kp, Ki; //四元数 private float q0, q1, q2, q3; //時間経過 private long tNow, tLast; private float tDelta; //積分補正値 private float iex, iey, iez; //デフォルトコンストラクタ：KpとKiはデフォルトのものを使用する public M_AHRS(){ Kp = ARHS_KP * 2.0; Ki = ARHS_KI * 2.0; q0 = 1.0; q1 = 0.0; q2 = 0.0; q3 = 0.0; tNow = 0; tDelta = 0.0; tLast = 0; iex = 0.0; iey = 0.0; iez = 0.0; }; //コンストラクタ：KpとKiを自分で指定する public M_AHRS(float myKp, float myKi){ Kp = myKp * 2.0; Ki = myKi * 2.0; q0 = 1.0; q1 = 0.0; q2 = 0.0; q3 = 0.0; tNow = 0; tDelta = 0.0; tLast = 0; iex = 0.0; iey = 0.0; iez = 0.0; }; //2015年現在最速っぽい平方根の逆数算出アルゴリズム private float invSqrt(float x){ float xhalf = 0.5f*x; int i = Float.floatToIntBits(x); i = 0x5f3759df - (i>>1); x = Float.intBitsToFloat(i); x = x*(1.5f - xhalf*x*x); return x; }; //private void update(float, ... 9); //MahonyAHRSupdate(9軸での四元数更新 ジャイロ+加速度+地磁気) //ジャイロの値については弧度法(rad)であることに注意 private void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz){ //地磁気のデータが不足している場合は6軸で計算する if((mx == 0.0) && (my == 0.0) && (mz == 0.0)) { update(gx, gy, gz, ax, ay, az); return; } float norm; float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3; float hx, hy, bx, bz; float vx, vy, vz, wx, wy, wz; float ex, ey, ez; float qa, qb, qc; norm = invSqrt(ax * ax + ay * ay + az * az); ax *= norm; ay *= norm; az *= norm; norm = invSqrt(mx * mx + my * my + mz * mz); mx *= norm; my *= norm; mz *= norm; q0q0 = q0 * q0; q0q1 = q0 * q1; q0q2 = q0 * q2; q0q3 = q0 * q3; q1q1 = q1 * q1; q1q2 = q1 * q2; q1q3 = q1 * q3; q2q2 = q2 * q2; q2q3 = q2 * q3; q3q3 = q3 * q3; float vA = 0.5 - q2q2 - q3q3; float vB = q1q2 - q0q3; float vC = 0.5 - q1q1 - q2q2; vx = q1q3 - q0q2; hx = 2.0 * (mx * vA + my * vB + mz * (q1q3 + q0q2)); hy = 2.0 * (mx * (q1q2 + q0q3) + my * (0.5 - q1q1 - q3q3) + mz * (q2q3 - q0q1)); bx = sqrt(hx * hx + hy * hy); bz = 2.0 * (mx * vx + my * (q2q3 + q0q1) + mz * vC); vy = 2.0 * (q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3; wx = 2.0 * (bx * vA + bz * vx); vx *= 2.0; wy = 2.0 * (bx * vB + bz * vy); wz = 2.0 * (bx * (q0q2 + q1q3) + bz * vC); ex = (ay * vz - az * vy) + (my * wz - mz * wy); ey = (az * vx - ax * vz) + (mz * wx - mx * wz); ez = (ax * vy - ay * vx) + (mx * wy - my * wx); if(Ki == 0.0){ iex = 0.0; iey = 0.0; iez = 0.0; }else{ iex += ex; iey += ey; iez += ez; } gx += Kp * ex + Ki * iex; gy += Kp * ey + Ki * iey; gz += Kp * ez + Ki * iez; qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz) * tDelta; q1 += (qa * gx + qc * gz - q3 * gy) * tDelta; q2 += (qa * gy - qb * gz + q3 * gx) * tDelta; q3 += (qa * gz + qb * gy - qc * gx) * tDelta; norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= norm; q1 *= norm; q2 *= norm; q3 *= norm; }; //private void update(float, ... 6); //MahonyAHRSupdate(6軸での四元数更新 ジャイロ+加速度) //ジャイロの値については弧度法(rad)であることに注意 private void update(float gx, float gy, float gz, float ax, float ay, float az){ float norm; float vx, vy, vz; float ex, ey, ez; float qa, qb, qc; vx = q1 * q3 - q0 * q2; vy = q0 * q1 + q2 * q3; vz = q0 * q0 - 0.5 + q3 * q3; ex = (ay * vz - az * vy); ey = (az * vx - ax * vz); ez = (ax * vy - ay * vx); if(Ki == 0.0){ iex = 0.0; iey = 0.0; iez = 0.0; }else{ iex += ex; iey += ey; iez += ez; } gx += Kp * ex + Ki * iex; gy += Kp * ey + Ki * iey; gz += Kp * ez + Ki * iez; qa = q0; qb = q1; qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz) * tDelta; q1 += (qa * gx + qc * gz - q3 * gy) * tDelta; q2 += (qa * gy - qb * gz + q3 * gx) * tDelta; q3 += (qa * gz + qb * gy - qc * gx) * tDelta; norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 *= norm; q1 *= norm; q2 *= norm; q3 *= norm; }; //計算蓄積をすべて破棄する public void reset(){ q0 = 1.0; q1 = 0.0; q2 = 0.0; q3 = 0.0; tNow = 0; tDelta = 0.0; tLast = 0; iex = 0.0; iey = 0.0; iez = 0.0; }; //比例ゲインとKpと積分ゲインKiのリセット(普通は使用しない) public void resetKpKi(float myKp, float myKi){ Kp = myKp * 2.0; Ki = myKi * 2.0; }; //計算を実行し、参照変数roll, pitch, yawを更新する //各軸の傾き(Roll = X軸, Pitch = Y軸, Yaw = Z軸) X軸を機首とした飛行機をイメージするとわかりやすい //ジャイロの値については弧度法(rad/s)であることに注意 public float[] calc( float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz ){ //計算結果は配列として返す float[] val = new float[3]; //経過時間を算出 tNow = System.nanoTime() / 1000; tDelta = ((tNow - tLast) / 1000000.0) / 2.0; tLast = tNow; //四元数を更新 update(gx, gy, gz, ax, ay, az, mx, my, mz); //傾きを算出して弧度法を度数法に変換 float q0q0 = q0 * q0, q1q1q2q2 = q1 * q1 - q2 * q2, q3q3 = q3 * q3; val[0] = degrees(atan2(2.0f * (q0 * q1 + q2 * q3), q0q0 - q1q1q2q2 + q3q3)); val[1] = degrees(-asin(2.0f * (q1 * q3 - q0 * q2))); val[2] = degrees(atan2(2.0f * (q1 * q2 + q0 * q3), q0q0 + q1q1q2q2 - q3q3)); return val; }; }; // [MEMBER] ========================================================= //public M_AHRS(); デフォルトコンストラクタ：KpとKiはデフォルトのものを使用する //public M_AHRS(float myKp, float myKi); コンストラクタ：KpとKiを自分で指定する //private float invSqrt(float x); 2015年現在最速っぽい平方根の逆数算出アルゴリズム //private void update(float, ... 9); MahonyAHRSupdate(9軸での四元数更新 ジャイロ+加速度+地磁気) //private void update(float, ... 6); MahonyAHRSupdate(6軸での四元数更新 ジャイロ+加速度) //public void reset(); 計算蓄積をすべて破棄する //public void resetKpKi(float myKp, float myKi); 比例ゲインとKpと積分ゲインKiのリセット(普通は使用しない) //public void calc(float&, ... 3, float, ... 9); 計算を実行し、参照変数roll, pitch, yawを更新する
今回厄介だったところ
地磁気センサー(HMC5883L)のXYZ軸のパラメータを連続で読みにいく場合、
X→Y→Z ではなくて、 X→Z→Y であるということ。
これに気が付くのが遅れて、後々の修正が結構大変だった。
あと、センサーの値をそれぞれの単位系に変換する際に、よそのサイトを見ても結構メチャクチャなことを書いていた。
ジャイロ(L3G4200D)はデータシートの最初のほうに、LSBあたりの量を書いている。
感度設定が+-500dpsの場合、RAWデータをdpsに直すには17.5mdps/LSBを掛けなければならない。
つまり、17.5 / 1000 = 0.0175。
加速度(ADXL345)の場合は、3.3Vで 境界値 / ビット数 でいいらしい。（いまだに半信半疑）
感度設定が+-16Gの場合、値は13bitで出力される。
つまり、(16*2) / (2^13) = 0.00390625。（それっぽい値が出るから良しとしてるけどホントにあってんのか？）
地磁気(HMC5883L)の場合は、データシートのわかりやすい場所に載っている。
測定レンジによって値が決まっていて、Gainが820の設定の場合は1.22らしい。
算出される単位はmGauss(ミリガウス)。
オーバーフローが怖かったので、ライブラリではミリガウスをガウスに変換している。
AHRSの式にブチ込む値については、単位変換前でも単位変換後でもどっちでもいいらしい。(というか四元数の計算だしそれぞれの単位系はあんまり関係ない)
ただし、計算を浮動小数で行う以上、先に単位変換してからブチ込むのが最善っぽい。
Processing側はGUIライブラリを最初 G4P で作ってたんだけど、3D描画を扱う P3D の動きが怪しいことがわかった。(×ボタンで閉じると検査例外(RuntimeException)がずらずら出る)
仕方なく ControlP5 で書き直したものの、ドキュメントがグッチャグチャで、おまけにGUIデザイナーもなくてホント苦労した。次からは大丈夫そうだけど、最初はハマるね。
さいごに
自分でもびっくりするほど記事が長くなったけど、細部までは触れていません。
もしこの記事をグーグルで見つけて、自分のほしい情報が見つけられそうで見つけられなかった場合
私の経験を基に解決策を提示できるかもしれません。そういうアレを期待している人は遠慮なくコメントをください。あと参考になったら拍手ボタンを無我夢中で押してください。
尚、数学式とか積分についてはほとんどわかっていない模様。
</graph_lastidx;>