Diff: main.cpp

Revision:

4:008160161171

Parent:

2:137282f468d0

--- a/main.cpp	Sat Jul 27 10:01:33 2013 +0000
+++ b/main.cpp	Fri Aug 16 08:23:45 2013 +0000
@@ -26,6 +26,7 @@
     Ticker logging;
 //for communication with mission_mbed
     myCAN can(p30, p29);
+    Ticker can_msg;
 
 //for pc_msg
     #define PC_BAUD 9600
@@ -36,6 +37,10 @@
     #define LOG_RATE 0.3//0.04
     char log_flg;
     void log_flg_on(void);
+//for can_msg
+    #define CAN_RATE 0.1
+    void can_flg_on(void);
+    char can_flg = 0;
 
 //Offsets for the gyroscope and the accelerometer.
     double w_xBias;
@@ -214,6 +219,10 @@
     pc_flg = 1;
 }
 
+void can_flg_on(void){
+    can_flg = 1;
+}
+
 void log_flg_on(void){
     log_flg = 1;
 }
@@ -241,19 +250,31 @@
     //for PC
         pc.baud(PC_BAUD);
         pc_msg.attach(&pc_flg_on,PC_RATE);
+        
+    //for CAN
+        can_msg.attach(&can_flg_on,CAN_RATE);
     
     //for logging
         logging.attach(&log_flg_on,LOG_RATE);
     
-    int a_zi;
-    a_zi=0;
+    int roll,pitch;
+    float g;
+    roll = 0;
+    pitch = 0;
     
     while(1){
         //for can send
-            if(a_z > 9.6){a_zi=1;}
-            else{a_zi=0;}         
-            can.make_logger_senddata(gps.get_time(),gps.get_satelite_number(),gps.get_str_latitude(),gps.get_str_longitude(),a_zi,(int)(scp1000.readTemperature()*20),scp1000.readPressure());
-            can.send(LOGGER);
+            if(can_flg == 1){
+                g = sqrt(a_x*a_x + a_y*a_y + a_z*a_z);
+                roll = (int)(acos(a_y/g)*180/3.1415926535 - 90.0); 
+                pitch = (int)(acos(a_x/g)*180/3.1415926535 - 90.0); 
+                if(roll<0){roll=roll+360.0;}
+                if(a_z<0){roll=-1*roll+180.0;} 
+                if(roll<0){roll=roll+360.0;}
+                can.make_logger_senddata(gps.get_time(),gps.get_satelite_number(),gps.get_str_latitude(),gps.get_str_longitude(),roll,pitch,(int)(scp1000.readTemperature()*20),scp1000.readPressure());
+                can.send(LOGGER);
+                can_flg = 0;
+            }
         //for logging
             if(log_flg == 1){
                 if(SD == 1){
@@ -263,10 +284,13 @@
                         //error("Could not open file for write\n");
                         pc.printf("SD_error");
                     }else{
-                        fprintf(fp,"time:%s,latitude:%f,longitude:%f,status:%c,satelite:%c,speed:%f,",gps.get_time(),gps.get_latitude(),gps.get_longitude(),gps.get_status(),gps.get_satelite_number(),gps.get_speed());
-                        //fprintf(fp,"roll:%f,pitch:%f,",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()));
-                        fprintf(fp,"pressure:%d,temperature:%f\r\n", scp1000.readPressure(), scp1000.readTemperature());
-                        fprintf(fp,"%s,%f,%f,%f,%f,%f,%f\r\n",gps.get_time(),w_y, w_x, w_z, a_y, a_x, a_z);
+                        fprintf(fp,"%s,%f,%f,",gps.get_time(),gps.get_latitude(),gps.get_longitude());
+                        fprintf(fp,"%c,%d,%f,",gps.get_status(),gps.get_satelite_number(),gps.get_speed());
+                        fprintf(fp,"%d,%f,", scp1000.readPressure(), scp1000.readTemperature());
+                        fprintf(fp,"%f,%f,%f,%f,%f,%f,",w_y, w_x, w_z, a_y, a_x, a_z);
+                        fprintf(fp,"%d,%d,",roll,pitch);
+                        fprintf(fp,"%d",can.get_mission_status());
+                        fprintf(fp,"\r\n",can.get_mission_status());
                         fclose(fp);
                     }
                 }
@@ -274,13 +298,12 @@
         
         //for pc debug
             if(pc_flg == 1){
-                pc.printf("%f,%f,%f,%f,%f,%f\r\n",w_y, w_x, w_z, a_y, a_x, a_z);
-                pc.printf("%d\r\n",a_zi);
+                //pc.printf("%f,%f,%f,%f,%f,%f\r\n",w_y, w_x, w_z, a_y, a_x, a_z);
+                pc.printf("%d,%d\r\n",roll,pitch);
                 //pc.printf("%f,%f,%f,%f,%f,%f\r\n",a_xBias, a_yBias, a_zBias, w_xBias, w_yBias, w_zBias);
                 //pc.printf("pressure:%d,temperature:%f\r\n", scp1000.readPressure(), scp1000.readTemperature());
-                pc.printf("time:%s,latitude:%f,longitude:%f,status:%c,satelite:%c,speed:%f\r\n",gps.get_time(),gps.get_latitude(),gps.get_longitude(),gps.get_status(),gps.get_satelite_number(),gps.get_speed());
-                //pc.printf("%f,%f,%d,%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),scp1000.readPressure(), scp1000.readTemperature());
-                pc.printf("\n\r");
+                //pc.printf("time:%s,latitude:%f,longitude:%f,status:%c,satelite:%d,speed:%f\r\n",gps.get_time(),gps.get_latitude(),gps.get_longitude(),gps.get_status(),gps.get_satelite_number(),gps.get_speed());
+                //pc.printf("\n\r");
                 pc_flg =0;
             }
     }