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@19:eef0e3ec32a0, 2015-08-28 (annotated)

Committer:
taoqiuyang
Date:
Fri Aug 28 08:04:57 2015 +0000
Revision:
19:eef0e3ec32a0
Parent:
18:9a30764847d2
Child:
20:a820531c78bc
SD card added

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