Nathan Ewin
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
Revision 12:894e648638e4, committed 2013-07-08
- Comitter:
- njewin
- Date:
- Mon Jul 08 16:28:57 2013 +0000
- Parent:
- 11:2b2537dcf504
- Child:
- 13:0782664b5fcb
- Commit message:
- gps working. UM6 data signal freeze when gps signals are received.
Changed in this revision
--- a/MODSERIAL.lib Mon Jun 24 20:37:05 2013 +0000 +++ b/MODSERIAL.lib Mon Jul 08 16:28:57 2013 +0000 @@ -1,1 +1,1 @@ -https://mbed.org/users/AjK/code/MODSERIAL/#76d474800ca5 +http://mbed.org/users/njewin/code/LogData_UM6-to-SDcard/#76d474800ca5
--- a/UM6_config/UM6_config.h Mon Jun 24 20:37:05 2013 +0000
+++ b/UM6_config/UM6_config.h Mon Jul 08 16:28:57 2013 +0000
@@ -35,7 +35,7 @@
// This setup makes it easy to make more data immediately available when needed - simply increase the array size, add code in
// the firmware that writes data to the new array location, and then make updates to the firmware definition on the PC side.
#define CONFIG_ARRAY_SIZE 44
-#define DATA_ARRAY_SIZE 37
+#define DATA_ARRAY_SIZE 13
#define COMMAND_COUNT 9
// original data array size 32
@@ -63,18 +63,19 @@
#define UM6_GYRO_RAW_Z (DATA_REG_START_ADDRESS + 2)
#define UM6_ACCEL_RAW_XY (DATA_REG_START_ADDRESS + 3) // Raw accel data is stored in 16-bit signed integers
#define UM6_ACCEL_RAW_Z (DATA_REG_START_ADDRESS + 4)
-#define UM6_MAG_RAW_XY (DATA_REG_START_ADDRESS + 5) // Raw mag data is stored in 16-bit signed integers
-#define UM6_MAG_RAW_Z (DATA_REG_START_ADDRESS + 6)
+//#define UM6_MAG_RAW_XY (DATA_REG_START_ADDRESS + 5) // Raw mag data is stored in 16-bit signed integers
+//#define UM6_MAG_RAW_Z (DATA_REG_START_ADDRESS + 6)
#define UM6_GYRO_PROC_XY (DATA_REG_START_ADDRESS + 7) // Processed gyro data has scale factors applied and alignment correction performed. Data is 16-bit signed integer.
#define UM6_GYRO_PROC_Z (DATA_REG_START_ADDRESS + 8)
#define UM6_ACCEL_PROC_XY (DATA_REG_START_ADDRESS + 9) // Processed accel data has scale factors applied and alignment correction performed. Data is 16-bit signed integer.
#define UM6_ACCEL_PROC_Z (DATA_REG_START_ADDRESS + 10)
-#define UM6_MAG_PROC_XY (DATA_REG_START_ADDRESS + 11) // Processed mag data has scale factors applied and alignment correction performed. Data is 16-bit signed integer.
-#define UM6_MAG_PROC_Z (DATA_REG_START_ADDRESS + 12)
+//#define UM6_MAG_PROC_XY (DATA_REG_START_ADDRESS + 11) // Processed mag data has scale factors applied and alignment correction performed. Data is 16-bit signed integer.
+//#define UM6_MAG_PROC_Z (DATA_REG_START_ADDRESS + 12)
#define UM6_EULER_PHI_THETA (DATA_REG_START_ADDRESS + 13) // Euler angles are 32-bit IEEE floating point
#define UM6_EULER_PSI (DATA_REG_START_ADDRESS + 14)
#define UM6_QUAT_AB (DATA_REG_START_ADDRESS + 15) // Quaternions are 16-bit signed integers.
#define UM6_QUAT_CD (DATA_REG_START_ADDRESS + 16)
+/*
#define UM6_ERROR_COV_00 (DATA_REG_START_ADDRESS + 17) // Error covariance is a 4x4 matrix of 32-bit IEEE floating point values
#define UM6_ERROR_COV_01 (DATA_REG_START_ADDRESS + 18)
#define UM6_ERROR_COV_02 (DATA_REG_START_ADDRESS + 19)
@@ -91,10 +92,12 @@
#define UM6_ERROR_COV_31 (DATA_REG_START_ADDRESS + 30)
#define UM6_ERROR_COV_32 (DATA_REG_START_ADDRESS + 31)
#define UM6_ERROR_COV_33 (DATA_REG_START_ADDRESS + 32)
-#define UM6_GPS_LONGITUDE (DATA_REG_START_ADDRESS + 34)
-#define UM6_GPS_LATITUDE (DATA_REG_START_ADDRESS + 35)
-#define UM6_GPS_ALTITUDE (DATA_REG_START_ADDRESS + 36)
-#define UM6_GPS_COURSE_SPEED (DATA_REG_START_ADDRESS + 40)
+*/
+// connected gps module directing to mbed using MODGPS
+//#define UM6_GPS_LONGITUDE (DATA_REG_START_ADDRESS + 34)
+//#define UM6_GPS_LATITUDE (DATA_REG_START_ADDRESS + 35)
+//#define UM6_GPS_ALTITUDE (DATA_REG_START_ADDRESS + 36)
+//#define UM6_GPS_COURSE_SPEED (DATA_REG_START_ADDRESS + 40)
@@ -137,17 +140,17 @@
float Accel_Proc_X;
float Accel_Proc_Y;
float Accel_Proc_Z;
-float Mag_Proc_X;
-float Mag_Proc_Y;
-float Mag_Proc_Z;
+//float Mag_Proc_X;
+//float Mag_Proc_Y;
+//float Mag_Proc_Z;
float Roll;
float Pitch;
float Yaw;
-float GPS_long;
-float GPS_lat;
-float GPS_alt;
-float GPS_course;
-float GPS_speed;
+//float GPS_long;
+//float GPS_lat;
+//float GPS_alt;
+//float GPS_course;
+//float GPS_speed;
};
UM6 data;
@@ -163,12 +166,13 @@
int16_t MY_DATA_ACCEL_PROC_X;
int16_t MY_DATA_ACCEL_PROC_Y;
int16_t MY_DATA_ACCEL_PROC_Z;
-int16_t MY_DATA_MAG_PROC_X;
-int16_t MY_DATA_MAG_PROC_Y;
-int16_t MY_DATA_MAG_PROC_Z;
+//int16_t MY_DATA_MAG_PROC_X;
+//int16_t MY_DATA_MAG_PROC_Y;
+//int16_t MY_DATA_MAG_PROC_Z;
int16_t MY_DATA_EULER_PHI;
int16_t MY_DATA_EULER_THETA;
int16_t MY_DATA_EULER_PSI;
+/*
int32_t MY_DATA_GPS_LONG;
int32_t MY_DATA_GPS_LONG0;
int32_t MY_DATA_GPS_LONG1;
@@ -183,7 +187,7 @@
int32_t MY_DATA_GPS_ALT2;
int16_t MY_DATA_GPS_COURSE;
int16_t MY_DATA_GPS_SPEED;
-
+*/
static uint8_t data_counter = 0;
@@ -400,6 +404,7 @@
// calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
// shown below.
// magnetic field = register_data* 0.000305176
+ /*
if (new_packet.address == UM6_MAG_PROC_XY) {
// MAG_PROC_X
@@ -429,6 +434,7 @@
data.Mag_Proc_Z = MY_DATA_MAG_PROC_Z*0.000305176;
} // end if(UM6_MAG_PROC)
+ */
//------------------------------------------------------------
@@ -468,7 +474,7 @@
}
-
+ /*
//-------------------------------------------------------------------
// GPS Ground/Speed
if (new_packet.address == UM6_GPS_COURSE_SPEED) {
@@ -498,7 +504,7 @@
//------------------------------------------------------------
//-------------------------------------------------------------------
// GPS lat
- /* if (new_packet.address == UM6_GPS_LATITUDE) {
+ if (new_packet.address == UM6_GPS_LATITUDE) {
// Latitude
//MY_DATA_GPS_LAT0 = (int32_t)new_packet.packet_data[0]<<24;
//MY_DATA_GPS_LAT1 = (int32_t)new_packet.packet_data[1]<<16;
@@ -544,26 +550,3 @@
#endif
-/* debugging GPS lat & long
- // code snippets and print out
-
-//code
-double GPS_long;
-double MY_DATA_GPS_LONG;
-MY_DATA_GPS_LONG = (int32_t)new_packet.packet_data;
-data.GPS_long = MY_DATA_GPS_LAT;
-//print out
-Long 0.000000
-Long -2.000001
-Long -2.000001
-Long -26815635079454453000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000
-
-//code
-int32_t GPS_long;
-int32_t MY_DATA_GPS_LONG;
-MY_DATA_GPS_LONG = (int32_t)new_packet.packet_data;
-data.GPS_long = MY_DATA_GPS_LAT;
-//print out
-Long nan
-Long 0.000000
-*/
\ No newline at end of file
--- a/main.cpp Mon Jun 24 20:37:05 2013 +0000
+++ b/main.cpp Mon Jul 08 16:28:57 2013 +0000
@@ -14,49 +14,58 @@
DigitalOut log_led(LED3); // debug LED
DigitalOut debug_led(LED4); // debug LED
DigitalIn enable(p10); // enable logging pin
-DigitalOut sync(p11); // sychronization (with CAN logger) pin
+DigitalOut sync(p17); // sychronization (with CAN logger) pin
SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
GPS gps(NC,p27);
+Timer t_debug;
+int counter;
// interupt function for processing uart messages --------------------//
+int flag_uart=0;
+float time_debug;
void rxCallback(MODSERIAL_IRQ_INFO *q) {
- if (um6_uart.rxBufferGetCount() >= MAX_PACKET_DATA) {
- uart_led = !uart_led; // Lights LED when uart RxBuff has > 40 bytes
+ if (um6_uart.rxBufferGetCount() >= MAX_PACKET_DATA) {
+ uart_led = 1; // Lights LED when uart RxBuff has > 40 bytes
+ t_debug.start();
Process_um6_packet();
- }
+ t_debug.stop();
+ time_debug=t_debug.read();
+ t_debug.reset();
+ uart_led = 0;
+ counter++;
+ }
}
//------------ LogData interrupt function ----------------------------//
-int flag=0;
+int flag_log=0;
void LogData() {
- flag=1; //interrupt sets flag that causes variables to be logged
+ flag_log=1; //interrupt sets flag that causes variables to be logged
}
//------------ Pc print interrupt function ----------------------------//
// to print to PC a a different rate to logging
-int flag1=0;
+int flag_pc=0;
void pcData() {
- flag1=1; //interrupt sets flag that causes variables to be logged
+ flag_pc=1; //interrupt sets flag that causes variables to be logged
}
//============= Main Program =========================================//
int main() {
Ticker tick;
Ticker tick1;
- Timer t;
+ Timer t;
+ sync = 0;
GPS_Time q1;
GPS_VTG v1;
pc.baud(115200); // baud rate to pc interface
um6_uart.baud(115200); // baud rate to um6 interface
- gps.baud(57600);
+ gps.baud(115200);
gps.format(8, GPS::None, 1);
//---- call interrupt functions --------------------------//
um6_uart.attach(&rxCallback, MODSERIAL::RxIrq); // attach interupt function to uart
- tick.attach(&LogData, 0.1); // attaches LogData function to 'tick' ticker interrupt every 0.5s
- tick1.attach(&pcData, 1); // attaches LogData function to 'tick' ticker interrupt every 0.5s
-
- t.start(); // start logging time
+ tick.attach(&LogData, 1); // attaches LogData function to 'tick' ticker interrupt every 0.5s
+ tick1.attach(&pcData, 0.5); // attaches LogData function to 'tick' ticker interrupt every 0.5s
//---------- setup sd card directory------------------------------//
int FileNo = 0;
@@ -78,14 +87,21 @@
if(fp == NULL) {
error("Could not open file for write\n");
}
- //--- print TEST signals to file, header
- //fprintf(fp,"time(s),Yaw(deg),Accel(m/s2),GPS Speed(m/s) \r");
- //--- print ALL signals to file, header
+ //--- print ALL signals headers to sd card
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(km/h),Latitude(deg),Longitude(deg) \r"); // sends header to file
- while (!pc.readable()) {
- if(flag==1) {
- log_led=1; // turns on LED3 to indicate logging
+ // start time
+ gps.timeNow(&q1);
+ 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);
+ // printf("%c %02d:%02d:%02d %02d/%02d/%04d\r\n",
+ // q1.status, q1.hour, q1.minute, q1.second, q1.day, q1.month, q1.year);
+ t.start(); // start logging timer
+ sync = 1; // sync step sent to VN8900
+
+ while (!pc.readable() && enable) {
+ if(flag_log==1) {
+ log_led= !log_led; // turns on LED3 to indicate logging
float time=t.read();
float Yaw=data.Yaw;
float Roll=data.Roll;
@@ -96,44 +112,52 @@
float AccelX=data.Accel_Proc_X;
float AccelY=data.Accel_Proc_Y;
float AccelZ=data.Accel_Proc_Z;
- double GPSlong=gps.longitude(); // currently not reading GPS longitude correctly
+ /* double GPSlong=gps.longitude(); // currently not reading GPS longitude correctly
double GPSlat=gps.latitude(); // currently not reading GPS latitude correctly
double GPSalt=gps.altitude();
gps.vtg(&v1);
float GPScourse=v1.track_true();
float GPSspeed=v1.velocity_kph();
-
- //----- print TEST signals----------------------------//
- // fprintf(fp,"%.3f,%.2f,%.4f,%.2f \r",time,Yaw,AccelX,GPSspeed);
- if(flag1==1) {
- pc.printf("time %.3f,Yaw %f, Lat %f, Long %f, Alt %f,Speed(kph):%lf,Track(true):%lf \n",
- time,Yaw,GPSlat,GPSlong,GPSalt,GPSspeed,GPScourse);
- gps.timeNow(&q1);
- 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);
- flag1=0;
- }
- //pc.printf("time %.3f,Yaw %f,Accel %f, Speed %f \n",time,Yaw,AccelX,GPSspeed);
- // pc.printf("time %f,LongIn 0x%08X, LongOut %f\n",time,GPSlong,*(int *)&GPSlong);
-
+ */
+
//----- print ALL signals to file --------------------//
- 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, %.4f,%.4f,%.4f\r",
- time, Yaw,Roll,Pitch, GyroX,GyroY,GyroZ, AccelX,AccelY,AccelZ, GPScourse,GPSspeed, GPSlat,GPSlong,GPSalt);
- flag=0; // reset LogData interrupt flag
- pc_led = !pc_led; // Lights LED1 when transmitting to PC screen
+ // 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, %f,%f,%f\r",
+ // time, Yaw,Roll,Pitch, GyroX,GyroY,GyroZ, AccelX,AccelY,AccelZ, GPScourse,GPSspeed, GPSlat,GPSlong,GPSalt);
+ flag_log=0; // reset LogData interrupt flag
} // end if(flag=1) loop
-
- if(enable==0) {
- break; // breaks while loop if enable switched off
- }
+
+ if(flag_pc==1) {
+ float time=t.read();
+ float Yaw=data.Yaw;
+ float AccelX=data.Accel_Proc_X;
+ float AccelY=data.Accel_Proc_Y;
+ float AccelZ=data.Accel_Proc_Z;
+ double GPSlong=gps.longitude(); // currently not reading GPS longitude correctly
+ double GPSlat=gps.latitude(); // currently not reading GPS latitude correctly
+ gps.vtg(&v1);
+ float GPScourse=v1.track_true();
+ float GPSspeed=v1.velocity_kph();
+
+ pc.printf("time %.3f,Yaw %3.0f,Accel %.2f, Lat %f, Long %f, kph %.lf \n",
+ time,Yaw,AccelX,GPSlat,GPSlong,GPSspeed);
+ pc.printf("debug time %f, counter %d \n", time_debug, counter);
+ counter = 0;
+ // pc.printf("time %.3f,Yaw %3.0f,Accel %.2f \n",
+ // time,Yaw,AccelX);
+ flag_pc=0;
+ pc_led = !pc_led; // Lights LED1 when transmitting to PC screen
+ }
+
} // end while(!pc.readable()) loop
+ sync = 0; // sync step end sent to VN900
+ fprintf(fp,"sync time %f \n",t.read());
pc.printf(" Done. \n"); // prints 'done when logging is finished/enable switched off
log_led=0; // turns off LED logging is finished/enable switched off
wait(0.5); // debug wait for pc.printf
fclose(fp); // closes log file
- pc.getc(); // Clear character from buffer
+ pc.getc(); // Clear character from buffer
- while (!pc.readable()) { // Wait for a key press to restart logging
+ while (!pc.readable() && !enable) { // Wait for a key press to restart logging
};
pc.getc(); // Clear character from buffer
FileNo++; // Increment file number
@@ -143,8 +167,17 @@
/* DEUBBING NOTES
+-----------------------------------------------------------------------------------------------
opening a file BEFORE calling interrupts OK
opening a file and print to it BEFORE calling interrupts NOT OK (stops rest of program)
open a (local) file and print to it AFTER calling interrupts NOT OK (stops rest of program)
open a (sd) file and print to it AFTER calling interrupts OK
+-----------------------------------------------------------------------------------------------
+measuring processor time for code
+ defining all of data variables including gps speed and course = 8us
+ printing all data to sd card = 250us - upto 25ms periodically
+ printing select data to pc = 400-500us
+
+-----------------------------------------------------------------------------------------------
+
*/
\ No newline at end of file