main.cpp

00001 #include "mbed.h"
00002 #include "SDFileSystem.h"
00003 #include "IMUfilter.h"
00004 #include "IMU_I2C.h"
00005 #include "nmea0813.h"
00006 #include "SCP1000.h"
00007 #include "myCAN.h"
00008 #include "IDDATA.h"
00009 
00010 //for debug
00011     DigitalOut myled(LED1);
00012     Serial pc(USBTX, USBRX);
00013     Ticker pc_msg;
00014 //for logging
00015     SDFileSystem sd(p5, p6, p7, p8, "sd");
00016     DigitalIn SD(p17);
00017     IMUfilter imuFilter(FILTER_RATE, 0.3);
00018     IMU_I2C accelerometer(p9, p10);
00019     GPS gps(p28,p27);
00020     DigitalOut gps_permission(p26);
00021     SCP1000 scp1000(p11,p12,p13,p21);
00022     DigitalOut scp1000_permission(p20);
00023     Ticker accelerometerTicker;
00024     Ticker gyroscopeTicker;
00025     Ticker filterTicker;
00026     Ticker logging;
00027 //for communication with mission_mbed
00028     myCAN can(p30, p29);
00029     Ticker can_msg;
00030 
00031 //for pc_msg
00032     #define PC_BAUD 9600
00033     #define PC_RATE 0.3
00034     void pc_flg_on(void);
00035     char pc_flg = 0;
00036 //for logging
00037     #define LOG_RATE 0.3//0.04
00038     char log_flg;
00039     void log_flg_on(void);
00040 //for can_msg
00041     #define CAN_RATE 0.1
00042     void can_flg_on(void);
00043     char can_flg = 0;
00044 
00045 //Offsets for the gyroscope and the accelerometer.
00046     double w_xBias;
00047     double w_yBias;
00048     double w_zBias;
00049     double a_xBias;
00050     double a_yBias;
00051     double a_zBias;
00052 
00053 //Accumulators used for oversampling and then averaging.
00054     volatile double a_xAccumulator = 0;
00055     volatile double a_yAccumulator = 0;
00056     volatile double a_zAccumulator = 0;
00057     volatile double w_xAccumulator = 0;
00058     volatile double w_yAccumulator = 0;
00059     volatile double w_zAccumulator = 0;
00060 
00061 //Accelerometer and gyroscope readings for x, y, z axes.
00062     volatile double a_x;
00063     volatile double a_y;
00064     volatile double a_z;
00065     volatile double w_x;
00066     volatile double w_y;
00067     volatile double w_z;
00068 
00069 //Buffer for accelerometer readings.
00070 int readings[3];
00071 //Number of accelerometer samples we're on.
00072 int accelerometerSamples = 0;
00073 //Number of gyroscope samples we're on.
00074 int gyroscopeSamples = 0;
00075 
00076 //Prototypes
00077 void initializeAcceleromter(void);//Set up the ADXL345 appropriately.
00078 void calibrateAccelerometer(void);//Calculate the null bias.
00079 void sampleAccelerometer(void);//Take a set of samples and average them.
00080 void initializeGyroscope(void);//Set up the ITG3200 appropriately.
00081 void calibrateGyroscope(void);//Calculate the null bias.
00082 void sampleGyroscope(void);//Take a set of samples and average them.
00083 void filter(void);//Update the filter and calculate the Euler angles.
00084 void IMU_Init(void);
00085 
00086 void initializeAccelerometer(void) {
00087     //Go into standby mode to configure the device.
00088     accelerometer.setPowerControl(0x00);
00089     //Full resolution, +/-16g, 4mg/LSB.
00090     accelerometer.setDataFormatControl(0x0B);
00091     //200Hz data rate.
00092     accelerometer.setDataRate(ADXL345_200HZ);
00093     //Measurement mode.
00094     accelerometer.setPowerControl(0x08);
00095     //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
00096     wait_ms(22);
00097 }
00098 
00099 void sampleAccelerometer(void) {
00100     //Have we taken enough samples?
00101     if (accelerometerSamples == SAMPLES) {
00102         //Average the samples, remove the bias, and calculate the acceleration
00103         //in m/s/s.
00104         a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
00105         a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
00106         a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
00107 
00108         a_xAccumulator = 0;
00109         a_yAccumulator = 0;
00110         a_zAccumulator = 0;
00111         accelerometerSamples = 0;
00112     } else {
00113         //Take another sample.
00114         accelerometer.getOutput(readings);
00115 
00116         a_xAccumulator += (int16_t) readings[0];
00117         a_yAccumulator += (int16_t) readings[1];
00118         a_zAccumulator += (int16_t) readings[2];
00119 
00120         accelerometerSamples++;
00121     }
00122 }
00123 
00124 void calibrateAccelerometer(void) {
00125     a_xAccumulator = 0;
00126     a_yAccumulator = 0;
00127     a_zAccumulator = 0;
00128     
00129     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00130         accelerometer.getOutput(readings);
00131 
00132         a_xAccumulator += (int16_t) readings[0];
00133         a_yAccumulator += (int16_t) readings[1];
00134         a_zAccumulator += (int16_t) readings[2];
00135 
00136         wait(ACC_RATE);
00137     }
00138     a_xAccumulator /= CALIBRATION_SAMPLES;
00139     a_yAccumulator /= CALIBRATION_SAMPLES;
00140     a_zAccumulator /= CALIBRATION_SAMPLES;
00141 
00142     //At 4mg/LSB, 250 LSBs is 1g.
00143     a_xBias = a_xAccumulator;
00144     a_yBias = a_yAccumulator;
00145     a_zBias = (a_zAccumulator - 250);
00146 
00147     a_xAccumulator = 0;
00148     a_yAccumulator = 0;
00149     a_zAccumulator = 0;
00150 }
00151 
00152 void initializeGyroscope(void) {
00153     accelerometer.setLpBandwidth(LPFBW_42HZ);//Low pass filter bandwidth of 42Hz.
00154     accelerometer.setSampleRateDivider(4);//Internal sample rate of 200Hz. (1kHz / 5).
00155 }
00156 
00157 void calibrateGyroscope(void) {
00158     w_xAccumulator = 0;
00159     w_yAccumulator = 0;
00160     w_zAccumulator = 0;
00161 
00162     for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
00163         w_xAccumulator += accelerometer.getGyroX();
00164         w_yAccumulator += accelerometer.getGyroY();
00165         w_zAccumulator += accelerometer.getGyroZ();
00166         wait(GYRO_RATE);
00167     }
00168     //Average the samples.
00169     w_xAccumulator /= CALIBRATION_SAMPLES;
00170     w_yAccumulator /= CALIBRATION_SAMPLES;
00171     w_zAccumulator /= CALIBRATION_SAMPLES;
00172 
00173     w_xBias = w_xAccumulator;
00174     w_yBias = w_yAccumulator;
00175     w_zBias = w_zAccumulator;
00176 
00177     w_xAccumulator = 0;
00178     w_yAccumulator = 0;
00179     w_zAccumulator = 0;
00180 }
00181 
00182 void sampleGyroscope(void) {
00183     if (gyroscopeSamples == SAMPLES) {
00184         //velocity in rad/s.
00185         w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
00186         w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
00187         w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
00188 
00189         w_xAccumulator = 0;
00190         w_yAccumulator = 0;
00191         w_zAccumulator = 0;
00192         gyroscopeSamples = 0;
00193     } else {
00194         w_xAccumulator += accelerometer.getGyroX();
00195         w_yAccumulator += accelerometer.getGyroY();
00196         w_zAccumulator += accelerometer.getGyroZ();
00197 
00198         gyroscopeSamples++;
00199     }
00200 }
00201 
00202 void filter(void) {
00203     imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);//Update the filter variables.
00204     imuFilter.computeEuler();//Calculate the new Euler angles.
00205 }
00206 
00207 void IMU_Init(void){
00208     initializeAccelerometer();
00209     calibrateAccelerometer();
00210     initializeGyroscope();
00211     calibrateGyroscope();
00212 
00213     accelerometerTicker.attach(&sampleAccelerometer, 0.005);//Accelerometer data rate is 200Hz, so we'll sample at this speed.
00214     gyroscopeTicker.attach(&sampleGyroscope, 0.005);//Gyroscope data rate is 200Hz, so we'll sample at this speed.
00215     filterTicker.attach(&filter, FILTER_RATE);//Initialize inertial sensors.
00216 }
00217 
00218 void pc_flg_on(void){
00219     pc_flg = 1;
00220 }
00221 
00222 void can_flg_on(void){
00223     can_flg = 1;
00224 }
00225 
00226 void log_flg_on(void){
00227     log_flg = 1;
00228 }
00229 
00230 int main() {
00231     gps_permission = 1;//off
00232     scp1000_permission = 1;//off
00233     
00234     IMU_Init();
00235     
00236     a_xBias=0;
00237     a_yBias=0;
00238     a_zBias=0;
00239     w_xBias=0;
00240     w_yBias=0;
00241     w_zBias=0;
00242     
00243     //wait(1.0);
00244     
00245     myled = 1;
00246     gps_permission = 0;//on
00247     scp1000_permission = 0;//on
00248     scp1000.init_scp1000();
00249     
00250     //for PC
00251         pc.baud(PC_BAUD);
00252         pc_msg.attach(&pc_flg_on,PC_RATE);
00253         
00254     //for CAN
00255         can_msg.attach(&can_flg_on,CAN_RATE);
00256     
00257     //for logging
00258         logging.attach(&log_flg_on,LOG_RATE);
00259     
00260     int roll,pitch;
00261     float g;
00262     roll = 0;
00263     pitch = 0;
00264     
00265     while(1){
00266         //for can send
00267             if(can_flg == 1){
00268                 g = sqrt(a_x*a_x + a_y*a_y + a_z*a_z);
00269                 roll = (int)(acos(a_y/g)*180/3.1415926535 - 90.0); 
00270                 pitch = (int)(acos(a_x/g)*180/3.1415926535 - 90.0); 
00271                 if(roll<0){roll=roll+360.0;}
00272                 if(a_z<0){roll=-1*roll+180.0;} 
00273                 if(roll<0){roll=roll+360.0;}
00274                 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());
00275                 can.send(LOGGER);
00276                 can_flg = 0;
00277             }
00278         //for logging
00279             if(log_flg == 1){
00280                 if(SD == 1){
00281                     mkdir("/sd/mydir", 0777);
00282                     FILE *fp = fopen("/sd/mydir/ver2.txt", "a");
00283                     if(fp == NULL){
00284                         //error("Could not open file for write\n");
00285                         pc.printf("SD_error");
00286                     }else{
00287                         fprintf(fp,"%s,%f,%f,",gps.get_time(),gps.get_latitude(),gps.get_longitude());
00288                         fprintf(fp,"%c,%d,%f,",gps.get_status(),gps.get_satelite_number(),gps.get_speed());
00289                         fprintf(fp,"%d,%f,", scp1000.readPressure(), scp1000.readTemperature());
00290                         fprintf(fp,"%f,%f,%f,%f,%f,%f,",w_y, w_x, w_z, a_y, a_x, a_z);
00291                         fprintf(fp,"%d,%d,",roll,pitch);
00292                         fprintf(fp,"%d",can.get_mission_status());
00293                         fprintf(fp,"\r\n",can.get_mission_status());
00294                         fclose(fp);
00295                     }
00296                 }
00297             }
00298         
00299         //for pc debug
00300             if(pc_flg == 1){
00301                 //pc.printf("%f,%f,%f,%f,%f,%f\r\n",w_y, w_x, w_z, a_y, a_x, a_z);
00302                 pc.printf("%d,%d\r\n",roll,pitch);
00303                 //pc.printf("%f,%f,%f,%f,%f,%f\r\n",a_xBias, a_yBias, a_zBias, w_xBias, w_yBias, w_zBias);
00304                 //pc.printf("pressure:%d,temperature:%f\r\n", scp1000.readPressure(), scp1000.readTemperature());
00305                 //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());
00306                 //pc.printf("\n\r");
00307                 pc_flg =0;
00308             }
00309     }
00310 }