File content as of revision 2:e023e162e559:

#include "mbed.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>


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
    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
    Heading = hmc5883l.getHeading();
    //float magData[3],mx,my,mz;
    hmc5883l.readMagData(magData);
    mx = magData[0];
    my = magData[1];
    mz = magData[2];
     //wait(1);       
        }
    }



void thread3(void const *args)
{
    alt_sensor.init();
    wait(2);
    while(true) {         // thread loop
       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);
       
        }
        
    }
    
/*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();
            

        }
    }
}
void toggle_led1() {ledToggle(1);}
void toggle_led2() {ledToggle(2);}