Diff: main.cpp

Revision:

0:583ea5b1e1e7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Dec 23 07:31:14 2019 +0000
@@ -0,0 +1,211 @@
+#include "mbed.h"
+#include "AsyncSerial.hpp"
+#include "TinyGPS.h"
+#include "JY901.h"
+#include "PowerControl.h"
+#include "EthernetPowerControl.h"
+      
+
+Serial pc(USBTX, USBRX);//CoolTermを用いてパソコン上に表示
+JY901 i2c(p28,p27);// sda, scl // gyro
+AsyncSerial Serial1(p9,p10,38400,128); // tx, rx // log(out),speed(in)
+AsyncSerial Serial2(p13,p14,9600,256); // tx, rx //display&twe-lite(out),gps(in)
+//DigitalOut myled(LED1);
+InterruptIn alt(p21);// hight
+InterruptIn rot(p22);// rot
+DigitalIn mid(p23);// bottun
+AnalogIn an1(p19);// dakaku,ELE
+AnalogIn an2(p20);// dakaku,RUD
+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, course;
+    unsigned long fix_age;
+    long lat, lon;
+    unsigned long time, date, speed;
+    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]=i2c.getXaxisAngle();
+    data.angle[1]=i2c.getYaxisAngle();
+    data.angle[2]=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=(t.read_us()/58.0*19.681);
+    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("%5.2f,%3d,%5.2f,%7f,%7f\r\n",(float)data.height/1000,data.rot_count,data.air_speed,data.analog[0],data.analog[1]);
+    //Serial2.printf("0.1,10,0.1,%7f,%7f",(float)data.height/1000,data.rot_count,data.air_speed,data.analog[0],data.analog[1]);
+  
+    /*pc.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]); */
+    
+     
+    /*
+    pc.printf("time:%5ds, hgt:%5.2fm, rot:%3drpm, spd:%5.2fm/s, ELE:%7f, RUD:%7f, X:%6.2f, Y:%6.2f, Z:%6.2f, gt:%6d, lat:%5.2f, lon:%5.2f, crs:%d, gspd:%5.2f, alt:%5.2f\r\n"
+    ,t2.read_ms()/1000,(float)data.height/1000,data.rot_count,data.air_speed
+    ,data.analog[0],data.analog[1],data.angle[0],data.angle[1],data.angle[2],data.time/100+90000
+    ,(float)data.lat/1000000,(float)data.lon/1000000,data.course,data.fmps,data.falt);
+    */
+    pc.printf("%5.2f,%3d,%5.2f,%7f,%7f\r\n",(float)data.height/1000,data.rot_count,data.air_speed,data.analog[0],data.analog[1]);
+    
+    
+    /*pc.printf("time=%5ds, hight=%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.hight/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;
+    
+    PHY_PowerDown();
+    
+    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
+                gps.f_course();// course in 100ths of a degree
+                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