Abe Takumi / Mbed 2 deprecated EM_Logger

For Cansat EM

Dependencies:   IMU_I2C IMUfilter SCP1000 SDFileSystem mbed nmea0813 myCAN_logger

main.cpp@0:657047909e74, 2013-07-13 (annotated)

Committer:
YSB
Date:
Sat Jul 13 16:50:55 2013 +0000
Revision:
0:657047909e74
Child:
2:137282f468d0
First Commit

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