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@21:99be83550601, 2015-08-29 (annotated)

Committer:
dem123456789
Date:
Sat Aug 29 03:35:10 2015 +0000
Revision:
21:99be83550601
Parent:
20:a820531c78bc
Child:
22:faba67585854
Servo tested

Who changed what in which revision?

UserRevisionLine numberNew contents of line
dem123456789 20:a820531c78bc 1 #include "Config.h"
dem123456789 11:1caacb994236 2
taoqiuyang 2:afb333543af5 3 DigitalOut led1(LED1);
taoqiuyang 2:afb333543af5 4 DigitalOut led2(LED2);
taoqiuyang 5:451b8203ef99 5 DigitalOut led3(LED3);
dem123456789 8:1f5a44bade4d 6 DigitalOut led4(LED4);
dem123456789 20:a820531c78bc 7
taoqiuyang 2:afb333543af5 8 Serial pc(USBTX, USBRX);
taoqiuyang 2:afb333543af5 9 Serial IMU(p28, p27); // tx, rx
taoqiuyang 5:451b8203ef99 10 Serial GPS(p13, p14); // tx, rx
dem123456789 21:99be83550601 11 Servo rudderServo(p25);
dem123456789 21:99be83550601 12 Servo wingServo(p26);
dem123456789 21:99be83550601 13 SDFileSystem sd(p5, p6, p7, p8, "sd");// mosi, miso, sck, cs
taoqiuyang 2:afb333543af5 14
taoqiuyang 2:afb333543af5 15 char IMU_message[256];
taoqiuyang 3:ab9f94d112c0 16 int IMU_message_counter=0;
taoqiuyang 5:451b8203ef99 17 char GPS_message[256];
taoqiuyang 5:451b8203ef99 18 int GPS_message_counter=0;
dem123456789 14:92bacb5af01b 19 char PC_message[256];
dem123456789 14:92bacb5af01b 20 int PC_message_counter=0;
dem123456789 11:1caacb994236 21
dem123456789 11:1caacb994236 22 string IMU_Y="N/A";
dem123456789 11:1caacb994236 23 string IMU_P="N/A";
dem123456789 11:1caacb994236 24 string IMU_R="N/A";
dem123456789 14:92bacb5af01b 25 string GPS_Quality="N/A";
dem123456789 11:1caacb994236 26 string GPS_UTC="N/A";
dem123456789 11:1caacb994236 27 string GPS_Latitude="N/A";
dem123456789 11:1caacb994236 28 string GPS_Longtitude="N/A";
dem123456789 11:1caacb994236 29 string GPS_Altitude="N/A";
dem123456789 11:1caacb994236 30 string GPS_Num_Satellite="N/A";
dem123456789 11:1caacb994236 31 string GPS_HDOP="N/A";
dem123456789 12:8644abfa86da 32 string GPS_VDOP="N/A";
dem123456789 12:8644abfa86da 33 string GPS_PDOP="N/A";
dem123456789 14:92bacb5af01b 34 string GPS_Date="N/A";
dem123456789 14:92bacb5af01b 35 string GPS_VelocityKnot="N/A";
dem123456789 14:92bacb5af01b 36 string GPS_VelocityKph="N/A";
dem123456789 14:92bacb5af01b 37
dem123456789 12:8644abfa86da 38 int asterisk_idx;
dem123456789 9:bf5939466e86 39
dem123456789 20:a820531c78bc 40 double Longtitude_Path[MAX_TASK_SIZE];
dem123456789 20:a820531c78bc 41 double Latitude_Path[MAX_TASK_SIZE];
dem123456789 20:a820531c78bc 42
dem123456789 9:bf5939466e86 43 vector<string> split(const string &s, char delim) {
dem123456789 9:bf5939466e86 44 stringstream ss(s);
dem123456789 9:bf5939466e86 45 string item;
dem123456789 9:bf5939466e86 46 vector<string> tokens;
dem123456789 9:bf5939466e86 47 while (getline(ss, item, delim)) {
dem123456789 15:dbf20c1209ae 48 if (item.empty()) {
dem123456789 20:a820531c78bc 49 item = "N/A";
dem123456789 15:dbf20c1209ae 50 }
dem123456789 9:bf5939466e86 51 tokens.push_back(item);
dem123456789 9:bf5939466e86 52 }
dem123456789 9:bf5939466e86 53 return tokens;
dem123456789 9:bf5939466e86 54 }
dem123456789 9:bf5939466e86 55
dem123456789 9:bf5939466e86 56 void updateIMU(string IMU_data) {
dem123456789 9:bf5939466e86 57 string IMU_data_string(IMU_data);
dem123456789 9:bf5939466e86 58 if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) {
dem123456789 9:bf5939466e86 59 IMU_data_string = IMU_data_string.substr(5);
dem123456789 9:bf5939466e86 60 vector<string> result = split(IMU_data_string, ',');
dem123456789 9:bf5939466e86 61 IMU_Y = result.at(0);
dem123456789 9:bf5939466e86 62 IMU_P = result.at(1);
dem123456789 15:dbf20c1209ae 63 IMU_R = result.at(2).substr(0, result.at(2).size()-1);
dem123456789 9:bf5939466e86 64 }
dem123456789 9:bf5939466e86 65 }
dem123456789 9:bf5939466e86 66
dem123456789 10:12ba6ed2d6f0 67 void updateGPS(string GPS_data) {
dem123456789 10:12ba6ed2d6f0 68 string GPS_data_string(GPS_data);
dem123456789 11:1caacb994236 69 if (GPS_data_string.substr(0,6) == "$GPGGA") {
dem123456789 11:1caacb994236 70 GPS_data_string = GPS_data_string.substr(7);
dem123456789 10:12ba6ed2d6f0 71 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 72 GPS_Quality = result.at(5);
dem123456789 14:92bacb5af01b 73 if(GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 11:1caacb994236 74 GPS_UTC = result.at(0);
dem123456789 11:1caacb994236 75 GPS_Latitude = result.at(2) + result.at(1);
dem123456789 11:1caacb994236 76 GPS_Longtitude = result.at(4) + result.at(3);
dem123456789 11:1caacb994236 77 GPS_Num_Satellite = result.at(6);
dem123456789 11:1caacb994236 78 GPS_HDOP = result.at(7);
dem123456789 11:1caacb994236 79 GPS_Altitude = result.at(8) + result.at(9);
dem123456789 11:1caacb994236 80 }
dem123456789 14:92bacb5af01b 81 } else if (GPS_data_string.substr(0,6) == "$GPGSA" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 82 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 83 vector<string> result = split(GPS_data_string, ',');
dem123456789 12:8644abfa86da 84 GPS_PDOP = result.at(14);
dem123456789 12:8644abfa86da 85 asterisk_idx = result.at(16).find('*');
dem123456789 12:8644abfa86da 86 GPS_VDOP = result.at(16).substr(0, asterisk_idx);
dem123456789 14:92bacb5af01b 87 } else if (GPS_data_string.substr(0,6) == "$GPRMC" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 88 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 89 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 90 GPS_Date = result.at(8);
dem123456789 14:92bacb5af01b 91 } else if (GPS_data_string.substr(0,6) == "$GPVTG" and GPS_Quality != "0" and GPS_Quality != "6") {
dem123456789 12:8644abfa86da 92 GPS_data_string = GPS_data_string.substr(7);
dem123456789 12:8644abfa86da 93 vector<string> result = split(GPS_data_string, ',');
dem123456789 14:92bacb5af01b 94 GPS_VelocityKnot = result.at(4) + result.at(5);
dem123456789 12:8644abfa86da 95 asterisk_idx = result.at(7).find('*');
dem123456789 14:92bacb5af01b 96 GPS_VelocityKph = result.at(6) + result.at(7).substr(0, asterisk_idx);
dem123456789 10:12ba6ed2d6f0 97 }
dem123456789 12:8644abfa86da 98
dem123456789 10:12ba6ed2d6f0 99 }
dem123456789 16:0ea992d9a390 100 /* Get data from SailBoat
dem123456789 16:0ea992d9a390 101 @GET=parameter
dem123456789 16:0ea992d9a390 102 Ex: @GET=GPS_Quality
dem123456789 16:0ea992d9a390 103 Supported parameter: GPS_Quality, GPS_UTC, GPS_Latitude, GPS_Longtitude, GPS_Altitude,
dem123456789 16:0ea992d9a390 104 GPS_Num_Satellite, GPS_HDOP, GPS_VDOP, GPS_PDOP, GPS_Date, GPS_VelocityKnot, GPS_VelocityKph
dem123456789 16:0ea992d9a390 105 Set path to SailBoat
dem123456789 20:a820531c78bc 106 @SET=PATH, Latitude, Longtitude, Task id
dem123456789 20:a820531c78bc 107 @SET=PATH, 33.776318, -84.407590, 3
dem123456789 16:0ea992d9a390 108 */
dem123456789 15:dbf20c1209ae 109 void decodePC(string PC_data) {
dem123456789 15:dbf20c1209ae 110 string PC_data_string(PC_data);
dem123456789 15:dbf20c1209ae 111 if (PC_data_string.substr(0,4) == "@GET") {
dem123456789 15:dbf20c1209ae 112 pc.printf("%s", PC_data_string.c_str());
dem123456789 15:dbf20c1209ae 113 PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
dem123456789 20:a820531c78bc 114 pc.printf("%s\n", decodeCommandGET(PC_data_string).c_str());
dem123456789 16:0ea992d9a390 115 } else if (PC_data_string.substr(0,4) == "@SET") {
dem123456789 20:a820531c78bc 116 pc.printf("%s", PC_data_string.c_str());
dem123456789 20:a820531c78bc 117 PC_data_string = PC_data_string.substr(5, PC_data_string.size()-6);
dem123456789 20:a820531c78bc 118 string claim = decodeCommandSET(PC_data_string);
dem123456789 20:a820531c78bc 119 if (claim == "DONE") {
dem123456789 20:a820531c78bc 120 pc.printf("Current Path: Longtitude, Latitude\n");
dem123456789 20:a820531c78bc 121 for (int i=0;i<MAX_TASK_SIZE;++i) {
dem123456789 20:a820531c78bc 122 pc.printf(" %f, %f\n", Longtitude_Path[i], Latitude_Path[i]);
dem123456789 20:a820531c78bc 123 }
dem123456789 20:a820531c78bc 124 }
dem123456789 20:a820531c78bc 125 pc.printf("%s\n", claim.c_str());
dem123456789 16:0ea992d9a390 126 }
dem123456789 15:dbf20c1209ae 127 }
dem123456789 15:dbf20c1209ae 128
dem123456789 15:dbf20c1209ae 129
dem123456789 11:1caacb994236 130 void printStateIMU() {
dem123456789 11:1caacb994236 131 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 132 }
dem123456789 11:1caacb994236 133
dem123456789 12:8644abfa86da 134 void printStateGPS() {
dem123456789 14:92bacb5af01b 135 pc.printf("GPS_Quality: %s, GPS_UTC: %s, GPS_Latitude: %s, GPS_Longtitude: %s, GPS_Altitude: %s, "
dem123456789 14:92bacb5af01b 136 "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 137 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 138 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 139 }
dem123456789 14:92bacb5af01b 140
dem123456789 11:1caacb994236 141 //#YPR=-183.74,-134.27,-114.39
taoqiuyang 4:37d118f2348c 142 void IMU_serial_ISR() {
taoqiuyang 3:ab9f94d112c0 143 char buf;
taoqiuyang 3:ab9f94d112c0 144
taoqiuyang 3:ab9f94d112c0 145 while (IMU.readable()) {
taoqiuyang 3:ab9f94d112c0 146 buf = IMU.getc();
taoqiuyang 3:ab9f94d112c0 147
dem123456789 14:92bacb5af01b 148 //pc.putc(buf);
taoqiuyang 3:ab9f94d112c0 149 IMU_message[IMU_message_counter]=buf;
dem123456789 9:bf5939466e86 150 IMU_message_counter+=1;
taoqiuyang 3:ab9f94d112c0 151
taoqiuyang 3:ab9f94d112c0 152 if (buf=='\n'){
dem123456789 9:bf5939466e86 153 string IMU_copy(IMU_message, IMU_message_counter);
dem123456789 11:1caacb994236 154 //pc.printf("%s", IMU_copy.c_str());
dem123456789 9:bf5939466e86 155 updateIMU(IMU_copy);
dem123456789 9:bf5939466e86 156 IMU_message_counter=0;
dem123456789 9:bf5939466e86 157 IMU_copy[0] = '\0';
dem123456789 9:bf5939466e86 158 }
dem123456789 9:bf5939466e86 159
taoqiuyang 3:ab9f94d112c0 160 }
taoqiuyang 2:afb333543af5 161 led2 = !led2;
taoqiuyang 2:afb333543af5 162 }
dem123456789 9:bf5939466e86 163
dem123456789 9:bf5939466e86 164
dem123456789 8:1f5a44bade4d 165
taoqiuyang 5:451b8203ef99 166 void GPS_serial_ISR() {
taoqiuyang 5:451b8203ef99 167 char buf;
taoqiuyang 5:451b8203ef99 168
taoqiuyang 5:451b8203ef99 169 while (GPS.readable()) {
taoqiuyang 5:451b8203ef99 170 buf = GPS.getc();
dem123456789 14:92bacb5af01b 171
dem123456789 14:92bacb5af01b 172 //pc.putc(buf);
dem123456789 10:12ba6ed2d6f0 173 GPS_message[GPS_message_counter]=buf;
dem123456789 10:12ba6ed2d6f0 174 GPS_message_counter+=1;
dem123456789 10:12ba6ed2d6f0 175
dem123456789 10:12ba6ed2d6f0 176 if (buf=='\n'){
dem123456789 10:12ba6ed2d6f0 177 string GPS_copy(GPS_message, GPS_message_counter);
dem123456789 11:1caacb994236 178 //pc.printf("%s", GPS_copy.c_str());
dem123456789 10:12ba6ed2d6f0 179 updateGPS(GPS_copy);
dem123456789 10:12ba6ed2d6f0 180 GPS_message_counter=0;
dem123456789 10:12ba6ed2d6f0 181 GPS_copy[0] = '\0';
dem123456789 10:12ba6ed2d6f0 182 }
taoqiuyang 5:451b8203ef99 183 }
taoqiuyang 5:451b8203ef99 184
taoqiuyang 5:451b8203ef99 185 led3 = !led3;
taoqiuyang 5:451b8203ef99 186 }
taoqiuyang 3:ab9f94d112c0 187
dem123456789 8:1f5a44bade4d 188 void PC_serial_ISR() {
dem123456789 8:1f5a44bade4d 189 char buf;
dem123456789 8:1f5a44bade4d 190
dem123456789 8:1f5a44bade4d 191 while (pc.readable()) {
dem123456789 8:1f5a44bade4d 192 buf = pc.getc();
dem123456789 14:92bacb5af01b 193
dem123456789 14:92bacb5af01b 194 //pc.putc(buf);
dem123456789 14:92bacb5af01b 195 PC_message[PC_message_counter]=buf;
dem123456789 14:92bacb5af01b 196 PC_message_counter+=1;
dem123456789 14:92bacb5af01b 197
dem123456789 14:92bacb5af01b 198 if (buf=='\n'){
dem123456789 14:92bacb5af01b 199 string PC_copy(PC_message, PC_message_counter);
dem123456789 14:92bacb5af01b 200 //pc.printf("%s", PC_copy.c_str());
dem123456789 14:92bacb5af01b 201 decodePC(PC_copy);
dem123456789 14:92bacb5af01b 202 PC_message_counter=0;
dem123456789 14:92bacb5af01b 203 PC_copy[0] = '\0';
dem123456789 14:92bacb5af01b 204 }
dem123456789 8:1f5a44bade4d 205 }
dem123456789 8:1f5a44bade4d 206
dem123456789 8:1f5a44bade4d 207 led4= !led4;
dem123456789 8:1f5a44bade4d 208 }
dem123456789 21:99be83550601 209
dem123456789 21:99be83550601 210 void set_servo_position(Servo targetServo, float angleDeg, float minDeg, float minNorVal, float maxDeg, float maxNorVal){
dem123456789 21:99be83550601 211 /*angleDeg: desired angle
dem123456789 21:99be83550601 212 minDeg: minimum angle in degrees
dem123456789 21:99be83550601 213 minNorVal: normalized value[0,1] when servo is at its minimum angle
dem123456789 21:99be83550601 214 maxDeg: maximum angle in degrees
dem123456789 21:99be83550601 215 maxNorVal: normalized value[0,1] when servo is at its maximum angle
dem123456789 21:99be83550601 216 */
dem123456789 21:99be83550601 217
dem123456789 21:99be83550601 218 float pos; //normalized angle, [0,1]
dem123456789 21:99be83550601 219 float scale; //scale factor for servo calibration
dem123456789 21:99be83550601 220
dem123456789 21:99be83550601 221 //extreme conditions
dem123456789 21:99be83550601 222 if (angleDeg<minDeg){
dem123456789 21:99be83550601 223 pos=minNorVal;
dem123456789 21:99be83550601 224 }
dem123456789 21:99be83550601 225 if (angleDeg>maxDeg){
dem123456789 21:99be83550601 226 pos=maxNorVal;
dem123456789 21:99be83550601 227 }
dem123456789 21:99be83550601 228
dem123456789 21:99be83550601 229 //Calculate scale factor for servo calibration
dem123456789 21:99be83550601 230 scale = (angleDeg-minDeg)/(maxDeg-minDeg);
dem123456789 21:99be83550601 231 //Calculate servo normalized value
dem123456789 21:99be83550601 232 pos = minNorVal + scale*(maxNorVal-minNorVal);
dem123456789 21:99be83550601 233
dem123456789 21:99be83550601 234 //send signal to servo
dem123456789 21:99be83550601 235 targetServo=pos;
dem123456789 21:99be83550601 236 }
dem123456789 21:99be83550601 237
dem123456789 21:99be83550601 238 int log_data_SD(){
dem123456789 21:99be83550601 239 FILE *fp = fopen("/sd/dataLog/dataLog.txt", "w");
dem123456789 21:99be83550601 240 if(fp == NULL) {
dem123456789 21:99be83550601 241 return 0;
dem123456789 21:99be83550601 242 }else{
dem123456789 21:99be83550601 243 //Write all the useful data to the SD card
dem123456789 21:99be83550601 244 fprintf(fp, "Nya Pass~");
dem123456789 21:99be83550601 245 fclose(fp);
dem123456789 21:99be83550601 246 return 1;
dem123456789 21:99be83550601 247 }
dem123456789 21:99be83550601 248 }
dem123456789 21:99be83550601 249
dem123456789 21:99be83550601 250
taoqiuyang 0:f4d390c06705 251 int main() {
taoqiuyang 2:afb333543af5 252 IMU.baud(57600);
taoqiuyang 4:37d118f2348c 253 IMU.attach(&IMU_serial_ISR);
taoqiuyang 5:451b8203ef99 254 GPS.baud(38400);
taoqiuyang 5:451b8203ef99 255 GPS.attach(&GPS_serial_ISR);
dem123456789 8:1f5a44bade4d 256 pc.baud(115200);
dem123456789 8:1f5a44bade4d 257 pc.attach(&PC_serial_ISR);
taoqiuyang 1:e7245ffb4820 258
dem123456789 21:99be83550601 259 float angle=20;
taoqiuyang 2:afb333543af5 260 while (1) {
dem123456789 21:99be83550601 261 if (angle>160){angle=20;}
dem123456789 21:99be83550601 262 set_servo_position(rudderServo, angle, 0, 0, 180, 1);
dem123456789 21:99be83550601 263 set_servo_position(wingServo, angle, 0, 0, 180, 1);
dem123456789 21:99be83550601 264 angle=angle+10;
dem123456789 15:dbf20c1209ae 265 wait(0.4);
dem123456789 15:dbf20c1209ae 266 //printStateIMU();
dem123456789 15:dbf20c1209ae 267 //printStateGPS();
dem123456789 21:99be83550601 268 led1 = !led1;
taoqiuyang 0:f4d390c06705 269 }
taoqiuyang 0:f4d390c06705 270 }