Diff: main.cpp

Revision:

0:e4bf955dac58

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 23 07:23:01 2019 +0000
@@ -0,0 +1,190 @@
+#include "mbed.h"
+#include "AsyncSerial.hpp"
+#include "TinyGPS.h"
+#include "JY901.h"
+
+Serial pc(USBTX, USBRX);//CoolTermを用いてパソコン上に表示
+JY901 i2c(p28,p27);// sda, scl
+AsyncSerial Serial1(p9,p10,38400,128); // tx, rx
+AsyncSerial Serial2(p13,p14,9600,256); // tx, rx
+DigitalOut myled(LED1);
+InterruptIn alt(p21);
+InterruptIn rot(p22);
+DigitalIn mid(p23);
+AnalogIn an1(p19);
+AnalogIn an2(p20);
+Ticker flipper;
+Ticker flipper2;
+TinyGPS gps;
+Timer t;
+Timer t2;
+int rot_count_buf=0;
+int send_f1=0;
+int send_f2=0;
+
+#define coef_a -21.05 //ELE
+#define coef_b 22.2 //RUD
+float mid_a=0.5,mid_b=0.5;
+int mid_t=0;
+
+struct DATA{
+    int height;
+    int rot_count;
+    float air_speed;
+    float analog[2];//0=ELE 1=RUD
+    float fmps,falt;
+    unsigned long fix_age;
+    long lat, lon;
+    unsigned long time, date, speed, course;
+    float angle[3];
+    int a,b;
+};
+DATA data;
+
+void ini(){
+    data.height=0;data.rot_count=0;data.air_speed=0;data.analog[0]=0.0;data.analog[1]=0.0;
+    data.time=0;data.date=0;
+    data.lat=0;data.lon=0.0;data.fmps=0.0;data.falt=0.0;data.fix_age=0;
+    data.course=0;
+    data.angle[0]=0.0;data.angle[1]=0.0;data.angle[2]=0.0;
+    send_f1=0;
+    send_f2=0;
+    mid.mode(PullUp);
+}
+float con(float a){
+    if(a>180.0)return a-360.0;
+    else return a;
+}
+
+void read_gyro(){
+    data.angle[0]=con(i2c.getXaxisAngle());
+    data.angle[1]=con(i2c.getYaxisAngle());
+    data.angle[2]=con(i2c.getZaxisAngle());
+}
+void read_analog(){
+    data.analog[0]=an1.read();//ELE
+    data.analog[1]=an2.read();//RUD
+    data.a=(int)((data.analog[0]-mid_a)*coef_a+4.5);
+    data.b=(int)((data.analog[1]-mid_b)*coef_b+4.5);
+    if(data.a>8)data.a=8;
+    if(data.a<0)data.a=0;
+    if(data.b>8)data.b=8;
+    if(data.b<0)data.b=0;
+}
+
+void mid1(){
+    mid_t=t2.read_ms();
+    mid_a=an1.read();
+    mid_b=an2.read(); 
+}
+
+void alt1() {
+    t.start();
+}
+void alt2() {
+    t.stop();
+    data.height=(int)(t.read_us()/58);
+    t.reset();
+}
+void rot1(){
+    rot_count_buf++;
+}
+
+void rot2(){
+    data.rot_count=rot_count_buf;
+    rot_count_buf=0;
+    send_f1=1;
+}
+
+
+void send(){//disp
+    //def
+    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]);
+    
+    //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]);
+    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"
+    ,t2.read_ms()/1000,data.height/100,data.rot_count,data.air_speed
+    ,data.analog[0],data.analog[1],data.date,data.time
+    ,data.lat,data.lon,data.course,data.fmps,data.falt,data.angle[0],data.angle[1],data.angle[2]);
+    //rudele
+    //Serial2.printf("ELE,RUD,%d,%d,%f,%f\r\n",data.a,data.b,data.analog[0],data.analog[1]);
+    
+    //Serial2.printf("%d,%d,%.2f,%.2f\r\n",data.a,data.b,data.analog[0],data.analog[1]);
+    /*Serial2.printf("%d,%d,%d,%.2f,%f,%f,%d,%d,%d,%d,%d,%.2f,%.2f,%.2f,%.2f,%.2f\r\n"
+    ,t2.read_ms(),data.height,data.rot_count,data.air_speed
+    ,data.analog[0],data.analog[1],data.date,data.time
+    ,data.lat,data.lon,data.course,data.fmps,data.falt,data.angle[0],data.angle[1],data.angle[2]);*/
+}
+
+void send2(){//log
+    Serial1.printf("%d,%d,%d,%.2f,%f,%f,%d,%d,%d,%d,%d,%.2f,%.2f,%.2f,%.2f,%.2f\r\n"
+    ,t2.read_ms(),data.height,data.rot_count,data.air_speed
+    ,data.analog[0],data.analog[1],data.date,data.time
+    ,data.lat,data.lon,data.course,data.fmps,data.falt,data.angle[0],data.angle[1],data.angle[2]);
+}
+
+
+void send2_f(){
+    send_f2=1;
+}
+    
+
+int main() {
+    char a[10];//バッファ
+    int i=0;//count用
+    int prev_m=0;
+    ini();
+    t.start();  
+    t2.start();  
+    alt.rise(&alt1);//立ち上がり
+    alt.fall(&alt2);//立ち下がり
+    rot.rise(&rot1);//立ち上がり
+    rot.fall(&rot1);//立ち下がり
+    //mid.fall(&mid1);//立ち上がり
+    
+    flipper.attach(&rot2, 0.5);//0.5sごとに回転数更新＆送信   
+    i2c.calibrateAll(3000);
+    flipper2.attach(&send2_f, 0.2);//0.2sごとに送信
+    while(1) {
+        while(Serial1.readable()>0){
+            a[i]=Serial1.getc();
+            if(a[i]=='\r'){
+                a[i]=Serial1.getc();//\n
+                a[i]='\0';
+                i=0;
+                data.air_speed=atof(a);
+            }else{
+                i++;
+            }   
+        }
+        
+        while(Serial2.readable()>0){
+            char c = Serial2.getc();
+            if(gps.encode(c)){
+                gps.get_position(&(data.lat), &(data.lon), &(data.fix_age));// retrieves +/- lat/long in 100000ths of a degree
+                gps.get_datetime(&(data.date), &(data.time), &(data.fix_age));// time in hhmmsscc
+                data.course = gps.course();// course in 100ths of a degree
+                data.fmps = gps.f_speed_mps(); // speed in m/sec
+                data.falt = gps.f_altitude(); // +/- altitude in meters
+            }
+        }
+        if(send_f1==1){
+            send_f1=0;
+            send(); 
+        }
+        if(send_f2==1){
+            read_gyro();
+            read_analog();
+            send_f2=0;
+            send2();
+            int a;
+            a=mid;
+            if(a==prev_m && !a && mid_t==0){
+                mid1();
+                Serial1.printf("##%f,%f\r\n",mid_a,mid_b);
+                Serial2.printf("##%f,%f\r\n",mid_a,mid_b);
+            }
+            prev_m=a;            
+        }
+    }
+}
\ No newline at end of file