Diff: main.cpp

Revision:

0:6ac6b2d2bf1a

Child:

1:cd11c1c592c7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jul 08 05:39:27 2016 +0000
@@ -0,0 +1,89 @@
+#include "mbed.h"
+#include "Barometer.h"
+#include "Ultrasonic.h"
+#include "math.h"
+Serial pc(USBTX, USBRX);
+Barometer barometer(p9, p10);
+Ultrasonic sonar(p24, p12); 
+Serial AHRS(p13, p14);
+Serial GPS(p28, p27);
+PwmOut Linear(p21);
+PwmOut Micro_gf(p22);
+PwmOut Micro_d(p23);
+
+float roll,pitch,yaw,accx,accy,accz;
+float p = 0.0f, t = 0.0f, alt = 0.0f;
+int i = 0, j=0, gps_ok=0, flag, lock
+int stat, chksum=0;
+volatile unsigned char GPS_buffer[2];
+char ns, ew;
+char msg[150];
+
+void GPS_isr(){
+    i++;
+    GPS_buffer[1] = GPS_buffer[0];
+    GPS_buffer[0] = GPS.getc();
+    if ((GPS_buffer[1]==13)&(GPS_buffer[0]==10))
+    {   
+        i=0;
+        if (flag == 1)
+        {
+            flag = 0;
+            gps_ok = 1;
+            j=0;
+        }
+    }
+    if ((i==5)&(GPS_buffer[0] == 'G')){flag=1;}
+    if (flag==1){msg[j]=GPS_buffer[0]; j++;}
+}
+
+float trunc(float v) {
+    if(v < 0.0) {
+        v*= -1.0;
+        v = floor(v);
+        v*=-1.0;
+    } else {v = floor(v);}
+    return v;
+}
+
+int main(void)
+{   AHRS.baud(9600);
+    GPS.baud(9600);
+    GPS.attach(&GPS_isr, Serial::RxIrq);
+    float longitude,latitude, time;
+    Linear.period(0.01); // set PWM period to 1ms
+    Micro_gf.period(0.01);
+    Micro_d.period(0.01);
+    //Linear=0.1;wait(1);
+    Linear=0.19;
+    Micro_d=0.2;
+    Micro_gf=0.2;
+    wait(1);
+    while(1) {
+        //stat should be added
+        barometer.update();
+        p = barometer.get_pressure();
+        t = barometer.get_temperature();
+        alt = barometer.get_altitude_m();
+        while(AHRS.getc() != '\n');
+        AHRS.scanf("*%f,%f,%f,%f,%f,%f \n", &roll, &pitch, &yaw, &accx,&accy,&accz);
+        if (gps_ok == 1)
+        {gps_ok = 0;
+         sscanf(msg, "GA,%f,%f,%c,%f,%c,%d", &time, &latitude, &ns, &longitude, &ew, &lock);
+            if(ns == 'S') {latitude  *= -1.0; }
+            if(ew == 'W') {longitude *= -1.0; }
+            float degrees = trunc(latitude / 100.0f);
+            float minutes = latitude - (degrees * 100.0f);
+            latitude = degrees + minutes / 60.0f;    
+            degrees = trunc(longitude / 100.0f);
+            minutes = longitude - (degrees * 100.0f);
+            longitude = degrees + minutes / 60.0f;
+            time = time + 90000;
+        }
+        unsigned int distance = sonar.distance();
+        if (distance<10){Linear =0.1;}
+        chksum=(int)stat+t+alt+roll+pitch+yaw+accz+time+latitude+longitude+distance;
+     pc.printf("\r\n Temperature(c):%f Altitude(m):%f Roll:%f Pitch:%f Yaw:%f Accz:%f Time:%f Latitude:%f Longitude:%f Sonar(cm)%u",stat,t,alt,roll,pitch,yaw,accz,time,latitude,longitude,distance,chksum);
+    }   
+}
+ 
\ No newline at end of file