Nathan Ewin
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.
Fork of
UM6_IMU_AHRS_2012
by 
lhiggs CSUM
Revision 3:0cfe2e18440d, committed 2013-05-02
- Comitter:
- njewin
- Date:
- Thu May 02 06:41:42 2013 +0000
- Parent:
- 2:db3bbd57b075
- Child:
- 4:aefc0f09fe1c
- Commit message:
- GPS setup but not printing
Changed in this revision
| UM6_config/UM6_config.h | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/UM6_config/UM6_config.h Wed May 01 23:32:16 2013 +0000
+++ b/UM6_config/UM6_config.h Thu May 02 06:41:42 2013 +0000
@@ -31,14 +31,17 @@
// 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 35
+#define DATA_ARRAY_SIZE 36
#define COMMAND_COUNT 9
+// original data array size 32
//
#define CONFIG_REG_START_ADDRESS 0
-#define DATA_REG_START_ADDRESS 85
+#define DATA_REG_START_ADDRESS 85
#define COMMAND_START_ADDRESS 170
+// hex 0x55 = dec 85
+// hex 0xAA = dec 170
// These preprocessor definitions make it easier to access specific configuration parameters in code
// They specify array locations associated with each register name. Note that in the comments below, many of the values are
// said to be 32-bit IEEE floating point. Obviously this isn't directly the case, since the arrays are actually 32-bit unsigned
@@ -84,9 +87,9 @@
#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_TEMPERATURE (DATA_REG_START_ADDRESS + 33)
#define UM6_GPS_LONGITUDE (DATA_REG_START_ADDRESS + 34)
-
+#define UM6_GPS_LATITUDE (DATA_REG_START_ADDRESS + 35)
+#define UM6_GPS_COURSE_SPEED (DATA_REG_START_ADDRESS + 40)
@@ -136,6 +139,9 @@
float Pitch;
float Yaw;
float GPS_long;
+float GPS_lat;
+float GPS_course;
+float GPS_speed;
};
UM6 data;
@@ -156,8 +162,10 @@
int16_t MY_DATA_EULER_PHI;
int16_t MY_DATA_EULER_THETA;
int16_t MY_DATA_EULER_PSI;
-
-int16_t MY_DATA_GPS_LONG;
+int32_t MY_DATA_GPS_LONG;
+int32_t MY_DATA_GPS_LAT;
+int16_t MY_DATA_GPS_COURSE;
+int16_t MY_DATA_GPS_SPEED;
@@ -443,40 +451,36 @@
}
+
//-------------------------------------------------------------------
- // GPS
- if (new_packet.address == UM6_GPS_LONGITUDE) {
- // GPS Longitude
- MY_DATA_GPS_LONG = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
- MY_DATA_GPS_LONG |= new_packet.packet_data[1];
- data.GPS_long = MY_DATA_GPS_LONG; // stores longitude in degrees
-
-
-
-
+ // GPS Ground/Speed
+ if (new_packet.address == UM6_GPS_COURSE_SPEED) {
+ // Ground course
+ MY_DATA_GPS_COURSE = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
+ MY_DATA_GPS_COURSE |= new_packet.packet_data[1];
+ data.GPS_course = MY_DATA_GPS_COURSE; // need to divide by 100 to get ground course in degrees
- // EULER_THETA (PITCH)
- // MY_DATA_EULER_THETA = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
- // MY_DATA_EULER_THETA |= new_packet.packet_data[3];
- // data.Pitch = MY_DATA_EULER_THETA*0.0109863;
+ // Speed
+ MY_DATA_GPS_SPEED = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
+ MY_DATA_GPS_SPEED |= new_packet.packet_data[3];
+ data.GPS_speed = MY_DATA_GPS_SPEED;
- //------------------------------------------------------------
-
- //------------------------------------------------------------
- // UM6_EULER_PSI (0x63) (YAW)
- // Stores the most recently computed yaw (psi) angle estimate. The angle estimate is stored as a 16-
- // bit 2's complement integer. To obtain the actual angle estimate in degrees, the register data
- // should be multiplied by the scale factor 0.0109863 as shown below
-
- // MY_DATA_EULER_PSI = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
- // MY_DATA_EULER_PSI |= new_packet.packet_data[5];
- // data.Yaw = MY_DATA_EULER_PSI*0.0109863;
-
-
+ //------------------------------------------------------------
}
- // end if(UM6_EULER_PHI_THETA)
+ //-------------------------------------------------------------------
+ // GPS long
+ // if (new_packet.address == UM6_GPS_LONGITUDE) {
+ // Longitude
+ // data.GPS_long = MY_DATA_GPS_LONG;
+ // }
//------------------------------------------------------------
-
+ //-------------------------------------------------------------------
+ // GPS lat
+ // if (new_packet.address == UM6_GPS_LATITUDE) {
+ // Latitude
+ // data.GPS_lat = MY_DATA_GPS_LAT;
+ // }
+ //------------------------------------------------------------
} // end if(ADDRESS_TYPE_DATA)
--- a/main.cpp Wed May 01 23:32:16 2013 +0000
+++ b/main.cpp Thu May 02 06:41:42 2013 +0000
@@ -17,14 +17,11 @@
DigitalOut pc_activity(LED1); // LED1 = PC SERIAL
DigitalOut uart_activity(LED2); // LED2 = UM6 SERIAL
DigitalOut logLED(LED3); // LED3 = logging active
-
+DigitalOut sync(p6);
DigitalIn enable(p5); // enable signal for logging data to file
-Timer t; // sets up timer for measuring data loggin time
-Timer t1; // sets up timer for reading data at set intervals
-//Ticker tick; // sets up ticker
-
-//int doRead = 0; // ticker interupt variable
+Timer t; // sets up timer for measuring data time stamp
+Timer t1;// sets up timer for measuring time to read/write data
void rxCallback(MODSERIAL_IRQ_INFO *q) {
if (um6_uart.rxBufferGetCount() >= MAX_PACKET_DATA) {
@@ -33,9 +30,6 @@
}
}
-//void print_um6() {
-// doRead = 1; // interupt to read signals from UM6
-// }
int main() {
@@ -47,23 +41,19 @@
// set UM6 serial uart baud 9600
um6_uart.baud(115200);
pc.baud(115200); // pc baud for UM6 to pc interface
- t.start(); // starts timer
// attach interupt function to uart
um6_uart.attach(&rxCallback, MODSERIAL::RxIrq);
-// tick.attach(&print_um6, 0.20); // ticker interupt
FILE *fp = fopen("/local/out3.csv", "w"); // Open "out.txt" on the local file system for writing
- fprintf(fp, "time1 (s),Yaw (deg),Roll (deg),Pitch (deg),GyroX,GyroY,GyroZ,AccelX,AccelY,AccelZ\r"); // sends header to file
-
-int n = 0; // while loop counter
-
- while (n < 10) {
+ fprintf(fp, "time (s),Yaw (deg),Roll (deg),Pitch (deg),GyroX,GyroY,GyroZ,AccelX,AccelY,AccelZ\r"); // sends header to file
+
+ t.start(); // start data log time
+ sync = 1;
+ while (enable) {
logLED = 1; // turns LED3 on when logging starts
- t1.start();
- wait(0.5);
- if (t1 > 0.5) {
+ wait(0.5);
float time1=t.read();
float Yaw=data.Yaw;
float Roll=data.Roll;
@@ -78,19 +68,21 @@
// float MagY=data.Mag_Proc_Y;
// float MagZ=data.Mag_Proc_Z;
float GPSlong=data.GPS_long;
+ float GPSlat=data.GPS_lat;
+ float GPScourse=data.GPS_course;
+ float GPSspeed=data.GPS_speed;
- pc.printf("time1 %3.3f s,Yaw %3.3f deg,Roll %3.3f deg,Pitch %3.3f deg\n",time1,Yaw,Roll,Pitch);
- //pc.printf("time1 %3.3f s,Yaw %3.3f deg,Rol l %3.3f deg,Pitch %3.3f deg, Longitude %f deg\n",time1,Yaw,Roll,Pitch,GPSlong);
- fprintf(fp, "%3.3f,%3.3f,%3.3f,%3.3f\n",time1,Yaw,Roll,Pitch);
+ // pc.printf("time1 %3.3f s,Yaw %3.3f deg,Roll %3.3f deg,Pitch %3.3f deg",time1,Yaw,Roll,Pitch);
+ pc.printf("time1 %3.3f s,Yaw %3.3f deg, Speed %f 0.01m/s,course %f 0.01m/s, long %f deg, lat %f deg\n",time1,Yaw,GPSspeed,GPScourse,GPSlong,GPSlat);
+ // fprintf(fp, "%3.3f,%3.3f,%3.3f,%3.3f \n",time1,Yaw,Roll,Pitch);
// fprintf(fp, "%3.3f,%3.3f,%3.3f,%3.3f,%3.3f,%3.3f,%3.3f\n",time1,Yaw,Roll,Pitch,GyroX,GyroY,GyroZ);
// fprintf(fp, "%3.3f,%3.3f,%3.3f,%3.3f,%3.3f,%3.3f,%3.3f,%3.3f,%3.3f,%3.3f\n",time1,Yaw,Roll,Pitch,GyroX,GyroY,GyroZ,AccelX,AccelY,AccelZ);
- pc_activity = !pc_activity; // Lights LED1 when uart RxBuff has > 40 bytes
- // doRead = 0; // reset ticker interupt variable
- t1.reset();
- n++;
- }
- } // end while(1) loopn
+ pc_activity = !pc_activity; // Lights LED1 when uart RxBuff has > 40 bytes
+
+ } // end while(1) loop
+ fprintf(fp,"%3.3f \n",t.read());
+ sync = 0;
wait(0.6); // debug - hold LED on for 0.6s, even when while loop not true
logLED = 0; // turns LED3 off when logging ends
fclose(fp);