A project similar to http://mbed.org/users/lhiggs/code/UM6_IMU_AHRS_2012/, where I'm trying to log data from a UM6 (CH Robotics orientation sensor) and a GPS transceiver to an sd card. I've adapted LHiggs code to include ModGPS. For sum reason a soon as I pick up a gps signal the UM6 data freezes i.e. the time and gps signals continue to print out but the UM6 signals fixes on a single value.
Dependencies: MODGPS MODSERIAL SDFileSystem mbed
main.cpp@10:d96e068f3595, 2013-06-07 (annotated)
- Committer:
- njewin
- Date:
- Fri Jun 07 08:49:59 2013 +0000
- Revision:
- 10:d96e068f3595
- Parent:
- 9:7dcfa24d5e7a
- Child:
- 11:2b2537dcf504
GPS working! printing lat/long/alt/speed/course to pc
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
njewin | 0:52295643d083 | 1 | #include "mbed.h" |
njewin | 4:8dcf0bdc25c8 | 2 | #include "SDFileSystem.h" // SD file system header from handbook/cookbook offical mbed library |
njewin | 4:8dcf0bdc25c8 | 3 | #include "MODSERIAL.h" |
njewin | 5:ac633cdbb75c | 4 | #include "UM6_usart.h" // UM6 USART HEADER |
njewin | 5:ac633cdbb75c | 5 | #include "UM6_config.h" // UM6 CONFIG HEADER |
njewin | 10:d96e068f3595 | 6 | #include "GPS.h" |
njewin | 0:52295643d083 | 7 | |
njewin | 1:9fe40d9ac0f5 | 8 | //------------ system and interface setup ----------------------------// |
njewin | 4:8dcf0bdc25c8 | 9 | MODSERIAL pc(USBTX, USBRX); // sets up serial connection to pc terminal |
njewin | 0:52295643d083 | 10 | |
njewin | 1:9fe40d9ac0f5 | 11 | //------------ Hardware setup ----------------------------------------// |
njewin | 9:7dcfa24d5e7a | 12 | DigitalOut pc_led(LED1); // LED1 = PC SERIAL |
njewin | 9:7dcfa24d5e7a | 13 | DigitalOut uart_led(LED2); // LED2 = UM6 SERIAL |
njewin | 9:7dcfa24d5e7a | 14 | DigitalOut log_led(LED3); // debug LED |
njewin | 9:7dcfa24d5e7a | 15 | DigitalOut debug_led(LED4); // debug LED |
njewin | 9:7dcfa24d5e7a | 16 | DigitalIn enable(p10); // enable logging pin |
njewin | 9:7dcfa24d5e7a | 17 | DigitalOut sync(p11); // sychronization (with CAN logger) pin |
njewin | 3:b3358ec2f57c | 18 | SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board |
njewin | 10:d96e068f3595 | 19 | GPS gps(NC,p27); |
njewin | 3:b3358ec2f57c | 20 | |
njewin | 5:ac633cdbb75c | 21 | // interupt function for processing uart messages --------------------// |
njewin | 5:ac633cdbb75c | 22 | void rxCallback(MODSERIAL_IRQ_INFO *q) { |
njewin | 5:ac633cdbb75c | 23 | if (um6_uart.rxBufferGetCount() >= MAX_PACKET_DATA) { |
njewin | 5:ac633cdbb75c | 24 | uart_led = !uart_led; // Lights LED when uart RxBuff has > 40 bytes |
njewin | 5:ac633cdbb75c | 25 | Process_um6_packet(); |
njewin | 5:ac633cdbb75c | 26 | } |
njewin | 5:ac633cdbb75c | 27 | } |
njewin | 5:ac633cdbb75c | 28 | |
njewin | 1:9fe40d9ac0f5 | 29 | //------------ LogData interrupt function ----------------------------// |
njewin | 9:7dcfa24d5e7a | 30 | int flag=0; |
njewin | 1:9fe40d9ac0f5 | 31 | void LogData() { |
njewin | 9:7dcfa24d5e7a | 32 | flag=1; //interrupt sets flag that causes variables to be logged |
njewin | 1:9fe40d9ac0f5 | 33 | } |
njewin | 1:9fe40d9ac0f5 | 34 | |
njewin | 1:9fe40d9ac0f5 | 35 | //============= Main Program =========================================// |
njewin | 0:52295643d083 | 36 | int main() { |
njewin | 10:d96e068f3595 | 37 | Ticker tick; |
njewin | 10:d96e068f3595 | 38 | Timer t; |
njewin | 10:d96e068f3595 | 39 | GPS_Time q1; |
njewin | 10:d96e068f3595 | 40 | GPS_VTG v1; |
njewin | 10:d96e068f3595 | 41 | |
njewin | 9:7dcfa24d5e7a | 42 | pc.baud(115200); // baud rate to pc interface |
njewin | 5:ac633cdbb75c | 43 | um6_uart.baud(115200); // baud rate to um6 interface |
njewin | 10:d96e068f3595 | 44 | gps.baud(57600); |
njewin | 10:d96e068f3595 | 45 | gps.format(8, GPS::None, 1); |
njewin | 9:7dcfa24d5e7a | 46 | |
njewin | 9:7dcfa24d5e7a | 47 | //---- call interrupt functions --------------------------// |
njewin | 5:ac633cdbb75c | 48 | um6_uart.attach(&rxCallback, MODSERIAL::RxIrq); // attach interupt function to uart |
njewin | 10:d96e068f3595 | 49 | tick.attach(&LogData, 2); // attaches LogData function to 'tick' ticker interrupt every 0.5s |
njewin | 9:7dcfa24d5e7a | 50 | t.start(); // start logging time |
njewin | 9:7dcfa24d5e7a | 51 | |
njewin | 9:7dcfa24d5e7a | 52 | //---------- setup sd card directory------------------------------// |
njewin | 9:7dcfa24d5e7a | 53 | int FileNo = 0; |
njewin | 9:7dcfa24d5e7a | 54 | mkdir("/sd/log_data", 0777); |
njewin | 1:9fe40d9ac0f5 | 55 | |
njewin | 9:7dcfa24d5e7a | 56 | //---- main while loop -------------------------------------------// |
njewin | 9:7dcfa24d5e7a | 57 | while(1) { |
njewin | 9:7dcfa24d5e7a | 58 | //---- Setup file on SD card ----------------------------// |
njewin | 9:7dcfa24d5e7a | 59 | printf("Opening sd card\n"); |
njewin | 9:7dcfa24d5e7a | 60 | char buffer[50]; |
njewin | 9:7dcfa24d5e7a | 61 | sprintf(buffer, "/sd/log_data/%i.csv", FileNo); |
njewin | 9:7dcfa24d5e7a | 62 | //**************************************************** |
njewin | 9:7dcfa24d5e7a | 63 | // This is necessary for card to work when reconnected |
njewin | 9:7dcfa24d5e7a | 64 | // Initialise disk |
njewin | 9:7dcfa24d5e7a | 65 | sd.disk_initialize(); |
njewin | 9:7dcfa24d5e7a | 66 | //**************************************************** |
njewin | 9:7dcfa24d5e7a | 67 | // Open a file for write |
njewin | 9:7dcfa24d5e7a | 68 | FILE *fp = fopen(buffer, "w"); |
njewin | 9:7dcfa24d5e7a | 69 | if(fp == NULL) { |
njewin | 9:7dcfa24d5e7a | 70 | error("Could not open file for write\n"); |
njewin | 3:b3358ec2f57c | 71 | } |
njewin | 9:7dcfa24d5e7a | 72 | //--- print TEST signals to file, header |
njewin | 9:7dcfa24d5e7a | 73 | //fprintf(fp,"time(s),Yaw(deg),Accel(m/s2),GPS Speed(m/s) \r"); |
njewin | 9:7dcfa24d5e7a | 74 | //--- print ALL signals to file, header |
njewin | 9:7dcfa24d5e7a | 75 | fprintf(fp, "time(s),Yaw(deg),Roll(deg),Pitch(deg),GyroX(deg/s),GyroY(deg/s),GyroZ(deg/s),AccelX(g),AccelY(g),AccelZ(g),GPScourse(deg),GPSspeed(m/s),Latitude(deg),Longitude(deg) \r"); // sends header to file |
njewin | 9:7dcfa24d5e7a | 76 | |
njewin | 9:7dcfa24d5e7a | 77 | while (!pc.readable()) { |
njewin | 9:7dcfa24d5e7a | 78 | if(flag==1) { |
njewin | 9:7dcfa24d5e7a | 79 | log_led=1; // turns on LED3 to indicate logging |
njewin | 9:7dcfa24d5e7a | 80 | float time=t.read(); |
njewin | 9:7dcfa24d5e7a | 81 | float Yaw=data.Yaw; |
njewin | 9:7dcfa24d5e7a | 82 | float Roll=data.Roll; |
njewin | 9:7dcfa24d5e7a | 83 | float Pitch=data.Pitch; |
njewin | 9:7dcfa24d5e7a | 84 | float GyroX=data.Gyro_Proc_X; |
njewin | 9:7dcfa24d5e7a | 85 | float GyroY=data.Gyro_Proc_Y; |
njewin | 9:7dcfa24d5e7a | 86 | float GyroZ=data.Gyro_Proc_Z; |
njewin | 9:7dcfa24d5e7a | 87 | float AccelX=data.Accel_Proc_X; |
njewin | 9:7dcfa24d5e7a | 88 | float AccelY=data.Accel_Proc_Y; |
njewin | 9:7dcfa24d5e7a | 89 | float AccelZ=data.Accel_Proc_Z; |
njewin | 10:d96e068f3595 | 90 | double GPSlong=gps.longitude(); // currently not reading GPS longitude correctly |
njewin | 10:d96e068f3595 | 91 | double GPSlat=gps.latitude(); // currently not reading GPS latitude correctly |
njewin | 10:d96e068f3595 | 92 | double GPSalt=gps.altitude(); |
njewin | 9:7dcfa24d5e7a | 93 | float GPScourse=data.GPS_course; |
njewin | 9:7dcfa24d5e7a | 94 | float GPSspeed=data.GPS_speed; |
njewin | 10:d96e068f3595 | 95 | |
njewin | 9:7dcfa24d5e7a | 96 | //----- print TEST signals----------------------------// |
njewin | 9:7dcfa24d5e7a | 97 | // fprintf(fp,"%.3f,%.2f,%.4f,%.2f \r",time,Yaw,AccelX,GPSspeed); |
njewin | 10:d96e068f3595 | 98 | pc.printf("time %.3f,Yaw %f, Lat %f, Long %f, Alt %f \n",time,Yaw,GPSlat,GPSlong,GPSalt); |
njewin | 10:d96e068f3595 | 99 | gps.timeNow(&q1); |
njewin | 10:d96e068f3595 | 100 | pc.printf("%c %02d:%02d:%02d %02d/%02d/%04d\r\n",q1.status, q1.hour, q1.minute, q1.second, q1.day, q1.month, q1.year); |
njewin | 10:d96e068f3595 | 101 | gps.vtg(&v1); |
njewin | 10:d96e068f3595 | 102 | pc.printf("Speed(kph):%lf,Track(true):%lf\r\n", v1.velocity_kph(), v1.track_true()); |
njewin | 10:d96e068f3595 | 103 | |
njewin | 9:7dcfa24d5e7a | 104 | //pc.printf("time %.3f,Yaw %f,Accel %f, Speed %f \n",time,Yaw,AccelX,GPSspeed); |
njewin | 9:7dcfa24d5e7a | 105 | // pc.printf("time %f,LongIn 0x%08X, LongOut %f\n",time,GPSlong,*(int *)&GPSlong); |
njewin | 9:7dcfa24d5e7a | 106 | |
njewin | 9:7dcfa24d5e7a | 107 | //----- print ALL signals to file --------------------// |
njewin | 10:d96e068f3595 | 108 | // fprintf(fp, "%3.3f, %3.1f,%3.1f,%3.1f, %3.2f,%3.2f,%3.2f, %3.4f,%3.4f,%3.4f, %3.1f,%3.2f,%d,%f\r",time,Yaw,Roll,Pitch,GyroX,GyroY,GyroZ,AccelX,AccelY,AccelZ,GPScourse,GPSspeed,GPSlat,GPSlong); |
njewin | 9:7dcfa24d5e7a | 109 | flag=0; // reset LogData interrupt flag |
njewin | 9:7dcfa24d5e7a | 110 | pc_led = !pc_led; // Lights LED1 when transmitting to PC screen |
njewin | 9:7dcfa24d5e7a | 111 | } // end if(flag=1) loop |
njewin | 9:7dcfa24d5e7a | 112 | |
njewin | 9:7dcfa24d5e7a | 113 | if(enable==0) { |
njewin | 9:7dcfa24d5e7a | 114 | break; // breaks while loop if enable switched off |
njewin | 9:7dcfa24d5e7a | 115 | } |
njewin | 9:7dcfa24d5e7a | 116 | } // end while(!pc.readable()) loop |
njewin | 9:7dcfa24d5e7a | 117 | pc.printf(" Done. \n"); // prints 'done when logging is finished/enable switched off |
njewin | 9:7dcfa24d5e7a | 118 | log_led=0; // turns off LED logging is finished/enable switched off |
njewin | 9:7dcfa24d5e7a | 119 | wait(0.5); // debug wait for pc.printf |
njewin | 9:7dcfa24d5e7a | 120 | fclose(fp); // closes log file |
njewin | 9:7dcfa24d5e7a | 121 | pc.getc(); // Clear character from buffer |
njewin | 9:7dcfa24d5e7a | 122 | |
njewin | 9:7dcfa24d5e7a | 123 | while (!pc.readable()) { // Wait for a key press to restart logging |
njewin | 9:7dcfa24d5e7a | 124 | }; |
njewin | 9:7dcfa24d5e7a | 125 | pc.getc(); // Clear character from buffer |
njewin | 9:7dcfa24d5e7a | 126 | FileNo++; // Increment file number |
njewin | 3:b3358ec2f57c | 127 | } // end while(1) loop |
njewin | 9:7dcfa24d5e7a | 128 | |
njewin | 5:ac633cdbb75c | 129 | } // end main() program |
njewin | 5:ac633cdbb75c | 130 | |
njewin | 5:ac633cdbb75c | 131 | |
njewin | 9:7dcfa24d5e7a | 132 | /* DEUBBING NOTES |
njewin | 9:7dcfa24d5e7a | 133 | opening a file BEFORE calling interrupts OK |
njewin | 5:ac633cdbb75c | 134 | opening a file and print to it BEFORE calling interrupts NOT OK (stops rest of program) |
njewin | 5:ac633cdbb75c | 135 | open a (local) file and print to it AFTER calling interrupts NOT OK (stops rest of program) |
njewin | 5:ac633cdbb75c | 136 | open a (sd) file and print to it AFTER calling interrupts OK |
njewin | 5:ac633cdbb75c | 137 | */ |