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@11:1caacb994236, 2015-08-25 (annotated)

Committer:
dem123456789
Date:
Tue Aug 25 05:32:46 2015 +0000
Revision:
11:1caacb994236
Parent:
10:12ba6ed2d6f0
Child:
12:8644abfa86da
GPS almost 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 11:1caacb994236 5 using namespace std;
dem123456789 11:1caacb994236 6
dem123456789 11:1caacb994236 7 #define MAX_IMU_SIZE 29
taoqiuyang 3:ab9f94d112c0 8
taoqiuyang 2:afb333543af5 9 DigitalOut led1(LED1);
taoqiuyang 2:afb333543af5 10 DigitalOut led2(LED2);
taoqiuyang 5:451b8203ef99 11 DigitalOut led3(LED3);
dem123456789 8:1f5a44bade4d 12 DigitalOut led4(LED4);
taoqiuyang 2:afb333543af5 13
taoqiuyang 2:afb333543af5 14 Serial pc(USBTX, USBRX);
taoqiuyang 2:afb333543af5 15 Serial IMU(p28, p27); // tx, rx
taoqiuyang 5:451b8203ef99 16 Serial GPS(p13, p14); // tx, rx
taoqiuyang 2:afb333543af5 17
taoqiuyang 2:afb333543af5 18 char IMU_message[256];
taoqiuyang 3:ab9f94d112c0 19 int IMU_message_counter=0;
taoqiuyang 5:451b8203ef99 20 char GPS_message[256];
taoqiuyang 5:451b8203ef99 21 int GPS_message_counter=0;
dem123456789 11:1caacb994236 22
dem123456789 11:1caacb994236 23 string IMU_Y="N/A";
dem123456789 11:1caacb994236 24 string IMU_P="N/A";
dem123456789 11:1caacb994236 25 string IMU_R="N/A";
dem123456789 11:1caacb994236 26 string GPS_quality="N/A";
dem123456789 11:1caacb994236 27 string GPS_UTC="N/A";
dem123456789 11:1caacb994236 28 string GPS_Latitude="N/A";
dem123456789 11:1caacb994236 29 string GPS_Longtitude="N/A";
dem123456789 11:1caacb994236 30 string GPS_Altitude="N/A";
dem123456789 11:1caacb994236 31 string GPS_Num_Satellite="N/A";
dem123456789 11:1caacb994236 32 string GPS_HDOP="N/A";
dem123456789 11:1caacb994236 33
dem123456789 11:1caacb994236 34
dem123456789 9:bf5939466e86 35
dem123456789 9:bf5939466e86 36 vector<string> split(const string &s, char delim) {
dem123456789 9:bf5939466e86 37 stringstream ss(s);
dem123456789 9:bf5939466e86 38 string item;
dem123456789 9:bf5939466e86 39 vector<string> tokens;
dem123456789 9:bf5939466e86 40 while (getline(ss, item, delim)) {
dem123456789 9:bf5939466e86 41 tokens.push_back(item);
dem123456789 9:bf5939466e86 42 }
dem123456789 9:bf5939466e86 43 return tokens;
dem123456789 9:bf5939466e86 44 }
dem123456789 9:bf5939466e86 45
dem123456789 9:bf5939466e86 46 void updateIMU(string IMU_data) {
dem123456789 9:bf5939466e86 47 string IMU_data_string(IMU_data);
dem123456789 9:bf5939466e86 48 if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) {
dem123456789 9:bf5939466e86 49 IMU_data_string = IMU_data_string.substr(5);
dem123456789 9:bf5939466e86 50 vector<string> result = split(IMU_data_string, ',');
dem123456789 9:bf5939466e86 51 IMU_Y = result.at(0);
dem123456789 9:bf5939466e86 52 IMU_P = result.at(1);
dem123456789 9:bf5939466e86 53 IMU_R = result.at(2).substr(0, result.at(2).size()-1);;
dem123456789 9:bf5939466e86 54 }
dem123456789 9:bf5939466e86 55 }
dem123456789 9:bf5939466e86 56
dem123456789 10:12ba6ed2d6f0 57 void updateGPS(string GPS_data) {
dem123456789 10:12ba6ed2d6f0 58 string GPS_data_string(GPS_data);
dem123456789 11:1caacb994236 59 if (GPS_data_string.substr(0,6) == "$GPGGA") {
dem123456789 11:1caacb994236 60 GPS_data_string = GPS_data_string.substr(7);
dem123456789 10:12ba6ed2d6f0 61 vector<string> result = split(GPS_data_string, ',');
dem123456789 11:1caacb994236 62 GPS_quality = result.at(5);
dem123456789 11:1caacb994236 63 if(GPS_quality == "1") {
dem123456789 11:1caacb994236 64 GPS_UTC = result.at(0);
dem123456789 11:1caacb994236 65 GPS_Latitude = result.at(2) + result.at(1);
dem123456789 11:1caacb994236 66 GPS_Longtitude = result.at(4) + result.at(3);
dem123456789 11:1caacb994236 67 GPS_Num_Satellite = result.at(6);
dem123456789 11:1caacb994236 68 GPS_HDOP = result.at(7);
dem123456789 11:1caacb994236 69 GPS_Altitude = result.at(8) + result.at(9);
dem123456789 11:1caacb994236 70 //pc.printf("%s, %s, %s, %s, %s, %s, %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());
dem123456789 11:1caacb994236 71 }
dem123456789 10:12ba6ed2d6f0 72 }
dem123456789 10:12ba6ed2d6f0 73 }
dem123456789 10:12ba6ed2d6f0 74
dem123456789 11:1caacb994236 75 void printStateIMU() {
dem123456789 11:1caacb994236 76 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 77 }
dem123456789 11:1caacb994236 78
dem123456789 11:1caacb994236 79 void printStateGPS() {
dem123456789 11:1caacb994236 80 pc.printf("%s, %s, %s, %s, %s, %s, %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());
dem123456789 9:bf5939466e86 81 }
dem123456789 10:12ba6ed2d6f0 82
dem123456789 10:12ba6ed2d6f0 83
dem123456789 11:1caacb994236 84 //#YPR=-183.74,-134.27,-114.39
taoqiuyang 4:37d118f2348c 85 void IMU_serial_ISR() {
taoqiuyang 3:ab9f94d112c0 86 char buf;
taoqiuyang 3:ab9f94d112c0 87
taoqiuyang 3:ab9f94d112c0 88 while (IMU.readable()) {
taoqiuyang 3:ab9f94d112c0 89 buf = IMU.getc();
taoqiuyang 3:ab9f94d112c0 90
dem123456789 9:bf5939466e86 91
taoqiuyang 3:ab9f94d112c0 92 IMU_message[IMU_message_counter]=buf;
dem123456789 9:bf5939466e86 93 IMU_message_counter+=1;
taoqiuyang 3:ab9f94d112c0 94
taoqiuyang 3:ab9f94d112c0 95 if (buf=='\n'){
dem123456789 9:bf5939466e86 96 string IMU_copy(IMU_message, IMU_message_counter);
dem123456789 11:1caacb994236 97 //pc.printf("%s", IMU_copy.c_str());
dem123456789 9:bf5939466e86 98 updateIMU(IMU_copy);
dem123456789 11:1caacb994236 99 printStateIMU();
dem123456789 9:bf5939466e86 100 IMU_message_counter=0;
dem123456789 9:bf5939466e86 101 IMU_copy[0] = '\0';
dem123456789 9:bf5939466e86 102 }
dem123456789 9:bf5939466e86 103
taoqiuyang 3:ab9f94d112c0 104 }
taoqiuyang 2:afb333543af5 105 led2 = !led2;
taoqiuyang 2:afb333543af5 106 }
dem123456789 9:bf5939466e86 107
dem123456789 9:bf5939466e86 108
dem123456789 8:1f5a44bade4d 109
taoqiuyang 5:451b8203ef99 110 void GPS_serial_ISR() {
taoqiuyang 5:451b8203ef99 111 char buf;
taoqiuyang 5:451b8203ef99 112
taoqiuyang 5:451b8203ef99 113 while (GPS.readable()) {
taoqiuyang 5:451b8203ef99 114 buf = GPS.getc();
dem123456789 10:12ba6ed2d6f0 115
dem123456789 10:12ba6ed2d6f0 116
dem123456789 10:12ba6ed2d6f0 117 GPS_message[GPS_message_counter]=buf;
dem123456789 10:12ba6ed2d6f0 118 GPS_message_counter+=1;
dem123456789 10:12ba6ed2d6f0 119
dem123456789 10:12ba6ed2d6f0 120 if (buf=='\n'){
dem123456789 10:12ba6ed2d6f0 121 string GPS_copy(GPS_message, GPS_message_counter);
dem123456789 11:1caacb994236 122 //pc.printf("%s", GPS_copy.c_str());
dem123456789 10:12ba6ed2d6f0 123 updateGPS(GPS_copy);
dem123456789 11:1caacb994236 124 printStateGPS();
dem123456789 10:12ba6ed2d6f0 125 GPS_message_counter=0;
dem123456789 10:12ba6ed2d6f0 126 GPS_copy[0] = '\0';
dem123456789 10:12ba6ed2d6f0 127 }
taoqiuyang 5:451b8203ef99 128 }
taoqiuyang 5:451b8203ef99 129
taoqiuyang 5:451b8203ef99 130 led3 = !led3;
taoqiuyang 5:451b8203ef99 131 }
taoqiuyang 3:ab9f94d112c0 132
dem123456789 8:1f5a44bade4d 133 void PC_serial_ISR() {
dem123456789 8:1f5a44bade4d 134 char buf;
dem123456789 8:1f5a44bade4d 135
dem123456789 8:1f5a44bade4d 136 while (pc.readable()) {
dem123456789 8:1f5a44bade4d 137 buf = pc.getc();
dem123456789 9:bf5939466e86 138 //pc.putc(buf);
dem123456789 8:1f5a44bade4d 139 }
dem123456789 8:1f5a44bade4d 140
dem123456789 8:1f5a44bade4d 141 led4= !led4;
dem123456789 8:1f5a44bade4d 142 }
taoqiuyang 3:ab9f94d112c0 143
taoqiuyang 0:f4d390c06705 144 int main() {
taoqiuyang 2:afb333543af5 145 IMU.baud(57600);
taoqiuyang 4:37d118f2348c 146 IMU.attach(&IMU_serial_ISR);
taoqiuyang 5:451b8203ef99 147 GPS.baud(38400);
taoqiuyang 5:451b8203ef99 148 GPS.attach(&GPS_serial_ISR);
dem123456789 8:1f5a44bade4d 149 pc.baud(115200);
dem123456789 8:1f5a44bade4d 150 pc.attach(&PC_serial_ISR);
taoqiuyang 1:e7245ffb4820 151
taoqiuyang 2:afb333543af5 152 while (1) {
dem123456789 8:1f5a44bade4d 153
taoqiuyang 2:afb333543af5 154 led1 = !led1;
taoqiuyang 5:451b8203ef99 155 wait(0.2);
taoqiuyang 0:f4d390c06705 156 }
taoqiuyang 0:f4d390c06705 157 }