Diff: main.cpp

Revision:

1:88ebad269e60

Parent:

0:187a5ea5239b

Child:

2:ba90f3e05798

--- a/main.cpp	Tue Dec 01 14:08:15 2020 +0000
+++ b/main.cpp	Tue Dec 01 14:41:24 2020 +0000
@@ -50,7 +50,7 @@
 int main()
 {
 //-----Initialization realted code-------//
-   //inital set the thruster esc to 1ms duty cycle
+    //inital set the thruster esc to 1ms duty cycle
     thruster.period(0.002);      // 2 ms period
     thruster.pulsewidth(1.0/1000.000);    /////IMU initial and begin
     thruster2.period(0.002);      // 2 ms period
@@ -73,9 +73,8 @@
     imu_ticker.attach(&IMU_update,0.1);  //10Hz
     log_ticker.attach(&log_data,0.5);
     wait(1);
-    while(1)
-    {
-      // put your main control code here
+    while(1) {
+        // put your main control code here
     }
 
 }
@@ -84,19 +83,16 @@
 ///-----------Welcome menu---------------------///
 void welcome()
 {
-    char buffer[100]={0};
+    char buffer[100]= {0};
     int flag=1;
     //Flush the port
-    while(BLE.readable())
-    {
-    BLE.getc();
+    while(BLE.readable()) {
+        BLE.getc();
     }
-    while(flag)
-    {
+    while(flag) {
         BLE.printf("### I am alive\r\n");
         BLE.printf("### Please enter the log file name you want\r\n");
-        if(BLE.readable())
-        {
+        if(BLE.readable()) {
             BLE.scanf("%s",buffer);
             sprintf(fname,"/sd/mydir/%s.txt",buffer); //make file name
 
@@ -111,11 +107,9 @@
     //open file test
     mkdir("/sd/mydir",0777); //keep 0777, this is magic #
     fp = fopen(fname, "a");
-    if(fp == NULL){
+    if(fp == NULL) {
         BLE.printf("Could not open file for write\n");
-    }
-    else
-    {
+    } else {
         BLE.printf("##file open good \n"); //open file and tell if open
         fprintf(fp, "Hello\r\n");
         fclose(fp);
@@ -128,12 +122,22 @@
 ///-----------log functions---------------------///
 void log_data()
 {
-    //log system time t.read()
-    // log imu data, log sciene data
-    // log pulse width
-    // log pressure sensor data.
-    //science sensor: temp.temp(), light.light()
-    //IMU sensor
+    IMU_update();
+    fp = fopen(fname, "a");
+
+    if(fp == NULL) { // If the file didn't open we'd want to know.
+        error("Could not open file for write\n");
+
+    }
+    fprintf(fp, "$NEMA,1,3,%.3f,%.3f,%.3f;", t.read(), //log system time t.read()
+            p_sensor.depth(), p_sensor.MS5837_Temperature(), // log pressure sensor data.
+            pw, // log pulse width
+            IMU.calcAccel(IMU.ax), IMU.calcAccel(IMU.ay), IMU.calcAccel(IMU.az), // log imu data, log sciene data
+            IMU.calcGyro(IMU.gx), IMU.calcGyro(IMU.gy), IMU.calcGyro(IMU.gz),
+            euler[0],euler[1],euler[2], //IMU sensor
+            temp.temp(), light.light()); //science sensor: temp.temp(), light.light()
+            
+    fclose();
 
 }
 
@@ -158,8 +162,9 @@
 {
     float bias[3] = {0.0793,0.0357,0.2333};
     float scale[3][3] = {{1.0070, 0.0705, 0.0368},
-                         {0.0705, 1.0807, 0.0265},
-                         {0.0368, 0.0265, 0.9250}};
+        {0.0705, 1.0807, 0.0265},
+        {0.0368, 0.0265, 0.9250}
+    };
     //mag_c = (mag-bias)*scale
 
     mag_c[0] = (mx - bias[0]) *scale[0][0] + (my - bias[1]) *scale[1][0] + (mz - bias[2]) *scale[2][0];
@@ -169,28 +174,25 @@
 
 void pose_estimate(float euler[3], float accel[3], float gyro[3], float mag[3])  //pose estimation function
 {
-        euler[0] =  atan2 (accel[1] , accel[2]/abs(accel[2])*(sqrt ((accel[0] * accel[0]) + (accel[2] * accel[2]))));
-        euler[1] = - atan2( -accel[0] ,( sqrt((accel[1] * accel[1]) + (accel[2] * accel[2]))));
-        float Yh = (mag[1] * cos(euler[0])) - (mag[2] * sin(euler[0]));
-        float Xh = (mag[0] * cos(euler[1]))+(mag[1] * sin(euler[0])*sin(euler[1]))
-                    + (mag[2] * cos(euler[0]) * sin(euler[1]));
-        euler[2] = atan2(Yh, Xh);
-        //convert into degrees
-        euler[0] *= 180.0f / PI;
-        euler[1] *= 180.0f / PI;
-        euler[2] *= 180.0f / PI;
-        //wrap the values to be within 0 to 360.
-        for (int i=0;i<3;i++)
-        {
-            if(euler[i]<=0)
-            {
-                euler[i]=euler[i]+360;
-            }
-            if(euler[i]>360)
-            {
-                euler[i]=euler[i]-360;
-            }
+    euler[0] =  atan2 (accel[1], accel[2]/abs(accel[2])*(sqrt ((accel[0] * accel[0]) + (accel[2] * accel[2]))));
+    euler[1] = - atan2( -accel[0],( sqrt((accel[1] * accel[1]) + (accel[2] * accel[2]))));
+    float Yh = (mag[1] * cos(euler[0])) - (mag[2] * sin(euler[0]));
+    float Xh = (mag[0] * cos(euler[1]))+(mag[1] * sin(euler[0])*sin(euler[1]))
+               + (mag[2] * cos(euler[0]) * sin(euler[1]));
+    euler[2] = atan2(Yh, Xh);
+    //convert into degrees
+    euler[0] *= 180.0f / PI;
+    euler[1] *= 180.0f / PI;
+    euler[2] *= 180.0f / PI;
+    //wrap the values to be within 0 to 360.
+    for (int i=0; i<3; i++) {
+        if(euler[i]<=0) {
+            euler[i]=euler[i]+360;
         }
+        if(euler[i]>360) {
+            euler[i]=euler[i]-360;
+        }
+    }
 
 }
 
@@ -201,8 +203,7 @@
 void thrust_on(float pw, float on_time)  //input is pulse width
 {
     float pw_max=2.0;
-    if(pw>pw_max)
-    {
+    if(pw>pw_max) {
         pw=pw_max; //hard limitation
     }
     Timer tt;
@@ -213,8 +214,7 @@
     thruster.pulsewidth(pw/1000.00);
     thruster2.pulsewidth(pw/1000.00);
     //PWM will be kept until time out
-    while(tt.read()<=on_time)
-    {
+    while(tt.read()<=on_time) {
     }
     //stop the timer
     tt.stop();