Nathan Ewin / Mbed 2 deprecated UM6_IMU_AHRS_edit_NJE

Trying to log data from UM6 sensor with GPS receiver LS20031. I have two problems: - I can't log to file at a fast rate (<0.5s) without data values freezing to a fixed value. Print to pc screen it works fine. Ideally I would do this with an interrupt (e.g. ticker) so that the time of each reading is a fixed interval - I removed this as I thought this was causing the problem. - I want to record GPS lat and long. I have setup the GPS ground speed so I know the sensor are communicating. So I possibly havent set the config file to correctly interpet these two signals.

Dependencies:   MODSERIAL mbed

Fork of UM6_IMU_AHRS_2012 by lhiggs CSUM

main.cpp@6:43029c69b9ac, 2013-05-04 (annotated)

Committer:
njewin
Date:
Sat May 04 09:34:15 2013 +0000
Revision:
6:43029c69b9ac
Parent:
5:b3cd0bcf2968 
publish version ; bugs need correcting for:; - logging to file at 10ms rate; - reading GPS lat and long data

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lhiggs 0:03c649c76388 1 #include "mbed.h" // MBED LIBRARY
lhiggs 0:03c649c76388 2 #include "MODSERIAL.h" // MBED BUFFERED SERIAL
lhiggs 0:03c649c76388 3
lhiggs 0:03c649c76388 4 #include "UM6_usart.h" // UM6 USART HEADER
lhiggs 0:03c649c76388 5 #include "UM6_config.h" // UM6 CONFIG HEADER
lhiggs 0:03c649c76388 6
njewin 4:aefc0f09fe1c 7 LocalFileSystem local("local"); // Create local filesystem under the name "local"
njewin 1:20201cda90d0 8
lhiggs 0:03c649c76388 9 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:03c649c76388 10 // SETUP (ASSIGN) SERIAL COMMUNICATION PINS ON MBED
lhiggs 0:03c649c76388 11 /////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:03c649c76388 12 MODSERIAL pc(USBTX, USBRX); // PC SERIAL OVER USB PORT ON MBED
lhiggs 0:03c649c76388 13
lhiggs 0:03c649c76388 14 ////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:03c649c76388 15 // SETUP (ASSIGN) MBED LED (1 thru 3) FOR VISUAL DEBUGGING ON MBED
lhiggs 0:03c649c76388 16 ////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:03c649c76388 17 DigitalOut pc_activity(LED1); // LED1 = PC SERIAL
lhiggs 0:03c649c76388 18 DigitalOut uart_activity(LED2); // LED2 = UM6 SERIAL
njewin 1:20201cda90d0 19 DigitalOut logLED(LED3); // LED3 = logging active
njewin 3:0cfe2e18440d 20 DigitalOut sync(p6);
njewin 1:20201cda90d0 21 DigitalIn enable(p5); // enable signal for logging data to file
njewin 1:20201cda90d0 22
njewin 3:0cfe2e18440d 23 Timer t; // sets up timer for measuring data time stamp
njewin 3:0cfe2e18440d 24 Timer t1;// sets up timer for measuring time to read/write data
lhiggs 0:03c649c76388 25
lhiggs 0:03c649c76388 26 void rxCallback(MODSERIAL_IRQ_INFO *q) {
lhiggs 0:03c649c76388 27 if (um6_uart.rxBufferGetCount() >= MAX_PACKET_DATA) {
lhiggs 0:03c649c76388 28 uart_activity = !uart_activity; // Lights LED when uart RxBuff has > 40 bytes
lhiggs 0:03c649c76388 29 Process_um6_packet();
lhiggs 0:03c649c76388 30 }
lhiggs 0:03c649c76388 31 }
njewin 1:20201cda90d0 32
lhiggs 0:03c649c76388 33
lhiggs 0:03c649c76388 34 int main() {
lhiggs 0:03c649c76388 35
njewin 2:db3bbd57b075 36
lhiggs 0:03c649c76388 37 /////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:03c649c76388 38 // SET SERIAL UART BAUD RATES
lhiggs 0:03c649c76388 39 /////////////////////////////////////////////////////////////////////////////////////////////////////
lhiggs 0:03c649c76388 40
njewin 6:43029c69b9ac 41 // set UM6 serial uart baud rate
njewin 6:43029c69b9ac 42 um6_uart.baud(115200); // baud rate to um6 interface
njewin 6:43029c69b9ac 43 pc.baud(115200); // baud rate to pc interface
njewin 1:20201cda90d0 44
lhiggs 0:03c649c76388 45 // attach interupt function to uart
lhiggs 0:03c649c76388 46 um6_uart.attach(&rxCallback, MODSERIAL::RxIrq);
lhiggs 0:03c649c76388 47
njewin 6:43029c69b9ac 48 // opens file for saving data to and sets up header row
njewin 1:20201cda90d0 49
njewin 1:20201cda90d0 50 FILE *fp = fopen("/local/out3.csv", "w"); // Open "out.txt" on the local file system for writing
njewin 5:b3cd0bcf2968 51 fprintf(fp, "time (s),Yaw (deg),Roll (deg),Pitch (deg),GyroX,GyroY,GyroZ,AccelX,AccelY,AccelZ,GPScourse(x100 deg),GPSspeed(x100 m/s)\r"); // sends header to file
njewin 3:0cfe2e18440d 52
njewin 3:0cfe2e18440d 53 t.start(); // start data log time
njewin 3:0cfe2e18440d 54 sync = 1;
njewin 3:0cfe2e18440d 55 while (enable) {
njewin 2:db3bbd57b075 56 logLED = 1; // turns LED3 on when logging starts
njewin 6:43029c69b9ac 57 wait(0.02);
njewin 1:20201cda90d0 58 float time1=t.read();
njewin 1:20201cda90d0 59 float Yaw=data.Yaw;
njewin 1:20201cda90d0 60 float Roll=data.Roll;
njewin 1:20201cda90d0 61 float Pitch=data.Pitch;
njewin 1:20201cda90d0 62 float GyroX=data.Gyro_Proc_X;
njewin 1:20201cda90d0 63 float GyroY=data.Gyro_Proc_Y;
njewin 1:20201cda90d0 64 float GyroZ=data.Gyro_Proc_Z;
njewin 1:20201cda90d0 65 float AccelX=data.Accel_Proc_X;
njewin 1:20201cda90d0 66 float AccelY=data.Accel_Proc_Y;
njewin 1:20201cda90d0 67 float AccelZ=data.Accel_Proc_Z;
njewin 6:43029c69b9ac 68 float GPSlong=data.GPS_long; // currently I can get GPS longitude to out data
njewin 6:43029c69b9ac 69 float GPSlat=data.GPS_lat; // currently I can get GPS latitude to out data
njewin 3:0cfe2e18440d 70 float GPScourse=data.GPS_course;
njewin 3:0cfe2e18440d 71 float GPSspeed=data.GPS_speed;
njewin 1:20201cda90d0 72
njewin 6:43029c69b9ac 73 // print to file every 20ms - this is what I would idealy do but it ends up freezing on a single value
njewin 6:43029c69b9ac 74 fprintf(fp, "%3.3f, %3.1f,%3.1f,%3.1f, %4.1f,%4.1f,%4.1f, %1.3f,%1.3f,%1.3f, %4.0f,%f\n",time1,Yaw,Roll,Pitch,GyroX,GyroY,GyroZ,AccelX,AccelY,AccelZ,GPScourse,GPSspeed);
njewin 6:43029c69b9ac 75
njewin 6:43029c69b9ac 76 // print to screen - I use this to check that data is outputting - works fine on its own, but also freezes when above line is used
njewin 6:43029c69b9ac 77 pc.printf("time %3.3f, yaw %3.1e,%3.1e,%3.1e, gyro %4.1e,%4.1e,%4.1e,accel %1.3e,%1.3e,%1.3e,speed %4.0e, course %5.0e\n",time1,Yaw,Roll,Pitch,GyroX,GyroY,GyroZ,AccelX,AccelY,AccelZ,GPScourse,GPSspeed);
njewin 6:43029c69b9ac 78
njewin 6:43029c69b9ac 79 pc_activity = !pc_activity; // Lights LED1 when uart RxBuff has > 40 bytes
njewin 3:0cfe2e18440d 80
njewin 3:0cfe2e18440d 81 } // end while(1) loop
njewin 6:43029c69b9ac 82 fprintf(fp,"%3.3f \n",t.read()); // records time at end of measurement
njewin 6:43029c69b9ac 83 sync = 0; // zeros synchronization digital output
njewin 2:db3bbd57b075 84 wait(0.6); // debug - hold LED on for 0.6s, even when while loop not true
njewin 1:20201cda90d0 85 logLED = 0; // turns LED3 off when logging ends
njewin 6:43029c69b9ac 86 fclose(fp); // closes log file, otherwise cannot access mbed drive
lhiggs 0:03c649c76388 87
lhiggs 0:03c649c76388 88 } // end main()