![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
GPS and IMU reading works
Dependencies: mbed Servo SDFileSystem
main.cpp
- Committer:
- dem123456789
- Date:
- 2015-08-29
- Revision:
- 20:a820531c78bc
- Parent:
- 19:eef0e3ec32a0
- Child:
- 21:99be83550601
File content as of revision 20:a820531c78bc:
#include "Config.h" DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); Serial pc(USBTX, USBRX); Serial IMU(p28, p27); // tx, rx Serial GPS(p13, p14); // tx, rx char IMU_message[256]; 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_UTC="N/A"; string GPS_Latitude="N/A"; string GPS_Longtitude="N/A"; string GPS_Altitude="N/A"; string GPS_Num_Satellite="N/A"; 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"; int asterisk_idx; double Longtitude_Path[MAX_TASK_SIZE]; double Latitude_Path[MAX_TASK_SIZE]; vector<string> split(const string &s, char delim) { stringstream ss(s); string item; vector<string> tokens; while (getline(ss, item, delim)) { if (item.empty()) { item = "N/A"; } tokens.push_back(item); } return tokens; } void updateIMU(string IMU_data) { string IMU_data_string(IMU_data); if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) { IMU_data_string = IMU_data_string.substr(5); vector<string> result = split(IMU_data_string, ','); IMU_Y = result.at(0); IMU_P = result.at(1); IMU_R = result.at(2).substr(0, result.at(2).size()-1); } } void updateGPS(string GPS_data) { string GPS_data_string(GPS_data); 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_UTC = result.at(0); GPS_Latitude = result.at(2) + result.at(1); GPS_Longtitude = result.at(4) + result.at(3); GPS_Num_Satellite = result.at(6); 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") { 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") { 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_data_string = GPS_data_string.substr(7); vector<string> result = split(GPS_data_string, ','); 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); } } /* Get data from SailBoat @GET=parameter Ex: @GET=GPS_Quality Supported parameter: GPS_Quality, GPS_UTC, GPS_Latitude, GPS_Longtitude, GPS_Altitude, GPS_Num_Satellite, GPS_HDOP, GPS_VDOP, GPS_PDOP, GPS_Date, GPS_VelocityKnot, GPS_VelocityKph Set path to SailBoat @SET=PATH, Latitude, Longtitude, Task id @SET=PATH, 33.776318, -84.407590, 3 */ void decodePC(string PC_data) { string PC_data_string(PC_data); if (PC_data_string.substr(0,4) == "@GET") { pc.printf("%s", PC_data_string.c_str()); PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6); pc.printf("%s\n", decodeCommandGET(PC_data_string).c_str()); } else if (PC_data_string.substr(0,4) == "@SET") { pc.printf("%s", PC_data_string.c_str()); PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6); string claim = decodeCommandSET(PC_data_string); if (claim == "DONE") { pc.printf("Current Path: Longtitude, Latitude\n"); for (int i=0;i<MAX_TASK_SIZE;++i) { pc.printf(" %f, %f\n", Longtitude_Path[i], Latitude_Path[i]); } } pc.printf("%s\n", claim.c_str()); } } void printStateIMU() { pc.printf("IMU_Y: %s, IMU_P: %s, IMU_R: %s\n",IMU_Y.c_str(), IMU_P.c_str(), IMU_R.c_str()); } 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()); } //#YPR=-183.74,-134.27,-114.39 void IMU_serial_ISR() { char buf; while (IMU.readable()) { buf = IMU.getc(); //pc.putc(buf); IMU_message[IMU_message_counter]=buf; IMU_message_counter+=1; if (buf=='\n'){ string IMU_copy(IMU_message, IMU_message_counter); //pc.printf("%s", IMU_copy.c_str()); updateIMU(IMU_copy); IMU_message_counter=0; IMU_copy[0] = '\0'; } } led2 = !led2; } void GPS_serial_ISR() { char buf; while (GPS.readable()) { buf = GPS.getc(); //pc.putc(buf); GPS_message[GPS_message_counter]=buf; GPS_message_counter+=1; if (buf=='\n'){ string GPS_copy(GPS_message, GPS_message_counter); //pc.printf("%s", GPS_copy.c_str()); updateGPS(GPS_copy); GPS_message_counter=0; GPS_copy[0] = '\0'; } } led3 = !led3; } void PC_serial_ISR() { char buf; while (pc.readable()) { buf = pc.getc(); //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; } int main() { IMU.baud(57600); IMU.attach(&IMU_serial_ISR); GPS.baud(38400); GPS.attach(&GPS_serial_ISR); pc.baud(115200); pc.attach(&PC_serial_ISR); while (1) { led1 = !led1; wait(0.4); //printStateIMU(); //printStateGPS(); } }