![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
Diff: main.cpp
- Revision:
- 14:92bacb5af01b
- Parent:
- 12:8644abfa86da
- Child:
- 15:dbf20c1209ae
diff -r e18e7b1cb900 -r 92bacb5af01b main.cpp --- a/main.cpp Wed Aug 26 06:29:50 2015 +0000 +++ b/main.cpp Wed Aug 26 16:33:26 2015 +0000 @@ -2,6 +2,7 @@ #include <string> #include <sstream> #include <vector> +#include "Get.h" using namespace std; #define MAX_IMU_SIZE 29 @@ -19,11 +20,13 @@ int IMU_message_counter=0; char GPS_message[256]; int GPS_message_counter=0; +char PC_message[256]; +int PC_message_counter=0; string IMU_Y="N/A"; string IMU_P="N/A"; string IMU_R="N/A"; -string GPS_quality="N/A"; +string GPS_Quality="N/A"; string GPS_UTC="N/A"; string GPS_Latitude="N/A"; string GPS_Longtitude="N/A"; @@ -32,9 +35,10 @@ string GPS_HDOP="N/A"; string GPS_VDOP="N/A"; string GPS_PDOP="N/A"; -string GPS_date="N/A"; -string GPS_velocityKnot="N/A"; -string GPS_velocityKph="N/A"; +string GPS_Date="N/A"; +string GPS_VelocityKnot="N/A"; +string GPS_VelocityKph="N/A"; + int asterisk_idx; vector<string> split(const string &s, char delim) { @@ -63,8 +67,8 @@ if (GPS_data_string.substr(0,6) == "$GPGGA") { GPS_data_string = GPS_data_string.substr(7); vector<string> result = split(GPS_data_string, ','); - GPS_quality = result.at(5); - if(GPS_quality != "0" and GPS_quality != "6") { + GPS_Quality = result.at(5); + if(GPS_Quality != "0" and GPS_Quality != "6") { GPS_UTC = result.at(0); GPS_Latitude = result.at(2) + result.at(1); GPS_Longtitude = result.at(4) + result.at(3); @@ -72,22 +76,22 @@ GPS_HDOP = result.at(7); GPS_Altitude = result.at(8) + result.at(9); } - } else if (GPS_data_string.substr(0,6) == "$GPGSA" and GPS_quality != "0" and GPS_quality != "6") { + } else if (GPS_data_string.substr(0,6) == "$GPGSA" and GPS_Quality != "0" and GPS_Quality != "6") { GPS_data_string = GPS_data_string.substr(7); vector<string> result = split(GPS_data_string, ','); GPS_PDOP = result.at(14); asterisk_idx = result.at(16).find('*'); GPS_VDOP = result.at(16).substr(0, asterisk_idx); - } else if (GPS_data_string.substr(0,6) == "$GPRMC" and GPS_quality != "0" and GPS_quality != "6") { + } else if (GPS_data_string.substr(0,6) == "$GPRMC" and GPS_Quality != "0" and GPS_Quality != "6") { GPS_data_string = GPS_data_string.substr(7); vector<string> result = split(GPS_data_string, ','); - GPS_date = result.at(8); - } else if (GPS_data_string.substr(0,6) == "$GPVTG" and GPS_quality != "0" and GPS_quality != "6") { + GPS_Date = result.at(8); + } else if (GPS_data_string.substr(0,6) == "$GPVTG" and GPS_Quality != "0" and GPS_Quality != "6") { GPS_data_string = GPS_data_string.substr(7); vector<string> result = split(GPS_data_string, ','); - GPS_velocityKnot = result.at(4) + result.at(5); + GPS_VelocityKnot = result.at(4) + result.at(5); asterisk_idx = result.at(7).find('*'); - GPS_velocityKph = result.at(6) + result.at(7).substr(0, asterisk_idx); + GPS_VelocityKph = result.at(6) + result.at(7).substr(0, asterisk_idx); } } @@ -97,11 +101,12 @@ } void printStateGPS() { - pc.printf("GPS_quality: %s, GPS_UTC: %s, GPS_Latitude: %s, GPS_Longtitude: %s, GPS_Altitude: %s, " - "GPS_Num_Satellite: %s, GPS_HDOP: %s, GPS_VDOP: %s, GPS_PDOP: %s, GPS_date: %s, GPS_velocityKnot: %s, GPS_velocityKph: %s\n", - GPS_quality.c_str(), GPS_UTC.c_str(), GPS_Latitude.c_str(), GPS_Longtitude.c_str(), GPS_Altitude.c_str(), GPS_Num_Satellite.c_str(), - GPS_HDOP.c_str(), GPS_VDOP.c_str(), GPS_PDOP.c_str(), GPS_date.c_str(), GPS_velocityKnot.c_str(), GPS_velocityKph.c_str()); + pc.printf("GPS_Quality: %s, GPS_UTC: %s, GPS_Latitude: %s, GPS_Longtitude: %s, GPS_Altitude: %s, " + "GPS_Num_Satellite: %s, GPS_HDOP: %s, GPS_VDOP: %s, GPS_PDOP: %s, GPS_Date: %s, GPS_VelocityKnot: %s, GPS_VelocityKph: %s\n", + GPS_Quality.c_str(), GPS_UTC.c_str(), GPS_Latitude.c_str(), GPS_Longtitude.c_str(), GPS_Altitude.c_str(), GPS_Num_Satellite.c_str(), + GPS_HDOP.c_str(), GPS_VDOP.c_str(), GPS_PDOP.c_str(), GPS_Date.c_str(), GPS_VelocityKnot.c_str(), GPS_VelocityKph.c_str()); } + //#YPR=-183.74,-134.27,-114.39 void IMU_serial_ISR() { char buf; @@ -109,7 +114,7 @@ while (IMU.readable()) { buf = IMU.getc(); - + //pc.putc(buf); IMU_message[IMU_message_counter]=buf; IMU_message_counter+=1; @@ -132,8 +137,8 @@ while (GPS.readable()) { buf = GPS.getc(); - - + + //pc.putc(buf); GPS_message[GPS_message_counter]=buf; GPS_message_counter+=1; @@ -154,7 +159,18 @@ while (pc.readable()) { buf = pc.getc(); - //pc.putc(buf); + + //pc.putc(buf); + PC_message[PC_message_counter]=buf; + PC_message_counter+=1; + + if (buf=='\n'){ + string PC_copy(PC_message, PC_message_counter); + //pc.printf("%s", PC_copy.c_str()); + decodePC(PC_copy); + PC_message_counter=0; + PC_copy[0] = '\0'; + } } led4= !led4;