electronics-lab / Mbed 2 deprecated mag_sensor2

磁気センサv4

Dependencies:   mbed

Fork of mag_sensor by Funai Ryotaro

main.cpp@1:21ba826811d6, 2018-08-12 (annotated)

Committer:
ponpoko1939
Date:
Sun Aug 12 06:34:00 2018 +0000
Revision:
1:21ba826811d6
Parent:
0:ae23b58fc2d4
Child:
2:8ba36609face
Child:
3:7644f7b86ae0
??????????????????...?

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ponpoko1939 0:ae23b58fc2d4 1 /*MPU9250_PROGRAM ver2.2 PROGRAMED BY RYOTARO FUNAI 2018/08/10*/
ponpoko1939 0:ae23b58fc2d4 2 #include <mbed.h>
ponpoko1939 0:ae23b58fc2d4 3 #include <math.h>
ponpoko1939 0:ae23b58fc2d4 4
ponpoko1939 0:ae23b58fc2d4 5 I2C i2c(p9, p10);
ponpoko1939 0:ae23b58fc2d4 6 Serial pc(USBTX, USBRX); //TWELITE使う予定なら13,14ピン
ponpoko1939 0:ae23b58fc2d4 7
ponpoko1939 0:ae23b58fc2d4 8 #define MPU9250_ADDRESS 0x68<<1 //I2CでのMPU9250のスレーブアドレス
ponpoko1939 0:ae23b58fc2d4 9 #define AK8963_ADDRESS 0x0c<<1 //磁気センサのスレーブアドレス
ponpoko1939 0:ae23b58fc2d4 10 #define Whoami 0x75 //who_am_iレジスタのアドレス、0x71が返ってくる
ponpoko1939 0:ae23b58fc2d4 11 #define Magadd 0x00 //ak8963のWho_am_i
ponpoko1939 0:ae23b58fc2d4 12 #define PWR 0x6b //スリープモードをonにするためのアドレス
ponpoko1939 0:ae23b58fc2d4 13 #define MAG_OPN 0x37 //mpu9250から磁気センサにアクセスできるようにする
ponpoko1939 0:ae23b58fc2d4 14 #define ACC_CONFIG 0x1c //加速度センサ設定用のアドレス
ponpoko1939 0:ae23b58fc2d4 15 #define ACC_2G 0x00 //加速度センサのレンジ(2G)
ponpoko1939 0:ae23b58fc2d4 16 #define ACC_4G 0x08 //加速度センサのレンジ(4G)
ponpoko1939 0:ae23b58fc2d4 17 #define ACC_8G 0x10 //加速度センサのレンジ(8G)
ponpoko1939 0:ae23b58fc2d4 18 #define ACC_16G 0x18 //加速度センサのレンジ(16Gまで計測可能)
ponpoko1939 0:ae23b58fc2d4 19 #define MAG_CONFIG 0x0a //磁気センサ設定用のアドレス
ponpoko1939 0:ae23b58fc2d4 20 #define MAG_8HZ 0x12 //磁気センサの出力周期(8Hz)
ponpoko1939 0:ae23b58fc2d4 21 #define MAG_100HZ 0x16 //磁気センサの出力周期(100Hz)
ponpoko1939 0:ae23b58fc2d4 22 #define accRange 16.0 //加速度センサの測定レンジ
ponpoko1939 0:ae23b58fc2d4 23 #define ST2 0x02 //磁気センサのステータスが入っているアドレス
ponpoko1939 0:ae23b58fc2d4 24 #define Ain 35
ponpoko1939 0:ae23b58fc2d4 25 #define SDA 21
ponpoko1939 0:ae23b58fc2d4 26 #define SCL 22
ponpoko1939 0:ae23b58fc2d4 27 #define led 2 //チェック用のLEDピン
ponpoko1939 0:ae23b58fc2d4 28
ponpoko1939 0:ae23b58fc2d4 29 void Ac_Read(int16_t*, int16_t*, int16_t*); //9軸から加速度の値を取得
ponpoko1939 0:ae23b58fc2d4 30 void Mag_Read(int16_t*, int16_t*, int16_t*);
ponpoko1939 0:ae23b58fc2d4 31 //addrにスレーブアドレス、regにアクセスするアドレスを入力する
ponpoko1939 0:ae23b58fc2d4 32 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data);
ponpoko1939 0:ae23b58fc2d4 33 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data);
ponpoko1939 0:ae23b58fc2d4 34 uint8_t IDcheck();
ponpoko1939 0:ae23b58fc2d4 35
ponpoko1939 0:ae23b58fc2d4 36 uint8_t accgyrodata[14];
ponpoko1939 0:ae23b58fc2d4 37 uint8_t magneticdata[7];
ponpoko1939 0:ae23b58fc2d4 38 uint8_t ST2_Bit; //磁気センサのステータスを入れておく
ponpoko1939 0:ae23b58fc2d4 39
ponpoko1939 0:ae23b58fc2d4 40 int16_t mx, my, mz;
ponpoko1939 0:ae23b58fc2d4 41 double magX, magY, magZ, mag;
ponpoko1939 1:21ba826811d6 42 int16_t ax, ay, az;
ponpoko1939 1:21ba826811d6 43 float accX, accY, accZ, acc;
ponpoko1939 1:21ba826811d6 44 double rad;
ponpoko1939 1:21ba826811d6 45 double degree;
ponpoko1939 0:ae23b58fc2d4 46 float ID;
ponpoko1939 1:21ba826811d6 47 double roll;
ponpoko1939 1:21ba826811d6 48 double pitch;
ponpoko1939 1:21ba826811d6 49
ponpoko1939 0:ae23b58fc2d4 50 int main() {
ponpoko1939 0:ae23b58fc2d4 51 i2cWrite(MPU9250_ADDRESS, PWR, 0x00); //スリープモードの解除
ponpoko1939 0:ae23b58fc2d4 52 double maveX = 0, maveY = 0, maveZ = 0;
ponpoko1939 0:ae23b58fc2d4 53 while(1) {
ponpoko1939 1:21ba826811d6 54 //加速度の値を取得し、落下判定
ponpoko1939 1:21ba826811d6 55 Ac_Read(&ax,&ay,&az);
ponpoko1939 1:21ba826811d6 56 accX = ax * accRange / 32768.0;//[G]に変換
ponpoko1939 1:21ba826811d6 57 accY = ay * accRange / 32768.0;//[G]に変換
ponpoko1939 1:21ba826811d6 58 accZ = az * accRange / 32768.0;//[G]に変換
ponpoko1939 1:21ba826811d6 59 roll = atan2(accY, accZ);
ponpoko1939 1:21ba826811d6 60 pitch = atan2(-accX, sin(accY) + cos(accZ));
ponpoko1939 1:21ba826811d6 61 i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
ponpoko1939 1:21ba826811d6 62 i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
ponpoko1939 1:21ba826811d6 63 for(int i = 0;i < 150;i++){
ponpoko1939 0:ae23b58fc2d4 64 Mag_Read(&mx, &my, &mz);
ponpoko1939 0:ae23b58fc2d4 65 maveX += mx;
ponpoko1939 0:ae23b58fc2d4 66 maveY += my;
ponpoko1939 0:ae23b58fc2d4 67 maveZ += mz;
ponpoko1939 1:21ba826811d6 68 wait_ms(10);
ponpoko1939 0:ae23b58fc2d4 69 }
ponpoko1939 1:21ba826811d6 70 maveX /= 150;
ponpoko1939 1:21ba826811d6 71 maveY /= 150;
ponpoko1939 1:21ba826811d6 72 maveZ /= 150;
ponpoko1939 0:ae23b58fc2d4 73 magX = (maveX - 168.75) / 32768.0f * 4800.0f;//[uT]に変換
ponpoko1939 0:ae23b58fc2d4 74 magY = (maveY - 18.75) / 32768.0f * 4800.0f;//[uT]に変換
ponpoko1939 1:21ba826811d6 75 magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
ponpoko1939 0:ae23b58fc2d4 76 pc.printf("%f,%f\n\r", magX, magY);
ponpoko1939 1:21ba826811d6 77 //ID = IDcheck();
ponpoko1939 0:ae23b58fc2d4 78 //pc.printf("%f\n\r", ID);
ponpoko1939 1:21ba826811d6 79 rad = atan2(magZ * sin(roll) - magY * cos(roll), magX * cos(pitch) + magY * sin(pitch) * sin(roll) + magZ * sin(pitch) * cos(roll));
ponpoko1939 1:21ba826811d6 80 degree = -((int)(rad * 180.0 / 3.141592 + 270.0 - 7.5) % 360 - 360.0);
ponpoko1939 1:21ba826811d6 81 /*
ponpoko1939 0:ae23b58fc2d4 82 if(degree < 0){
ponpoko1939 0:ae23b58fc2d4 83 degree += 360;
ponpoko1939 0:ae23b58fc2d4 84 }
ponpoko1939 1:21ba826811d6 85 */
ponpoko1939 1:21ba826811d6 86 pc.printf("%d\n\r",(int)degree);
ponpoko1939 0:ae23b58fc2d4 87 }
ponpoko1939 0:ae23b58fc2d4 88 }
ponpoko1939 0:ae23b58fc2d4 89
ponpoko1939 1:21ba826811d6 90 //mpu9250から加速度センサのみ引っ張ってくる
ponpoko1939 1:21ba826811d6 91 void Ac_Read(int16_t* ax, int16_t* ay, int16_t* az){
ponpoko1939 1:21ba826811d6 92 i2cWrite(MPU9250_ADDRESS, PWR, 0x00); //スリープモードの解除
ponpoko1939 1:21ba826811d6 93 i2cWrite(MPU9250_ADDRESS, ACC_CONFIG, ACC_16G);//加速度センサの測定レンジの設定
ponpoko1939 1:21ba826811d6 94 i2cRead(MPU9250_ADDRESS, 0x3b, 14, accgyrodata);
ponpoko1939 1:21ba826811d6 95 *ax = (accgyrodata[0] << 8) | accgyrodata[1];//accGyroTempData[0]を左に8シフトし(<<)，accGyroTempData[1]を足し合わせる(|) |は論理和
ponpoko1939 1:21ba826811d6 96 *ay = (accgyrodata[2] << 8) | accgyrodata[3];//accGyroTempData[2]を左に8シフトし(<<)，accGyroTempData[3]を足し合わせる(|)
ponpoko1939 1:21ba826811d6 97 *az = (accgyrodata[4] << 8) | accgyrodata[5];//accGyroTempData[4]を左に8シフトし(<<)，accGyroTempData[5]を足し合わせる(|)
ponpoko1939 1:21ba826811d6 98 }
ponpoko1939 1:21ba826811d6 99
ponpoko1939 0:ae23b58fc2d4 100 //mpu9250から磁気センサのみ引っ張ってくる
ponpoko1939 0:ae23b58fc2d4 101 void Mag_Read(int16_t* mx, int16_t* my, int16_t* mz){
ponpoko1939 0:ae23b58fc2d4 102 i2cRead(AK8963_ADDRESS, ST2, 1, &ST2_Bit);//読み出し準備ができたか確認
ponpoko1939 0:ae23b58fc2d4 103 if(ST2_Bit & 0x01){ //ちゃんと読めたかをST2レジスタの値を読んで確認
ponpoko1939 0:ae23b58fc2d4 104 i2cRead(AK8963_ADDRESS, 0x03, 7, magneticdata);
ponpoko1939 0:ae23b58fc2d4 105 }
ponpoko1939 0:ae23b58fc2d4 106 else pc.printf("ERROR!!\n");
ponpoko1939 0:ae23b58fc2d4 107 *mx = (magneticdata[0] << 8) | magneticdata[1];
ponpoko1939 0:ae23b58fc2d4 108 *my = (magneticdata[2] << 8) | magneticdata[3];
ponpoko1939 0:ae23b58fc2d4 109 *mz = (magneticdata[4] << 8) | magneticdata[5];
ponpoko1939 0:ae23b58fc2d4 110 }
ponpoko1939 0:ae23b58fc2d4 111
ponpoko1939 0:ae23b58fc2d4 112 //mpu9250からデータを取得(bytesに受け取るデータのバイト数、dataに実際のデータを挿入していく)
ponpoko1939 0:ae23b58fc2d4 113 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data){
ponpoko1939 0:ae23b58fc2d4 114 char cmd[1];
ponpoko1939 0:ae23b58fc2d4 115 char written_data[14];
ponpoko1939 0:ae23b58fc2d4 116 cmd[0] = reg;
ponpoko1939 0:ae23b58fc2d4 117 i2c.write(addr, cmd, 1, 1);
ponpoko1939 0:ae23b58fc2d4 118 i2c.read(addr, written_data, bytes, 0);
ponpoko1939 0:ae23b58fc2d4 119 for(int ii = 0; ii < bytes; ii++) {
ponpoko1939 0:ae23b58fc2d4 120 data[ii] = written_data[ii];
ponpoko1939 0:ae23b58fc2d4 121 }
ponpoko1939 0:ae23b58fc2d4 122 }
ponpoko1939 0:ae23b58fc2d4 123
ponpoko1939 0:ae23b58fc2d4 124 //mpu9250にデータを送信(dataに送信するデータを入力する)
ponpoko1939 0:ae23b58fc2d4 125 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data){
ponpoko1939 0:ae23b58fc2d4 126 char cmd[2];
ponpoko1939 0:ae23b58fc2d4 127 cmd[0] = reg; //レジスタ指定
ponpoko1939 0:ae23b58fc2d4 128 cmd[1] = data; //送信するデータ
ponpoko1939 0:ae23b58fc2d4 129 i2c.write(addr, cmd, 2); //レジスタ指定はどうする?
ponpoko1939 0:ae23b58fc2d4 130 }
ponpoko1939 0:ae23b58fc2d4 131 /*
ponpoko1939 0:ae23b58fc2d4 132 void readMagData(int16_t * destination)
ponpoko1939 0:ae23b58fc2d4 133 {
ponpoko1939 0:ae23b58fc2d4 134 uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
ponpoko1939 0:ae23b58fc2d4 135 if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
ponpoko1939 0:ae23b58fc2d4 136 readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
ponpoko1939 0:ae23b58fc2d4 137 uint8_t c = rawData[6]; // End data read by reading ST2 register
ponpoko1939 0:ae23b58fc2d4 138 if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
ponpoko1939 0:ae23b58fc2d4 139 destination[0] = (int16_t)(((int16_t)rawData[1] << 8) | rawData[0]); // Turn the MSB and LSB into a signed 16-bit value
ponpoko1939 0:ae23b58fc2d4 140 destination[1] = (int16_t)(((int16_t)rawData[3] << 8) | rawData[2]) ; // Data stored as little Endian
ponpoko1939 0:ae23b58fc2d4 141 destination[2] = (int16_t)(((int16_t)rawData[5] << 8) | rawData[4]) ;
ponpoko1939 0:ae23b58fc2d4 142 }
ponpoko1939 0:ae23b58fc2d4 143 }
ponpoko1939 0:ae23b58fc2d4 144 }
ponpoko1939 0:ae23b58fc2d4 145
ponpoko1939 0:ae23b58fc2d4 146 void getMres() {
ponpoko1939 0:ae23b58fc2d4 147 switch (Mscale)
ponpoko1939 0:ae23b58fc2d4 148 {
ponpoko1939 0:ae23b58fc2d4 149 // Possible magnetometer scales (and their register bit settings) are:
ponpoko1939 0:ae23b58fc2d4 150 // 14 bit resolution (0) and 16 bit resolution (1)
ponpoko1939 0:ae23b58fc2d4 151 case MFS_14BITS:
ponpoko1939 0:ae23b58fc2d4 152 mRes = 10.0*4912.0/8190.0; // Proper scale to return milliGauss
ponpoko1939 0:ae23b58fc2d4 153 break;
ponpoko1939 0:ae23b58fc2d4 154 case MFS_16BITS:
ponpoko1939 0:ae23b58fc2d4 155 mRes = 10.0*4912.0/32760.0; // Proper scale to return milliGauss
ponpoko1939 0:ae23b58fc2d4 156 break;
ponpoko1939 0:ae23b58fc2d4 157 }
ponpoko1939 0:ae23b58fc2d4 158 }
ponpoko1939 0:ae23b58fc2d4 159 */
ponpoko1939 0:ae23b58fc2d4 160
ponpoko1939 0:ae23b58fc2d4 161 //Who_am_Iアドレスで接続確認ができる。0x48もしくは10進数で72が返ってくればok
ponpoko1939 0:ae23b58fc2d4 162 uint8_t IDcheck(){
ponpoko1939 0:ae23b58fc2d4 163 uint8_t address;
ponpoko1939 0:ae23b58fc2d4 164 i2cRead(AK8963_ADDRESS, Magadd, 1, &address);
ponpoko1939 0:ae23b58fc2d4 165 return address;
ponpoko1939 0:ae23b58fc2d4 166 }