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;
    }
}