temp
Dependencies: mbed SDFileSystem MS5607 ADXL345_I2C FATFileSystem
main.cpp
- Committer:
- IKobayashi
- Date:
- 2020-03-16
- Revision:
- 0:c88c3b616c00
File content as of revision 0:c88c3b616c00:
#include<mbed.h> #include"MS5607I2C.h" #include"ADXL345_I2C.h" #include "SDFileSystem.h" #include "HMC5883L.h" #include<stdio.h> //#include<math.h> #define SDA p9 #define SCL p10 #define Rs p21 //right servo motor #define Ls p22 //left servo motor #define PI 3.14159265 #define N_SAMPLE 256 #define WAIT_TIME 50 #define N_DIM 4 #define EPS 1e-8 #define goallat 4008.544783 #define goallon 13959.2389 SDFileSystem sd(p5, p6, p7, p8, "sd"); Serial pc(USBTX, USBRX); Serial xbee(p28,p27); Serial gps(p13, p14); MS5607I2C ms5607(SDA, SCL, true); ADXL345_I2C accelerometer(SDA, SCL); HMC5883L hmc5883l(SDA, SCL); Timer timer; Ticker decision; DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); InterruptIn StartPort(p24); PwmOut servoLeft(Ls); PwmOut servoRight(Rs); FILE *dt; FILE *lo; bool flight_pin = false; bool gp = false; int i,rlock,mode,num; char gps_data[256]; char ns,ew; float w_time,hokui,tokei; float g_hokui,g_tokei; // float d_hokui,m_hokui,d_tokei,m_tokei; unsigned char c; double dev = 0; double gpsdata[4]; //gps data void ini(); void FP(); void getGPS(); void getdata(double *aldata, double acdata[], double mgdata[]); void servo(float x); void correction(double ac[], double mg[], double delta[]); void dev_calc(double exam[], double *s, double *t); void calc_dir_lati_longi(double newl[], double *result); int main(){ pc.baud(9600); //sekect baudrate gps.baud(9600); xbee.baud(9600); double aldata; double acdata[3]; double mgdata[3]; double delta[2]; double result; double chigh = 2000; double dhigh = 0; int k; mkdir("/sd/log", 0777); mkdir("/sd/sensordata", 0777); lo = fopen("/sd/log/log.txt", "a"); if(lo == NULL) { error("Could not open file for write\n"); } dt = fopen("/sd/sensordata/data.csv", "a"); if(dt == NULL) { error("Could not open file for write\n"); } fprintf(dt,"UTC,lon,lat,satnum,altitude,acce_x,acce_y,acce_z,mag_x,mag_y,mag_z\n"); fclose(dt); pc.printf("Cansat activate.....\r\n"); xbee.printf("Cansat activate.....\r\n"); fprintf(lo,"Cansat activate.....\r\n"); pc.printf("make directory and start initialize\r\n"); xbee.printf("make directory and start initialize\r\n"); fprintf(lo,"make directory and start initialize\r\n"); ini(); //initialize fclose(lo); StartPort.mode(PullUp); StartPort.fall(&FP); servo(0); while(flight_pin != true){ led1 =! led1; wait(0.1); } led1 = 0; while(true){ while(true){ gps.attach(getGPS,Serial::RxIrq); if(gp == true)break; } gp=false; getdata(&aldata,acdata,mgdata); dhigh = chigh - aldata; chigh = aldata; dt = fopen("/sd/sensordata/data.csv", "a"); if(dt == NULL) { error("Could not open file for write\n"); } fprintf(dt,"%lf,%lf,%lf,%2.0lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",gpsdata[0],gpsdata[1],gpsdata[2],gpsdata[3],aldata,acdata[0],acdata[1],acdata[2],mgdata[0],mgdata[1],mgdata[2]); fclose(dt); correction(acdata,mgdata,delta); calc_dir_lati_longi(gpsdata,&result); dev_calc(delta,&result,&dev); lo = fopen("/sd/log/log.txt", "a"); if(lo == NULL) { error("Could not open file for write\n"); } if (-180 < dev && dev <= -150) { servo(0); fprintf(lo,"Left:180\r\n"); pc.printf("Left:180\r\n"); } else if (-150 < dev && dev <= -120) { servo(30); fprintf(lo,"Left:150\r\n"); pc.printf("Left:150\r\n"); } else if (-120 < dev && dev <= -90) { servo(60); fprintf(lo,"Left:120\r\n"); pc.printf("Left:120\r\n"); } else if (-90 < dev && dev <= -60) { servo(90); fprintf(lo,"Left:90\r\n"); pc.printf("Left:90\r\n"); } else if (-60 < dev && dev <= -30) { servo(120); fprintf(lo,"Left:60\r\n"); pc.printf("Left:60\r\n"); } else if(-30 < dev && dev < 0) { servo(150); fprintf(lo,"Left:30\r\n"); pc.printf("Left:30\r\n"); } else if (dev == 0) { servo(0); fprintf(lo,"Straight\r\n"); pc.printf("Straight\r\n"); } else if (0 < dev && dev <= 30) { servo(30); fprintf(lo,"Right:30\r\n"); pc.printf("Right:30\r\n"); } else if (30 < dev && dev <= 60) { servo(60); fprintf(lo,"Right:60\r\n"); pc.printf("Right:60\r\n"); } else if (60 < dev && dev <= 90) { servo(90); fprintf(lo,"Right:90\r\n"); pc.printf("Right:90\r\n"); } else if (90 < dev && dev <= 120) { servo(120); fprintf(lo,"Right:120\r\n"); pc.printf("Right:120\r\n"); } else if (120 < dev && dev <= 150) { servo(150); fprintf(lo,"Right:150\r\n"); pc.printf("Right:150\r\n"); } else if (150 < dev && dev <= 180) { servo(180); fprintf(lo,"Right:180\r\n"); pc.printf("Right:180\r\n"); } fclose(lo); /* if (-0.1 < dhigh < 0.1) { k++; if (k >= 20) { break; } } else { k = 0; } */ } pc.printf("Cansat touchdown.....\r\n"); xbee.printf("Cansat touchdown.....\r\n"); fprintf(lo,"Cansat touchdown.....\r\n"); fclose(lo); while(true){ led4=!led4; } return 0; } void ini(){ //accelermeter initialization accelerometer.setPowerControl(0x00); //Go into standby mode to configure the device. accelerometer.setDataFormatControl(0x0B); //Full resolution, +/-16g, 4mg/LSB. accelerometer.setDataRate(ADXL345_3200HZ); //3.2kHz data rate. accelerometer.setPowerControl(0x08); //Measurement mode. pc.printf("All sensor is initialized\r\n"); xbee.printf("All sensor is initialized\r\n"); fprintf(lo,"All sensor is initialized\r\n"); } void FP(){ flight_pin = true; } void getGPS() { c = gps.getc(); if( c=='$' || i == 256){ mode = 0; i = 0; for(int j=0; j<256; j++){ gps_data[j]=NULL; } } if(mode==0){ if((gps_data[i]=c) != '\r'){ i++; }else{ gps_data[i]='\0'; if( sscanf(gps_data, "$GPGGA,%f,%f,%c,%f,%c,%d,%d",&w_time,&hokui,&ns,&tokei,&ew,&rlock,&num) >= 1){ led1=!led1; if(rlock==1){ //pc.printf("Status:Lock(%d)\n\r",rlock); //logitude // d_tokei= int(tokei/100); // m_tokei= (tokei-d_tokei*100)/60; // g_tokei= d_tokei+m_tokei; //Latitude // d_hokui=int(hokui/100); // m_hokui=(hokui-d_hokui*100)/60; // g_hokui=d_hokui+m_hokui; pc.printf("Lon:%.6f, Lat:%.6f, num:%d\n\r",tokei, hokui, num); xbee.printf("Lon:%.6f, Lat:%.6f, num:%d\n\r",tokei, hokui, num); gpsdata[0] = (double)w_time; gpsdata[2] = (double)tokei; gpsdata[1] = (double)hokui; gpsdata[3] = (double)num; gp = true; } else{ // pc.printf("\n\rStatus:unLock(%d)\n\r",rlock); pc.printf("%s\r\n",gps_data); //xbee.printf("\n\rStatus:unLock(%d)\n\r",rlock); xbee.printf("%s\r\n",gps_data); // gp = true; } sprintf(gps_data, ""); }//if } } } void getdata(double *aldata, double acdata[], double mgdata[]){ led2=!led2; int readings[3]; *aldata = ms5607.getAltitude(); accelerometer.getOutput(readings); acdata[0] = (int16_t)readings[1]*0.039; acdata[1] = (int16_t)readings[0]*0.039*(-1); acdata[2] = (int16_t)readings[2]*0.039; mgdata[0] = (double)hmc5883l.getMy(); mgdata[1] = (double)hmc5883l.getMx()*(-1); mgdata[2] = (double)hmc5883l.getMz(); } void correction(double ac[], double mg[], double delta[2]) { double Hx, Hy, r, p; if ((-1 < ac[0])&&(ac[0] < 1) && (-1 < (ac[1] / sqrt(1 - ac[0] * ac[0])))&&((ac[1] / sqrt(1 - ac[0] * ac[0])) < 1)) { led3=!led3; r = asin(ac[0]); p = asin(ac[1] / sqrt(1 - ac[0] * ac[0])); Hx = cos(r) * mg[0] + sin(p) * sin(r) * mg[1] - cos(p) * sin(r) * mg[2]; Hy = cos(p) * mg[1] + sin(p) * mg[2]; delta[0]=Hx; delta[1]=Hy; /* *delta = atan2(Hy, Hx); if (*delta < 0) *delta += 2 * PI; if (*delta > 2 * PI) *delta -= 2 * PI; *delta = *delta * 180 / PI; */ } } void servo(float x) { float y; y = 0.000284*(x / 30) + 0.0007; //left if (dev < 0) { servoLeft.pulsewidth(y); } else if(dev == 0){ servoLeft.pulsewidth(y+0.000284*(180 / 30)); servoRight.pulsewidth(y); } else { servoRight.pulsewidth(y); } } void dev_calc(double exam[], double *s, double *t) { double nowdir; nowdir = atan(exam[1] / exam[0]); if(nowdir < 0)nowdir += 2*PI; if(nowdir > 2*PI)nowdir -= 2*PI; nowdir = nowdir * 180 / PI; if (*s > nowdir) { if ((180 - (*s - nowdir)) < 0) { *t = *s - nowdir - 360; //dir_lati_longi:*s dev;*t } else { *t = *s - nowdir; } } else { if ((180 - (nowdir - *s)) < 0) { *t = 360 - nowdir + *s; } else { *t = *s - nowdir; } } pc.printf("%lf\r\n",*t); } void calc_dir_lati_longi(double newl[], double *result) { double diflongi, diflati; //difference diflati = (goallat) - newl[1]; //difference diflongi = (goallon) - newl[2]; if(diflongi == 0 && diflati >= 0)*result = 0; else if(diflongi == 0 && diflati < 0)*result = 180; else if(diflongi > 0 && diflati == 0)*result = 90; else if(diflongi < 0 && diflati == 0)*result = 270; else if (diflongi >= 0 && diflati >= 0) { //1 *result = atan(diflongi / diflati) * 180 / PI; } else if (diflongi >= 0 && diflati < 0) { //4 *result = atan(diflongi / diflati) * 180 / PI + 180; } else if (diflongi < 0 && diflati >= 0) { //2 *result = atan(diflongi / diflati) * 180 / PI + 360; } else if (diflongi < 0 && diflati < 0) { //3 *result = atan(diflongi / diflati) * 180 / PI + 180; } }