Abe Takumi / Mbed 2 deprecated EM_Logger

For Cansat EM

Dependencies:   IMU_I2C IMUfilter SCP1000 SDFileSystem mbed nmea0813 myCAN_logger

main.cpp@4:008160161171, 2013-08-16 (annotated)

Committer:
YSB
Date:
Fri Aug 16 08:23:45 2013 +0000
Revision:
4:008160161171
Parent:
2:137282f468d0 
20130816ver

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YSB 0:657047909e74 1 #include "mbed.h"
YSB 0:657047909e74 2 #include "SDFileSystem.h"
YSB 0:657047909e74 3 #include "IMUfilter.h"
YSB 0:657047909e74 4 #include "IMU_I2C.h"
YSB 0:657047909e74 5 #include "nmea0813.h"
YSB 0:657047909e74 6 #include "SCP1000.h"
YSB 2:137282f468d0 7 #include "myCAN.h"
YSB 2:137282f468d0 8 #include "IDDATA.h"
YSB 0:657047909e74 9
YSB 0:657047909e74 10 //for debug
YSB 0:657047909e74 11 DigitalOut myled(LED1);
YSB 0:657047909e74 12 Serial pc(USBTX, USBRX);
YSB 0:657047909e74 13 Ticker pc_msg;
YSB 0:657047909e74 14 //for logging
YSB 0:657047909e74 15 SDFileSystem sd(p5, p6, p7, p8, "sd");
YSB 2:137282f468d0 16 DigitalIn SD(p17);
YSB 0:657047909e74 17 IMUfilter imuFilter(FILTER_RATE, 0.3);
YSB 0:657047909e74 18 IMU_I2C accelerometer(p9, p10);
YSB 0:657047909e74 19 GPS gps(p28,p27);
YSB 0:657047909e74 20 DigitalOut gps_permission(p26);
YSB 0:657047909e74 21 SCP1000 scp1000(p11,p12,p13,p21);
YSB 0:657047909e74 22 DigitalOut scp1000_permission(p20);
YSB 0:657047909e74 23 Ticker accelerometerTicker;
YSB 0:657047909e74 24 Ticker gyroscopeTicker;
YSB 0:657047909e74 25 Ticker filterTicker;
YSB 2:137282f468d0 26 Ticker logging;
YSB 0:657047909e74 27 //for communication with mission_mbed
YSB 2:137282f468d0 28 myCAN can(p30, p29);
YSB 4:008160161171 29 Ticker can_msg;
YSB 0:657047909e74 30
YSB 0:657047909e74 31 //for pc_msg
YSB 0:657047909e74 32 #define PC_BAUD 9600
YSB 0:657047909e74 33 #define PC_RATE 0.3
YSB 0:657047909e74 34 void pc_flg_on(void);
YSB 0:657047909e74 35 char pc_flg = 0;
YSB 2:137282f468d0 36 //for logging
YSB 2:137282f468d0 37 #define LOG_RATE 0.3//0.04
YSB 2:137282f468d0 38 char log_flg;
YSB 2:137282f468d0 39 void log_flg_on(void);
YSB 4:008160161171 40 //for can_msg
YSB 4:008160161171 41 #define CAN_RATE 0.1
YSB 4:008160161171 42 void can_flg_on(void);
YSB 4:008160161171 43 char can_flg = 0;
YSB 0:657047909e74 44
YSB 0:657047909e74 45 //Offsets for the gyroscope and the accelerometer.
YSB 0:657047909e74 46 double w_xBias;
YSB 0:657047909e74 47 double w_yBias;
YSB 0:657047909e74 48 double w_zBias;
YSB 0:657047909e74 49 double a_xBias;
YSB 0:657047909e74 50 double a_yBias;
YSB 0:657047909e74 51 double a_zBias;
YSB 0:657047909e74 52
YSB 0:657047909e74 53 //Accumulators used for oversampling and then averaging.
YSB 0:657047909e74 54 volatile double a_xAccumulator = 0;
YSB 0:657047909e74 55 volatile double a_yAccumulator = 0;
YSB 0:657047909e74 56 volatile double a_zAccumulator = 0;
YSB 0:657047909e74 57 volatile double w_xAccumulator = 0;
YSB 0:657047909e74 58 volatile double w_yAccumulator = 0;
YSB 0:657047909e74 59 volatile double w_zAccumulator = 0;
YSB 0:657047909e74 60
YSB 0:657047909e74 61 //Accelerometer and gyroscope readings for x, y, z axes.
YSB 0:657047909e74 62 volatile double a_x;
YSB 0:657047909e74 63 volatile double a_y;
YSB 0:657047909e74 64 volatile double a_z;
YSB 0:657047909e74 65 volatile double w_x;
YSB 0:657047909e74 66 volatile double w_y;
YSB 0:657047909e74 67 volatile double w_z;
YSB 0:657047909e74 68
YSB 0:657047909e74 69 //Buffer for accelerometer readings.
YSB 0:657047909e74 70 int readings[3];
YSB 0:657047909e74 71 //Number of accelerometer samples we're on.
YSB 0:657047909e74 72 int accelerometerSamples = 0;
YSB 0:657047909e74 73 //Number of gyroscope samples we're on.
YSB 0:657047909e74 74 int gyroscopeSamples = 0;
YSB 0:657047909e74 75
YSB 0:657047909e74 76 //Prototypes
YSB 0:657047909e74 77 void initializeAcceleromter(void);//Set up the ADXL345 appropriately.
YSB 0:657047909e74 78 void calibrateAccelerometer(void);//Calculate the null bias.
YSB 0:657047909e74 79 void sampleAccelerometer(void);//Take a set of samples and average them.
YSB 0:657047909e74 80 void initializeGyroscope(void);//Set up the ITG3200 appropriately.
YSB 0:657047909e74 81 void calibrateGyroscope(void);//Calculate the null bias.
YSB 0:657047909e74 82 void sampleGyroscope(void);//Take a set of samples and average them.
YSB 0:657047909e74 83 void filter(void);//Update the filter and calculate the Euler angles.
YSB 0:657047909e74 84 void IMU_Init(void);
YSB 0:657047909e74 85
YSB 0:657047909e74 86 void initializeAccelerometer(void) {
YSB 0:657047909e74 87 //Go into standby mode to configure the device.
YSB 0:657047909e74 88 accelerometer.setPowerControl(0x00);
YSB 0:657047909e74 89 //Full resolution, +/-16g, 4mg/LSB.
YSB 0:657047909e74 90 accelerometer.setDataFormatControl(0x0B);
YSB 0:657047909e74 91 //200Hz data rate.
YSB 0:657047909e74 92 accelerometer.setDataRate(ADXL345_200HZ);
YSB 0:657047909e74 93 //Measurement mode.
YSB 0:657047909e74 94 accelerometer.setPowerControl(0x08);
YSB 0:657047909e74 95 //See http://www.analog.com/static/imported-files/application_notes/AN-1077.pdf
YSB 0:657047909e74 96 wait_ms(22);
YSB 0:657047909e74 97 }
YSB 0:657047909e74 98
YSB 0:657047909e74 99 void sampleAccelerometer(void) {
YSB 0:657047909e74 100 //Have we taken enough samples?
YSB 0:657047909e74 101 if (accelerometerSamples == SAMPLES) {
YSB 0:657047909e74 102 //Average the samples, remove the bias, and calculate the acceleration
YSB 0:657047909e74 103 //in m/s/s.
YSB 0:657047909e74 104 a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * ACCELEROMETER_GAIN;
YSB 0:657047909e74 105 a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * ACCELEROMETER_GAIN;
YSB 0:657047909e74 106 a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * ACCELEROMETER_GAIN;
YSB 0:657047909e74 107
YSB 0:657047909e74 108 a_xAccumulator = 0;
YSB 0:657047909e74 109 a_yAccumulator = 0;
YSB 0:657047909e74 110 a_zAccumulator = 0;
YSB 0:657047909e74 111 accelerometerSamples = 0;
YSB 0:657047909e74 112 } else {
YSB 0:657047909e74 113 //Take another sample.
YSB 0:657047909e74 114 accelerometer.getOutput(readings);
YSB 0:657047909e74 115
YSB 0:657047909e74 116 a_xAccumulator += (int16_t) readings[0];
YSB 0:657047909e74 117 a_yAccumulator += (int16_t) readings[1];
YSB 0:657047909e74 118 a_zAccumulator += (int16_t) readings[2];
YSB 0:657047909e74 119
YSB 0:657047909e74 120 accelerometerSamples++;
YSB 0:657047909e74 121 }
YSB 0:657047909e74 122 }
YSB 0:657047909e74 123
YSB 0:657047909e74 124 void calibrateAccelerometer(void) {
YSB 0:657047909e74 125 a_xAccumulator = 0;
YSB 0:657047909e74 126 a_yAccumulator = 0;
YSB 0:657047909e74 127 a_zAccumulator = 0;
YSB 0:657047909e74 128
YSB 0:657047909e74 129 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
YSB 0:657047909e74 130 accelerometer.getOutput(readings);
YSB 0:657047909e74 131
YSB 0:657047909e74 132 a_xAccumulator += (int16_t) readings[0];
YSB 0:657047909e74 133 a_yAccumulator += (int16_t) readings[1];
YSB 0:657047909e74 134 a_zAccumulator += (int16_t) readings[2];
YSB 0:657047909e74 135
YSB 0:657047909e74 136 wait(ACC_RATE);
YSB 0:657047909e74 137 }
YSB 0:657047909e74 138 a_xAccumulator /= CALIBRATION_SAMPLES;
YSB 0:657047909e74 139 a_yAccumulator /= CALIBRATION_SAMPLES;
YSB 0:657047909e74 140 a_zAccumulator /= CALIBRATION_SAMPLES;
YSB 0:657047909e74 141
YSB 0:657047909e74 142 //At 4mg/LSB, 250 LSBs is 1g.
YSB 0:657047909e74 143 a_xBias = a_xAccumulator;
YSB 0:657047909e74 144 a_yBias = a_yAccumulator;
YSB 0:657047909e74 145 a_zBias = (a_zAccumulator - 250);
YSB 0:657047909e74 146
YSB 0:657047909e74 147 a_xAccumulator = 0;
YSB 0:657047909e74 148 a_yAccumulator = 0;
YSB 0:657047909e74 149 a_zAccumulator = 0;
YSB 0:657047909e74 150 }
YSB 0:657047909e74 151
YSB 0:657047909e74 152 void initializeGyroscope(void) {
YSB 0:657047909e74 153 accelerometer.setLpBandwidth(LPFBW_42HZ);//Low pass filter bandwidth of 42Hz.
YSB 0:657047909e74 154 accelerometer.setSampleRateDivider(4);//Internal sample rate of 200Hz. (1kHz / 5).
YSB 0:657047909e74 155 }
YSB 0:657047909e74 156
YSB 0:657047909e74 157 void calibrateGyroscope(void) {
YSB 0:657047909e74 158 w_xAccumulator = 0;
YSB 0:657047909e74 159 w_yAccumulator = 0;
YSB 0:657047909e74 160 w_zAccumulator = 0;
YSB 0:657047909e74 161
YSB 0:657047909e74 162 for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
YSB 0:657047909e74 163 w_xAccumulator += accelerometer.getGyroX();
YSB 0:657047909e74 164 w_yAccumulator += accelerometer.getGyroY();
YSB 0:657047909e74 165 w_zAccumulator += accelerometer.getGyroZ();
YSB 0:657047909e74 166 wait(GYRO_RATE);
YSB 0:657047909e74 167 }
YSB 0:657047909e74 168 //Average the samples.
YSB 0:657047909e74 169 w_xAccumulator /= CALIBRATION_SAMPLES;
YSB 0:657047909e74 170 w_yAccumulator /= CALIBRATION_SAMPLES;
YSB 0:657047909e74 171 w_zAccumulator /= CALIBRATION_SAMPLES;
YSB 0:657047909e74 172
YSB 0:657047909e74 173 w_xBias = w_xAccumulator;
YSB 0:657047909e74 174 w_yBias = w_yAccumulator;
YSB 0:657047909e74 175 w_zBias = w_zAccumulator;
YSB 0:657047909e74 176
YSB 0:657047909e74 177 w_xAccumulator = 0;
YSB 0:657047909e74 178 w_yAccumulator = 0;
YSB 0:657047909e74 179 w_zAccumulator = 0;
YSB 0:657047909e74 180 }
YSB 0:657047909e74 181
YSB 0:657047909e74 182 void sampleGyroscope(void) {
YSB 0:657047909e74 183 if (gyroscopeSamples == SAMPLES) {
YSB 0:657047909e74 184 //velocity in rad/s.
YSB 0:657047909e74 185 w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias) * GYROSCOPE_GAIN);
YSB 0:657047909e74 186 w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias) * GYROSCOPE_GAIN);
YSB 0:657047909e74 187 w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias) * GYROSCOPE_GAIN);
YSB 0:657047909e74 188
YSB 0:657047909e74 189 w_xAccumulator = 0;
YSB 0:657047909e74 190 w_yAccumulator = 0;
YSB 0:657047909e74 191 w_zAccumulator = 0;
YSB 0:657047909e74 192 gyroscopeSamples = 0;
YSB 0:657047909e74 193 } else {
YSB 0:657047909e74 194 w_xAccumulator += accelerometer.getGyroX();
YSB 0:657047909e74 195 w_yAccumulator += accelerometer.getGyroY();
YSB 0:657047909e74 196 w_zAccumulator += accelerometer.getGyroZ();
YSB 0:657047909e74 197
YSB 0:657047909e74 198 gyroscopeSamples++;
YSB 0:657047909e74 199 }
YSB 0:657047909e74 200 }
YSB 0:657047909e74 201
YSB 0:657047909e74 202 void filter(void) {
YSB 0:657047909e74 203 imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);//Update the filter variables.
YSB 0:657047909e74 204 imuFilter.computeEuler();//Calculate the new Euler angles.
YSB 0:657047909e74 205 }
YSB 0:657047909e74 206
YSB 0:657047909e74 207 void IMU_Init(void){
YSB 0:657047909e74 208 initializeAccelerometer();
YSB 0:657047909e74 209 calibrateAccelerometer();
YSB 0:657047909e74 210 initializeGyroscope();
YSB 0:657047909e74 211 calibrateGyroscope();
YSB 0:657047909e74 212
YSB 0:657047909e74 213 accelerometerTicker.attach(&sampleAccelerometer, 0.005);//Accelerometer data rate is 200Hz, so we'll sample at this speed.
YSB 0:657047909e74 214 gyroscopeTicker.attach(&sampleGyroscope, 0.005);//Gyroscope data rate is 200Hz, so we'll sample at this speed.
YSB 0:657047909e74 215 filterTicker.attach(&filter, FILTER_RATE);//Initialize inertial sensors.
YSB 0:657047909e74 216 }
YSB 0:657047909e74 217
YSB 0:657047909e74 218 void pc_flg_on(void){
YSB 0:657047909e74 219 pc_flg = 1;
YSB 0:657047909e74 220 }
YSB 0:657047909e74 221
YSB 4:008160161171 222 void can_flg_on(void){
YSB 4:008160161171 223 can_flg = 1;
YSB 4:008160161171 224 }
YSB 4:008160161171 225
YSB 2:137282f468d0 226 void log_flg_on(void){
YSB 2:137282f468d0 227 log_flg = 1;
YSB 0:657047909e74 228 }
YSB 0:657047909e74 229
YSB 0:657047909e74 230 int main() {
YSB 0:657047909e74 231 gps_permission = 1;//off
YSB 0:657047909e74 232 scp1000_permission = 1;//off
YSB 0:657047909e74 233
YSB 0:657047909e74 234 IMU_Init();
YSB 0:657047909e74 235
YSB 0:657047909e74 236 a_xBias=0;
YSB 0:657047909e74 237 a_yBias=0;
YSB 0:657047909e74 238 a_zBias=0;
YSB 0:657047909e74 239 w_xBias=0;
YSB 0:657047909e74 240 w_yBias=0;
YSB 0:657047909e74 241 w_zBias=0;
YSB 0:657047909e74 242
YSB 0:657047909e74 243 //wait(1.0);
YSB 0:657047909e74 244
YSB 0:657047909e74 245 myled = 1;
YSB 0:657047909e74 246 gps_permission = 0;//on
YSB 0:657047909e74 247 scp1000_permission = 0;//on
YSB 0:657047909e74 248 scp1000.init_scp1000();
YSB 0:657047909e74 249
YSB 0:657047909e74 250 //for PC
YSB 0:657047909e74 251 pc.baud(PC_BAUD);
YSB 0:657047909e74 252 pc_msg.attach(&pc_flg_on,PC_RATE);
YSB 4:008160161171 253
YSB 4:008160161171 254 //for CAN
YSB 4:008160161171 255 can_msg.attach(&can_flg_on,CAN_RATE);
YSB 0:657047909e74 256
YSB 2:137282f468d0 257 //for logging
YSB 2:137282f468d0 258 logging.attach(&log_flg_on,LOG_RATE);
YSB 2:137282f468d0 259
YSB 4:008160161171 260 int roll,pitch;
YSB 4:008160161171 261 float g;
YSB 4:008160161171 262 roll = 0;
YSB 4:008160161171 263 pitch = 0;
YSB 0:657047909e74 264
YSB 0:657047909e74 265 while(1){
YSB 2:137282f468d0 266 //for can send
YSB 4:008160161171 267 if(can_flg == 1){
YSB 4:008160161171 268 g = sqrt(a_x*a_x + a_y*a_y + a_z*a_z);
YSB 4:008160161171 269 roll = (int)(acos(a_y/g)*180/3.1415926535 - 90.0);
YSB 4:008160161171 270 pitch = (int)(acos(a_x/g)*180/3.1415926535 - 90.0);
YSB 4:008160161171 271 if(roll<0){roll=roll+360.0;}
YSB 4:008160161171 272 if(a_z<0){roll=-1*roll+180.0;}
YSB 4:008160161171 273 if(roll<0){roll=roll+360.0;}
YSB 4:008160161171 274 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());
YSB 4:008160161171 275 can.send(LOGGER);
YSB 4:008160161171 276 can_flg = 0;
YSB 4:008160161171 277 }
YSB 0:657047909e74 278 //for logging
YSB 2:137282f468d0 279 if(log_flg == 1){
YSB 2:137282f468d0 280 if(SD == 1){
YSB 2:137282f468d0 281 mkdir("/sd/mydir", 0777);
YSB 2:137282f468d0 282 FILE *fp = fopen("/sd/mydir/ver2.txt", "a");
YSB 2:137282f468d0 283 if(fp == NULL){
YSB 2:137282f468d0 284 //error("Could not open file for write\n");
YSB 2:137282f468d0 285 pc.printf("SD_error");
YSB 2:137282f468d0 286 }else{
YSB 4:008160161171 287 fprintf(fp,"%s,%f,%f,",gps.get_time(),gps.get_latitude(),gps.get_longitude());
YSB 4:008160161171 288 fprintf(fp,"%c,%d,%f,",gps.get_status(),gps.get_satelite_number(),gps.get_speed());
YSB 4:008160161171 289 fprintf(fp,"%d,%f,", scp1000.readPressure(), scp1000.readTemperature());
YSB 4:008160161171 290 fprintf(fp,"%f,%f,%f,%f,%f,%f,",w_y, w_x, w_z, a_y, a_x, a_z);
YSB 4:008160161171 291 fprintf(fp,"%d,%d,",roll,pitch);
YSB 4:008160161171 292 fprintf(fp,"%d",can.get_mission_status());
YSB 4:008160161171 293 fprintf(fp,"\r\n",can.get_mission_status());
YSB 2:137282f468d0 294 fclose(fp);
YSB 2:137282f468d0 295 }
YSB 2:137282f468d0 296 }
YSB 2:137282f468d0 297 }
YSB 0:657047909e74 298
YSB 0:657047909e74 299 //for pc debug
YSB 0:657047909e74 300 if(pc_flg == 1){
YSB 4:008160161171 301 //pc.printf("%f,%f,%f,%f,%f,%f\r\n",w_y, w_x, w_z, a_y, a_x, a_z);
YSB 4:008160161171 302 pc.printf("%d,%d\r\n",roll,pitch);
YSB 0:657047909e74 303 //pc.printf("%f,%f,%f,%f,%f,%f\r\n",a_xBias, a_yBias, a_zBias, w_xBias, w_yBias, w_zBias);
YSB 2:137282f468d0 304 //pc.printf("pressure:%d,temperature:%f\r\n", scp1000.readPressure(), scp1000.readTemperature());
YSB 4:008160161171 305 //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());
YSB 4:008160161171 306 //pc.printf("\n\r");
YSB 0:657047909e74 307 pc_flg =0;
YSB 0:657047909e74 308 }
YSB 0:657047909e74 309 }
YSB 0:657047909e74 310 }