Agus Cahya / Mbed 2 deprecated RTOS-OBDH

-

Dependencies:   mbed MPU6050 mbed-rtos SpiFlash25 HMC5883L BMP085 flash-fs ledControl2

main.cpp@2:e023e162e559, 2021-04-22 (annotated)

Committer:
aguscahya
Date:
Thu Apr 22 03:52:55 2021 +0000
Revision:
2:e023e162e559
Parent:
1:42e428d934fe 
-

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mbentata 0:b487734d687d 1 #include "mbed.h"
mbentata 0:b487734d687d 2 #include "rtos.h"
mbentata 0:b487734d687d 3 #include "stdio.h"
aguscahya 1:42e428d934fe 4 #include "MPU6050.h"
aguscahya 1:42e428d934fe 5 #include "HMC5883L.h"
aguscahya 1:42e428d934fe 6 #include "BMP085.h"
aguscahya 1:42e428d934fe 7 #include "ledControl.h"
aguscahya 1:42e428d934fe 8 #include "mbed.h"
aguscahya 1:42e428d934fe 9 #include "SpiFlash25.h"
aguscahya 1:42e428d934fe 10 #include "spiffs.h"
aguscahya 1:42e428d934fe 11 #include <string>
mbentata 0:b487734d687d 12
aguscahya 1:42e428d934fe 13
aguscahya 1:42e428d934fe 14 I2C i2c(D14,D15); // setup i2c (SDA,SCL)
aguscahya 1:42e428d934fe 15 BMP085 alt_sensor(i2c);
aguscahya 1:42e428d934fe 16 //Serial pc(USBTX,USBRX);
aguscahya 1:42e428d934fe 17 Serial pc(PB_10, PB_11);
aguscahya 1:42e428d934fe 18 //Serial pc(PC_10, PC_11);
aguscahya 1:42e428d934fe 19 MPU6050 mpu6050; // class: MPU6050, object: mpu6050
aguscahya 1:42e428d934fe 20 HMC5883L hmc5883l;
aguscahya 1:42e428d934fe 21 Timer t;
aguscahya 1:42e428d934fe 22 Thread *t1, *t2, *t3, *t4, *t5, *t6;
aguscahya 1:42e428d934fe 23 Ticker toggler1;
aguscahya 1:42e428d934fe 24 Ticker filter;
aguscahya 1:42e428d934fe 25 AnalogIn Battery(PA_0);
aguscahya 1:42e428d934fe 26
aguscahya 1:42e428d934fe 27 void toggle_led1();
aguscahya 1:42e428d934fe 28 void toggle_led2();
aguscahya 1:42e428d934fe 29 void compFilter();
aguscahya 1:42e428d934fe 30
aguscahya 1:42e428d934fe 31 char responatt;
aguscahya 1:42e428d934fe 32 float pitchAngle = 0;
aguscahya 1:42e428d934fe 33 float rollAngle = 0;
aguscahya 1:42e428d934fe 34 double Heading;
aguscahya 1:42e428d934fe 35 float pitchAcc, rollAcc, pitch, roll;
aguscahya 1:42e428d934fe 36 float magData[3],mx,my,mz,alti;
aguscahya 1:42e428d934fe 37 int temperatur,press;
aguscahya 1:42e428d934fe 38 int waktu = 0;
aguscahya 1:42e428d934fe 39 int packet= 0;
aguscahya 1:42e428d934fe 40 bool start;
aguscahya 1:42e428d934fe 41 float a, Vcc, R1=10000, R2=1500;
aguscahya 1:42e428d934fe 42
aguscahya 1:42e428d934fe 43 // this value represents the number of files you can have open at the same time
aguscahya 1:42e428d934fe 44 // adjust it according to your requirements
mbentata 0:b487734d687d 45
mbentata 0:b487734d687d 46
mbentata 0:b487734d687d 47 void thread1(void const *args)
mbentata 0:b487734d687d 48 {
aguscahya 1:42e428d934fe 49
aguscahya 1:42e428d934fe 50 mpu6050.whoAmI(); // Communication test: WHO_AM_I register reading
aguscahya 1:42e428d934fe 51 wait(1);
aguscahya 1:42e428d934fe 52 mpu6050.calibrate(accelBias,gyroBias); // Calibrate MPU6050 and load biases into bias registers
aguscahya 1:42e428d934fe 53 pc.printf("Calibration is completed. \r\n");
aguscahya 1:42e428d934fe 54 mpu6050.init(); // Initialize the sensor
aguscahya 1:42e428d934fe 55 wait(1);
aguscahya 1:42e428d934fe 56 pc.printf("MPU6050 is initialized for operation.. \r\n\r\n");
aguscahya 1:42e428d934fe 57 mpu6050.getAres();
mbentata 0:b487734d687d 58 while(true) { // thread loop
aguscahya 1:42e428d934fe 59 mpu6050.readAccelData(accelData);
aguscahya 1:42e428d934fe 60 mpu6050.getAres();
aguscahya 1:42e428d934fe 61 ax = accelData[0]*aRes - accelBias[0];
aguscahya 1:42e428d934fe 62 ay = accelData[1]*aRes - accelBias[1];
aguscahya 1:42e428d934fe 63 az = accelData[2]*aRes - accelBias[2];
aguscahya 1:42e428d934fe 64
aguscahya 1:42e428d934fe 65 mpu6050.readGyroData(gyroData);
aguscahya 1:42e428d934fe 66 mpu6050.getGres();
aguscahya 1:42e428d934fe 67 gx = gyroData[0]*gRes; // - gyroBias[0]; // Results are better without extracting gyroBias[i]
aguscahya 1:42e428d934fe 68 gy = gyroData[1]*gRes; // - gyroBias[1];
aguscahya 1:42e428d934fe 69 gz = gyroData[2]*gRes; // - gyroBias[2];
aguscahya 1:42e428d934fe 70
aguscahya 1:42e428d934fe 71 //float pitchAcc, rollAcc, pitch, roll;
aguscahya 1:42e428d934fe 72 /* Integrate the gyro data(deg/s) over time to get angle */
aguscahya 1:42e428d934fe 73 pitch += gx * dt; // Angle around the X-axis
aguscahya 1:42e428d934fe 74 roll -= gy * dt; // Angle around the Y-axis
aguscahya 1:42e428d934fe 75
aguscahya 1:42e428d934fe 76 /* Turning around the X-axis results in a vector on the Y-axis
aguscahya 1:42e428d934fe 77 whereas turning around the Y-axis results in a vector on the X-axis. */
aguscahya 1:42e428d934fe 78 pitchAcc = atan2f(accelData[1], accelData[2])*180/PI;
aguscahya 1:42e428d934fe 79 rollAcc = atan2f(accelData[0], accelData[2])*180/PI;
aguscahya 1:42e428d934fe 80
aguscahya 1:42e428d934fe 81 /* Apply Complementary Filter */
aguscahya 1:42e428d934fe 82 pitch = pitch * 0.98 + pitchAcc * 0.02;
aguscahya 1:42e428d934fe 83 roll = roll * 0.98 + rollAcc * 0.02;
aguscahya 1:42e428d934fe 84
aguscahya 1:42e428d934fe 85 /*if(pitchAcc > 0){
aguscahya 1:42e428d934fe 86 pc.printf("pitch up, ");}
aguscahya 1:42e428d934fe 87 else { pc.printf("pitch down, ");}
aguscahya 1:42e428d934fe 88 if(rollAcc > 0){
aguscahya 1:42e428d934fe 89 pc.printf("roll kanan, ");}
aguscahya 1:42e428d934fe 90 else {pc.printf("roll kiri, ");}*/
aguscahya 1:42e428d934fe 91 //pc.printf("roll=%.3f pitch=%.3f \r\n",rollAcc,pitchAcc);
aguscahya 1:42e428d934fe 92
aguscahya 1:42e428d934fe 93 //pc.printf("| mag Data | mx=%.3f | my=%.3f | mz=%.3f \r\n",mx,my,mz);
aguscahya 1:42e428d934fe 94
aguscahya 1:42e428d934fe 95 /*pc.printf(" _______________\r\n");
aguscahya 1:42e428d934fe 96 pc.printf("| Heading: %.1f \r\n", Heading);
aguscahya 1:42e428d934fe 97 pc.printf("|_______________\r\n\r\n");
aguscahya 1:42e428d934fe 98
aguscahya 1:42e428d934fe 99 pc.printf("Temperature: %d\r\n", alt_sensor.get_temperature());
aguscahya 1:42e428d934fe 100 pc.printf("Pressure: %d\r\n", alt_sensor.get_pressure());
aguscahya 1:42e428d934fe 101 pc.printf("Altitude: %f\r\n", alt_sensor.get_altitude_m()); */
aguscahya 1:42e428d934fe 102 //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());
aguscahya 1:42e428d934fe 103 //wait(1);
mbentata 0:b487734d687d 104 }
mbentata 0:b487734d687d 105 }
mbentata 0:b487734d687d 106
mbentata 0:b487734d687d 107 void thread2(void const *args)
mbentata 0:b487734d687d 108 {
aguscahya 1:42e428d934fe 109 hmc5883l.init();
aguscahya 1:42e428d934fe 110 wait(2);
mbentata 0:b487734d687d 111 while(true) { // thread loop
aguscahya 1:42e428d934fe 112 Heading = hmc5883l.getHeading();
aguscahya 1:42e428d934fe 113 //float magData[3],mx,my,mz;
aguscahya 1:42e428d934fe 114 hmc5883l.readMagData(magData);
aguscahya 1:42e428d934fe 115 mx = magData[0];
aguscahya 1:42e428d934fe 116 my = magData[1];
aguscahya 1:42e428d934fe 117 mz = magData[2];
aguscahya 1:42e428d934fe 118 //wait(1);
mbentata 0:b487734d687d 119 }
mbentata 0:b487734d687d 120 }
aguscahya 1:42e428d934fe 121
mbentata 0:b487734d687d 122
mbentata 0:b487734d687d 123
mbentata 0:b487734d687d 124 void thread3(void const *args)
mbentata 0:b487734d687d 125 {
aguscahya 1:42e428d934fe 126 alt_sensor.init();
aguscahya 1:42e428d934fe 127 wait(2);
mbentata 0:b487734d687d 128 while(true) { // thread loop
aguscahya 1:42e428d934fe 129 temperatur = alt_sensor.get_temperature()-4;
aguscahya 1:42e428d934fe 130 press = alt_sensor.get_pressure();
aguscahya 1:42e428d934fe 131 alti = alt_sensor.get_altitude_m();
aguscahya 1:42e428d934fe 132 //wait(1);
aguscahya 1:42e428d934fe 133 }
aguscahya 1:42e428d934fe 134
aguscahya 1:42e428d934fe 135 }
aguscahya 1:42e428d934fe 136
aguscahya 1:42e428d934fe 137 void thread4(void const *args)
aguscahya 1:42e428d934fe 138 {
aguscahya 1:42e428d934fe 139 //t.start();
aguscahya 1:42e428d934fe 140 while(true) { // thread loop
aguscahya 1:42e428d934fe 141 pc.printf("%.1f, %.1f, %.1f, %.1f, %.3f, %.3f, %.3f, %d, %d, %.1f \n" ,Vcc,rollAcc,pitchAcc,Heading,mx,my,mz,temperatur,press,alti);
aguscahya 1:42e428d934fe 142 //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);
aguscahya 1:42e428d934fe 143 //wait(1);
aguscahya 1:42e428d934fe 144 }
aguscahya 1:42e428d934fe 145
aguscahya 1:42e428d934fe 146 }
aguscahya 1:42e428d934fe 147
aguscahya 1:42e428d934fe 148 void thread5(void const *args)
aguscahya 1:42e428d934fe 149 {
aguscahya 1:42e428d934fe 150 //t.start();
aguscahya 1:42e428d934fe 151 while(true) { // thread loop
aguscahya 1:42e428d934fe 152
aguscahya 1:42e428d934fe 153
aguscahya 1:42e428d934fe 154 a=Battery.read();
aguscahya 1:42e428d934fe 155
aguscahya 1:42e428d934fe 156 Vcc = (a*3.3*(R1+R2)/R2)-1.75;
aguscahya 1:42e428d934fe 157
aguscahya 1:42e428d934fe 158 //wait(1);
aguscahya 1:42e428d934fe 159
mbentata 0:b487734d687d 160 }
aguscahya 1:42e428d934fe 161
aguscahya 1:42e428d934fe 162 }
aguscahya 1:42e428d934fe 163
aguscahya 1:42e428d934fe 164 /*id thread6(void const *args)
aguscahya 1:42e428d934fe 165 {
aguscahya 1:42e428d934fe 166 //t.start();
aguscahya 1:42e428d934fe 167 while(true) { // thread loop
aguscahya 1:42e428d934fe 168 if(pitchAcc > 0 && rollAcc > 0){
aguscahya 1:42e428d934fe 169 responatt = 'pitch naik, roll kanan';
aguscahya 1:42e428d934fe 170 }
aguscahya 1:42e428d934fe 171 else if( pitchAcc < 0 && rollAcc < 0){
aguscahya 1:42e428d934fe 172 responatt = 'pitch turun, roll kiri';
aguscahya 1:42e428d934fe 173 }
aguscahya 1:42e428d934fe 174 else if( pitchAcc < 0 && rollAcc > 0){
aguscahya 1:42e428d934fe 175 responatt = 'pitch turun, roll kanan';
aguscahya 1:42e428d934fe 176 }
aguscahya 1:42e428d934fe 177 else if( pitchAcc > 0 && rollAcc < 0){
aguscahya 1:42e428d934fe 178 responatt = 'pitch naik, roll kiri';
aguscahya 1:42e428d934fe 179 }
aguscahya 1:42e428d934fe 180 else if( pitchAcc == 0 && rollAcc == 0){
aguscahya 1:42e428d934fe 181 responatt = 'steady';
aguscahya 1:42e428d934fe 182 }
aguscahya 1:42e428d934fe 183 }
aguscahya 1:42e428d934fe 184
aguscahya 1:42e428d934fe 185 }*/
aguscahya 1:42e428d934fe 186
aguscahya 1:42e428d934fe 187 int main() {
aguscahya 1:42e428d934fe 188
aguscahya 1:42e428d934fe 189 pc.baud(57600); // baud rate: 9600
aguscahya 1:42e428d934fe 190 t1 = new Thread(thread1);
aguscahya 1:42e428d934fe 191 t2 = new Thread(thread2);
aguscahya 1:42e428d934fe 192 t3 = new Thread(thread3);
aguscahya 1:42e428d934fe 193 t5 = new Thread(thread5);
aguscahya 1:42e428d934fe 194 //t6 = new Thread(thread6);
aguscahya 1:42e428d934fe 195 wait(2);
aguscahya 1:42e428d934fe 196
aguscahya 1:42e428d934fe 197 t4 = new Thread(thread4);
aguscahya 1:42e428d934fe 198 t.start();
aguscahya 1:42e428d934fe 199 while(1){
aguscahya 1:42e428d934fe 200 if(t.read()>20)
aguscahya 1:42e428d934fe 201 {
aguscahya 1:42e428d934fe 202 t.stop();
aguscahya 1:42e428d934fe 203 t.reset();
aguscahya 1:42e428d934fe 204 pc.printf("OBDH SCHEDULE RESTART (Watchdog Timer)\n");
aguscahya 1:42e428d934fe 205 t4->terminate();
aguscahya 1:42e428d934fe 206 delete t4;
aguscahya 1:42e428d934fe 207 pc.printf("restarted\n");
aguscahya 1:42e428d934fe 208 wait(5);
aguscahya 1:42e428d934fe 209 t4=new Thread(thread4);
aguscahya 1:42e428d934fe 210 t.start();
aguscahya 1:42e428d934fe 211
aguscahya 1:42e428d934fe 212
aguscahya 1:42e428d934fe 213 }
mbentata 0:b487734d687d 214 }
mbentata 0:b487734d687d 215 }
aguscahya 1:42e428d934fe 216 void toggle_led1() {ledToggle(1);}
aguscahya 1:42e428d934fe 217 void toggle_led2() {ledToggle(2);}