Senior_Design_Sailboat / Mbed 2 deprecated ECE4012_Sailboat

GPS and IMU reading works

Dependencies:   mbed Servo SDFileSystem

/media/uploads/taoqiuyang/img_2352.jpg

main.cpp@15:dbf20c1209ae, 2015-08-26 (annotated)

Committer:
dem123456789
Date:
Wed Aug 26 22:41:36 2015 +0000
Revision:
15:dbf20c1209ae
Parent:
14:92bacb5af01b
Child:
16:0ea992d9a390
PC GET part done

Who changed what in which revision?

UserRevisionLine numberNew contents of line
taoqiuyang 0:f4d390c06705 1 #include "mbed.h"
taoqiuyang 3:ab9f94d112c0 2 #include <string>
dem123456789 9:bf5939466e86 3 #include <sstream>
dem123456789 9:bf5939466e86 4 #include <vector>
dem123456789 14:92bacb5af01b 5 #include "Get.h"
dem123456789 11:1caacb994236 6 using namespace std;
dem123456789 11:1caacb994236 7
dem123456789 11:1caacb994236 8 #define MAX_IMU_SIZE 29
taoqiuyang 3:ab9f94d112c0 9
taoqiuyang 2:afb333543af5 10 DigitalOut led1(LED1);
taoqiuyang 2:afb333543af5 11 DigitalOut led2(LED2);
taoqiuyang 5:451b8203ef99 12 DigitalOut led3(LED3);
dem123456789 8:1f5a44bade4d 13 DigitalOut led4(LED4);
taoqiuyang 2:afb333543af5 14
taoqiuyang 2:afb333543af5 15 Serial pc(USBTX, USBRX);
taoqiuyang 2:afb333543af5 16 Serial IMU(p28, p27); // tx, rx
taoqiuyang 5:451b8203ef99 17 Serial GPS(p13, p14); // tx, rx
taoqiuyang 2:afb333543af5 18
taoqiuyang 2:afb333543af5 19 char IMU_message[256];
taoqiuyang 3:ab9f94d112c0 20 int IMU_message_counter=0;
taoqiuyang 5:451b8203ef99 21 char GPS_message[256];
taoqiuyang 5:451b8203ef99 22 int GPS_message_counter=0;
dem123456789 14:92bacb5af01b 23 char PC_message[256];
dem123456789 14:92bacb5af01b 24 int PC_message_counter=0;
dem123456789 11:1caacb994236 25
dem123456789 11:1caacb994236 26 string IMU_Y="N/A";
dem123456789 11:1caacb994236 27 string IMU_P="N/A";
dem123456789 11:1caacb994236 28 string IMU_R="N/A";
dem123456789 14:92bacb5af01b 29 string GPS_Quality="N/A";
dem123456789 11:1caacb994236 30 string GPS_UTC="N/A";
dem123456789 11:1caacb994236 31 string GPS_Latitude="N/A";
dem123456789 11:1caacb994236 32 string GPS_Longtitude="N/A";
dem123456789 11:1caacb994236 33 string GPS_Altitude="N/A";
dem123456789 11:1caacb994236 34 string GPS_Num_Satellite="N/A";
dem123456789 11:1caacb994236 35 string GPS_HDOP="N/A";
dem123456789 12:8644abfa86da 36 string GPS_VDOP="N/A";
dem123456789 12:8644abfa86da 37 string GPS_PDOP="N/A";
dem123456789 14:92bacb5af01b 38 string GPS_Date="N/A";
dem123456789 14:92bacb5af01b 39 string GPS_VelocityKnot="N/A";
dem123456789 14:92bacb5af01b 40 string GPS_VelocityKph="N/A";
dem123456789 14:92bacb5af01b 41
dem123456789 12:8644abfa86da 42 int asterisk_idx;
dem123456789 9:bf5939466e86 43
dem123456789 9:bf5939466e86 44 vector<string> split(const string &s, char delim) {
dem123456789 9:bf5939466e86 45 stringstream ss(s);
dem123456789 9:bf5939466e86 46 string item;
dem123456789 9:bf5939466e86 47 vector<string> tokens;
dem123456789 9:bf5939466e86 48 while (getline(ss, item, delim)) {
dem123456789 15:dbf20c1209ae 49 if (item.empty()) {
dem123456789 15:dbf20c1209ae 50 item == "N/A";
dem123456789 15:dbf20c1209ae 51 }
dem123456789 9:bf5939466e86 52 tokens.push_back(item);
dem123456789 9:bf5939466e86 53 }
dem123456789 9:bf5939466e86 54 return tokens;
dem123456789 9:bf5939466e86 55 }
dem123456789 9:bf5939466e86 56
dem123456789 9:bf5939466e86 57 void updateIMU(string IMU_data) {
dem123456789 9:bf5939466e86 58 string IMU_data_string(IMU_data);
dem123456789 9:bf5939466e86 59 if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) {
dem123456789 9:bf5939466e86 60 IMU_data_string = IMU_data_string.substr(5);
dem123456789 9:bf5939466e86 61 vector<string> result = split(IMU_data_string, ',');
dem123456789 9:bf5939466e86 62 IMU_Y = result.at(0);
dem123456789 9:bf5939466e86 63 IMU_P = result.at(1);
dem123456789 15:dbf20c1209ae 64 IMU_R = result.at(2).substr(0, result.at(2).size()-1);
dem123456789 9:bf5939466e86 65 }
dem123456789 9:bf5939466e86 66 }
dem123456789 9:bf5939466e86 67
dem123456789 10:12ba6ed2d6f0 68 void updateGPS(string GPS_data) {
dem123456789 10:12ba6ed2d6f0 69 string GPS_data_string(GPS_data);
dem123456789 11:1caacb994236 70 if (GPS_data_string.substr(0,6) == "$GPGGA") {
dem123456789 11:1caacb994236 71 GPS_data_string = GPS_data_string.substr(7);
dem123456789 10:12ba6ed2d6f0 72 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 73 GPS_Quality = result.at(5);
dem123456789 14:92bacb5af01b 74 if(GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 11:1caacb994236 75 GPS_UTC = result.at(0);
dem123456789 11:1caacb994236 76 GPS_Latitude = result.at(2) + result.at(1);
dem123456789 11:1caacb994236 77 GPS_Longtitude = result.at(4) + result.at(3);
dem123456789 11:1caacb994236 78 GPS_Num_Satellite = result.at(6);
dem123456789 11:1caacb994236 79 GPS_HDOP = result.at(7);
dem123456789 11:1caacb994236 80 GPS_Altitude = result.at(8) + result.at(9);
dem123456789 11:1caacb994236 81 }
dem123456789 14:92bacb5af01b 82 } else if (GPS_data_string.substr(0,6) == "$GPGSA" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 83 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 84 vector<string> result = split(GPS_data_string, ',');
dem123456789 12:8644abfa86da 85 GPS_PDOP = result.at(14);
dem123456789 12:8644abfa86da 86 asterisk_idx = result.at(16).find('*');
dem123456789 12:8644abfa86da 87 GPS_VDOP = result.at(16).substr(0, asterisk_idx);
dem123456789 14:92bacb5af01b 88 } else if (GPS_data_string.substr(0,6) == "$GPRMC" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 89 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 90 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 91 GPS_Date = result.at(8);
dem123456789 14:92bacb5af01b 92 } else if (GPS_data_string.substr(0,6) == "$GPVTG" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 93 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 94 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 95 GPS_VelocityKnot = result.at(4) + result.at(5);
dem123456789 12:8644abfa86da 96 asterisk_idx = result.at(7).find('*');
dem123456789 14:92bacb5af01b 97 GPS_VelocityKph = result.at(6) + result.at(7).substr(0, asterisk_idx);
dem123456789 10:12ba6ed2d6f0 98 }
dem123456789 12:8644abfa86da 99
dem123456789 10:12ba6ed2d6f0 100 }
dem123456789 10:12ba6ed2d6f0 101
dem123456789 15:dbf20c1209ae 102 //@GET=GPS_Quality
dem123456789 15:dbf20c1209ae 103 void decodePC(string PC_data) {
dem123456789 15:dbf20c1209ae 104 string PC_data_string(PC_data);
dem123456789 15:dbf20c1209ae 105 if (PC_data_string.substr(0,4) == "@GET") {
dem123456789 15:dbf20c1209ae 106 pc.printf("%s", PC_data_string.c_str());
dem123456789 15:dbf20c1209ae 107 PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
dem123456789 15:dbf20c1209ae 108 pc.printf("%s\n", decodeCommand(PC_data_string).c_str());
dem123456789 15:dbf20c1209ae 109 }
dem123456789 15:dbf20c1209ae 110 }
dem123456789 15:dbf20c1209ae 111
dem123456789 15:dbf20c1209ae 112
dem123456789 11:1caacb994236 113 void printStateIMU() {
dem123456789 11:1caacb994236 114 pc.printf("IMU_Y: %s, IMU_P: %s, IMU_R: %s\n",IMU_Y.c_str(), IMU_P.c_str(), IMU_R.c_str());
dem123456789 11:1caacb994236 115 }
dem123456789 11:1caacb994236 116
dem123456789 12:8644abfa86da 117 void printStateGPS() {
dem123456789 14:92bacb5af01b 118 pc.printf("GPS_Quality: %s, GPS_UTC: %s, GPS_Latitude: %s, GPS_Longtitude: %s, GPS_Altitude: %s, "
dem123456789 14:92bacb5af01b 119 "GPS_Num_Satellite: %s, GPS_HDOP: %s, GPS_VDOP: %s, GPS_PDOP: %s, GPS_Date: %s, GPS_VelocityKnot: %s, GPS_VelocityKph: %s\n",
dem123456789 14:92bacb5af01b 120 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(),
dem123456789 14:92bacb5af01b 121 GPS_HDOP.c_str(), GPS_VDOP.c_str(), GPS_PDOP.c_str(), GPS_Date.c_str(), GPS_VelocityKnot.c_str(), GPS_VelocityKph.c_str());
dem123456789 12:8644abfa86da 122 }
dem123456789 14:92bacb5af01b 123
dem123456789 11:1caacb994236 124 //#YPR=-183.74,-134.27,-114.39
taoqiuyang 4:37d118f2348c 125 void IMU_serial_ISR() {
taoqiuyang 3:ab9f94d112c0 126 char buf;
taoqiuyang 3:ab9f94d112c0 127
taoqiuyang 3:ab9f94d112c0 128 while (IMU.readable()) {
taoqiuyang 3:ab9f94d112c0 129 buf = IMU.getc();
taoqiuyang 3:ab9f94d112c0 130
dem123456789 14:92bacb5af01b 131 //pc.putc(buf);
taoqiuyang 3:ab9f94d112c0 132 IMU_message[IMU_message_counter]=buf;
dem123456789 9:bf5939466e86 133 IMU_message_counter+=1;
taoqiuyang 3:ab9f94d112c0 134
taoqiuyang 3:ab9f94d112c0 135 if (buf=='\n'){
dem123456789 9:bf5939466e86 136 string IMU_copy(IMU_message, IMU_message_counter);
dem123456789 11:1caacb994236 137 //pc.printf("%s", IMU_copy.c_str());
dem123456789 9:bf5939466e86 138 updateIMU(IMU_copy);
dem123456789 9:bf5939466e86 139 IMU_message_counter=0;
dem123456789 9:bf5939466e86 140 IMU_copy[0] = '\0';
dem123456789 9:bf5939466e86 141 }
dem123456789 9:bf5939466e86 142
taoqiuyang 3:ab9f94d112c0 143 }
taoqiuyang 2:afb333543af5 144 led2 = !led2;
taoqiuyang 2:afb333543af5 145 }
dem123456789 9:bf5939466e86 146
dem123456789 9:bf5939466e86 147
dem123456789 8:1f5a44bade4d 148
taoqiuyang 5:451b8203ef99 149 void GPS_serial_ISR() {
taoqiuyang 5:451b8203ef99 150 char buf;
taoqiuyang 5:451b8203ef99 151
taoqiuyang 5:451b8203ef99 152 while (GPS.readable()) {
taoqiuyang 5:451b8203ef99 153 buf = GPS.getc();
dem123456789 14:92bacb5af01b 154
dem123456789 14:92bacb5af01b 155 //pc.putc(buf);
dem123456789 10:12ba6ed2d6f0 156 GPS_message[GPS_message_counter]=buf;
dem123456789 10:12ba6ed2d6f0 157 GPS_message_counter+=1;
dem123456789 10:12ba6ed2d6f0 158
dem123456789 10:12ba6ed2d6f0 159 if (buf=='\n'){
dem123456789 10:12ba6ed2d6f0 160 string GPS_copy(GPS_message, GPS_message_counter);
dem123456789 11:1caacb994236 161 //pc.printf("%s", GPS_copy.c_str());
dem123456789 10:12ba6ed2d6f0 162 updateGPS(GPS_copy);
dem123456789 10:12ba6ed2d6f0 163 GPS_message_counter=0;
dem123456789 10:12ba6ed2d6f0 164 GPS_copy[0] = '\0';
dem123456789 10:12ba6ed2d6f0 165 }
taoqiuyang 5:451b8203ef99 166 }
taoqiuyang 5:451b8203ef99 167
taoqiuyang 5:451b8203ef99 168 led3 = !led3;
taoqiuyang 5:451b8203ef99 169 }
taoqiuyang 3:ab9f94d112c0 170
dem123456789 8:1f5a44bade4d 171 void PC_serial_ISR() {
dem123456789 8:1f5a44bade4d 172 char buf;
dem123456789 8:1f5a44bade4d 173
dem123456789 8:1f5a44bade4d 174 while (pc.readable()) {
dem123456789 8:1f5a44bade4d 175 buf = pc.getc();
dem123456789 14:92bacb5af01b 176
dem123456789 14:92bacb5af01b 177 //pc.putc(buf);
dem123456789 14:92bacb5af01b 178 PC_message[PC_message_counter]=buf;
dem123456789 14:92bacb5af01b 179 PC_message_counter+=1;
dem123456789 14:92bacb5af01b 180
dem123456789 14:92bacb5af01b 181 if (buf=='\n'){
dem123456789 14:92bacb5af01b 182 string PC_copy(PC_message, PC_message_counter);
dem123456789 14:92bacb5af01b 183 //pc.printf("%s", PC_copy.c_str());
dem123456789 14:92bacb5af01b 184 decodePC(PC_copy);
dem123456789 14:92bacb5af01b 185 PC_message_counter=0;
dem123456789 14:92bacb5af01b 186 PC_copy[0] = '\0';
dem123456789 14:92bacb5af01b 187 }
dem123456789 8:1f5a44bade4d 188 }
dem123456789 8:1f5a44bade4d 189
dem123456789 8:1f5a44bade4d 190 led4= !led4;
dem123456789 8:1f5a44bade4d 191 }
taoqiuyang 3:ab9f94d112c0 192
taoqiuyang 0:f4d390c06705 193 int main() {
taoqiuyang 2:afb333543af5 194 IMU.baud(57600);
taoqiuyang 4:37d118f2348c 195 IMU.attach(&IMU_serial_ISR);
taoqiuyang 5:451b8203ef99 196 GPS.baud(38400);
taoqiuyang 5:451b8203ef99 197 GPS.attach(&GPS_serial_ISR);
dem123456789 8:1f5a44bade4d 198 pc.baud(115200);
dem123456789 8:1f5a44bade4d 199 pc.attach(&PC_serial_ISR);
taoqiuyang 1:e7245ffb4820 200
taoqiuyang 2:afb333543af5 201 while (1) {
taoqiuyang 2:afb333543af5 202 led1 = !led1;
dem123456789 15:dbf20c1209ae 203 wait(0.4);
dem123456789 15:dbf20c1209ae 204 //printStateIMU();
dem123456789 15:dbf20c1209ae 205 //printStateGPS();
taoqiuyang 0:f4d390c06705 206 }
taoqiuyang 0:f4d390c06705 207 }