main.cpp

00001 #include "mbed.h"
00002 #include "AsyncSerial.hpp"
00003 #include "TinyGPS.h"
00004 #include "JY901.h"
00005 
00006 Serial pc(USBTX, USBRX);//CoolTermを用いてパソコン上に表示
00007 JY901 i2c(p28,p27);// sda, scl
00008 AsyncSerial Serial1(p9,p10,38400,128); // tx, rx
00009 AsyncSerial Serial2(p13,p14,9600,256); // tx, rx
00010 DigitalOut myled(LED1);
00011 InterruptIn alt(p21);
00012 InterruptIn rot(p22);
00013 DigitalIn mid(p23);
00014 AnalogIn an1(p19);
00015 AnalogIn an2(p20);
00016 Ticker flipper;
00017 Ticker flipper2;
00018 TinyGPS gps;
00019 Timer t;
00020 Timer t2;
00021 int rot_count_buf=0;
00022 int send_f1=0;
00023 int send_f2=0;
00024 
00025 #define coef_a -21.05 //ELE
00026 #define coef_b 22.2 //RUD
00027 float mid_a=0.5,mid_b=0.5;
00028 int mid_t=0;
00029 
00030 struct DATA{
00031     int height;
00032     int rot_count;
00033     float air_speed;
00034     float analog[2];//0=ELE 1=RUD
00035     float fmps,falt;
00036     unsigned long fix_age;
00037     long lat, lon;
00038     unsigned long time, date, speed, course;
00039     float angle[3];
00040     int a,b;
00041 };
00042 DATA data;
00043 
00044 void ini(){
00045     data.height=0;data.rot_count=0;data.air_speed=0;data.analog[0]=0.0;data.analog[1]=0.0;
00046     data.time=0;data.date=0;
00047     data.lat=0;data.lon=0.0;data.fmps=0.0;data.falt=0.0;data.fix_age=0;
00048     data.course=0;
00049     data.angle[0]=0.0;data.angle[1]=0.0;data.angle[2]=0.0;
00050     send_f1=0;
00051     send_f2=0;
00052     mid.mode(PullUp);
00053 }
00054 float con(float a){
00055     if(a>180.0)return a-360.0;
00056     else return a;
00057 }
00058 
00059 void read_gyro(){
00060     data.angle[0]=con(i2c.getXaxisAngle());
00061     data.angle[1]=con(i2c.getYaxisAngle());
00062     data.angle[2]=con(i2c.getZaxisAngle());
00063 }
00064 void read_analog(){
00065     data.analog[0]=an1.read();//ELE
00066     data.analog[1]=an2.read();//RUD
00067     data.a=(int)((data.analog[0]-mid_a)*coef_a+4.5);
00068     data.b=(int)((data.analog[1]-mid_b)*coef_b+4.5);
00069     if(data.a>8)data.a=8;
00070     if(data.a<0)data.a=0;
00071     if(data.b>8)data.b=8;
00072     if(data.b<0)data.b=0;
00073 }
00074 
00075 void mid1(){
00076     mid_t=t2.read_ms();
00077     mid_a=an1.read();
00078     mid_b=an2.read(); 
00079 }
00080 
00081 void alt1() {
00082     t.start();
00083 }
00084 void alt2() {
00085     t.stop();
00086     data.height=(int)(t.read_us()/58);
00087     t.reset();
00088 }
00089 void rot1(){
00090     rot_count_buf++;
00091 }
00092 
00093 void rot2(){
00094     data.rot_count=rot_count_buf;
00095     rot_count_buf=0;
00096     send_f1=1;
00097 }
00098 
00099 
00100 void send(){//disp
00101     //def
00102     Serial2.printf("%d,%d,%.2f,%d,%d,%.2f,%.2f,%f,%f\r\n",data.height,data.rot_count,data.air_speed,data.a,data.b,data.fmps,data.angle[0],data.analog[0],data.analog[1]);
00103     
00104     //pc.printf("%d,%d,%.2f,%d,%d,%.2f,%.2f,%f,%f\r\n",data.height,data.rot_count,data.air_speed,data.a,data.b,data.fmps,data.angle[0],data.analog[0],data.analog[1]);
00105     pc.printf("time=%5ds, height=%4.2fm, rot=%3drpm, speed=%4.2fm/s, ELE=%f, RUD=%f, %d, %d, %d, %d, %d, %.2f, %.2f, %.2f, %.2f, %.2f\r\n"
00106     ,t2.read_ms()/1000,data.height/100,data.rot_count,data.air_speed
00107     ,data.analog[0],data.analog[1],data.date,data.time
00108     ,data.lat,data.lon,data.course,data.fmps,data.falt,data.angle[0],data.angle[1],data.angle[2]);
00109     //rudele
00110     //Serial2.printf("ELE,RUD,%d,%d,%f,%f\r\n",data.a,data.b,data.analog[0],data.analog[1]);
00111     
00112     //Serial2.printf("%d,%d,%.2f,%.2f\r\n",data.a,data.b,data.analog[0],data.analog[1]);
00113     /*Serial2.printf("%d,%d,%d,%.2f,%f,%f,%d,%d,%d,%d,%d,%.2f,%.2f,%.2f,%.2f,%.2f\r\n"
00114     ,t2.read_ms(),data.height,data.rot_count,data.air_speed
00115     ,data.analog[0],data.analog[1],data.date,data.time
00116     ,data.lat,data.lon,data.course,data.fmps,data.falt,data.angle[0],data.angle[1],data.angle[2]);*/
00117 }
00118 
00119 void send2(){//log
00120     Serial1.printf("%d,%d,%d,%.2f,%f,%f,%d,%d,%d,%d,%d,%.2f,%.2f,%.2f,%.2f,%.2f\r\n"
00121     ,t2.read_ms(),data.height,data.rot_count,data.air_speed
00122     ,data.analog[0],data.analog[1],data.date,data.time
00123     ,data.lat,data.lon,data.course,data.fmps,data.falt,data.angle[0],data.angle[1],data.angle[2]);
00124 }
00125 
00126 
00127 void send2_f(){
00128     send_f2=1;
00129 }
00130     
00131 
00132 int main() {
00133     char a[10];//バッファ
00134     int i=0;//count用
00135     int prev_m=0;
00136     ini();
00137     t.start();  
00138     t2.start();  
00139     alt.rise(&alt1);//立ち上がり
00140     alt.fall(&alt2);//立ち下がり
00141     rot.rise(&rot1);//立ち上がり
00142     rot.fall(&rot1);//立ち下がり
00143     //mid.fall(&mid1);//立ち上がり
00144     
00145     flipper.attach(&rot2, 0.5);//0.5sごとに回転数更新＆送信   
00146     i2c.calibrateAll(3000);
00147     flipper2.attach(&send2_f, 0.2);//0.2sごとに送信
00148     while(1) {
00149         while(Serial1.readable()>0){
00150             a[i]=Serial1.getc();
00151             if(a[i]=='\r'){
00152                 a[i]=Serial1.getc();//\n
00153                 a[i]='\0';
00154                 i=0;
00155                 data.air_speed=atof(a);
00156             }else{
00157                 i++;
00158             }   
00159         }
00160         
00161         while(Serial2.readable()>0){
00162             char c = Serial2.getc();
00163             if(gps.encode(c)){
00164                 gps.get_position(&(data.lat), &(data.lon), &(data.fix_age));// retrieves +/- lat/long in 100000ths of a degree
00165                 gps.get_datetime(&(data.date), &(data.time), &(data.fix_age));// time in hhmmsscc
00166                 data.course = gps.course();// course in 100ths of a degree
00167                 data.fmps = gps.f_speed_mps(); // speed in m/sec
00168                 data.falt = gps.f_altitude(); // +/- altitude in meters
00169             }
00170         }
00171         if(send_f1==1){
00172             send_f1=0;
00173             send(); 
00174         }
00175         if(send_f2==1){
00176             read_gyro();
00177             read_analog();
00178             send_f2=0;
00179             send2();
00180             int a;
00181             a=mid;
00182             if(a==prev_m && !a && mid_t==0){
00183                 mid1();
00184                 Serial1.printf("##%f,%f\r\n",mid_a,mid_b);
00185                 Serial2.printf("##%f,%f\r\n",mid_a,mid_b);
00186             }
00187             prev_m=a;            
00188         }
00189     }
00190 }