Revision 0:657047909e74, committed 2013-07-13

Comitter:

YSB

Date:

Sat Jul 13 16:50:55 2013 +0000

Child:

1:ecddbd10ab4c

Commit message:

First Commit

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMU_I2C.lib	Sat Jul 13 16:50:55 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/YSB/code/IMU_I2C/#7b3acf8e2a6f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/IMUfilter.lib	Sat Jul 13 16:50:55 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/YSB/code/IMUfilter/#fae851819c43

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SCP1000.lib	Sat Jul 13 16:50:55 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/kadams6/code/SCP1000/#53f432e521a4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/SDFileSystem.lib	Sat Jul 13 16:50:55 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/SDFileSystem/#c8f66dc765d4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Jul 13 16:50:55 2013 +0000
@@ -0,0 +1,306 @@
+#include "mbed.h"
+#include "SDFileSystem.h"
+#include "IMUfilter.h"
+#include "IMU_I2C.h"
+#include "nmea0813.h"
+#include "SCP1000.h"
+
+//for debug
+    DigitalOut myled(LED1);
+    Serial pc(USBTX, USBRX);
+    Ticker pc_msg;
+//for logging
+    SDFileSystem sd(p5, p6, p7, p8, "sd");
+    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;
+//for communication with mission_mbed
+    CAN 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 CAN
+    #define CAN_BAUD 4000000
+    #define CAN_RATE 0.05
+    void can_flg_on(void);
+    char can_flg,can_flg2;
+    int  id;
+    char dmsg[8];
+
+
+//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;
+}
+
+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.frequency(CAN_BAUD);
+        can_msg.attach(&can_flg_on,CAN_RATE);
+        CANMessage msg;
+    
+    while(1){
+    
+        if(can.read(msg)) {
+            pc.printf("Message received: %d\n\r", msg.data[0]);
+            if(msg.data[0]==1){can_flg2=1;}
+        } 
+    
+        //for logging
+            
+            mkdir("/sd/mydir", 0777);
+            FILE *fp = fopen("/sd/mydir/kikyuu.txt", "a");
+            if(fp == NULL){error("Could not open file for write\n");}
+            fprintf(fp,"time:%s,latitude:%f,longitude:%f,status:%c,satelite:%c,speed:%f,",gps.get_time(),gps.get_latitude(),gps.get_longitude(),gps.get_status(),gps.get_satelite_number(),gps.get_speed());
+            //fprintf(fp,"roll:%f,pitch:%f,",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()));
+            fprintf(fp,"pressure:%d,temperature:%f\r\n", scp1000.readPressure(), scp1000.readTemperature());
+            fprintf(fp,"%s,%f,%f,%f,%f,%f,%f\r\n",gps.get_time(),w_y, w_x, w_z, a_y, a_x, a_z);
+            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("%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:%c,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("%f,%f,%d,%f\r\n",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),scp1000.readPressure(), scp1000.readTemperature());
+                pc.printf("\n\r");
+                //pc.printf("%i,%i\n\r",dmsg[0],dmsg[2]);
+                //pc.printf("\n\r");
+                pc_flg =0;
+            }
+        //for can send
+            if(can_flg == 1){
+                if(can_flg2==1){
+                    id = 1;
+                    if(a_z > 9.5){
+                        dmsg[0]=1;//roll}
+                        dmsg[2]=1;
+                    }
+                    else{dmsg[0]=0;}
+                    dmsg[1]=0;//pitch
+                    //msg[2]=0;
+                    /*
+                    dmsg[3]=0;
+                    dmsg[4]=0;
+                    dmsg[5]=0;
+                    dmsg[6]=0;
+                    dmsg[7]=0;
+                    */
+                    can.write(CANMessage(id,dmsg)); 
+                    can_flg=0;
+                }
+            }
+    }
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sat Jul 13 16:50:55 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/b3110cd2dd17
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nmea0813.lib	Sat Jul 13 16:50:55 2013 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/YSB/code/nmea0813/#7870c69fa58c