2020/08/31 bmx055のライブラリ及びプログラム内容を一部変更. 変換レートを更新する必要あり?
Dependencies: mbed BMX055_rev2 SDFileSystem GPS_Interface
main.cpp@10:c60fec9b4e1b, 2020-08-30 (annotated)
- Committer:
- MatsumotoKouki
- Date:
- Sun Aug 30 07:01:19 2020 +0000
- Revision:
- 10:c60fec9b4e1b
- Parent:
- 9:fc0559c27628
- Child:
- 11:2fc33706edd4
2020/08/30; latest
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
MatsumotoKouki | 0:34d043558630 | 1 | #include "mbed.h" |
MatsumotoKouki | 0:34d043558630 | 2 | #include "BMX055.h" |
MatsumotoKouki | 0:34d043558630 | 3 | #include "SDFileSystem.h" |
MatsumotoKouki | 10:c60fec9b4e1b | 4 | #include "GPS.h" |
MatsumotoKouki | 1:4c7bace668f6 | 5 | |
MatsumotoKouki | 0:34d043558630 | 6 | |
MatsumotoKouki | 0:34d043558630 | 7 | |
MatsumotoKouki | 10:c60fec9b4e1b | 8 | GPS gps(PA_9, PA_10); // tx, rx |
MatsumotoKouki | 0:34d043558630 | 9 | DigitalIn FlightPin(PB_0); |
MatsumotoKouki | 1:4c7bace668f6 | 10 | |
MatsumotoKouki | 10:c60fec9b4e1b | 11 | Serial pc(USBTX, USBRX, 9600);//パソコン側からもマイコン⇒IM920のデータはみれる |
falconsyunya | 2:28ccb8bdac86 | 12 | Serial uart(PA_2, PA_3, 19200);//TX(IM920_RX), RX(IM920_TX) |
falconsyunya | 2:28ccb8bdac86 | 13 | |
MatsumotoKouki | 0:34d043558630 | 14 | DigitalOut fet1(PA_8); //MOSFET PIN:GATE |
MatsumotoKouki | 0:34d043558630 | 15 | DigitalOut fet2(PA_11); |
MatsumotoKouki | 0:34d043558630 | 16 | BMX055 bmx(PB_7,PB_6);//SDA,SCL |
MatsumotoKouki | 0:34d043558630 | 17 | SDFileSystem sd(PA_7, PA_6, PA_5, PA_4, "sd"); // the pinout on the mbed Cool Components workshop board |
MatsumotoKouki | 1:4c7bace668f6 | 18 | |
MatsumotoKouki | 0:34d043558630 | 19 | Ticker warikomi; |
MatsumotoKouki | 0:34d043558630 | 20 | Timeout t; |
MatsumotoKouki | 0:34d043558630 | 21 | //double a[3],b[3],c[3]; |
MatsumotoKouki | 9:fc0559c27628 | 22 | int i,j=0,rlock,mode; |
MatsumotoKouki | 0:34d043558630 | 23 | char ns,ew; |
MatsumotoKouki | 0:34d043558630 | 24 | float w_time;//,hokui,tokei; |
MatsumotoKouki | 0:34d043558630 | 25 | float hokui,tokei; |
MatsumotoKouki | 0:34d043558630 | 26 | float g_hokui,g_tokei; |
MatsumotoKouki | 0:34d043558630 | 27 | float d_hokui,m_hokui,d_tokei,m_tokei; |
MatsumotoKouki | 0:34d043558630 | 28 | char gps_data[256]; |
MatsumotoKouki | 0:34d043558630 | 29 | char str[256]; |
MatsumotoKouki | 1:4c7bace668f6 | 30 | |
MatsumotoKouki | 0:34d043558630 | 31 | |
MatsumotoKouki | 10:c60fec9b4e1b | 32 | //void getGPS(float *hokui_n,float *tokei_n); |
MatsumotoKouki | 0:34d043558630 | 33 | //void callback (); |
MatsumotoKouki | 0:34d043558630 | 34 | void FlightPinDriver(); |
MatsumotoKouki | 0:34d043558630 | 35 | void nichrome_ON(); |
MatsumotoKouki | 0:34d043558630 | 36 | void buzzer(); |
MatsumotoKouki | 0:34d043558630 | 37 | |
MatsumotoKouki | 0:34d043558630 | 38 | |
MatsumotoKouki | 0:34d043558630 | 39 | int main() { |
MatsumotoKouki | 10:c60fec9b4e1b | 40 | // float *hokui_n,*tokei_n; |
MatsumotoKouki | 0:34d043558630 | 41 | |
MatsumotoKouki | 0:34d043558630 | 42 | mkdir("/sd/mydir00", 0777); |
MatsumotoKouki | 7:e68ead6d3c1a | 43 | FILE *fp = fopen("/sd/mydir00/sddata.csv", "w"); |
MatsumotoKouki | 0:34d043558630 | 44 | if(fp == NULL) { |
MatsumotoKouki | 0:34d043558630 | 45 | error("Could not open file for write\n"); |
MatsumotoKouki | 0:34d043558630 | 46 | } |
MatsumotoKouki | 0:34d043558630 | 47 | |
MatsumotoKouki | 10:c60fec9b4e1b | 48 | //gps.baud(9600); |
MatsumotoKouki | 0:34d043558630 | 49 | //pc.baud(9600); |
MatsumotoKouki | 0:34d043558630 | 50 | //gps.attach(getGPS(&hokui_n,&tokei_n),Serial::RxIrq); |
MatsumotoKouki | 10:c60fec9b4e1b | 51 | fprintf(fp,"N,E,acc:x,acc:y,acc:z,gyr:x,gyr:y,gyr:z,mag:x,mag:y,mag:z\r\n"); |
MatsumotoKouki | 0:34d043558630 | 52 | warikomi.attach(&FlightPinDriver,0.5); |
MatsumotoKouki | 0:34d043558630 | 53 | |
MatsumotoKouki | 0:34d043558630 | 54 | |
falconsyunya | 3:aad6f83b618e | 55 | while(1) { |
falconsyunya | 3:aad6f83b618e | 56 | //BMX055部分 |
falconsyunya | 2:28ccb8bdac86 | 57 | bmx.getAcc();//加速度の取得 |
falconsyunya | 2:28ccb8bdac86 | 58 | bmx.getGyro();//角力加速度の取得 |
MatsumotoKouki | 7:e68ead6d3c1a | 59 | bmx.getMag();//地磁気の取得 |
MatsumotoKouki | 10:c60fec9b4e1b | 60 | |
MatsumotoKouki | 10:c60fec9b4e1b | 61 | gps.getgps(); |
MatsumotoKouki | 10:c60fec9b4e1b | 62 | int x = (float)(gps.longtitude-135.0)*1000000; |
MatsumotoKouki | 10:c60fec9b4e1b | 63 | int y = (float)(gps.latitude-34.0)*1000000; |
MatsumotoKouki | 10:c60fec9b4e1b | 64 | if(x==NULL)x=0; |
MatsumotoKouki | 10:c60fec9b4e1b | 65 | if(y==NULL)y=0; |
MatsumotoKouki | 10:c60fec9b4e1b | 66 | |
MatsumotoKouki | 9:fc0559c27628 | 67 | |
MatsumotoKouki | 10:c60fec9b4e1b | 68 | //float hokui_n=0; |
MatsumotoKouki | 10:c60fec9b4e1b | 69 | //float tokei_n=0; |
MatsumotoKouki | 10:c60fec9b4e1b | 70 | |
MatsumotoKouki | 10:c60fec9b4e1b | 71 | //getGPS(hokui_n,tokei_n);//GPS取得 |
MatsumotoKouki | 10:c60fec9b4e1b | 72 | |
MatsumotoKouki | 10:c60fec9b4e1b | 73 | // printf("%f,%f\n\r",*hokui_n,*tokei_n); |
MatsumotoKouki | 10:c60fec9b4e1b | 74 | |
MatsumotoKouki | 10:c60fec9b4e1b | 75 | fprintf(fp,"%f,%f,%f,%f,%f,%2.4f,%2.4f,%2.4f,%2.4f,%2.4f,%2.4f\n\r",gps.longtitude,gps.latitude,(bmx.accel[0]/512)*9.8, |
MatsumotoKouki | 10:c60fec9b4e1b | 76 | (bmx.accel[1]/512)*9.8,(bmx.accel[2]/512)*9.8,(bmx.gyroscope[0]*125)/2048, |
MatsumotoKouki | 10:c60fec9b4e1b | 77 | (bmx.gyroscope[1]*125)/2048,(bmx.gyroscope[2]*125)/2048,bmx.magnet[0],bmx.magnet[1],bmx.magnet[2]); |
MatsumotoKouki | 7:e68ead6d3c1a | 78 | int a[3],b[3],c[3]; |
falconsyunya | 3:aad6f83b618e | 79 | for(int i=0;i<3;i++){ |
falconsyunya | 3:aad6f83b618e | 80 | a[i]=bmx.accel[i]; |
falconsyunya | 3:aad6f83b618e | 81 | if(a[i]<0)a[i] += 4096;//負値の場合+4096 |
falconsyunya | 3:aad6f83b618e | 82 | b[i]=bmx.gyroscope[i]; |
MatsumotoKouki | 7:e68ead6d3c1a | 83 | if(b[i]<0)b[i] += 4096;//負値の場合+4096 |
MatsumotoKouki | 7:e68ead6d3c1a | 84 | c[i]=bmx.magnet[i]; |
MatsumotoKouki | 7:e68ead6d3c1a | 85 | if(c[i]<0)c[i] += 4096;//負値の場合+4096 |
MatsumotoKouki | 7:e68ead6d3c1a | 86 | |
falconsyunya | 3:aad6f83b618e | 87 | } |
falconsyunya | 3:aad6f83b618e | 88 | //GPS部分 |
MatsumotoKouki | 8:c2812fae50df | 89 | |
falconsyunya | 3:aad6f83b618e | 90 | //fprintf(fp,"北緯:%f,東経:%f,加速度:%f,%f,%f,ジャイロ:%2.4f,%2.4f,%2.4f\n\r",hokui_n,tokei_n,(bmx.accel[0]/512)*9.8,(bmx.accel[1]/512)*9.8,(bmx.accel[2]/512)*9.8,(bmx.gyroscope[0]*125)/2048,(bmx.gyroscope[1]*125)/2048,(bmx.gyroscope[2]*125)/2048); |
MatsumotoKouki | 1:4c7bace668f6 | 91 | |
falconsyunya | 3:aad6f83b618e | 92 | //IM920部分 |
falconsyunya | 2:28ccb8bdac86 | 93 | //ノード番号(00~FF)を設定.データを送るごとに1増やす. |
falconsyunya | 2:28ccb8bdac86 | 94 | uart.printf("STNN %02x\r\n",j); |
falconsyunya | 2:28ccb8bdac86 | 95 | j++; |
falconsyunya | 2:28ccb8bdac86 | 96 | if(j==256){j=0;} |
falconsyunya | 2:28ccb8bdac86 | 97 | |
falconsyunya | 3:aad6f83b618e | 98 | uart.printf("TXDA ");//可変長データ送信コマンド |
falconsyunya | 2:28ccb8bdac86 | 99 | |
falconsyunya | 3:aad6f83b618e | 100 | uart.printf("%04x%04x%04x",a[0],a[1],a[2]);//加速度の書き出し |
falconsyunya | 3:aad6f83b618e | 101 | uart.printf("%04x%04x%04x",b[0],b[1],b[2]);//角加速度の書き出し |
MatsumotoKouki | 8:c2812fae50df | 102 | |
MatsumotoKouki | 8:c2812fae50df | 103 | |
MatsumotoKouki | 10:c60fec9b4e1b | 104 | int north=x * 1000000; |
MatsumotoKouki | 10:c60fec9b4e1b | 105 | int east=y * 1000000; |
MatsumotoKouki | 8:c2812fae50df | 106 | |
MatsumotoKouki | 8:c2812fae50df | 107 | uart.printf("%08x%08x",east,north);//GPS書き出し |
falconsyunya | 2:28ccb8bdac86 | 108 | |
falconsyunya | 2:28ccb8bdac86 | 109 | uart.printf("\r\n"); |
MatsumotoKouki | 10:c60fec9b4e1b | 110 | //printf("%d%d\n\r",north,east); |
falconsyunya | 2:28ccb8bdac86 | 111 | |
falconsyunya | 2:28ccb8bdac86 | 112 | wait(0.34);//IM920長距離モードの場合,3パケット/sが理論上最大速度. |
MatsumotoKouki | 0:34d043558630 | 113 | } |
MatsumotoKouki | 0:34d043558630 | 114 | |
MatsumotoKouki | 0:34d043558630 | 115 | |
falconsyunya | 3:aad6f83b618e | 116 | //fclose(fp); |
MatsumotoKouki | 0:34d043558630 | 117 | |
MatsumotoKouki | 0:34d043558630 | 118 | return 0; |
MatsumotoKouki | 0:34d043558630 | 119 | |
MatsumotoKouki | 0:34d043558630 | 120 | } |
MatsumotoKouki | 0:34d043558630 | 121 | |
MatsumotoKouki | 0:34d043558630 | 122 | |
MatsumotoKouki | 10:c60fec9b4e1b | 123 | /*void getGPS(float *hokui_n,float *tokei_n) { //関数getGPS定義 |
MatsumotoKouki | 0:34d043558630 | 124 | unsigned char c = gps.getc(); |
MatsumotoKouki | 0:34d043558630 | 125 | if( c=='$' || i == 256){ |
MatsumotoKouki | 0:34d043558630 | 126 | mode = 0; |
MatsumotoKouki | 0:34d043558630 | 127 | i = 0; |
MatsumotoKouki | 0:34d043558630 | 128 | } |
MatsumotoKouki | 0:34d043558630 | 129 | if(mode==0){ |
MatsumotoKouki | 0:34d043558630 | 130 | if((gps_data[i]=c) != '\r'){ |
MatsumotoKouki | 0:34d043558630 | 131 | i++; |
MatsumotoKouki | 0:34d043558630 | 132 | }else{ |
MatsumotoKouki | 0:34d043558630 | 133 | gps_data[i]='\0'; |
MatsumotoKouki | 0:34d043558630 | 134 | // pc.printf("%s\r\n",gps_data); |
MatsumotoKouki | 0:34d043558630 | 135 | if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){ |
MatsumotoKouki | 0:34d043558630 | 136 | if(rlock==1){ |
MatsumotoKouki | 0:34d043558630 | 137 | // pc.printf("Status:Lock(%d)\n\r",rlock); |
MatsumotoKouki | 0:34d043558630 | 138 | //logitude |
MatsumotoKouki | 0:34d043558630 | 139 | d_tokei= int(tokei/100); |
MatsumotoKouki | 0:34d043558630 | 140 | m_tokei= (tokei-d_tokei*100)/60; |
MatsumotoKouki | 0:34d043558630 | 141 | g_tokei= d_tokei+m_tokei; |
MatsumotoKouki | 0:34d043558630 | 142 | //pc.printf("Log:%4.5f,",g_tokei); //Latitude |
MatsumotoKouki | 0:34d043558630 | 143 | d_hokui=int(hokui/100); |
MatsumotoKouki | 0:34d043558630 | 144 | m_hokui=(hokui-d_hokui*100)/60; |
MatsumotoKouki | 0:34d043558630 | 145 | g_hokui=d_hokui+m_hokui; |
MatsumotoKouki | 0:34d043558630 | 146 | // pc.printf("Lat:%4.5f\n\r",g_hokui); |
MatsumotoKouki | 0:34d043558630 | 147 | |
MatsumotoKouki | 0:34d043558630 | 148 | *hokui_n=g_hokui; |
MatsumotoKouki | 0:34d043558630 | 149 | *tokei_n=g_tokei; |
MatsumotoKouki | 0:34d043558630 | 150 | } |
MatsumotoKouki | 10:c60fec9b4e1b | 151 | /*if(rlock==0){ |
MatsumotoKouki | 0:34d043558630 | 152 | //pc.printf("\n\rStatus:unLock(%d)\n\r",rlock); |
MatsumotoKouki | 0:34d043558630 | 153 | //pc.printf("%s",gps_data); |
MatsumotoKouki | 10:c60fec9b4e1b | 154 | *hokui_n=0; |
MatsumotoKouki | 10:c60fec9b4e1b | 155 | *tokei_n=0; |
MatsumotoKouki | 10:c60fec9b4e1b | 156 | } |
MatsumotoKouki | 0:34d043558630 | 157 | }//if |
MatsumotoKouki | 0:34d043558630 | 158 | *hokui_n=hokui; |
MatsumotoKouki | 0:34d043558630 | 159 | *tokei_n=tokei; |
MatsumotoKouki | 0:34d043558630 | 160 | } //else |
MatsumotoKouki | 10:c60fec9b4e1b | 161 | } //L.123 if |
MatsumotoKouki | 10:c60fec9b4e1b | 162 | }*/ |
MatsumotoKouki | 0:34d043558630 | 163 | |
MatsumotoKouki | 1:4c7bace668f6 | 164 | /*void callback () { |
MatsumotoKouki | 0:34d043558630 | 165 | int i; |
MatsumotoKouki | 0:34d043558630 | 166 | char buf[65]; |
MatsumotoKouki | 0:34d043558630 | 167 | |
MatsumotoKouki | 0:34d043558630 | 168 | //i = im920.recv(buf, 64); |
MatsumotoKouki | 0:34d043558630 | 169 | buf[i] = 0; |
MatsumotoKouki | 0:34d043558630 | 170 | printf("recv: '%s' (%d)\r\n", buf, i); |
MatsumotoKouki | 1:4c7bace668f6 | 171 | }*/ |
MatsumotoKouki | 0:34d043558630 | 172 | |
MatsumotoKouki | 0:34d043558630 | 173 | void FlightPinDriver(){ |
MatsumotoKouki | 0:34d043558630 | 174 | |
MatsumotoKouki | 0:34d043558630 | 175 | if(FlightPin==1){ |
MatsumotoKouki | 8:c2812fae50df | 176 | printf("Flight Pin Worked!!\n\r"); |
MatsumotoKouki | 0:34d043558630 | 177 | warikomi.detach(); |
MatsumotoKouki | 0:34d043558630 | 178 | t.attach(nichrome_ON,3);//ニクロムを作動させるまでの時間 |
MatsumotoKouki | 0:34d043558630 | 179 | } |
MatsumotoKouki | 0:34d043558630 | 180 | |
MatsumotoKouki | 0:34d043558630 | 181 | } |
MatsumotoKouki | 0:34d043558630 | 182 | |
MatsumotoKouki | 0:34d043558630 | 183 | void nichrome_ON(){ |
MatsumotoKouki | 0:34d043558630 | 184 | printf("テグスカット!\n\r"); |
MatsumotoKouki | 0:34d043558630 | 185 | fet1=1; |
MatsumotoKouki | 0:34d043558630 | 186 | wait(2.0);//テグスを切るまでにかかる時間 |
MatsumotoKouki | 0:34d043558630 | 187 | fet1=0; |
MatsumotoKouki | 0:34d043558630 | 188 | t.detach(); |
MatsumotoKouki | 0:34d043558630 | 189 | t.attach(buzzer,6);//ブザー作動までの時間 |
MatsumotoKouki | 0:34d043558630 | 190 | } |
MatsumotoKouki | 0:34d043558630 | 191 | |
MatsumotoKouki | 0:34d043558630 | 192 | void buzzer(){ |
MatsumotoKouki | 0:34d043558630 | 193 | printf("ブザー作動\n\r"); |
MatsumotoKouki | 0:34d043558630 | 194 | fet2=1; |
MatsumotoKouki | 0:34d043558630 | 195 | } |