For Cansat EM
Dependencies: IMU_I2C IMUfilter SCP1000 SDFileSystem mbed nmea0813 myCAN_logger
main.cpp
- Committer:
- YSB
- Date:
- 2013-07-21
- Revision:
- 2:137282f468d0
- Parent:
- 0:657047909e74
- Child:
- 4:008160161171
File content as of revision 2:137282f468d0:
#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); //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); //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 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 logging logging.attach(&log_flg_on,LOG_RATE); int a_zi; a_zi=0; while(1){ //for can send if(a_z > 9.6){a_zi=1;} else{a_zi=0;} can.make_logger_senddata(gps.get_time(),gps.get_satelite_number(),gps.get_str_latitude(),gps.get_str_longitude(),a_zi,(int)(scp1000.readTemperature()*20),scp1000.readPressure()); can.send(LOGGER); //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,"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("%d\r\n",a_zi); //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_flg =0; } } }