File content as of revision 4:008160161171:

#include "mbed.h"
#include "SDFileSystem.h"
#include "IMUfilter.h"
#include "IMU_I2C.h"
#include "nmea0813.h"
#include "SCP1000.h"
#include "myCAN.h"
#include "IDDATA.h"

//for debug
    DigitalOut myled(LED1);
    Serial pc(USBTX, USBRX);
    Ticker pc_msg;
//for logging
    SDFileSystem sd(p5, p6, p7, p8, "sd");
    DigitalIn SD(p17);
    IMUfilter imuFilter(FILTER_RATE, 0.3);
    IMU_I2C accelerometer(p9, p10);
    GPS gps(p28,p27);
    DigitalOut gps_permission(p26);
    SCP1000 scp1000(p11,p12,p13,p21);
    DigitalOut scp1000_permission(p20);
    Ticker accelerometerTicker;
    Ticker gyroscopeTicker;
    Ticker filterTicker;
    Ticker logging;
//for communication with mission_mbed
    myCAN can(p30, p29);
    Ticker can_msg;

//for pc_msg
    #define PC_BAUD 9600
    #define PC_RATE 0.3
    void pc_flg_on(void);
    char pc_flg = 0;
//for logging
    #define LOG_RATE 0.3//0.04
    char log_flg;
    void log_flg_on(void);
//for can_msg
    #define CAN_RATE 0.1
    void can_flg_on(void);
    char can_flg = 0;

//Offsets for the gyroscope and the accelerometer.
    double w_xBias;
    double w_yBias;
    double w_zBias;
    double a_xBias;
    double a_yBias;
    double a_zBias;

//Accumulators used for oversampling and then averaging.
    volatile double a_xAccumulator = 0;
    volatile double a_yAccumulator = 0;
    volatile double a_zAccumulator = 0;
    volatile double w_xAccumulator = 0;
    volatile double w_yAccumulator = 0;
    volatile double w_zAccumulator = 0;

//Accelerometer and gyroscope readings for x, y, z axes.
    volatile double a_x;
    volatile double a_y;
    volatile double a_z;
    volatile double w_x;
    volatile double w_y;
    volatile double w_z;

//Buffer for accelerometer readings.
int readings[3];
//Number of accelerometer samples we're on.
int accelerometerSamples = 0;
//Number of gyroscope samples we're on.
int gyroscopeSamples = 0;

//Prototypes
void initializeAcceleromter(void);//Set up the ADXL345 appropriately.
void calibrateAccelerometer(void);//Calculate the null bias.
void sampleAccelerometer(void);//Take a set of samples and average them.
void initializeGyroscope(void);//Set up the ITG3200 appropriately.
void calibrateGyroscope(void);//Calculate the null bias.
void sampleGyroscope(void);//Take a set of samples and average them.
void filter(void);//Update the filter and calculate the Euler angles.
void IMU_Init(void);

void initializeAccelerometer(void) {
    //Go into standby mode to configure the device.
    accelerometer.setPowerControl(0x00);
    //Full resolution, +/-16g, 4mg/LSB.
    accelerometer.setDataFormatControl(0x0B);
    //200Hz data rate.
    accelerometer.setDataRate(ADXL345_200HZ);
    //Measurement mode.
    accelerometer.setPowerControl(0x08);
    //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
    wait_ms(22);
}

void sampleAccelerometer(void) {
    //Have we taken enough samples?
    if (accelerometerSamples == SAMPLES) {
        //Average the samples, remove the bias, and calculate the acceleration
        //in m/s/s.
        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;

        a_xAccumulator = 0;
        a_yAccumulator = 0;
        a_zAccumulator = 0;
        accelerometerSamples = 0;
    } else {
        //Take another sample.
        accelerometer.getOutput(readings);

        a_xAccumulator += (int16_t) readings[0];
        a_yAccumulator += (int16_t) readings[1];
        a_zAccumulator += (int16_t) readings[2];

        accelerometerSamples++;
    }
}

void calibrateAccelerometer(void) {
    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
    
    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
        accelerometer.getOutput(readings);

        a_xAccumulator += (int16_t) readings[0];
        a_yAccumulator += (int16_t) readings[1];
        a_zAccumulator += (int16_t) readings[2];

        wait(ACC_RATE);
    }
    a_xAccumulator /= CALIBRATION_SAMPLES;
    a_yAccumulator /= CALIBRATION_SAMPLES;
    a_zAccumulator /= CALIBRATION_SAMPLES;

    //At 4mg/LSB, 250 LSBs is 1g.
    a_xBias = a_xAccumulator;
    a_yBias = a_yAccumulator;
    a_zBias = (a_zAccumulator - 250);

    a_xAccumulator = 0;
    a_yAccumulator = 0;
    a_zAccumulator = 0;
}

void initializeGyroscope(void) {
    accelerometer.setLpBandwidth(LPFBW_42HZ);//Low pass filter bandwidth of 42Hz.
    accelerometer.setSampleRateDivider(4);//Internal sample rate of 200Hz. (1kHz / 5).
}

void calibrateGyroscope(void) {
    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;

    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
        w_xAccumulator += accelerometer.getGyroX();
        w_yAccumulator += accelerometer.getGyroY();
        w_zAccumulator += accelerometer.getGyroZ();
        wait(GYRO_RATE);
    }
    //Average the samples.
    w_xAccumulator /= CALIBRATION_SAMPLES;
    w_yAccumulator /= CALIBRATION_SAMPLES;
    w_zAccumulator /= CALIBRATION_SAMPLES;

    w_xBias = w_xAccumulator;
    w_yBias = w_yAccumulator;
    w_zBias = w_zAccumulator;

    w_xAccumulator = 0;
    w_yAccumulator = 0;
    w_zAccumulator = 0;
}

void sampleGyroscope(void) {
    if (gyroscopeSamples == SAMPLES) {
        //velocity in rad/s.
        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);

        w_xAccumulator = 0;
        w_yAccumulator = 0;
        w_zAccumulator = 0;
        gyroscopeSamples = 0;
    } else {
        w_xAccumulator += accelerometer.getGyroX();
        w_yAccumulator += accelerometer.getGyroY();
        w_zAccumulator += accelerometer.getGyroZ();

        gyroscopeSamples++;
    }
}

void filter(void) {
    imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);//Update the filter variables.
    imuFilter.computeEuler();//Calculate the new Euler angles.
}

void IMU_Init(void){
    initializeAccelerometer();
    calibrateAccelerometer();
    initializeGyroscope();
    calibrateGyroscope();

    accelerometerTicker.attach(&sampleAccelerometer, 0.005);//Accelerometer data rate is 200Hz, so we'll sample at this speed.
    gyroscopeTicker.attach(&sampleGyroscope, 0.005);//Gyroscope data rate is 200Hz, so we'll sample at this speed.
    filterTicker.attach(&filter, FILTER_RATE);//Initialize inertial sensors.
}

void pc_flg_on(void){
    pc_flg = 1;
}

void can_flg_on(void){
    can_flg = 1;
}

void log_flg_on(void){
    log_flg = 1;
}

int main() {
    gps_permission = 1;//off
    scp1000_permission = 1;//off
    
    IMU_Init();
    
    a_xBias=0;
    a_yBias=0;
    a_zBias=0;
    w_xBias=0;
    w_yBias=0;
    w_zBias=0;
    
    //wait(1.0);
    
    myled = 1;
    gps_permission = 0;//on
    scp1000_permission = 0;//on
    scp1000.init_scp1000();
    
    //for PC
        pc.baud(PC_BAUD);
        pc_msg.attach(&pc_flg_on,PC_RATE);
        
    //for CAN
        can_msg.attach(&can_flg_on,CAN_RATE);
    
    //for logging
        logging.attach(&log_flg_on,LOG_RATE);
    
    int roll,pitch;
    float g;
    roll = 0;
    pitch = 0;
    
    while(1){
        //for can send
            if(can_flg == 1){
                g = sqrt(a_x*a_x + a_y*a_y + a_z*a_z);
                roll = (int)(acos(a_y/g)*180/3.1415926535 - 90.0); 
                pitch = (int)(acos(a_x/g)*180/3.1415926535 - 90.0); 
                if(roll<0){roll=roll+360.0;}
                if(a_z<0){roll=-1*roll+180.0;} 
                if(roll<0){roll=roll+360.0;}
                can.make_logger_senddata(gps.get_time(),gps.get_satelite_number(),gps.get_str_latitude(),gps.get_str_longitude(),roll,pitch,(int)(scp1000.readTemperature()*20),scp1000.readPressure());
                can.send(LOGGER);
                can_flg = 0;
            }
        //for logging
            if(log_flg == 1){
                if(SD == 1){
                    mkdir("/sd/mydir", 0777);
                    FILE *fp = fopen("/sd/mydir/ver2.txt", "a");
                    if(fp == NULL){
                        //error("Could not open file for write\n");
                        pc.printf("SD_error");
                    }else{
                        fprintf(fp,"%s,%f,%f,",gps.get_time(),gps.get_latitude(),gps.get_longitude());
                        fprintf(fp,"%c,%d,%f,",gps.get_status(),gps.get_satelite_number(),gps.get_speed());
                        fprintf(fp,"%d,%f,", scp1000.readPressure(), scp1000.readTemperature());
                        fprintf(fp,"%f,%f,%f,%f,%f,%f,",w_y, w_x, w_z, a_y, a_x, a_z);
                        fprintf(fp,"%d,%d,",roll,pitch);
                        fprintf(fp,"%d",can.get_mission_status());
                        fprintf(fp,"\r\n",can.get_mission_status());
                        fclose(fp);
                    }
                }
            }
        
        //for pc debug
            if(pc_flg == 1){
                //pc.printf("%f,%f,%f,%f,%f,%f\r\n",w_y, w_x, w_z, a_y, a_x, a_z);
                pc.printf("%d,%d\r\n",roll,pitch);
                //pc.printf("%f,%f,%f,%f,%f,%f\r\n",a_xBias, a_yBias, a_zBias, w_xBias, w_yBias, w_zBias);
                //pc.printf("pressure:%d,temperature:%f\r\n", scp1000.readPressure(), scp1000.readTemperature());
                //pc.printf("time:%s,latitude:%f,longitude:%f,status:%c,satelite:%d,speed:%f\r\n",gps.get_time(),gps.get_latitude(),gps.get_longitude(),gps.get_status(),gps.get_satelite_number(),gps.get_speed());
                //pc.printf("\n\r");
                pc_flg =0;
            }
    }
}