electronics-lab / Mbed 2 deprecated CanSat2018_5

CanSat2018_4にGPSとtweliteをつけ足して関数化したもの

Dependencies:   mbed

main.cpp@2:b257a71f0655, 2018-08-13 (annotated)

Committer:
ponpoko1939
Date:
Mon Aug 13 08:20:44 2018 +0000
Revision:
2:b257a71f0655
Parent:
1:342818b81499 
?????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kurikuri 0:704adb172e40 1 /*MPU9250_PROGRAM ver2.1 PROGRAMED BY RYOTARO FUNAI 2018/08/10*/
kurikuri 0:704adb172e40 2 #include <mbed.h>
kurikuri 0:704adb172e40 3 #include <math.h>
kurikuri 0:704adb172e40 4 #define PI 3.14159265359
kurikuri 0:704adb172e40 5
kurikuri 0:704adb172e40 6 DigitalOut motor1(p21);
kurikuri 0:704adb172e40 7 DigitalOut motor2(p22);
kurikuri 0:704adb172e40 8 DigitalOut motor3(p23);
kurikuri 0:704adb172e40 9 DigitalOut motor4(p24);
kurikuri 0:704adb172e40 10 I2C i2c(p9, p10);
kurikuri 0:704adb172e40 11 Serial pc(USBTX, USBRX); //TWELITE使う予定なら13,14ピン
kurikuri 0:704adb172e40 12 Serial gps(p28,p27); // tx, rx
kurikuri 0:704adb172e40 13 Serial twelite(p13,p14); //twelite.printfでtwilite経由で通信できる
kurikuri 0:704adb172e40 14 DigitalOut led1(p12);
kurikuri 0:704adb172e40 15 AnalogIn Lx_in(p15);
kurikuri 0:704adb172e40 16
kurikuri 0:704adb172e40 17 //GPS用変数群
kurikuri 0:704adb172e40 18 int i,rlock,mode;
kurikuri 0:704adb172e40 19 char gps_data[256];
kurikuri 0:704adb172e40 20 char ns,ew;
kurikuri 0:704adb172e40 21 float x=135.585800, y=34.648525; //実験用初期位地
kurikuri 0:704adb172e40 22 float w_time,hokui,tokei;
kurikuri 0:704adb172e40 23 float g_hokui,g_tokei,old_hokui, old_tokei; //oldとg_を比べることで距離と角度を出す
kurikuri 0:704adb172e40 24 float d_hokui,m_hokui,d_tokei,m_tokei;
kurikuri 0:704adb172e40 25 unsigned char c;
kurikuri 0:704adb172e40 26
kurikuri 0:704adb172e40 27 #define MPU9250_ADDRESS 0x68<<1 //I2CでのMPU9250のスレーブアドレス
kurikuri 0:704adb172e40 28 #define AK8963_ADDRESS 0x0c<<1 //磁気センサのスレーブアドレス
kurikuri 0:704adb172e40 29 #define Whoami 0x75 //who_am_iレジスタのアドレス、0x71が返ってくる
kurikuri 0:704adb172e40 30 #define PWR 0x6b //スリープモードをonにするためのアドレス
kurikuri 0:704adb172e40 31 #define MAG_OPN 0x37 //mpu9250から磁気センサにアクセスできるようにする
kurikuri 0:704adb172e40 32 #define ACC_CONFIG 0x1c //加速度センサ設定用のアドレス
kurikuri 0:704adb172e40 33 #define ACC_2G 0x00 //加速度センサのレンジ(2G)
kurikuri 0:704adb172e40 34 #define ACC_4G 0x08 //加速度センサのレンジ(4G)
kurikuri 0:704adb172e40 35 #define ACC_8G 0x10 //加速度センサのレンジ(8G)
kurikuri 0:704adb172e40 36 #define ACC_16G 0x18 //加速度センサのレンジ(16Gまで計測可能)
kurikuri 0:704adb172e40 37 #define MAG_CONFIG 0x0a //磁気センサ設定用のアドレス
kurikuri 0:704adb172e40 38 #define MAG_8HZ 0x12 //磁気センサの出力周期(8Hz)
kurikuri 0:704adb172e40 39 #define MAG_100HZ 0x16 //磁気センサの出力周期(100Hz)
kurikuri 0:704adb172e40 40 #define accRange 16.0 //加速度センサの測定レンジ
kurikuri 0:704adb172e40 41 #define ST2 0x02 //磁気センサのステータスが入っているアドレス
kurikuri 0:704adb172e40 42 #define Ain 35
kurikuri 0:704adb172e40 43 #define SDA 21
kurikuri 0:704adb172e40 44 #define SCL 22
kurikuri 0:704adb172e40 45 #define led 2 //チェック用のLEDピン
kurikuri 0:704adb172e40 46
kurikuri 0:704adb172e40 47 //void LCD_Reset(); //m5stackLCDを更新
kurikuri 0:704adb172e40 48 float Lx_Read(); //cdsセルから値を取得
kurikuri 0:704adb172e40 49 void Ac_Read(int16_t*, int16_t*, int16_t*); //9軸から加速度の値を取得
kurikuri 0:704adb172e40 50 void Mag_Read(int16_t*, int16_t*, int16_t*);
kurikuri 0:704adb172e40 51 //addrにスレーブアドレス、regにアクセスするアドレスを入力する
kurikuri 0:704adb172e40 52 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data);
kurikuri 0:704adb172e40 53 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data);
kurikuri 0:704adb172e40 54 uint8_t IDcheck();
kurikuri 0:704adb172e40 55 void Breky();
kurikuri 0:704adb172e40 56 void Turn();
kurikuri 0:704adb172e40 57 void Return();
kurikuri 0:704adb172e40 58 void Right();
kurikuri 0:704adb172e40 59 void Left();
kurikuri 0:704adb172e40 60 void release();
kurikuri 0:704adb172e40 61 void Faling();
kurikuri 0:704adb172e40 62 void nikurom();
kurikuri 0:704adb172e40 63 void getGPS();
kurikuri 0:704adb172e40 64 void distance();
kurikuri 0:704adb172e40 65 void geoDirection();
kurikuri 0:704adb172e40 66 void compass();
kurikuri 0:704adb172e40 67
kurikuri 0:704adb172e40 68 uint8_t accgyrodata[14];
kurikuri 0:704adb172e40 69 uint8_t magneticdata[7];
kurikuri 0:704adb172e40 70 uint8_t ST2_Bit; //磁気センサのステータスを入れておく
kurikuri 0:704adb172e40 71
kurikuri 0:704adb172e40 72 int main() {
kurikuri 0:704adb172e40 73 gps.baud(9600);
kurikuri 0:704adb172e40 74 twelite.baud(115200);
kurikuri 0:704adb172e40 75 void Release(); //放出判定
kurikuri 0:704adb172e40 76 gps.attach(getGPS,Serial::RxIrq); //GPS割り込み
kurikuri 0:704adb172e40 77 void Faling();//落下判定
kurikuri 0:704adb172e40 78 void nikurom(); //ニクロム線を切る
kurikuri 0:704adb172e40 79 void compass();//九軸で進む方向決める
kurikuri 0:704adb172e40 80
kurikuri 0:704adb172e40 81 }
kurikuri 0:704adb172e40 82
kurikuri 0:704adb172e40 83
kurikuri 0:704adb172e40 84 /****************関数群****************/
kurikuri 0:704adb172e40 85 //cdsセルからアナログ値を持ってくる
kurikuri 0:704adb172e40 86 float Lx_Read(){
kurikuri 0:704adb172e40 87 float lx;
kurikuri 0:704adb172e40 88 lx = Lx_in.read();
kurikuri 0:704adb172e40 89 return lx;
kurikuri 0:704adb172e40 90 }
kurikuri 0:704adb172e40 91
kurikuri 0:704adb172e40 92 //Who_am_Iアドレスで接続確認ができる。0x71もしくは10進数で113が返ってくればok
kurikuri 0:704adb172e40 93 uint8_t IDcheck(){
kurikuri 0:704adb172e40 94 uint8_t address;
kurikuri 0:704adb172e40 95 i2cRead(MPU9250_ADDRESS, Whoami, 1, &address);
kurikuri 0:704adb172e40 96 return address;
kurikuri 0:704adb172e40 97 }
kurikuri 0:704adb172e40 98
kurikuri 0:704adb172e40 99 //mpu9250から加速度センサのみ引っ張ってくる
kurikuri 0:704adb172e40 100 void Ac_Read(int16_t* ax, int16_t* ay, int16_t* az){
kurikuri 0:704adb172e40 101 i2cWrite(MPU9250_ADDRESS, PWR, 0x00); //スリープモードの解除
kurikuri 0:704adb172e40 102 i2cWrite(MPU9250_ADDRESS, ACC_CONFIG, ACC_16G);//加速度センサの測定レンジの設定
kurikuri 0:704adb172e40 103 i2cRead(MPU9250_ADDRESS, 0x3b, 14, accgyrodata);
kurikuri 0:704adb172e40 104 *ax = (accgyrodata[0] << 8) | accgyrodata[1];//accGyroTempData[0]を左に8シフトし(<<)，accGyroTempData[1]を足し合わせる(|) |は論理和
kurikuri 0:704adb172e40 105 *ay = (accgyrodata[2] << 8) | accgyrodata[3];//accGyroTempData[2]を左に8シフトし(<<)，accGyroTempData[3]を足し合わせる(|)
kurikuri 0:704adb172e40 106 *az = (accgyrodata[4] << 8) | accgyrodata[5];//accGyroTempData[4]を左に8シフトし(<<)，accGyroTempData[5]を足し合わせる(|)
kurikuri 0:704adb172e40 107 }
kurikuri 0:704adb172e40 108
kurikuri 0:704adb172e40 109 //mpu9250から磁気センサのみ引っ張ってくる
kurikuri 0:704adb172e40 110 void Mag_Read(int16_t* mx, int16_t* my, int16_t* mz){
kurikuri 0:704adb172e40 111 i2cWrite(MPU9250_ADDRESS, PWR, 0x00); //スリープモードの解除
kurikuri 0:704adb172e40 112 i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
kurikuri 0:704adb172e40 113 i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
kurikuri 0:704adb172e40 114 i2cRead(AK8963_ADDRESS, ST2, 1, &ST2_Bit);//読み出し準備ができたか確認
kurikuri 0:704adb172e40 115 if(ST2_Bit & 0x01){ //ちゃんと読めたかをST2レジスタの値を読んで確認
kurikuri 0:704adb172e40 116 i2cRead(AK8963_ADDRESS, 0x03, 7, magneticdata);
kurikuri 0:704adb172e40 117 }
kurikuri 0:704adb172e40 118 else pc.printf("ERROR!!\n");
kurikuri 0:704adb172e40 119 *mx = (magneticdata[0] << 8) | magneticdata[1];
kurikuri 0:704adb172e40 120 *my = (magneticdata[2] << 8) | magneticdata[3];
kurikuri 0:704adb172e40 121 *mz = (magneticdata[4] << 8) | magneticdata[5];
kurikuri 0:704adb172e40 122 }
kurikuri 0:704adb172e40 123
kurikuri 0:704adb172e40 124 //mpu9250からデータを取得(bytesに受け取るデータのバイト数、dataに実際のデータを挿入していく)
kurikuri 0:704adb172e40 125 void i2cRead(uint8_t addr,uint8_t reg, uint8_t bytes,uint8_t* data){
kurikuri 0:704adb172e40 126 char cmd[1];
kurikuri 0:704adb172e40 127 char written_data[14];
kurikuri 0:704adb172e40 128 cmd[0] = reg;
kurikuri 0:704adb172e40 129 i2c.write(addr, cmd, 1, 1);
kurikuri 0:704adb172e40 130 i2c.read(addr, written_data, bytes, 0);
kurikuri 0:704adb172e40 131 for(int ii = 0; ii < bytes; ii++) {
kurikuri 0:704adb172e40 132 data[ii] = written_data[ii];
kurikuri 0:704adb172e40 133 }
kurikuri 0:704adb172e40 134 }
kurikuri 0:704adb172e40 135
kurikuri 0:704adb172e40 136 //mpu9250にデータを送信(dataに送信するデータを入力する)
kurikuri 0:704adb172e40 137 void i2cWrite(uint8_t addr,uint8_t reg, uint8_t data){
kurikuri 0:704adb172e40 138 char cmd[2];
kurikuri 0:704adb172e40 139 cmd[0] = reg; //レジスタ指定
kurikuri 0:704adb172e40 140 cmd[1] = data; //送信するデータ
kurikuri 0:704adb172e40 141 i2c.write(addr, cmd, 2); //レジスタ指定はどうする?
kurikuri 0:704adb172e40 142 }
kurikuri 0:704adb172e40 143
kurikuri 0:704adb172e40 144 //各種モータの関数
kurikuri 0:704adb172e40 145 void Breky(){
kurikuri 0:704adb172e40 146 motor1 = 0;
kurikuri 0:704adb172e40 147 motor2 = 0;
kurikuri 0:704adb172e40 148 motor3 = 0;
kurikuri 0:704adb172e40 149 motor4 = 0;
kurikuri 0:704adb172e40 150 }
kurikuri 0:704adb172e40 151
kurikuri 0:704adb172e40 152 void Turn(){
kurikuri 0:704adb172e40 153 motor1 = 1;
kurikuri 0:704adb172e40 154 motor2 = 0;
kurikuri 0:704adb172e40 155 motor3 = 1;
kurikuri 0:704adb172e40 156 motor4 = 0;
kurikuri 0:704adb172e40 157 }
kurikuri 0:704adb172e40 158
kurikuri 0:704adb172e40 159 void Return(){
kurikuri 0:704adb172e40 160 motor1 = 0;
kurikuri 0:704adb172e40 161 motor2 = 1;
kurikuri 0:704adb172e40 162 motor3 = 0;
kurikuri 0:704adb172e40 163 motor4 = 1;
kurikuri 0:704adb172e40 164 }
kurikuri 0:704adb172e40 165
kurikuri 0:704adb172e40 166 void Left(){
kurikuri 0:704adb172e40 167 motor1 = 1;
kurikuri 0:704adb172e40 168 motor2 = 0;
kurikuri 0:704adb172e40 169 motor3 = 0;
kurikuri 0:704adb172e40 170 motor4 = 1;
kurikuri 0:704adb172e40 171 }
kurikuri 0:704adb172e40 172
kurikuri 0:704adb172e40 173 void Right(){
kurikuri 0:704adb172e40 174 motor1 = 0;
kurikuri 0:704adb172e40 175 motor2 = 1;
kurikuri 0:704adb172e40 176 motor3 = 1;
kurikuri 0:704adb172e40 177 motor4 = 0;
kurikuri 0:704adb172e40 178 }
kurikuri 0:704adb172e40 179
kurikuri 0:704adb172e40 180 void release(){
kurikuri 0:704adb172e40 181 while(1){
kurikuri 0:704adb172e40 182 //cdsセルの値の確認
kurikuri 0:704adb172e40 183 //暗いときは0.8～1.0程度の値
kurikuri 0:704adb172e40 184 float blight;
kurikuri 0:704adb172e40 185 blight = Lx_Read();
kurikuri 0:704adb172e40 186 pc.printf("%4.1f\n\r",blight);
kurikuri 0:704adb172e40 187 if(blight<=0.6) break;// 明るさが0.6以下なら次のフェーズに進む
kurikuri 0:704adb172e40 188 }
kurikuri 0:704adb172e40 189 }
kurikuri 0:704adb172e40 190
kurikuri 0:704adb172e40 191 void Faling(){
kurikuri 0:704adb172e40 192 while(1){
kurikuri 0:704adb172e40 193 int16_t ax, ay, az;
kurikuri 0:704adb172e40 194 float accX, accY, accZ, acc;
kurikuri 0:704adb172e40 195 //加速度の値を取得し、落下判定
kurikuri 0:704adb172e40 196 Ac_Read(&ax,&ay,&az);
kurikuri 0:704adb172e40 197 accX = ax * accRange / 32768.0;//[G]に変換
kurikuri 0:704adb172e40 198 accY = ay * accRange / 32768.0;//[G]に変換
kurikuri 0:704adb172e40 199 accZ = az * accRange / 32768.0;//[G]に変換
kurikuri 0:704adb172e40 200 acc = sqrt((accX * accX) + (accY * accY) + (accZ * accZ));
kurikuri 0:704adb172e40 201 // 各軸のGを表示
kurikuri 0:704adb172e40 202 pc.printf("accX :%f\n\raccY :%f\n\raccZ :%f\n\racc :%f\n\r",accX, accY, accZ, acc);
kurikuri 0:704adb172e40 203 //静止時は1.0程度、落下時はそれを超える為、電源を入れるのは1.1以下にした
kurikuri 0:704adb172e40 204 if(acc <= 1.10){
kurikuri 0:704adb172e40 205 pc.printf("PWRON!!");
kurikuri 0:704adb172e40 206 Turn();
kurikuri 0:704adb172e40 207 break;
kurikuri 0:704adb172e40 208 //led1 = 0;
kurikuri 0:704adb172e40 209 }else{
kurikuri 0:704adb172e40 210 Breky();
kurikuri 0:704adb172e40 211 //led1 = 0;
kurikuri 0:704adb172e40 212 }
kurikuri 0:704adb172e40 213 }
kurikuri 0:704adb172e40 214 }
kurikuri 0:704adb172e40 215
kurikuri 0:704adb172e40 216 void nikurom(){
kurikuri 0:704adb172e40 217 wait(60);
kurikuri 0:704adb172e40 218 }
kurikuri 0:704adb172e40 219
kurikuri 0:704adb172e40 220 void getGPS() {
kurikuri 0:704adb172e40 221 c = gps.getc();
kurikuri 0:704adb172e40 222 if( c=='$' || i == 256){
kurikuri 0:704adb172e40 223 mode = 0;
kurikuri 0:704adb172e40 224 i = 0;
kurikuri 0:704adb172e40 225 for(int j=0; j<256; j++){
kurikuri 0:704adb172e40 226 gps_data[j]=NULL;
kurikuri 0:704adb172e40 227 }
kurikuri 0:704adb172e40 228 }
kurikuri 0:704adb172e40 229
kurikuri 0:704adb172e40 230 if(mode==0){
kurikuri 0:704adb172e40 231 if((gps_data[i]=c) != '\r'){
kurikuri 0:704adb172e40 232 i++;
kurikuri 0:704adb172e40 233 }else{
kurikuri 0:704adb172e40 234 gps_data[i]='\0';
kurikuri 0:704adb172e40 235
kurikuri 0:704adb172e40 236 if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
kurikuri 0:704adb172e40 237 if(rlock==1){
kurikuri 0:704adb172e40 238 // pc.printf("Status:Lock(%d)\n\r",rlock);
kurikuri 0:704adb172e40 239 //logitude
kurikuri 0:704adb172e40 240 d_tokei= int(tokei/100);
kurikuri 0:704adb172e40 241 m_tokei= (tokei-d_tokei*100)/60;
kurikuri 0:704adb172e40 242 g_tokei= d_tokei+m_tokei;
kurikuri 0:704adb172e40 243 //Latitude
kurikuri 0:704adb172e40 244 d_hokui=int(hokui/100);
kurikuri 0:704adb172e40 245 m_hokui=(hokui-d_hokui*100)/60;
kurikuri 0:704adb172e40 246 g_hokui=d_hokui+m_hokui;
kurikuri 0:704adb172e40 247 //現在地のgpsデータ
kurikuri 0:704adb172e40 248 pc.printf("Lat:%.6f, Lon:%.6f\n\r",g_hokui, g_tokei);
kurikuri 0:704adb172e40 249 twelite.printf("%.6f,%.6f\n\r",g_hokui, g_tokei);
kurikuri 0:704adb172e40 250 //前回の計測からの変化量
kurikuri 0:704adb172e40 251 //pc.printf("ch_Lat:%.6f, ch_Lon:%.6f\n\r",old_hokui-g_hokui, old_tokei-g_tokei);
kurikuri 0:704adb172e40 252 //pc.printf("dis : %fm\n\r", 1000*6378.137*acos(sin(y*PI/180)*sin(g_hokui*PI/180)+cos(y*PI/180)*cos(g_hokui*PI/180)*cos(x*PI/180-g_tokei*PI/180))); //別の距離の出し方
kurikuri 0:704adb172e40 253 distance();
kurikuri 0:704adb172e40 254 geoDirection();
kurikuri 0:704adb172e40 255 old_hokui=g_hokui;
kurikuri 0:704adb172e40 256 old_tokei=g_tokei;
kurikuri 0:704adb172e40 257 }else{//gpsデータ未受信時
kurikuri 0:704adb172e40 258 //pc.printf("\n\rStatus:unLock(%d)\n\r",rlock);
kurikuri 0:704adb172e40 259 ////pc.printf("%s",gps_data);
kurikuri 0:704adb172e40 260 }
kurikuri 0:704adb172e40 261 }
kurikuri 0:704adb172e40 262 }
kurikuri 0:704adb172e40 263 }
ponpoko1939 2:b257a71f0655 264 __disable_irq(); //割り込み禁止
kurikuri 0:704adb172e40 265 }
kurikuri 0:704adb172e40 266
kurikuri 0:704adb172e40 267 //ヒューベニの公式で距離出す
kurikuri 0:704adb172e40 268 void distance(){
kurikuri 0:704adb172e40 269 float rlat1 = y*PI/180, rlon1 = x*PI/180;
kurikuri 0:704adb172e40 270 float rlat2 = g_hokui*PI/180, rlon2 = g_tokei*PI/180;
kurikuri 0:704adb172e40 271 float a=6378137.000, a2=pow(a,2), e=sqrt((a2-pow(6356752.314,2))/a2);
kurikuri 0:704adb172e40 272 float lat_diff = rlat1-rlat2;
kurikuri 0:704adb172e40 273 float lon_diff = rlon1-rlon2;
kurikuri 0:704adb172e40 274 float lat_ave = (rlat1+rlat2)/2;
kurikuri 0:704adb172e40 275 float w = sqrt(1-pow(e,2)*pow(sin(lat_ave),2));
kurikuri 0:704adb172e40 276 float meridian = a*(1-pow(e,2))/pow(w,3);
kurikuri 0:704adb172e40 277 float n=a/w;
kurikuri 0:704adb172e40 278 float distance2 = sqrt(pow(lat_diff*meridian,2)+pow(lon_diff*n*cos(lat_ave),2));
kurikuri 0:704adb172e40 279 //pc.printf("dis2 : %fm\n\r",distance2);
kurikuri 0:704adb172e40 280 }
kurikuri 0:704adb172e40 281
kurikuri 0:704adb172e40 282 //角度出す
kurikuri 0:704adb172e40 283 void geoDirection() {
kurikuri 0:704adb172e40 284 // 緯度 g_hokui(lat1),経度 g_tokei(lng1) の点を出発として、緯度 y(lat2),経度 x(lng2) への方位
kurikuri 0:704adb172e40 285 // 北を０度で右回りの角度０～３６０度
kurikuri 0:704adb172e40 286 float Y = cos(x*PI/180)*sin(y*PI/180-g_hokui*PI/180);
kurikuri 0:704adb172e40 287 float X = cos(g_tokei*PI/180)*sin(x*PI/180)-sin(g_tokei*PI/180)*cos(x*PI/180)*cos(y*PI/180-g_hokui*PI/180);
kurikuri 0:704adb172e40 288 float dirE0 = 180*atan2(Y, X)/PI;
kurikuri 0:704adb172e40 289 if(dirE0 < 0){
kurikuri 0:704adb172e40 290 dirE0 = dirE0 + 360;
kurikuri 0:704adb172e40 291 }
kurikuri 0:704adb172e40 292 float dirN0 = (int)(dirE0 + 90) % 360;
kurikuri 0:704adb172e40 293 pc.printf("%f\n\r",dirN0);
kurikuri 0:704adb172e40 294 }
kurikuri 0:704adb172e40 295
kurikuri 0:704adb172e40 296 //方角だす
kurikuri 0:704adb172e40 297 void compass(){
ponpoko1939 1:342818b81499 298 double maveX = 0, maveY = 0, maveZ = 0;
ponpoko1939 1:342818b81499 299 int16_t ax, ay, az;
ponpoko1939 1:342818b81499 300 float accX, accY, accZ, acc;
kurikuri 0:704adb172e40 301 int16_t mx, my, mz;
kurikuri 0:704adb172e40 302 float magX, magY, magZ, mag;
ponpoko1939 1:342818b81499 303 double rad;
ponpoko1939 1:342818b81499 304 double degree;
ponpoko1939 1:342818b81499 305 double roll;
ponpoko1939 1:342818b81499 306 double pitch;
ponpoko1939 1:342818b81499 307 //補正する為に取得
ponpoko1939 1:342818b81499 308 accX = ax * accRange / 32768.0;//[G]に変換
ponpoko1939 1:342818b81499 309 accY = ay * accRange / 32768.0;//[G]に変換
ponpoko1939 1:342818b81499 310 accZ = az * accRange / 32768.0;//[G]に変換
ponpoko1939 1:342818b81499 311 roll = atan2(accY, accZ);
ponpoko1939 1:342818b81499 312 pitch = atan2(-accX, sin(accY) + cos(accZ));
kurikuri 0:704adb172e40 313 //磁気の値を取得し、方位判定
ponpoko1939 1:342818b81499 314 for(int i = 0;i < 150;i++){
ponpoko1939 1:342818b81499 315 Mag_Read(&mx, &my, &mz);
ponpoko1939 1:342818b81499 316 maveX += mx;
ponpoko1939 1:342818b81499 317 maveY += my;
ponpoko1939 1:342818b81499 318 maveZ += mz;
ponpoko1939 1:342818b81499 319 wait_ms(10);
ponpoko1939 1:342818b81499 320 }
ponpoko1939 1:342818b81499 321 maveX /= 150;
ponpoko1939 1:342818b81499 322 maveY /= 150;
ponpoko1939 1:342818b81499 323 maveZ /= 150;
ponpoko1939 1:342818b81499 324 magX = (maveX - 168.75) / 32768.0f * 4800.0f;//[uT]に変換
ponpoko1939 1:342818b81499 325 magY = (maveY - 18.75) / 32768.0f * 4800.0f;//[uT]に変換
kurikuri 0:704adb172e40 326 magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
ponpoko1939 1:342818b81499 327 pc.printf("%f,%f\n\r", magX, magY);
ponpoko1939 1:342818b81499 328 //ID = IDcheck();
ponpoko1939 1:342818b81499 329 //pc.printf("%f\n\r", ID);
ponpoko1939 1:342818b81499 330 rad = atan2(magZ * sin(roll) - magY * cos(roll), magX * cos(pitch) + magY * sin(pitch) * sin(roll) + magZ * sin(pitch) * cos(roll));
ponpoko1939 1:342818b81499 331 degree = -((int)(rad * 180.0 / 3.141592 + 270.0 - 7.5) % 360 - 360.0);
ponpoko1939 1:342818b81499 332 /*
ponpoko1939 1:342818b81499 333 if(degree < 0){
ponpoko1939 1:342818b81499 334 degree += 360;
ponpoko1939 1:342818b81499 335 }
ponpoko1939 1:342818b81499 336 */
ponpoko1939 1:342818b81499 337 pc.printf("%d\n\r",(int)degree);
ponpoko1939 2:b257a71f0655 338 __enable_irq(); //割り込み許可
kurikuri 0:704adb172e40 339 }