electronics-lab / Mbed 2 deprecated Cansat2018_4

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@9:674a1131da7b, 2018-08-10 (annotated)

Committer:
kurikuri
Date:
Fri Aug 10 18:13:43 2018 +0000
Revision:
9:674a1131da7b
Parent:
8:01fdbe4e967a 
????????????????

Who changed what in which revision?

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