Diff: main.cpp

Revision:

0:f93d0be13473

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Feb 08 09:54:38 2017 +0000
@@ -0,0 +1,88 @@
+#include "mbed.h"
+#include "MPU6050.h"
+#include "MS5607I2C.h"
+
+DigitalOut led_st(p19);
+DigitalOut led_act(p20);
+DigitalIn sw(p21);
+MPU6050 mpu(p9,p10);
+MS5607I2C ms5607(p9, p10, false);
+Serial gps(p28,p27);
+Serial pc(USBTX, USBRX);
+
+char gps_data[256];
+int i,rlock,mode;
+float a[3];
+float g[3];
+float pressure,temperature,altitude;
+
+void getGPS();
+void standby();
+void action();
+
+int main() {
+   mpu.setAcceleroRange(0);
+   mpu.setGyroRange(0);
+   gps.baud(9600);
+   gps.attach(getGPS,Serial::RxIrq);
+   while(1){
+      if(sw)action();
+      else standby();
+   }
+}
+
+void standby(){
+   led_act = 0;
+   led_st = 1;
+   pc.printf("mode(standby):%6.2f\t%6.2f\t%6.2f\r\n",ms5607.getPressure(),ms5607.getTemperature(),ms5607.getAltitude());
+   wait(3);
+}
+void action(){
+   led_act = 1;
+   led_st = 0;
+   mpu.getAccelero(a);
+   mpu.getGyro(g);
+   pressure = ms5607.getPressure();
+   temperature = ms5607.getTemperature();
+   altitude = ms5607.getAltitude();
+   pc.printf("mode(action):%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%4.3f,%6.2f,%6.2f,%6.2f\r\n",a[0],a[1],a[2],g[0],g[1],g[2],pressure,temperature,altitude);
+}
+
+void getGPS() {
+   unsigned char c = gps.getc();
+   if( c=='$' || i == 256){
+      mode = 0;
+      i = 0;
+      sprintf(gps_data,"");
+   }
+   if(mode==0){
+      char ns,ew;
+      float w_time,hokui,tokei;
+      float g_hokui,g_tokei;
+      float d_hokui,m_hokui,d_tokei,m_tokei;
+      if((gps_data[i]=c) != '\r'){
+         i++;
+      }else{
+         gps_data[i]='\0';
+         if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock) >= 1){
+            if(rlock==1){
+               pc.printf("Status:Lock(%d)\n\r",rlock);
+               //logitude
+               d_tokei= int(tokei/100);
+               m_tokei= (tokei-d_tokei*100)/60;
+               g_tokei= d_tokei+m_tokei;
+               //Latitude
+               d_hokui=int(hokui/100);
+               m_hokui=(hokui-d_hokui*100)/60;
+               g_hokui=d_hokui+m_hokui;
+               pc.printf("Lon:%.6f, Lat:%.6f\n\r",g_tokei, g_hokui);
+            }
+            else{
+               pc.printf("\n\rStatus:unLock(%d)\n\r",rlock);
+               pc.printf("%s\r\n",gps_data);
+            }
+            sprintf(gps_data, "");
+         }//if
+      }
+   }
+}