Atsumi Toda / Mbed 2 deprecated All_system_kai2

今までのメインプログラムとGPSのプログラムを統合した。

Dependencies:   HMC5883L SDFileSystem TextOLED mbed

Fork of GPS_Test05 by basyo matsuo

main.cpp@4:39845036d492, 2018-02-15 (annotated)

Committer:
Joeatsumi
Date:
Thu Feb 15 05:47:19 2018 +0000
Revision:
4:39845036d492
Parent:
2:9f8fa9035357 
GPS???????????????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
takeuchi 0:f3ff53b5b707 1 //GPS GT-720F Test05
basyo38 2:9f8fa9035357 2
basyo38 2:9f8fa9035357 3
takeuchi 0:f3ff53b5b707 4 #include "mbed.h"
basyo38 2:9f8fa9035357 5 #include "TextOLED.h"
basyo38 2:9f8fa9035357 6 #include "SDFileSystem.h"
basyo38 2:9f8fa9035357 7 #include "stdio.h"
Joeatsumi 1:04787f83fac1 8
takeuchi 0:f3ff53b5b707 9
takeuchi 0:f3ff53b5b707 10 #define ON 1
takeuchi 0:f3ff53b5b707 11 #define OFF 0
basyo38 2:9f8fa9035357 12
basyo38 2:9f8fa9035357 13
basyo38 2:9f8fa9035357 14 AnalogIn alt(p20); //高度計読み取り
basyo38 2:9f8fa9035357 15 InterruptIn speedcount(p17); //機速用のフォトインタラプタ読み取り
basyo38 2:9f8fa9035357 16 InterruptIn rpmcount(p21); //回転数用のフォトインタラプタ読み取り
basyo38 2:9f8fa9035357 17 InterruptIn Log_Switch(p26);//ログスイッチ読み取り
basyo38 2:9f8fa9035357 18
basyo38 2:9f8fa9035357 19
basyo38 2:9f8fa9035357 20 Serial pc(USBTX, USBRX);
basyo38 2:9f8fa9035357 21 Serial control_com(p28, p27);//制御基板との通信用
basyo38 2:9f8fa9035357 22 Serial android_com(p9, p10); //Androidとの通信用txrx!!
basyo38 2:9f8fa9035357 23
basyo38 2:9f8fa9035357 24 Serial gps(p13,p14);// tx, rx
basyo38 2:9f8fa9035357 25
basyo38 2:9f8fa9035357 26 TextOLED pilot_oled(p11, p12 ,p30 ,p29 ,p15, p16); // RS, E, DB4, DB5, DB6, DB7 有機EL!!
basyo38 2:9f8fa9035357 27 SDFileSystem sd(p5, p6, p7, p8, "sd");//SDpinの番号 3,7,5,2 //1pin--Vout,4pin--Vout,6pin--GND,8pin--Vout
basyo38 2:9f8fa9035357 28
basyo38 2:9f8fa9035357 29
basyo38 2:9f8fa9035357 30 DigitalOut led1(LED1);
basyo38 2:9f8fa9035357 31 DigitalOut led2(LED2);
basyo38 2:9f8fa9035357 32 DigitalOut led3(LED3);
basyo38 2:9f8fa9035357 33 DigitalOut led4(LED4);
basyo38 2:9f8fa9035357 34
basyo38 2:9f8fa9035357 35
basyo38 2:9f8fa9035357 36 //音システム用
basyo38 2:9f8fa9035357 37 DigitalOut soundsystem1(p19);
basyo38 2:9f8fa9035357 38 DigitalOut soundsystem2(p24);
basyo38 2:9f8fa9035357 39 DigitalOut soundsystem3(p25);
basyo38 2:9f8fa9035357 40
basyo38 2:9f8fa9035357 41 DigitalOut option(p22);//toGPS and jailo
basyo38 2:9f8fa9035357 42
basyo38 2:9f8fa9035357 43
basyo38 2:9f8fa9035357 44 //タイマー割り込み宣言
basyo38 2:9f8fa9035357 45 Ticker ticker;//updateのタイマー割り込み
basyo38 2:9f8fa9035357 46 Timer t,t1;
basyo38 2:9f8fa9035357 47
basyo38 2:9f8fa9035357 48 int speedcounter = 0, rpmcounter = 0, min = 0, sec = 0, i;
basyo38 2:9f8fa9035357 49 int oldspeed = 0, oldrpm = 0;
basyo38 2:9f8fa9035357 50 int log_count = 0;
basyo38 2:9f8fa9035357 51 int SD_count = 0;
basyo38 2:9f8fa9035357 52 int up2=0,up1=0,down1=0,down2=0,E_neut=0;
basyo38 2:9f8fa9035357 53 int left1=0,left2=0,right1=0,right2=0,R_neut=0;
basyo38 2:9f8fa9035357 54 int send_compass=0;
basyo38 2:9f8fa9035357 55
basyo38 2:9f8fa9035357 56
basyo38 2:9f8fa9035357 57 double time_s = 0,time_ms=0,time1=0,time2=0, speed = 0, rpm = 0, height = 0,compass = 0;
basyo38 2:9f8fa9035357 58
basyo38 2:9f8fa9035357 59 char ele;
basyo38 2:9f8fa9035357 60 char rud;
basyo38 2:9f8fa9035357 61 char e_trim='e',r_trim='r';
basyo38 2:9f8fa9035357 62 char compass_array[4] = "000";
basyo38 2:9f8fa9035357 63 char data;
basyo38 2:9f8fa9035357 64 char send_buff1[24]={0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0};
basyo38 2:9f8fa9035357 65
basyo38 2:9f8fa9035357 66
basyo38 2:9f8fa9035357 67 unsigned char Log_flag = OFF;
basyo38 2:9f8fa9035357 68 unsigned char SD_flag = OFF;
basyo38 2:9f8fa9035357 69
Joeatsumi 4:39845036d492 70 char c;
Joeatsumi 4:39845036d492 71 int i3,rlock,stn;
Joeatsumi 4:39845036d492 72 char gps_data[256];
Joeatsumi 4:39845036d492 73 char ns,ew;
Joeatsumi 4:39845036d492 74 float time3,hokui,tokei;
Joeatsumi 4:39845036d492 75 float g_hokui,g_tokei;
Joeatsumi 4:39845036d492 76 float d_hokui,m_hokui,d_tokei,m_tokei;
Joeatsumi 4:39845036d492 77
Joeatsumi 4:39845036d492 78 int h_time=0,m_time=0,s_time=0;
Joeatsumi 4:39845036d492 79
basyo38 2:9f8fa9035357 80
basyo38 2:9f8fa9035357 81
basyo38 2:9f8fa9035357 82 //プロトタイプ宣言
basyo38 2:9f8fa9035357 83 //void init();
basyo38 2:9f8fa9035357 84 //void myled(int a,int b,int c,int d);
basyo38 2:9f8fa9035357 85 void SD_log();
basyo38 2:9f8fa9035357 86 void OLEDdisplay_print();
basyo38 2:9f8fa9035357 87 void spd_f();
basyo38 2:9f8fa9035357 88 void rpm_f();
basyo38 2:9f8fa9035357 89 void spd_c();
basyo38 2:9f8fa9035357 90 void rpm_c();
basyo38 2:9f8fa9035357 91 void control_communication();
basyo38 2:9f8fa9035357 92 void android_communication();
basyo38 2:9f8fa9035357 93 void update();
basyo38 2:9f8fa9035357 94 void altitude_f();
basyo38 2:9f8fa9035357 95 void compass_f();
basyo38 2:9f8fa9035357 96 void Log_Switch_f();
basyo38 2:9f8fa9035357 97 void CGRAM_1();
basyo38 2:9f8fa9035357 98 void CGRAM_2();
basyo38 2:9f8fa9035357 99 void CGRAM_3();
basyo38 2:9f8fa9035357 100 void CGRAM_4();
basyo38 2:9f8fa9035357 101 void CGRAM_5();
basyo38 2:9f8fa9035357 102
basyo38 2:9f8fa9035357 103
basyo38 2:9f8fa9035357 104 //=======================================================================//
basyo38 2:9f8fa9035357 105
basyo38 2:9f8fa9035357 106
Joeatsumi 1:04787f83fac1 107
Joeatsumi 1:04787f83fac1 108 LocalFileSystem local("local");
takeuchi 0:f3ff53b5b707 109
Joeatsumi 1:04787f83fac1 110
Joeatsumi 1:04787f83fac1 111 FILE *fp;
takeuchi 0:f3ff53b5b707 112
takeuchi 0:f3ff53b5b707 113 int main() {
takeuchi 0:f3ff53b5b707 114
Joeatsumi 4:39845036d492 115
Joeatsumi 1:04787f83fac1 116
basyo38 2:9f8fa9035357 117 //mkdir("/sd/gps_data", 0777);//まずsdのフォルダをつくる。
Joeatsumi 1:04787f83fac1 118
takeuchi 0:f3ff53b5b707 119 gps.baud(9600);
takeuchi 0:f3ff53b5b707 120
basyo38 2:9f8fa9035357 121 pc.printf("Start TBT!!\n");
basyo38 2:9f8fa9035357 122
basyo38 2:9f8fa9035357 123 led1=1;
basyo38 2:9f8fa9035357 124 led2=1;
basyo38 2:9f8fa9035357 125 led3=1;
basyo38 2:9f8fa9035357 126 led4=1;
basyo38 2:9f8fa9035357 127 wait(0.01);
basyo38 2:9f8fa9035357 128 pilot_oled.cls();
basyo38 2:9f8fa9035357 129 wait(0.001);
basyo38 2:9f8fa9035357 130 CGRAM_1();
basyo38 2:9f8fa9035357 131 CGRAM_2();
basyo38 2:9f8fa9035357 132 CGRAM_3();
basyo38 2:9f8fa9035357 133 CGRAM_4();
basyo38 2:9f8fa9035357 134 CGRAM_5();
basyo38 2:9f8fa9035357 135 // myled(1,1,0,0);
basyo38 2:9f8fa9035357 136 wait(2);
basyo38 2:9f8fa9035357 137 mkdir("/sd/mydir", 0777);
basyo38 2:9f8fa9035357 138 android_com.baud(115200);
basyo38 2:9f8fa9035357 139 android_com.format(8,Serial::None,1);
basyo38 2:9f8fa9035357 140
basyo38 2:9f8fa9035357 141 control_com.attach(control_communication, Serial::RxIrq);
basyo38 2:9f8fa9035357 142 control_com.baud(115200);
basyo38 2:9f8fa9035357 143
basyo38 2:9f8fa9035357 144 t.start();
basyo38 2:9f8fa9035357 145 t1.start();
basyo38 2:9f8fa9035357 146
basyo38 2:9f8fa9035357 147 Log_Switch.rise(&Log_Switch_f);
basyo38 2:9f8fa9035357 148 speedcount.rise(&spd_c);
basyo38 2:9f8fa9035357 149 rpmcount.rise(&rpm_c);
basyo38 2:9f8fa9035357 150 ticker.attach(update,1);//1s更新
basyo38 2:9f8fa9035357 151 //time1 = t1.read_ms();
basyo38 2:9f8fa9035357 152
basyo38 2:9f8fa9035357 153
basyo38 2:9f8fa9035357 154
takeuchi 0:f3ff53b5b707 155 while (1) {
basyo38 2:9f8fa9035357 156
basyo38 2:9f8fa9035357 157
basyo38 2:9f8fa9035357 158 // pc.printf("There is TBT!!\n");
basyo38 2:9f8fa9035357 159 led3=!led3;
basyo38 2:9f8fa9035357 160 if(min >=30) { //30分経過時、タイマーリセット
basyo38 2:9f8fa9035357 161 t.reset();
basyo38 2:9f8fa9035357 162 }
basyo38 2:9f8fa9035357 163 if(Log_flag == OFF) { //log取るごとにリセット
basyo38 2:9f8fa9035357 164 t.stop();
basyo38 2:9f8fa9035357 165 t.reset();
basyo38 2:9f8fa9035357 166 } else {
basyo38 2:9f8fa9035357 167 t.start();
basyo38 2:9f8fa9035357 168 }
basyo38 2:9f8fa9035357 169 time_s = t.read();//時間取得
basyo38 2:9f8fa9035357 170 min = (int)(time_s) / 60;
basyo38 2:9f8fa9035357 171 sec = (int)time_s % 60;
basyo38 2:9f8fa9035357 172 wait(0.0002);
basyo38 2:9f8fa9035357 173
basyo38 2:9f8fa9035357 174
basyo38 2:9f8fa9035357 175 i3=0;
takeuchi 0:f3ff53b5b707 176 while(gps.getc()!='$'){
takeuchi 0:f3ff53b5b707 177 }
takeuchi 0:f3ff53b5b707 178
basyo38 2:9f8fa9035357 179 while( (gps_data[i3]=gps.getc()) != '\r'){
basyo38 2:9f8fa9035357 180 i3++;
basyo38 2:9f8fa9035357 181 if(i3==256){
Joeatsumi 1:04787f83fac1 182
basyo38 2:9f8fa9035357 183 i3=255;
takeuchi 0:f3ff53b5b707 184 break;
takeuchi 0:f3ff53b5b707 185 }
takeuchi 0:f3ff53b5b707 186 }
basyo38 2:9f8fa9035357 187 gps_data[i3]='\0';
takeuchi 0:f3ff53b5b707 188
takeuchi 0:f3ff53b5b707 189 //test
takeuchi 0:f3ff53b5b707 190 /* Test data
takeuchi 0:f3ff53b5b707 191 rlock=1;
takeuchi 0:f3ff53b5b707 192 hokui=3532.25024; //=>35.537502
takeuchi 0:f3ff53b5b707 193 tokei=13751.86820;//=>137.864471
takeuchi 0:f3ff53b5b707 194 */
Joeatsumi 4:39845036d492 195 if( sscanf(gps_data, "GPGGA,%f,%f,%c,%f,%c,%d,%d",&time3,&hokui,&ns,&tokei,&ew,&rlock,&stn) >= 1){
takeuchi 0:f3ff53b5b707 196 if(rlock >= 1){
Joeatsumi 1:04787f83fac1 197
Joeatsumi 1:04787f83fac1 198 //time set
Joeatsumi 4:39845036d492 199 h_time=int(time3/10000);
Joeatsumi 4:39845036d492 200 m_time=int((time3-h_time*10000)/100);
Joeatsumi 4:39845036d492 201 s_time=int(time3-h_time*10000-m_time*100);
Joeatsumi 1:04787f83fac1 202 h_time=h_time+9;//UTC =>JST
Joeatsumi 1:04787f83fac1 203
takeuchi 0:f3ff53b5b707 204 //hokui
takeuchi 0:f3ff53b5b707 205 d_hokui=int(hokui/100);
takeuchi 0:f3ff53b5b707 206 m_hokui=(hokui-d_hokui*100)/60;
takeuchi 0:f3ff53b5b707 207 g_hokui=d_hokui+m_hokui;
takeuchi 0:f3ff53b5b707 208 //tokei
takeuchi 0:f3ff53b5b707 209 d_tokei=int(tokei/100);
takeuchi 0:f3ff53b5b707 210 m_tokei=(tokei-d_tokei*100)/60;
takeuchi 0:f3ff53b5b707 211 g_tokei=d_tokei+m_tokei;
takeuchi 0:f3ff53b5b707 212 //g_hokui=int(hokui/100)+(hokui-int(hokui/100))/60;
takeuchi 0:f3ff53b5b707 213 //g_tokei=int(tokei/100)+(tokei-int(tokei/100))/60;
Joeatsumi 1:04787f83fac1 214
takeuchi 0:f3ff53b5b707 215
Joeatsumi 1:04787f83fac1 216 //==================================================//
Joeatsumi 1:04787f83fac1 217 pc.printf("%2d,%2d,%2d,%4.6f,,%3.6f,\r\n",h_time,m_time,s_time,g_tokei,g_hokui);
Joeatsumi 1:04787f83fac1 218 //==================================================//
Joeatsumi 1:04787f83fac1 219
Joeatsumi 1:04787f83fac1 220
Joeatsumi 1:04787f83fac1 221
Joeatsumi 1:04787f83fac1 222 //}
takeuchi 0:f3ff53b5b707 223 }
takeuchi 0:f3ff53b5b707 224 }//if
takeuchi 0:f3ff53b5b707 225 }//while
takeuchi 0:f3ff53b5b707 226 }//main
takeuchi 0:f3ff53b5b707 227
takeuchi 0:f3ff53b5b707 228
basyo38 2:9f8fa9035357 229 void SD_log() //SDログ関数
basyo38 2:9f8fa9035357 230 {
basyo38 2:9f8fa9035357 231 // __disable_irq();
basyo38 2:9f8fa9035357 232 led2 = 1;
basyo38 2:9f8fa9035357 233 if (Log_flag == ON) {
basyo38 2:9f8fa9035357 234 FILE *fp;
basyo38 2:9f8fa9035357 235 /*
basyo38 2:9f8fa9035357 236 fp = fopen("/sd/log.txt", "a");
basyo38 2:9f8fa9035357 237
basyo38 2:9f8fa9035357 238 fprintf(fp,"%2d,%2d,%2d,%4.6f,,%3.6f,\r\n",h_time,m_time,s_time,g_tokei,g_hokui);
basyo38 2:9f8fa9035357 239 fprintf(fp,"%4.6f,%3.6f,\r\n",g_tokei,g_hokui);
basyo38 2:9f8fa9035357 240 fclose(fp);
basyo38 2:9f8fa9035357 241 */
basyo38 2:9f8fa9035357 242
basyo38 2:9f8fa9035357 243 fp = fopen("/sd/mydir/TP.txt", "a");
basyo38 2:9f8fa9035357 244
Joeatsumi 4:39845036d492 245
basyo38 2:9f8fa9035357 246 if (NULL == fp) {
basyo38 2:9f8fa9035357 247 pilot_oled.locate(0, 0);
basyo38 2:9f8fa9035357 248 pilot_oled.printf("There is no SD!!");
basyo38 2:9f8fa9035357 249 pc.printf("There is no SD!!\n");
basyo38 2:9f8fa9035357 250 error("Could not open file for write \n");
basyo38 2:9f8fa9035357 251 } else {
Joeatsumi 4:39845036d492 252 fprintf(fp, "NO:%d,%02d:%02d,%1.0f,%0.3f,%0.3f,%0.3f,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d\r\n",
basyo38 2:9f8fa9035357 253 SD_count, min, sec, rpm, speed, height,compass,up2,up1,E_neut,down1,down2,left2,left1,R_neut,right1,right2);
Joeatsumi 4:39845036d492 254
Joeatsumi 4:39845036d492 255 // fprintf(fp,";%3.6f;,%3.6f,;\r\n",g_tokei,g_hokui);
basyo38 2:9f8fa9035357 256 fclose(fp);
basyo38 2:9f8fa9035357 257 //操舵データ
basyo38 2:9f8fa9035357 258 }
Joeatsumi 4:39845036d492 259 fp = fopen("/sd/log.txt", "a");
Joeatsumi 4:39845036d492 260
Joeatsumi 4:39845036d492 261 // fprintf(fp,"%2d,%2d,%2d,%4.6f,,%3.6f,\r\n",h_time,m_time,s_time,g_tokei,g_hokui);
Joeatsumi 4:39845036d492 262 fprintf(fp,"%4.6f,%3.6f,\r\n",g_tokei,g_hokui);
Joeatsumi 4:39845036d492 263 fclose(fp);
basyo38 2:9f8fa9035357 264 }
basyo38 2:9f8fa9035357 265 led2 = 0;
basyo38 2:9f8fa9035357 266 // __enable_irq();
basyo38 2:9f8fa9035357 267 }
takeuchi 0:f3ff53b5b707 268
basyo38 2:9f8fa9035357 269 void Log_Switch_f()//ログスイッチ
basyo38 2:9f8fa9035357 270 {
basyo38 2:9f8fa9035357 271 log_count++;
basyo38 2:9f8fa9035357 272 if (log_count == 1) {
basyo38 2:9f8fa9035357 273 Log_flag = ON;
basyo38 2:9f8fa9035357 274 SD_count++;
basyo38 2:9f8fa9035357 275 } else if (log_count == 2) {
basyo38 2:9f8fa9035357 276 Log_flag = OFF;
basyo38 2:9f8fa9035357 277 log_count = 0;
basyo38 2:9f8fa9035357 278 }
basyo38 2:9f8fa9035357 279 }
basyo38 2:9f8fa9035357 280
basyo38 2:9f8fa9035357 281 void OLEDdisplay_print() //有機EL表示用関数
basyo38 2:9f8fa9035357 282 {
basyo38 2:9f8fa9035357 283 led3!=led3;
basyo38 2:9f8fa9035357 284 //__disable_irq();
basyo38 2:9f8fa9035357 285 /* if(Log_flag == OFF) {
basyo38 2:9f8fa9035357 286 wait_ms(0.7);
basyo38 2:9f8fa9035357 287 pilot_oled.locate(0, 0);
basyo38 2:9f8fa9035357 288 pilot_oled.printf("log");//時間表示
basyo38 2:9f8fa9035357 289 pilot_oled.locate(3, 0);
basyo38 2:9f8fa9035357 290 pilot_oled.putc(0xB5);//オ
basyo38 2:9f8fa9035357 291 pilot_oled.locate(4, 0);
basyo38 2:9f8fa9035357 292 pilot_oled.putc(0xBE);//セ
basyo38 2:9f8fa9035357 293 pilot_oled.locate(5, 0);
basyo38 2:9f8fa9035357 294 pilot_oled.putc(0x21);//！
basyo38 2:9f8fa9035357 295 } else {
basyo38 2:9f8fa9035357 296 pilot_oled.locate(0, 0);
basyo38 2:9f8fa9035357 297 pilot_oled.printf("%02d:%02d ", min, sec);//時間表示
basyo38 2:9f8fa9035357 298 }
basyo38 2:9f8fa9035357 299 wait_ms(0.7);*/
basyo38 2:9f8fa9035357 300 pilot_oled.locate(0, 1);
basyo38 2:9f8fa9035357 301 pilot_oled.printf("r%3.0f s%4.1f h%4.1f", rpm,speed,height);//センサーの値
basyo38 2:9f8fa9035357 302 wait_ms(0.7);
basyo38 2:9f8fa9035357 303
basyo38 2:9f8fa9035357 304 pilot_oled.locate(4, 0);
basyo38 2:9f8fa9035357 305 pilot_oled.printf("%1d", send_compass);
basyo38 2:9f8fa9035357 306 pilot_oled.locate(11, 0);
basyo38 2:9f8fa9035357 307 if (Log_flag == ON) pilot_oled.printf("ON ");
basyo38 2:9f8fa9035357 308 else pilot_oled.printf("OFF");
basyo38 2:9f8fa9035357 309
basyo38 2:9f8fa9035357 310 // pilot_oled.locate(0, 0);
basyo38 2:9f8fa9035357 311 // pilot_oled.printf("count:%d",rpmcounter);//時間表示
basyo38 2:9f8fa9035357 312 wait_ms(0.7);
basyo38 2:9f8fa9035357 313 // __enable_irq();
basyo38 2:9f8fa9035357 314 }
basyo38 2:9f8fa9035357 315
basyo38 2:9f8fa9035357 316 void compass_f()//高度の変換
basyo38 2:9f8fa9035357 317 {
basyo38 2:9f8fa9035357 318 compass=atoi(compass_array);
basyo38 2:9f8fa9035357 319
basyo38 2:9f8fa9035357 320 if(compass<22.5&&compass>=0||compass<360&&compass>=337.5)send_compass=1;//北
basyo38 2:9f8fa9035357 321 else if(compass<67.5&&compass>=22.5)send_compass=2;//
basyo38 2:9f8fa9035357 322 else if(compass<112.5&&compass>=67.5)send_compass=3;//東
basyo38 2:9f8fa9035357 323 else if(compass<157.5&&compass>=112.5)send_compass=4;//
basyo38 2:9f8fa9035357 324 else if(compass<202.5&&compass>=157.5)send_compass=5;//南
basyo38 2:9f8fa9035357 325 else if(compass<247.5&&compass>=202.5)send_compass=6;//
basyo38 2:9f8fa9035357 326 else if(compass<292.5&&compass>=247.5)send_compass=7;//西
basyo38 2:9f8fa9035357 327 else if(compass<337.5&&compass>=292.5)send_compass=8;//
basyo38 2:9f8fa9035357 328
basyo38 2:9f8fa9035357 329
basyo38 2:9f8fa9035357 330
basyo38 2:9f8fa9035357 331 }
basyo38 2:9f8fa9035357 332
basyo38 2:9f8fa9035357 333 void altitude_f()//高度の変換
basyo38 2:9f8fa9035357 334 {
basyo38 2:9f8fa9035357 335 height=alt.read()*3.3*1000/1.6/100;
basyo38 2:9f8fa9035357 336 }
basyo38 2:9f8fa9035357 337
basyo38 2:9f8fa9035357 338 void rpm_c() //回転数のフォトインタラプタ
basyo38 2:9f8fa9035357 339 {
basyo38 2:9f8fa9035357 340 rpmcounter++;
basyo38 2:9f8fa9035357 341 }
basyo38 2:9f8fa9035357 342
basyo38 2:9f8fa9035357 343 void rpm_f() //回転数計測
basyo38 2:9f8fa9035357 344 {
basyo38 2:9f8fa9035357 345 // rpm = (double)(rpmcounter * 60000/time_ms/48);
basyo38 2:9f8fa9035357 346 rpm=(double)(rpmcounter);
basyo38 2:9f8fa9035357 347 rpmcounter = 0;
basyo38 2:9f8fa9035357 348 }
basyo38 2:9f8fa9035357 349
basyo38 2:9f8fa9035357 350 void spd_c() //機速のフォトインタラプタ
basyo38 2:9f8fa9035357 351 {
basyo38 2:9f8fa9035357 352 speedcounter++;
basyo38 2:9f8fa9035357 353 }
basyo38 2:9f8fa9035357 354
basyo38 2:9f8fa9035357 355 void spd_f()
basyo38 2:9f8fa9035357 356 {
basyo38 2:9f8fa9035357 357 int x = 0;
basyo38 2:9f8fa9035357 358 x = (double)(speedcounter * 60000/time_ms/8);
basyo38 2:9f8fa9035357 359 speed = (double)(0.0014*x + 0.9815);
basyo38 2:9f8fa9035357 360 //最初の機速校正時の式は、0.0015*x + 0.6063
basyo38 2:9f8fa9035357 361 if(speed<=0.9815) {
basyo38 2:9f8fa9035357 362 speed = 0;
basyo38 2:9f8fa9035357 363 }
basyo38 2:9f8fa9035357 364 speedcounter = 0;
basyo38 2:9f8fa9035357 365 }
basyo38 2:9f8fa9035357 366
basyo38 2:9f8fa9035357 367
basyo38 2:9f8fa9035357 368 void control_communication() //操舵基板との通信用
basyo38 2:9f8fa9035357 369 {
basyo38 2:9f8fa9035357 370 // myled(9,9,1,9);
basyo38 2:9f8fa9035357 371
basyo38 2:9f8fa9035357 372 // __disable_irq();
basyo38 2:9f8fa9035357 373 // t1.stop();
basyo38 2:9f8fa9035357 374 for (int i = 0; i <3; i++) {
basyo38 2:9f8fa9035357 375 compass_array[i] = control_com.getc();//高度の受信
basyo38 2:9f8fa9035357 376 }
basyo38 2:9f8fa9035357 377 ele = control_com.getc();//舵角の受信
basyo38 2:9f8fa9035357 378 rud = control_com.getc();//舵角の受信
basyo38 2:9f8fa9035357 379 e_trim = control_com.getc();//trimの受信
basyo38 2:9f8fa9035357 380 r_trim = control_com.getc();//trimの受信
basyo38 2:9f8fa9035357 381 //////////////ER操舵データ表示////////////////////
basyo38 2:9f8fa9035357 382 /* pilot_oled.locate(6, 0);
basyo38 2:9f8fa9035357 383 pilot_oled.printf("%c", ele);
basyo38 2:9f8fa9035357 384 wait_ms(0.7);
basyo38 2:9f8fa9035357 385 pilot_oled.locate(8, 0);
basyo38 2:9f8fa9035357 386 pilot_oled.printf("%c", rud);
basyo38 2:9f8fa9035357 387 wait_ms(0.7);*/
basyo38 2:9f8fa9035357 388 pilot_oled.locate(0, 0);
basyo38 2:9f8fa9035357 389 pilot_oled.printf("%1c%1c", e_trim,r_trim);
basyo38 2:9f8fa9035357 390 pilot_oled.locate(14, 0);
basyo38 2:9f8fa9035357 391 pilot_oled.printf("%2d", SD_count);
basyo38 2:9f8fa9035357 392 wait_ms(0.7);
basyo38 2:9f8fa9035357 393 ///////////エレベータログデータ条件分岐////////////////////////////
basyo38 2:9f8fa9035357 394 switch(ele) {
basyo38 2:9f8fa9035357 395 case 'U':
basyo38 2:9f8fa9035357 396 pilot_oled.locate(6, 0);
basyo38 2:9f8fa9035357 397 pilot_oled.putc(0x02);//
basyo38 2:9f8fa9035357 398 // pilot_oled.printf("U");//
basyo38 2:9f8fa9035357 399 up2 = 5 ;
basyo38 2:9f8fa9035357 400 break;
basyo38 2:9f8fa9035357 401 case 'u':
basyo38 2:9f8fa9035357 402 pilot_oled.locate(6, 0);
basyo38 2:9f8fa9035357 403 pilot_oled.putc(0x5E);//^
basyo38 2:9f8fa9035357 404 // pilot_oled.printf("u");//
basyo38 2:9f8fa9035357 405 up1 = 4 ;
basyo38 2:9f8fa9035357 406 break;
basyo38 2:9f8fa9035357 407 case 'n':
basyo38 2:9f8fa9035357 408 pilot_oled.locate(6, 0);
basyo38 2:9f8fa9035357 409 pilot_oled.putc(0x2D);//-
basyo38 2:9f8fa9035357 410 E_neut = 3 ;
basyo38 2:9f8fa9035357 411 break;
basyo38 2:9f8fa9035357 412 case 'd':
basyo38 2:9f8fa9035357 413 pilot_oled.locate(6, 0);
basyo38 2:9f8fa9035357 414 pilot_oled.putc(0x00);//vみたいな文字
basyo38 2:9f8fa9035357 415 // pilot_oled.printf("d");//
basyo38 2:9f8fa9035357 416 down1 = 2 ;
basyo38 2:9f8fa9035357 417 break;
basyo38 2:9f8fa9035357 418 case 'D':
basyo38 2:9f8fa9035357 419 pilot_oled.locate(6, 0);
basyo38 2:9f8fa9035357 420 pilot_oled.putc(0x01);
basyo38 2:9f8fa9035357 421 // pilot_oled.printf("D");//
basyo38 2:9f8fa9035357 422 down2 = 1;
basyo38 2:9f8fa9035357 423 break;
basyo38 2:9f8fa9035357 424 default:
basyo38 2:9f8fa9035357 425 break;
basyo38 2:9f8fa9035357 426 }
basyo38 2:9f8fa9035357 427 /////////ラダーログデータ条件分岐/////////////////////////////////////
basyo38 2:9f8fa9035357 428 switch(rud) {
basyo38 2:9f8fa9035357 429 case 'L':
basyo38 2:9f8fa9035357 430 pilot_oled.locate(8, 0);
basyo38 2:9f8fa9035357 431 pilot_oled.putc(0x03);
basyo38 2:9f8fa9035357 432 // pilot_oled.printf("L");//
basyo38 2:9f8fa9035357 433 left2 =1;
basyo38 2:9f8fa9035357 434 break;
basyo38 2:9f8fa9035357 435 case 'l':
basyo38 2:9f8fa9035357 436 pilot_oled.locate(8, 0);
basyo38 2:9f8fa9035357 437 pilot_oled.putc(0x3C);//<
basyo38 2:9f8fa9035357 438 left1 =2;
basyo38 2:9f8fa9035357 439 break;
basyo38 2:9f8fa9035357 440 case 'N':
basyo38 2:9f8fa9035357 441 pilot_oled.locate(8, 0);
basyo38 2:9f8fa9035357 442 pilot_oled.putc(0x7C);//|
basyo38 2:9f8fa9035357 443 R_neut=3;
basyo38 2:9f8fa9035357 444 break;
basyo38 2:9f8fa9035357 445 case 'r':
basyo38 2:9f8fa9035357 446 pilot_oled.locate(8, 0);
basyo38 2:9f8fa9035357 447 pilot_oled.putc(0x3E);//>
basyo38 2:9f8fa9035357 448 right1=4;
basyo38 2:9f8fa9035357 449 break;
basyo38 2:9f8fa9035357 450 case 'R':
basyo38 2:9f8fa9035357 451 pilot_oled.locate(8, 0);
basyo38 2:9f8fa9035357 452 pilot_oled.putc(0x04);
basyo38 2:9f8fa9035357 453 // pilot_oled.printf("R");//
basyo38 2:9f8fa9035357 454 right2=5;
basyo38 2:9f8fa9035357 455 break;
basyo38 2:9f8fa9035357 456 default:
basyo38 2:9f8fa9035357 457 break;
basyo38 2:9f8fa9035357 458 }
basyo38 2:9f8fa9035357 459 // t1.start();
basyo38 2:9f8fa9035357 460 //__enable_irq();
basyo38 2:9f8fa9035357 461 }
basyo38 2:9f8fa9035357 462
basyo38 2:9f8fa9035357 463 void android_communication()
basyo38 2:9f8fa9035357 464 {
basyo38 2:9f8fa9035357 465 // myled(9,9,9,1);
basyo38 2:9f8fa9035357 466
basyo38 2:9f8fa9035357 467 int srpm,sspeed,sheight;
basyo38 2:9f8fa9035357 468 srpm=(int)rpm;
basyo38 2:9f8fa9035357 469 sspeed=(int)(speed*10);
basyo38 2:9f8fa9035357 470 sheight=(int)(height*10);
basyo38 2:9f8fa9035357 471 // if(srpm<100)srpm+=900;
basyo38 2:9f8fa9035357 472 // if(sspeed<100)sspeed+=900;
basyo38 2:9f8fa9035357 473 // if(sheight<100)sheight+=900;
basyo38 2:9f8fa9035357 474
basyo38 2:9f8fa9035357 475 snprintf(send_buff1,10,"r%xh%xv%x",srpm,sheight,sspeed);
basyo38 2:9f8fa9035357 476 for(i=0;i<10;i++){
basyo38 2:9f8fa9035357 477 // pc.putc(send_buff1[i]);
basyo38 2:9f8fa9035357 478 android_com.putc(send_buff1[i]);
basyo38 2:9f8fa9035357 479 }
basyo38 2:9f8fa9035357 480 }
basyo38 2:9f8fa9035357 481
basyo38 2:9f8fa9035357 482 void update() //データ更新。最優先関数、割り込み禁止絶対入れろ。
basyo38 2:9f8fa9035357 483 {
basyo38 2:9f8fa9035357 484 // myled(1,0,0,0);
basyo38 2:9f8fa9035357 485 __disable_irq();
basyo38 2:9f8fa9035357 486 //time2 = t1.read_ms();
basyo38 2:9f8fa9035357 487 time_ms = t1.read_ms();//周期時間読み取り
basyo38 2:9f8fa9035357 488 t1.stop();
basyo38 2:9f8fa9035357 489 t1.reset();
basyo38 2:9f8fa9035357 490 led4 =! led4;
basyo38 2:9f8fa9035357 491 //time_ms =time2-time1;
basyo38 2:9f8fa9035357 492 rpm_f();
basyo38 2:9f8fa9035357 493 spd_f();
basyo38 2:9f8fa9035357 494 altitude_f();
basyo38 2:9f8fa9035357 495 compass_f();
basyo38 2:9f8fa9035357 496 SD_log();
basyo38 2:9f8fa9035357 497 OLEDdisplay_print();
basyo38 2:9f8fa9035357 498 android_communication();
basyo38 2:9f8fa9035357 499 pc.printf("NO:%d,%02d:%02d,R:%1.0f,S:%0.3f,H:%0.3f,%c%c%c,compass:%3.0f,%c%c,e:%c,r:%c,et:%c,rt:%c,%.3f\r\n", SD_count, min, sec, rpm, speed, height,send_buff1[4],send_buff1[5],send_buff1[6],compass,compass_array[0],compass_array[1], ele, rud,e_trim,r_trim,time_ms);
basyo38 2:9f8fa9035357 500 // pc.printf("TBT\n");
basyo38 2:9f8fa9035357 501 up2 = 0;
basyo38 2:9f8fa9035357 502 up1 = 0;
basyo38 2:9f8fa9035357 503 E_neut = 0;
basyo38 2:9f8fa9035357 504 down1=0;
basyo38 2:9f8fa9035357 505 down2=0;
basyo38 2:9f8fa9035357 506 left1= 0;
basyo38 2:9f8fa9035357 507 left2 =0;
basyo38 2:9f8fa9035357 508 R_neut = 0;
basyo38 2:9f8fa9035357 509 right1=0;
basyo38 2:9f8fa9035357 510 right2=0;
basyo38 2:9f8fa9035357 511 t1.start();
basyo38 2:9f8fa9035357 512 // time1 = t1.read_ms();
basyo38 2:9f8fa9035357 513 __enable_irq();
basyo38 2:9f8fa9035357 514 }
basyo38 2:9f8fa9035357 515
basyo38 2:9f8fa9035357 516
basyo38 2:9f8fa9035357 517 void CGRAM_1()
basyo38 2:9f8fa9035357 518 {
basyo38 2:9f8fa9035357 519 // CGRAMアドレスの指定
basyo38 2:9f8fa9035357 520 // CGRAM(1)にビット・パターンを登録します。
basyo38 2:9f8fa9035357 521 pilot_oled.writeCommand(0x40) ;
basyo38 2:9f8fa9035357 522 // 40μ秒待ちます。
basyo38 2:9f8fa9035357 523 wait_us(40);
basyo38 2:9f8fa9035357 524
basyo38 2:9f8fa9035357 525 // ここから文字のビット・パターンを1行ずつ設定していきます。
basyo38 2:9f8fa9035357 526 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 527 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 528 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 529 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 530 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 531 pilot_oled.writeData((int)0x11) ;//10001
basyo38 2:9f8fa9035357 532 pilot_oled.writeData((int)0x0A) ;//01010
basyo38 2:9f8fa9035357 533 pilot_oled.writeData((int)0x04) ;//00100
basyo38 2:9f8fa9035357 534 wait_us(40);
basyo38 2:9f8fa9035357 535 pilot_oled.cls();
basyo38 2:9f8fa9035357 536 }
basyo38 2:9f8fa9035357 537
basyo38 2:9f8fa9035357 538 void CGRAM_2()
basyo38 2:9f8fa9035357 539 {
basyo38 2:9f8fa9035357 540 // CGRAMアドレスの指定
basyo38 2:9f8fa9035357 541 // CGRAM(2)にビット・パターンを登録します。
basyo38 2:9f8fa9035357 542 pilot_oled.writeCommand(0x48) ;
basyo38 2:9f8fa9035357 543 // 40μ秒待ちます。
basyo38 2:9f8fa9035357 544 wait_us(40);
basyo38 2:9f8fa9035357 545
basyo38 2:9f8fa9035357 546 // ここから文字のビット・パターンを1行ずつ設定していきます。
basyo38 2:9f8fa9035357 547 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 548 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 549 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 550 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 551 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 552 pilot_oled.writeData((int)0x1F) ;//11111
basyo38 2:9f8fa9035357 553 pilot_oled.writeData((int)0x0E) ;//01110
basyo38 2:9f8fa9035357 554 pilot_oled.writeData((int)0x04) ;//00100
basyo38 2:9f8fa9035357 555 wait_us(40);
basyo38 2:9f8fa9035357 556 pilot_oled.cls();
basyo38 2:9f8fa9035357 557 }
basyo38 2:9f8fa9035357 558
basyo38 2:9f8fa9035357 559 void CGRAM_3()
basyo38 2:9f8fa9035357 560 {
basyo38 2:9f8fa9035357 561 // CGRAMアドレスの指定
basyo38 2:9f8fa9035357 562 // CGRAM(3)にビット・パターンを登録します。
basyo38 2:9f8fa9035357 563 pilot_oled.writeCommand(0x50) ;
basyo38 2:9f8fa9035357 564 // 40μ秒待ちます。
basyo38 2:9f8fa9035357 565 wait_us(40);
basyo38 2:9f8fa9035357 566
basyo38 2:9f8fa9035357 567 // ここから文字のビット・パターンを1行ずつ設定していきます。
basyo38 2:9f8fa9035357 568 pilot_oled.writeData((int)0x04) ;//00100
basyo38 2:9f8fa9035357 569 pilot_oled.writeData((int)0x0E) ;//01110
basyo38 2:9f8fa9035357 570 pilot_oled.writeData((int)0x1F) ;//11111
basyo38 2:9f8fa9035357 571 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 572 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 573 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 574 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 575 pilot_oled.writeData((int)0x00) ;
basyo38 2:9f8fa9035357 576 wait_us(40);
basyo38 2:9f8fa9035357 577 pilot_oled.cls();
basyo38 2:9f8fa9035357 578 }
basyo38 2:9f8fa9035357 579
basyo38 2:9f8fa9035357 580 void CGRAM_4()
basyo38 2:9f8fa9035357 581 {
basyo38 2:9f8fa9035357 582 // CGRAMアドレスの指定
basyo38 2:9f8fa9035357 583 // CGRAM(4)にビット・パターンを登録します。
basyo38 2:9f8fa9035357 584 pilot_oled.writeCommand(0x58) ;
basyo38 2:9f8fa9035357 585 // 40μ秒待ちます。
basyo38 2:9f8fa9035357 586 wait_us(40);
basyo38 2:9f8fa9035357 587
basyo38 2:9f8fa9035357 588 // ここから文字のビット・パターンを1行ずつ設定していきます。
basyo38 2:9f8fa9035357 589 pilot_oled.writeData((int)0x02) ;//00010
basyo38 2:9f8fa9035357 590 pilot_oled.writeData((int)0x06) ;//00110
basyo38 2:9f8fa9035357 591 pilot_oled.writeData((int)0x0E) ;//01110
basyo38 2:9f8fa9035357 592 pilot_oled.writeData((int)0x1E) ;//11110
basyo38 2:9f8fa9035357 593 pilot_oled.writeData((int)0x0E) ;//01110
basyo38 2:9f8fa9035357 594 pilot_oled.writeData((int)0x06) ;//00110
basyo38 2:9f8fa9035357 595 pilot_oled.writeData((int)0x02) ;//00010
basyo38 2:9f8fa9035357 596 pilot_oled.writeData((int)0x00) ;//00000
basyo38 2:9f8fa9035357 597 wait_us(40);
basyo38 2:9f8fa9035357 598 pilot_oled.cls();
basyo38 2:9f8fa9035357 599 }
basyo38 2:9f8fa9035357 600
basyo38 2:9f8fa9035357 601 void CGRAM_5()
basyo38 2:9f8fa9035357 602 {
basyo38 2:9f8fa9035357 603 // CGRAMアドレスの指定
basyo38 2:9f8fa9035357 604 // CGRAM(5)にビット・パターンを登録します。
basyo38 2:9f8fa9035357 605 pilot_oled.writeCommand(0x60) ;
basyo38 2:9f8fa9035357 606 // 40μ秒待ちます。
basyo38 2:9f8fa9035357 607 wait_us(40);
basyo38 2:9f8fa9035357 608
basyo38 2:9f8fa9035357 609 // ここから文字のビット・パターンを1行ずつ設定していきます。
basyo38 2:9f8fa9035357 610 pilot_oled.writeData((int)0x08) ;//01000
basyo38 2:9f8fa9035357 611 pilot_oled.writeData((int)0x0C) ;//01100
basyo38 2:9f8fa9035357 612 pilot_oled.writeData((int)0x0E) ;//01110
basyo38 2:9f8fa9035357 613 pilot_oled.writeData((int)0x0F) ;//01111
basyo38 2:9f8fa9035357 614 pilot_oled.writeData((int)0x0E) ;//01110
basyo38 2:9f8fa9035357 615 pilot_oled.writeData((int)0x0C) ;//01100
basyo38 2:9f8fa9035357 616 pilot_oled.writeData((int)0x08) ;//01000
basyo38 2:9f8fa9035357 617 pilot_oled.writeData((int)0x00) ;//00000
basyo38 2:9f8fa9035357 618 wait_us(40);
basyo38 2:9f8fa9035357 619 pilot_oled.cls();
basyo38 2:9f8fa9035357 620 }
basyo38 2:9f8fa9035357 621
basyo38 2:9f8fa9035357 622
basyo38 2:9f8fa9035357 623