Diff: main.cpp

Revision:

1:42e428d934fe

Parent:

0:b487734d687d

--- a/main.cpp	Tue Oct 16 14:31:19 2018 +0000
+++ b/main.cpp	Thu Apr 22 03:49:52 2021 +0000
@@ -1,75 +1,217 @@
 #include "mbed.h"
-#include "uLCD_4DGL.h"
 #include "rtos.h"
 #include "stdio.h"
+#include "MPU6050.h"
+#include "HMC5883L.h"
+#include "BMP085.h"
+#include "ledControl.h"
+#include "mbed.h"
+#include "SpiFlash25.h"
+#include "spiffs.h"
+#include <string>
 
-uLCD_4DGL uLCD(p13,p14,p30);
-PwmOut RGBLED_r(p23);
-PwmOut RGBLED_g(p24);
-PwmOut RGBLED_b(p25);
-Mutex lcd_mutex;
+
+I2C i2c(D14,D15);         // setup i2c (SDA,SCL)  
+BMP085 alt_sensor(i2c);
+//Serial pc(USBTX,USBRX);   
+Serial pc(PB_10, PB_11);
+//Serial pc(PC_10, PC_11);
+MPU6050 mpu6050;           // class: MPU6050, object: mpu6050 
+HMC5883L hmc5883l;   
+Timer t;
+Thread *t1, *t2, *t3, *t4, *t5, *t6;
+Ticker toggler1;
+Ticker filter;           
+AnalogIn Battery(PA_0);
+
+void toggle_led1();
+void toggle_led2();
+void compFilter();
+
+char responatt;
+float pitchAngle = 0;
+float rollAngle = 0;
+double Heading;
+float pitchAcc, rollAcc, pitch, roll;
+float magData[3],mx,my,mz,alti;
+int temperatur,press;
+int waktu = 0;
+int packet= 0;
+bool start;
+float a, Vcc, R1=10000, R2=1500;
+
+// this value represents the number of files you can have open at the same time
+// adjust it according to your requirements
 
 
 void thread1(void const *args)
 {
+    
+    mpu6050.whoAmI();                           // Communication test: WHO_AM_I register reading 
+    wait(1);
+    mpu6050.calibrate(accelBias,gyroBias);      // Calibrate MPU6050 and load biases into bias registers
+    pc.printf("Calibration is completed. \r\n");
+    mpu6050.init();                             // Initialize the sensor
+    wait(1);
+    pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
+    mpu6050.getAres();
     while(true) {         // thread loop
-        RGBLED_r = 0.5 + (rand() % 11)/20.0;
-        RGBLED_g = 0.5 + (rand() % 11)/20.0;
-        RGBLED_b = 0.5 + (rand() % 11)/20.0;
-        Thread::wait(300);    // wait 1.5s
+    mpu6050.readAccelData(accelData);
+    mpu6050.getAres();
+    ax = accelData[0]*aRes - accelBias[0];
+    ay = accelData[1]*aRes - accelBias[1];
+    az = accelData[2]*aRes - accelBias[2]; 
+    
+    mpu6050.readGyroData(gyroData);
+    mpu6050.getGres();     
+    gx = gyroData[0]*gRes;  // - gyroBias[0];      // Results are better without extracting gyroBias[i]
+    gy = gyroData[1]*gRes;  // - gyroBias[1]; 
+    gz = gyroData[2]*gRes;  // - gyroBias[2]; 
+    
+    //float pitchAcc, rollAcc, pitch, roll;
+        /* Integrate the gyro data(deg/s) over time to get angle */
+    pitch += gx * dt;  // Angle around the X-axis
+    roll -=  gy * dt;  // Angle around the Y-axis
+    
+    /* Turning around the X-axis results in a vector on the Y-axis
+    whereas turning around the Y-axis results in a vector on the X-axis. */
+    pitchAcc = atan2f(accelData[1], accelData[2])*180/PI;
+    rollAcc  = atan2f(accelData[0], accelData[2])*180/PI;
+  
+    /* Apply Complementary Filter */
+    pitch = pitch * 0.98 + pitchAcc * 0.02;
+    roll  = roll  * 0.98 + rollAcc  * 0.02;  
+    
+    /*if(pitchAcc > 0){
+        pc.printf("pitch up, ");}
+        else { pc.printf("pitch down, ");}
+    if(rollAcc > 0){
+        pc.printf("roll kanan, ");}
+        else {pc.printf("roll kiri, ");}*/
+    //pc.printf("roll=%.3f pitch=%.3f \r\n",rollAcc,pitchAcc);
+
+    //pc.printf("| mag Data | mx=%.3f | my=%.3f | mz=%.3f                \r\n",mx,my,mz);
+        
+    /*pc.printf(" _______________\r\n");
+    pc.printf("| Heading: %.1f   \r\n", Heading);
+    pc.printf("|_______________\r\n\r\n");
+        
+    pc.printf("Temperature: %d\r\n", alt_sensor.get_temperature());
+    pc.printf("Pressure: %d\r\n", alt_sensor.get_pressure());
+    pc.printf("Altitude: %f\r\n", alt_sensor.get_altitude_m()); */
+    //pc.printf("%.1f , %.1f, %.1f, %.3f, %.3f, %.3f, %d, %d, %f \n",rollAcc,pitchAcc,Heading,mx,my,mz,alt_sensor.get_temperature()-2,alt_sensor.get_pressure(),alt_sensor.get_altitude_m());
+     //wait(1);
     }
 }
 
 void thread2(void const *args)
 {
+    hmc5883l.init();
+    wait(2);
     while(true) {         // thread loop
-        lcd_mutex.lock();
-        // basic printf demo = 16 by 18 characters on screen
-        uLCD.printf("\nHello uLCD World\n"); //Default Green on black text
-        uLCD.printf("\n  Starting Demo...");
-        uLCD.text_width(4); //4X size text
-        uLCD.text_height(4);
-        uLCD.color(RED);
-        for (int i=10; i>=6; --i) {
-            uLCD.locate(1,2);
-            uLCD.printf("%2D",i);
-            wait(.5);
+    Heading = hmc5883l.getHeading();
+    //float magData[3],mx,my,mz;
+    hmc5883l.readMagData(magData);
+    mx = magData[0];
+    my = magData[1];
+    mz = magData[2];
+     //wait(1);       
         }
-        uLCD.cls();
-        lcd_mutex.unlock();
-        Thread::wait(1000);
     }
-}
+
 
 
 void thread3(void const *args)
 {
+    alt_sensor.init();
+    wait(2);
     while(true) {         // thread loop
-        lcd_mutex.lock();
-        // basic printf demo = 16 by 18 characters on screen
-        uLCD.printf("\nHello uLCD World\n"); //Default Green on black text
-        uLCD.printf("\n  Starting Demo...");
-        uLCD.text_width(4); //4X size text
-        uLCD.text_height(4);
-        uLCD.color(RED);
-        for (int i=5; i>=0; --i) {
-            uLCD.locate(1,2);
-            uLCD.printf("%2D",i);
-            wait(.5);
+       temperatur = alt_sensor.get_temperature()-4;
+       press = alt_sensor.get_pressure();
+       alti = alt_sensor.get_altitude_m();
+       //wait(1);
+        }
+        
+    }
+
+void thread4(void const *args)
+{
+    //t.start();
+    while(true) {         // thread loop
+       pc.printf("%.1f, %.1f, %.1f, %.1f, %.3f, %.3f, %.3f, %d, %d, %.1f  \n" ,Vcc,rollAcc,pitchAcc,Heading,mx,my,mz,temperatur,press,alti);
+       //pc.printf("6800,%d,%d,0,%d,%d,5 V,0,0,0,%.1f,%.1f,%.3f,%.3f,%.3f,%.3f,%.3f,%.3f\n",waktu++,packet++,alt_sensor.get_pressure(),temperatur,alt_sensor.get_altitude_m(),Heading,mx,my,mz,gx,gy,gz);
+       //wait(1);
+        }
+        
+    }
+    
+void thread5(void const *args)
+{
+    //t.start();
+    while(true) {         // thread loop
+
+    
+    a=Battery.read();
+    
+    Vcc = (a*3.3*(R1+R2)/R2)-1.75;
+    
+    //wait(1);
+       
         }
-        uLCD.cls();
-        lcd_mutex.unlock();
-        Thread::wait(1000);
+        
+    }
+    
+/*id thread6(void const *args)
+{
+    //t.start();
+    while(true) {         // thread loop
+    if(pitchAcc > 0 && rollAcc > 0){
+       responatt = 'pitch naik, roll kanan';
+       }
+    else if( pitchAcc < 0 && rollAcc < 0){
+       responatt = 'pitch turun, roll kiri';
+       }
+    else if( pitchAcc < 0 && rollAcc > 0){
+       responatt = 'pitch turun, roll kanan';
+       } 
+    else if( pitchAcc > 0 && rollAcc < 0){
+       responatt = 'pitch naik, roll kiri';
+       }     
+    else if( pitchAcc == 0 && rollAcc == 0){
+       responatt = 'steady';
+       }      
+        }
+        
+    }*/
+
+int main() {
+    
+    pc.baud(57600);     // baud rate: 9600
+     t1 = new Thread(thread1);
+     t2 = new Thread(thread2);
+     t3 = new Thread(thread3);
+     t5 = new Thread(thread5);
+     //t6 = new Thread(thread6);
+     wait(2);
+
+     t4 = new Thread(thread4);
+     t.start();
+     while(1){
+     if(t.read()>20)
+       {
+            t.stop();
+            t.reset();
+            pc.printf("OBDH SCHEDULE RESTART (Watchdog Timer)\n");
+            t4->terminate();
+            delete t4;
+            pc.printf("restarted\n");
+            wait(5);
+            t4=new Thread(thread4);
+            t.start();
+            
+
+        }
     }
 }
-
-int main() {
-    Thread t1(thread1);
-    Thread t2(thread2);
-    Thread t3(thread3);
-    while(1) {
-        lcd_mutex.lock();
-        lcd_mutex.unlock();
-        Thread::wait(500);
-    }
-}
+void toggle_led1() {ledToggle(1);}
+void toggle_led2() {ledToggle(2);}
\ No newline at end of file