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 1:20201cda90d0, committed 2013-05-01
- Comitter:
- njewin
- Date:
- Wed May 01 19:06:43 2013 +0000
- Parent:
- 0:03c649c76388
- Child:
- 2:db3bbd57b075
- Commit message:
- working_logUM6data_toFile;
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 Fri Sep 28 00:40:29 2012 +0000
+++ b/UM6_config/UM6_config.h Wed May 01 19:06:43 2013 +0000
@@ -31,7 +31,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 33
+#define DATA_ARRAY_SIZE 35
#define COMMAND_COUNT 9
//
@@ -48,7 +48,8 @@
// Now for data register locations.
-// In the communication protocol, data registers are labeled with number ranging from 128 to 255. The value of 128 will be subtracted from these numbers
+// In the communication protocol, data registers are labeled with number ranging from 128 to 255.
+// The value of 128 will be subtracted from these numbers
// to produce the actual array index labeled below
#define UM6_STATUS DATA_REG_START_ADDRESS // Status register defines error codes with individual bits
#define UM6_GYRO_RAW_XY (DATA_REG_START_ADDRESS + 1) // Raw gyro data is stored in 16-bit signed integers
@@ -83,6 +84,8 @@
#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)
@@ -132,6 +135,7 @@
float Roll;
float Pitch;
float Yaw;
+float GPS_long;
};
UM6 data;
@@ -153,6 +157,8 @@
int16_t MY_DATA_EULER_THETA;
int16_t MY_DATA_EULER_PSI;
+int16_t MY_DATA_GPS_LONG;
+
static uint8_t data_counter = 0;
@@ -436,7 +442,39 @@
data.Yaw = MY_DATA_EULER_PSI*0.0109863;
- } // end if(UM6_EULER_PHI_THETA)
+ }
+ //-------------------------------------------------------------------
+ // 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
+
+
+
+
+
+ // 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;
+
+ //------------------------------------------------------------
+
+ //------------------------------------------------------------
+ // 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)
//------------------------------------------------------------
} // end if(ADDRESS_TYPE_DATA)
--- a/main.cpp Fri Sep 28 00:40:29 2012 +0000
+++ b/main.cpp Wed May 01 19:06:43 2013 +0000
@@ -4,6 +4,8 @@
#include "UM6_usart.h" // UM6 USART HEADER
#include "UM6_config.h" // UM6 CONFIG HEADER
+LocalFileSystem local("local"); // Create the local filesystem under the name "local"
+
/////////////////////////////////////////////////////////////////////////////////////////////
// SETUP (ASSIGN) SERIAL COMMUNICATION PINS ON MBED
/////////////////////////////////////////////////////////////////////////////////////////////
@@ -14,7 +16,12 @@
////////////////////////////////////////////////////////////////////////////////////////////////
DigitalOut pc_activity(LED1); // LED1 = PC SERIAL
DigitalOut uart_activity(LED2); // LED2 = UM6 SERIAL
+DigitalOut logLED(LED3); // LED3 = logging active
+DigitalIn enable(p5); // enable signal for logging data to file
+
+Timer t; // sets up timer 't'
+Ticker tick; // sets up ticker
void rxCallback(MODSERIAL_IRQ_INFO *q) {
if (um6_uart.rxBufferGetCount() >= MAX_PACKET_DATA) {
@@ -22,7 +29,13 @@
Process_um6_packet();
}
}
-
+
+int doRead = 0;
+
+void print_um6() {
+ doRead = 1; // interupt to read signals from UM6
+
+}
int main() {
@@ -31,27 +44,47 @@
/////////////////////////////////////////////////////////////////////////////////////////////////////
// set UM6 serial uart baud 9600
- um6_uart.baud(9600);
- pc.baud(9600); // pc baud for UM6 to pc interface
-
+ 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);
- int Roll_Counter = 0;
+ tick.attach(&print_um6, 0.50);
+
+ 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,MagX,MagY,MagZ \r"); // sends header to file
- while (1) {
- Roll_Counter++;
- if (Roll_Counter > 5000000) {
- pc.printf("Gyro_Proc_X %f deg/s, ",data.Gyro_Proc_X);
- pc.printf("Gyro_Proc_Y %f deg/s, ",data.Gyro_Proc_Y);
- pc.printf("Gyro_Proc_Z %f deg/s, ",data.Gyro_Proc_Z);
- pc.printf("Roll %f deg, ",data.Roll);
- pc.printf("Pitch %f deg, ",data.Pitch);
- pc.printf("Yaw %f deg \r\n",data.Yaw);
- pc_activity = !pc_activity; // Lights LED when uart RxBuff has > 40 bytes
- Roll_Counter = 0;
+ while (enable) {
+ logLED = 1; // turns LED3 on when logging starts
+ if (doRead) {
+ float time1=t.read();
+ float Yaw=data.Yaw;
+ float Roll=data.Roll;
+ float Pitch=data.Pitch;
+ float GyroX=data.Gyro_Proc_X;
+ float GyroY=data.Gyro_Proc_Y;
+ float GyroZ=data.Gyro_Proc_Z;
+ float AccelX=data.Accel_Proc_X;
+ float AccelY=data.Accel_Proc_Y;
+ float AccelZ=data.Accel_Proc_Z;
+ float MagX=data.Mag_Proc_X;
+ float MagY=data.Mag_Proc_Y;
+ float MagZ=data.Mag_Proc_Z;
+ float GPSlong=data.GPS_long;
+
+ 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,Roll %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);
+ //%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,MagX,MagY,MagZ);
+
+ pc_activity = !pc_activity; // Lights LED1 when uart RxBuff has > 40 bytes
+ doRead = 0;
}
-
+
} // end while(1) loop
+ logLED = 0; // turns LED3 off when logging ends
+ fclose(fp);
} // end main()
\ No newline at end of file