RCV GPS/IMU Description
21 January 2014
RCV GPS/IMU Description
Information on the implementation of Arduino GPS/IMU for the
Research Concept Vehicle
January 21, 2014
Mikael Nybacka
Postal address
Royal Institute of Technology
KTH Vehicle Dynamics
SE-100 44 Stockholm
Sweden
Visiting address
Teknikringen 8
Stockholm
Telephone
+46 8 790 6000
Telefax
+46 8 790 9304
i
Internet
www.ave.kth.se
RCV GPS/IMU Description
21 January 2014
Contents
1 Introduction............................................................................................................................ 3 2 Components used ................................................................................................................... 3 2.1 Arduino MEGA 2560........................................................................................................ 3 2.2 Sparkfun and additional components ................................................................................ 4 3 Output ..................................................................................................................................... 5 4 Data logging and calibration................................................................................................. 6 5 Code ........................................................................................................................................ 6 6 Comparison with VBOX3i INS ............................................................................................ 7 7 Appendix Code – Main_RCV ............................................................................................... 8 8 Appendix Code – IMU_data ............................................................................................... 12 9 Appendix Code – Calibrate_sensors .................................................................................. 14 10 Appendix Code – Display_update .................................................................................... 17 11 Appendix Code – LCD_print ............................................................................................ 17 12 Appendix Code – Log_data............................................................................................... 18 13 Appendix Code – Serial_Data........................................................................................... 19 ii
1 Introduction
This document describes briefly the design of the GPS/IMU implementation using
Arduino components and software for the Research Concept Vehicle at KTH
Transport Labs1.
2 Components used
2.1 Arduino MEGA 2560
The main board of the IMU is the Arduino MEGA 2560 that is a 5V board that also
have output for 3.3V.
The Arduino Mega 2560 is a microcontroller board based on the ATmega2560
(datasheet). It has 54 digital input/output pins (of which 15 can be used as PWM
outputs), 16 analog inputs, 4 UARTs (hardware serial ports), a 16 MHz crystal
oscillator, a USB connection, a power jack, an ICSP header, and a reset button. It
contains everything needed to support the microcontroller; simply connect it to a
computer with a USB cable or power it with a AC-to-DC adapter or battery to get
started. The Mega is compatible with most shields designed for the Arduino
Duemilanove or Diecimila. The Mega 2560 is an update to the Arduino Mega,
which it replaces. The Mega2560 differs from all preceding boards in that it does not
use the FTDI USB-to-serial driver chip. Instead, it features the ATmega16U2
(ATmega8U2 in the revision 1 and revision 2 boards) programmed as a USB-toserial converter.
Revision 2 of the Mega2560 board has a resistor pulling the 8U2 HWB line to
ground, making it easier to put into DFU mode.
Revision 3 of the board has the following new features:
• pinout: added SDA and SCL pins that are near to the AREF pin and two
other new pins placed near to the RESET pin, the IOREF that allow the
shields to adapt to the voltage provided from the board. In future, shields
will be compatible both with the board that use the AVR, which operate
with 5V and with the Arduino Due that operate with 3.3V. The second one
is a not connected pin, that is reserved for future purposes.
• Stronger RESET circuit.
• Atmega 16U2 replace the 8U2.
Microcontroller
Operating Voltage
Input Voltage (recommended)
Input Voltage (limits)
Digital I/O Pins
1
2
ATmega2560
5V
7-12V
6-20V
54 (of which 15 provide PWM output)
http://www.kth.se/en/forskning/forskningsplattformar/transport/initiativ/t-labs
http://www.velocitybox.co.uk/index.php/en/products-main/gps-data-loggers/144-vbox-3i-dual-antenna-vb3isl
3
RCV GPS/IMU Description
21 January 2014
Analog Input Pins
DC Current per I/O Pin
DC Current for 3.3V Pin
Flash Memory
SRAM
EEPROM
Clock Speed
16
40 mA
50 mA
256 KB of which 8 KB used by bootloader
8 KB
4 KB
16 MHz
2.2 Sparkfun and additional components
The following components are also added and installed to the Arduino MEGA 2560.
1. GPS Shield with GPS EM-406A
a. https://www.sparkfun.com/products/10710
b. https://www.sparkfun.com/products/465
2. microSD Shield with Logic Level Converter
a. https://www.sparkfun.com/products/465
b. https://www.sparkfun.com/products/11978
3. Basic 16x2 Character LCD - Red on Black 5V with Trimpot 10K with
Knob
a. https://www.sparkfun.com/products/791
b. http://www.lawicel-shop.se/prod/Trimpot-10K-withKnob_873824/Sparkfun_64668/SWE/SEK
4. 5-Way Tactile Switch BoB
a. http://www.lawicel-shop.se/prod/5-Way-Tactile-SwitchBoB_882404/Sparkfun_64668/SWE/SEK
5. 9 Degrees of Freedom - Razor IMU
a. https://www.sparkfun.com/products/10736
6. FTDI Basic Breakout - 3.3V
a. https://www.sparkfun.com/products/9873
Item 6 is needed in order to program item 5.
4
RCV GPS/IMU Description
21 January 2014
3 Output
The output is serial via the black and brown wires. The black wire is TX3 (Serial 3
Transmit) and the brown wire is RX3 (Serial 3 Read). The output has a baudrate of
57600 and sends new data every 10 milli seconds.
The format of the output data is as below:
3
4
5
6
7
1
2
Milli
seconds
Milli s
Int
Sattelites
in view
No unit
Int
Latitude
deg
Float
Longitude
deg
Float
Velocity
m/s
Float
AccX
2
m/s
Float
8
AccY
2
m/s
Float
9
AccZ
2
m/s
Float
10
11
Rollrate
Pitchrate
Yawrate
rad/s
Float
rad/s
Float
rad/s
Float
12
13
Roll
angle
rad
Float
Pitch
angle
rad
Float
Special care need to be taken since the Angles (output 12, 13, and 14) is only valid
at low speed or low lateral and longitudinal acceleration, meaning when the car do
not drive dynamically. The angles is calculated with a Kalman filter using
atan(ax/az) as input and correcting with gyro.
5
14
Heading
euler
Float
RCV GPS/IMU Description
21 January 2014
4 Data logging and calibration
The IMU/GPS has a joystick on the top of the unit, if pressing this DOWN
(direction from the LCD) you will calibrate the IMU data. This should only be done
when standing still and on a flat surface. Memory function is not implemented yet so
this has to be done for each drive.
If pressing the joystick UP (direction towards the LCD) the logging to the internal
memory card will start and to stop it you press the joystick. Name of the log file will
show on the display for a second.
5 Code
The code used is altered code from the following open source projects:
• Kristian Lauszus, http://blog.tkjelectronics.dk/2012/09/a-practical-approachto-kalman-filter-and-how-to-implement-it/,
https://github.com/TKJElectronics/Example-Sketch-for-IMU-includingKalman-filter.
• Peter Bartz, https://github.com/ptrbrtz/razor-9dof-ahrs
• TinyGPS++ library, http://arduiniana.org/libraries/tinygpsplus/
• Alexander
Breivig
TimedAction
Library,
http://playground.arduino.cc/Code/TimedAction
6
RCV GPS/IMU Description
21 January 2014
6 Comparison with VBOX3i INS
The RCV IMU/GPS was benchmarked to the VBOX3i (RLVB3iSL)2 where the
Kalman Filter in the VBOX was turned off and the elevation mask was set to default
value.
RCV Lat/Long (blue), VBOX Lat/Long (red)
RCV Vx (blue), VBOX Vx (red)
−18.0712
14
−18.0714
12
−18.0716
Longitudinal speed (m/s)
10
Long (deg)
−18.0718
−18.072
−18.0722
8
6
4
−18.0724
2
−18.0726
−18.0728
59.3482
59.3484
59.3486
59.3488
Lat (deg)
59.349
59.3492
0
59.3494
0
10
20
30
40
50
Time (s)
60
70
80
90
100
Figure 1. GPS data long vs lat in the left graph and GPS velocity in the right graph.
RCV IMU (blue) and VBOX (red).
RCV Ax (blue), VBOX Ax (red)
RCV Ay (blue), VBOX Ay (red)
4
12
10
2
8
1
6
Lateral acceleration (m/s2)
Longitudinal acceleration (m/s2)
3
0
−1
−2
4
2
0
−3
−2
−4
−4
−5
−6
−6
0
10
20
30
40
50
Time (s)
60
70
80
90
−8
100
0
10
20
30
40
50
Time (s)
60
70
80
90
100
Figure 2. Longitudinal acceleration (left) and Lateral acceleration (right). RCV IMU
(blue) and VBOX (red).
RCV yaw−rate (blue), VBOX yaw−rate (red)
40
RCV roll angle (blue), VBOX roll angle (red)
30
30
20
20
10
0
0
Roll angle (deg)
Yaw Rate (deg/s)
10
−10
−20
−10
−20
−30
−30
−40
−40
−50
−60
0
10
20
30
40
50
Time (s)
60
70
80
90
−50
100
0
10
20
30
40
50
Time (s)
60
70
80
90
100
Figure 3. Yaw rate (left) and roll angle (right). Note that roll angle is not valid during
dynamic lateral driving. RCV IMU (blue) and VBOX (red).
2
http://www.velocitybox.co.uk/index.php/en/products-main/gps-data-loggers/144-vbox-3i-dual-antenna-vb3isl
7
RCV GPS/IMU Description
21 January 2014
7 Appendix Code – Main_RCV
#include <Thread.h>
#include <ThreadController.h>
#include <LiquidCrystal.h>
#include <SPI.h>
#include <SD.h>
#include <TinyGPS++.h>
// Initialize file for logging on flash
File file;
// Initialize the library with the numbers of the interface pins
LiquidCrystal LCD(7, 6, 5, 4, 3, 2);
// The TinyGPS++ object
TinyGPSPlus gps;
// Thread for logging
Thread loggingThread = Thread();
//Thread for serial
Thread serialThread = Thread();
// Thread for serial debug
Thread serialDebugThread = Thread();
// Thread for reading and filtering IMU data
Thread readIMUThread = Thread();
// Thread for reading and filtering IMU data
Thread readGPSThread = Thread();
// Define Joystick connection
#define UP A1
#define CLICK A2
#define LEFT A3
#define DOWN A4
#define RIGHT A5
// Define the different baud rates
#define GPSRATE 4800
#define SERIAL_SPEED 57600
#define IMU_SPEED 115200
int LED2 = 8;
int LED3 = 7;
// Defining variables for IMU data parsing
char incomingString[64];
float in_values[9];
static float accel_corr[3];
static float angles_corr[3];
static float gyro_corr[3];
boolean started = false;
8
RCV GPS/IMU Description
21 January 2014
boolean ended = false;
#define SOP '['
#define EOP ']'
byte index = 0;
char filename[] = "LOGGER00.CSV";
char data_in;
int startTime=0;
long int lastTimeLog=0;
long int lastTimeSerial=0;
int LOGGED_BUTTON_PRESSED=0;
int CALIBRATE_BUTTON_PRESSED=0;
long int lastDisplUpdate=0;
long int lastDisplUpdate2=0;
String headerString = "";
int counter=0;
//----------------------------------//-------SETUP----------------------//----------------------------------void setup() {
//Serial.begin(SERIAL_SPEED);
Serial1.begin(GPSRATE);
Serial2.begin(IMU_SPEED);
Serial3.begin(SERIAL_SPEED);
pinMode(UP,INPUT);
pinMode(DOWN,INPUT);
pinMode(LEFT,INPUT);
pinMode(RIGHT,INPUT);
pinMode(CLICK,INPUT);
digitalWrite(UP, HIGH);
/* Enable internal pull-ups */
digitalWrite(DOWN, HIGH);
digitalWrite(LEFT, HIGH);
digitalWrite(RIGHT, HIGH);
digitalWrite(CLICK, HIGH);
loggingThread.onRun(log_data);
loggingThread.setInterval(60);
serialThread.onRun(serial_data);
serialThread.setInterval(10);
serialDebugThread.onRun(serial_debug_data);
serialDebugThread.setInterval(10);
readIMUThread.onRun(read_imu_data);
readIMUThread.setInterval(10);
readGPSThread.onRun(smartDelay);
readGPSThread.setInterval(500);
LCD.begin(16,2);
/* Setup serial LCD and clear the screen */
LCD.clear();
LCD_print("RCV KTH LOGGER!",0,0,1);
// make sure that the default chip select pin is set to output, even if you don't use it:
pinMode(SS, OUTPUT);
digitalWrite(SS, HIGH); // davekw7x: If it's low, the Wiznet chip corrupts the SPI bus
9
RCV GPS/IMU Description
21 January 2014
// see if the card is present and can be initialized:
if (!SD.begin(10,11,12,13)) {
//Serial.println("Card failed, or not present ");
// don't do anything more:
while (1) ;
}
headerString += String("Milli sec");
headerString += String(",");
headerString += String("Time step milli");
headerString += String(",");
headerString += String("Sattelites in view");
headerString += String(",");
headerString += String("Lat (deg)");
headerString += String(",");
headerString += String("Long (deg)");
headerString += String(",");
headerString += String("Velocity (m/s)");
headerString += String(",");
headerString += String("AccX (m/s^2)");
headerString += String(",");
headerString += String("AccY (m/s^2)");
headerString += String(",");
headerString += String("AccZ (m/s^2)");
headerString += String(",");
headerString += String("Roll-rate (rad/s)");
headerString += String(",");
headerString += String("Pitch-rate (rad/s)");
headerString += String(",");
headerString += String("Yaw-rate (rad/s)");
headerString += String(",");
headerString += String("Roll (rad)");
headerString += String(",");
headerString += String("Pitch (rad)");
headerString += String(",");
headerString += String("Heading (euler)");
LCD.clear();
LCD_print("Setup complete",1,0,1);
//delay(1000);
LCD_print("Click DOWN",0,0,1);
LCD_print("Then UP to calib.",1,1,0);
//delay(2000);
}
//----------------------------------//--------LOOP----------------------//----------------------------------void loop() {
displayUpdate();
if(readGPSThread.shouldRun())
readGPSThread.run();
if(readIMUThread.shouldRun())
readIMUThread.run();
// -------------------------------------------// ------------CORRECTING IMU DATA-------------
10
RCV GPS/IMU Description
21 January 2014
// -------------------------------------------if (digitalRead(DOWN) == 0 ){ /* Check for DOWN Button */
lastDisplUpdate=millis();
LCD_print("Calibrating",0,0,1);
LCD_print("IMU",1,1,0);
accel_corr[0]=calibrateAccX(in_values);
accel_corr[1]=calibrateAccY(in_values);
accel_corr[2]=0;//calibrateAccZ(in_values);
gyro_corr[0]=calibrateGyroX(in_values);
gyro_corr[1]=calibrateGyroY(in_values);
gyro_corr[2]=calibrateGyroZ(in_values);
angles_corr[0]=calibrateAngleX(in_values);
angles_corr[1]=calibrateAngleY(in_values);
angles_corr[2]=0;//calibrateAngleZ(in_values);
LCD_print("Calibrating",0,0,1);
LCD_print("DONE!",1,1,0);
}
// -------------------------------------------// ------------SERIAL TRANSMISSION------------// -------------------------------------------// checks if thread should run
//if(serialDebugThread.shouldRun())
//
serialDebugThread.run();
if(serialThread.shouldRun())
serialThread.run();
// uncoment for rel life running on serial 3
// -------------------------------------------// ------------LOGGING TO FILE----------------// -------------------------------------------if (digitalRead(UP) == 0 || LOGGED_BUTTON_PRESSED){ /* Check for Up button */
if(LOGGED_BUTTON_PRESSED==0){
for (uint8_t i = 0; i < 100; i++) {
filename[6] = i/10 + '0';
filename[7] = i%10 + '0';
if (! SD.exists(filename)) {
// only open a new file if it doesn't exist
file = SD.open(filename, FILE_WRITE);
delay(10);
file.println(headerString);
file.println();
file.flush();
break; // leave the loop!
}
}
}
if(millis()>lastDisplUpdate2+3000){
LCD_print("LOGGING",0,0,1);
LCD_print("CLICK 2 END",1,1,0);
lastDisplUpdate2=millis();
lastDisplUpdate=millis();
}
displayUpdate();
LOGGED_BUTTON_PRESSED=1;
int startTime = millis();
11
RCV GPS/IMU Description
21 January 2014
lastDisplUpdate=millis();
if(loggingThread.shouldRun())
loggingThread.run();
if (digitalRead(CLICK) == 0){
LOGGED_BUTTON_PRESSED=0;
LCD_print("SAVED",0,0,1);
LCD_print(filename,1,1,0);
file.close();
//lastDisplUpdate2=millis();
lastDisplUpdate=millis();
displayUpdate();
}
}
}
8 Appendix Code – IMU_data
void read_imu_data(){
while (Serial2.available() > 0){
data_in = Serial2.read();
//delay(1);
//DID WE FOUND THE BEGIN MARKER?
if (data_in == SOP){
started = true;
index = 0;
incomingString[index] = '\0';
}
//DID WE FOUND THE END MARKER?
else if (data_in == EOP){
ended = true;
break;
}
//READ 64 BITES INTO THE STR_IN ARRAY
else{
if (index < 53-1){
incomingString[index++] = data_in;
incomingString[index] = '\0';
}
}
}
//Serial.println(String(incomingString).length());
if(String(incomingString).length() >= 46){
parse_imu(incomingString,",",10);
}
}
void parse_imu(char* str, char *delim, int index){
char *act, *ptr;
static char copy[64];
int i;
strcpy(copy, str);
for (i = 0, act = copy; i <= index; i++, act = NULL) {
in_values[i] = atof(strtok_r(act, delim, &ptr));
if (in_values[i] == NULL) {
12
RCV GPS/IMU Description
21 January 2014
break;
}
}
}
13
RCV GPS/IMU Description
21 January 2014
9 Appendix Code –
Calibrate_sensors
//This is pretty simple. It takes 100 readings and calculate the average.
//gyros
float resultGyroX;//x-axis
float resultGyroY;//y-axis
float resultGyroZ;//z-axis
//accelerometers
float resultAccX;//x-axis
float resultAccY;//y-axis
float resultAccZ;//z-axis
//angles
float resultAngleX;//x-axis
float resultAngleY;//y-axis
float resultAngleZ;//z-axis
//gyros
float calibrateGyroX(float in_values[])
{
for(int i=0;i<100;i++)
{
read_imu_data();
resultGyroX += in_values[3];
delay(1);
}
resultGyroX = resultGyroX/100;
return resultGyroX;
}
float calibrateGyroY(float in_values[])
{
for(int i=0;i<100;i++)
{
read_imu_data();
resultGyroY += in_values[4];
delay(1);
}
resultGyroY = resultGyroY/100;
return resultGyroY;
}
float calibrateGyroZ(float in_values[])
{
for(int i=0;i<100;i++)
{
read_imu_data();
resultGyroZ += in_values[5];
delay(1);
}
resultGyroZ = resultGyroZ/100;
return resultGyroZ;
CALIBRATE_BUTTON_PRESSED=0;
14
RCV GPS/IMU Description
21 January 2014
}
//accelerometers
float calibrateAccX(float in_values[])
{
for(int i=0;i<100;i++)
{
read_imu_data();
resultAccX += in_values[0];
delay(1);
}
resultAccX = resultAccX/100;
return resultAccX;
}
float calibrateAccY(float in_values[])
{
for(int i=0;i<100;i++)
{
read_imu_data();
resultAccY += in_values[1];
delay(1);
}
resultAccY = resultAccY/100;
return resultAccY;
}
float calibrateAccZ(float in_values[])
{
for(int i=0;i<100;i++)
{
read_imu_data();
resultAccZ += in_values[2];
delay(1);
}
resultAccZ = resultAccZ/100;
return resultAccZ;
}
//Angles
float calibrateAngleX(float in_values[])
{
for(int i=0;i<100;i++)
{
read_imu_data();
resultAngleX += in_values[6];
delay(1);
}
resultAngleX = resultAngleX/100;
return resultAngleX;
}
float calibrateAngleY(float in_values[])
{
for(int i=0;i<100;i++)
{
read_imu_data();
resultAngleY += in_values[7];
delay(1);
}
resultAngleY = resultAngleY/100;
return resultAngleY;
15
RCV GPS/IMU Description
21 January 2014
}
float calibrateAngleZ(float in_values[])
{
for(int i=0;i<100;i++)
{
read_imu_data();
resultAngleZ += in_values[8];
delay(1);
}
resultAngleZ = resultAngleZ/100;
return resultAngleZ;
}
16
RCV GPS/IMU Description
21 January 2014
10
Appendix Code –
Display_update
void displayUpdate(){
if(millis()>lastDisplUpdate+1000){
LCD_print("KTH TL RCV",0,0,1);
LCD.setCursor(0,1);
LCD.print(gps.speed.kmph());
lastDisplUpdate=millis();
}
}
11
Appendix
LCD_print
Code
–
/*LCD_print(const char *str, int line, int pos, int clrscrn)
str is the pointer to the characters to be written to the screen line is the line number that the user wants
to have the characters posted to. Mind you screen used in this demo had only two lines, there is no
support for going past the available number of lines. pos is the position relative to the start of the line
clrscrn will clear the screen if it is 1 and do nothing if it is anything else*/
#define SCREEN 35
int LCD_print(const char *str, int line, int pos, int clrscrn) /* assumes 2*16 screen size*/
{
int counter = 0;
if(line == 0){
line = pos;
if(clrscrn == 1){
LCD.clear();
LCD.setCursor(0,0);
}
LCD.setCursor(0,line);
while(*str){
LCD.print(*str++); /*not the most efficient but it works...*/
counter++;
if(counter >= SCREEN) /* doesn't allow overlap onto first screen*/
{
break;
}
}
}
else if (line == 1){
line = pos;
if(clrscrn == 1){
LCD.clear();
LCD.setCursor(0,line);
}
LCD.setCursor(0,line);
17
RCV GPS/IMU Description
21 January 2014
while(*str){
LCD.print(*str++); /*not the most efficient but it works...*/
counter++;
if(counter >= (SCREEN/2)) /*doesn't allow overlap onto first screen*/
{
break;
}
}
}
else{
return -1;
}
}
12
Appendix Code – Log_data
void log_data(){
//smartDelay(0);
char tmp[10];
String logDataString = String(millis());
logDataString += ",";
logDataString += String(millis()-lastTimeLog);
logDataString += ",";
dtostrf(gps.satellites.value(),1,2,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(gps.location.lat(),1,6,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(gps.location.lng(),1,6,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(gps.speed.kmph(),1,2,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(in_values[0]-accel_corr[0],1,2,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(in_values[1]-accel_corr[1],1,2,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(in_values[2]-accel_corr[2],1,2,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(in_values[3]-gyro_corr[0],1,2,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(in_values[4]-gyro_corr[1],1,2,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(in_values[5]-gyro_corr[2],1,2,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(in_values[6]-angles_corr[0],1,2,tmp);
18
RCV GPS/IMU Description
21 January 2014
logDataString += tmp;
logDataString += ",";
dtostrf(in_values[7]-angles_corr[1],1,2,tmp);
logDataString += tmp;
logDataString += ",";
dtostrf(in_values[8]-angles_corr[2],1,2,tmp);
logDataString += tmp;
file.println(logDataString);
//file.print(millis());file.print(", ");
//file.print(gps.satellites.value());file.print(", ");
//file.print(gps.location.lat(),6);file.print(", ");
//file.print(gps.location.lng(),6);file.print(", ");
//file.print(gps.speed.kmph());file.print(", ");
//file.print(in_values[0]-accel_corr[0]);file.print(", ");
//file.print(in_values[1]-accel_corr[1]);file.print(", ");
//file.print(in_values[2]-accel_corr[2]);file.print(", ");
//file.print(in_values[3]-gyro_corr[0]);file.print(", ");
//file.print(in_values[4]-gyro_corr[1]);file.print(", ");
//file.print(in_values[5]-gyro_corr[2]);file.print(", ");
//file.print(in_values[6]-angles_corr[0]);file.print(", ");
//file.print(in_values[7]-angles_corr[1]);file.print(", ");
//file.print(in_values[8]-angles_corr[2]);file.print(", ");
//file.print(millis()-lastTimeLog);
//
//file.println();
lastTimeLog=millis();
file.flush();
}
//static void smartDelay(unsigned long ms)
static void smartDelay()
{
unsigned long start = millis();
do
{
while (Serial1.available())
gps.encode(Serial1.read());
} while (millis() - start < 0);
}
13
Appendix Code –
Serial_Data
void serial_data(){
Serial3.print(millis());Serial3.print(",");
Serial3.print(gps.satellites.value());Serial3.print(",");
Serial3.print(gps.location.lat(),6);Serial3.print(",");
Serial3.print(gps.location.lng(),6);Serial3.print(",");
Serial3.print(gps.speed.kmph());Serial3.print(",");
Serial3.print(in_values[0]-accel_corr[0]);Serial3.print(",");
Serial3.print(in_values[1]-accel_corr[1]);Serial3.print(",");
19
RCV GPS/IMU Description
21 January 2014
Serial3.print(in_values[2]-accel_corr[2]);Serial3.print(",");
Serial3.print(in_values[3]-gyro_corr[0]);Serial3.print(",");
Serial3.print(in_values[4]-gyro_corr[1]);Serial3.print(",");
Serial3.print(in_values[5]-gyro_corr[2]);Serial3.print(",");
Serial3.print(in_values[6]-angles_corr[0]);Serial3.print(",");
Serial3.print(in_values[7]-angles_corr[1]);Serial3.print(",");
Serial3.print(in_values[8]-angles_corr[2]);
Serial3.println();
//lastTimeSerial=millis();
}
void serial_debug_data(){
//Serial.print(millis());Serial.print("\t");
//Serial.print(gps.satellites.value());Serial.print(", ");
//Serial.print(gps.location.lat(),6);Serial.print(", ");
//Serial.print(gps.location.lng(),6);Serial.print(", ");
//Serial.print(gps.speed.kmph());Serial.print("\t ");
//Serial.print(in_values[0]-accel_corr[0]);Serial.print("\t");
//Serial.print(in_values[1]-accel_corr[1]);Serial.print("\t");
//Serial.print(in_values[2]-accel_corr[2]);Serial.print("\t");
//Serial.print(in_values[3]-gyro_corr[0]);Serial.print("\t");
//Serial.print(in_values[4]-gyro_corr[1]);Serial.print("\t");
//Serial.print(in_values[5]-gyro_corr[2]);Serial.print("\t");
//Serial.print(in_values[6]-angles_corr[0]);Serial.print("\t");
//Serial.print(in_values[7]-angles_corr[1]);Serial.print("\t");
//Serial.print(in_values[8]-angles_corr[2]);
Serial.print(in_values[0]-accel_corr[0]);Serial.print("\t");
Serial.print(in_values[1]-accel_corr[1]);Serial.print("\t");
//Serial.print(in_values[2]-accel_corr[2]);Serial.print(",");
Serial.print(in_values[3]-gyro_corr[0]);Serial.print("\t");
Serial.print(in_values[4]-gyro_corr[1]);Serial.print("\t");
//Serial.print(in_values[5]-gyro_corr[2]);Serial.print(",");
Serial.print(in_values[6]-angles_corr[0]);Serial.print("\t");
Serial.print(in_values[7]-angles_corr[1]);Serial.print("\t");
//Serial.print(in_values[8]-angles_corr[2]);//Serial.print("]");
Serial.println();
//lastTimeSerial=millis();
}
20
© Copyright 2024 ExpyDoc
