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@10:12ba6ed2d6f0, 2015-08-24 (annotated)

Committer:
dem123456789
Date:
Mon Aug 24 07:32:13 2015 +0000
Revision:
10:12ba6ed2d6f0
Parent:
9:bf5939466e86
Child:
11:1caacb994236
GPS half progress

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 9:bf5939466e86 5 #define MAX_IMU_SIZE 28
dem123456789 10:12ba6ed2d6f0 6 #define MAX_GPS_GPGGA_SIZE 72
dem123456789 10:12ba6ed2d6f0 7 #define MAX_GPS_GPGSV_SIZE 210
dem123456789 10:12ba6ed2d6f0 8 #define MAX_GPS_GPRMC_SIZE 70
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 10:12ba6ed2d6f0 23 string IMU_Y="Not ready";
dem123456789 10:12ba6ed2d6f0 24 string IMU_P="Not ready";
dem123456789 10:12ba6ed2d6f0 25 string IMU_R="Not ready";
dem123456789 9:bf5939466e86 26
dem123456789 9:bf5939466e86 27 vector<string> split(const string &s, char delim) {
dem123456789 9:bf5939466e86 28 stringstream ss(s);
dem123456789 9:bf5939466e86 29 string item;
dem123456789 9:bf5939466e86 30 vector<string> tokens;
dem123456789 9:bf5939466e86 31 while (getline(ss, item, delim)) {
dem123456789 9:bf5939466e86 32 tokens.push_back(item);
dem123456789 9:bf5939466e86 33 }
dem123456789 9:bf5939466e86 34 return tokens;
dem123456789 9:bf5939466e86 35 }
dem123456789 9:bf5939466e86 36
dem123456789 9:bf5939466e86 37 void updateIMU(string IMU_data) {
dem123456789 9:bf5939466e86 38 string IMU_data_string(IMU_data);
dem123456789 9:bf5939466e86 39 if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) {
dem123456789 9:bf5939466e86 40 IMU_data_string = IMU_data_string.substr(5);
dem123456789 9:bf5939466e86 41 vector<string> result = split(IMU_data_string, ',');
dem123456789 9:bf5939466e86 42 IMU_Y = result.at(0);
dem123456789 9:bf5939466e86 43 IMU_P = result.at(1);
dem123456789 9:bf5939466e86 44 IMU_R = result.at(2).substr(0, result.at(2).size()-1);;
dem123456789 9:bf5939466e86 45 }
dem123456789 9:bf5939466e86 46 }
dem123456789 9:bf5939466e86 47
dem123456789 10:12ba6ed2d6f0 48 void updateGPS(string GPS_data) {
dem123456789 10:12ba6ed2d6f0 49 string GPS_data_string(GPS_data);
dem123456789 10:12ba6ed2d6f0 50 if (GPS_data_string.substr(0,6) == "$GPGGA" and GPS_data_string.size() <= MAX_GPS_GPGGA_SIZE) {
dem123456789 10:12ba6ed2d6f0 51 GPS_data_string = IMU_data_string.substr(5);
dem123456789 10:12ba6ed2d6f0 52 vector<string> result = split(GPS_data_string, ',');
dem123456789 10:12ba6ed2d6f0 53 IMU_Y = result.at(0);
dem123456789 10:12ba6ed2d6f0 54 IMU_P = result.at(1);
dem123456789 10:12ba6ed2d6f0 55 IMU_R = result.at(2).substr(0, result.at(2).size()-1);
dem123456789 10:12ba6ed2d6f0 56 } else if (GPS_data_string.substr(0,6) == "%GPGSV" and GPS_data_string.size() <= MAX_GPS_GPGSV_SIZE) {
dem123456789 10:12ba6ed2d6f0 57 GPS_data_string = IMU_data_string.substr(5);
dem123456789 10:12ba6ed2d6f0 58 vector<string> result = split(GPS_data_string, ',');
dem123456789 10:12ba6ed2d6f0 59 IMU_Y = result.at(0);
dem123456789 10:12ba6ed2d6f0 60 IMU_P = result.at(1);
dem123456789 10:12ba6ed2d6f0 61 IMU_R = result.at(2).substr(0, result.at(2).size()-1);
dem123456789 10:12ba6ed2d6f0 62 } else if (GPS_data_string.substr(0,6) == "%GPRMC" and GPS_data_string.size() <= MAX_GPS_GPRMC_SIZE) {
dem123456789 10:12ba6ed2d6f0 63 GPS_data_string = IMU_data_string.substr(5);
dem123456789 10:12ba6ed2d6f0 64 vector<string> result = split(GPS_data_string, ',');
dem123456789 10:12ba6ed2d6f0 65 IMU_Y = result.at(0);
dem123456789 10:12ba6ed2d6f0 66 IMU_P = result.at(1);
dem123456789 10:12ba6ed2d6f0 67 IMU_R = result.at(2).substr(0, result.at(2).size()-1);
dem123456789 10:12ba6ed2d6f0 68 }
dem123456789 10:12ba6ed2d6f0 69 }
dem123456789 10:12ba6ed2d6f0 70
dem123456789 9:bf5939466e86 71 void printState() {
dem123456789 9:bf5939466e86 72 pc.printf("IMU_Y: %s, IMU_P: %s, IMU_R: %s\n",IMU_Y,IMU_P,IMU_R);
dem123456789 9:bf5939466e86 73 }
dem123456789 10:12ba6ed2d6f0 74
dem123456789 10:12ba6ed2d6f0 75
dem123456789 9:bf5939466e86 76 //#YPR=-183.-174,-134.27,-114.39
taoqiuyang 4:37d118f2348c 77 void IMU_serial_ISR() {
taoqiuyang 3:ab9f94d112c0 78 char buf;
taoqiuyang 3:ab9f94d112c0 79
taoqiuyang 3:ab9f94d112c0 80 while (IMU.readable()) {
taoqiuyang 3:ab9f94d112c0 81 buf = IMU.getc();
taoqiuyang 3:ab9f94d112c0 82
dem123456789 9:bf5939466e86 83
taoqiuyang 3:ab9f94d112c0 84 IMU_message[IMU_message_counter]=buf;
dem123456789 9:bf5939466e86 85 IMU_message_counter+=1;
taoqiuyang 3:ab9f94d112c0 86
taoqiuyang 3:ab9f94d112c0 87 if (buf=='\n'){
dem123456789 9:bf5939466e86 88 string IMU_copy(IMU_message, IMU_message_counter);
dem123456789 9:bf5939466e86 89 //pc.printf("%s\n", IMU_copy);
dem123456789 9:bf5939466e86 90 updateIMU(IMU_copy);
dem123456789 9:bf5939466e86 91 printState();
dem123456789 9:bf5939466e86 92 IMU_message_counter=0;
dem123456789 9:bf5939466e86 93 IMU_copy[0] = '\0';
dem123456789 9:bf5939466e86 94 }
dem123456789 9:bf5939466e86 95
taoqiuyang 3:ab9f94d112c0 96 }
taoqiuyang 2:afb333543af5 97 led2 = !led2;
taoqiuyang 2:afb333543af5 98 }
dem123456789 9:bf5939466e86 99
dem123456789 9:bf5939466e86 100
dem123456789 8:1f5a44bade4d 101
taoqiuyang 5:451b8203ef99 102 void GPS_serial_ISR() {
taoqiuyang 5:451b8203ef99 103 char buf;
taoqiuyang 5:451b8203ef99 104
taoqiuyang 5:451b8203ef99 105 while (GPS.readable()) {
taoqiuyang 5:451b8203ef99 106 buf = GPS.getc();
dem123456789 10:12ba6ed2d6f0 107
dem123456789 10:12ba6ed2d6f0 108
dem123456789 10:12ba6ed2d6f0 109 GPS_message[GPS_message_counter]=buf;
dem123456789 10:12ba6ed2d6f0 110 GPS_message_counter+=1;
dem123456789 10:12ba6ed2d6f0 111
dem123456789 10:12ba6ed2d6f0 112 if (buf=='\n'){
dem123456789 10:12ba6ed2d6f0 113 string GPS_copy(GPS_message, GPS_message_counter);
dem123456789 10:12ba6ed2d6f0 114 //pc.printf("%s\n", IMU_copy);
dem123456789 10:12ba6ed2d6f0 115 updateGPS(GPS_copy);
dem123456789 10:12ba6ed2d6f0 116 printState();
dem123456789 10:12ba6ed2d6f0 117 GPS_message_counter=0;
dem123456789 10:12ba6ed2d6f0 118 GPS_copy[0] = '\0';
dem123456789 10:12ba6ed2d6f0 119 }
taoqiuyang 5:451b8203ef99 120 }
taoqiuyang 5:451b8203ef99 121
taoqiuyang 5:451b8203ef99 122 led3 = !led3;
taoqiuyang 5:451b8203ef99 123 }
taoqiuyang 3:ab9f94d112c0 124
dem123456789 8:1f5a44bade4d 125 void PC_serial_ISR() {
dem123456789 8:1f5a44bade4d 126 char buf;
dem123456789 8:1f5a44bade4d 127
dem123456789 8:1f5a44bade4d 128 while (pc.readable()) {
dem123456789 8:1f5a44bade4d 129 buf = pc.getc();
dem123456789 9:bf5939466e86 130 //pc.putc(buf);
dem123456789 8:1f5a44bade4d 131 }
dem123456789 8:1f5a44bade4d 132
dem123456789 8:1f5a44bade4d 133 led4= !led4;
dem123456789 8:1f5a44bade4d 134 }
taoqiuyang 3:ab9f94d112c0 135
taoqiuyang 0:f4d390c06705 136 int main() {
taoqiuyang 2:afb333543af5 137 IMU.baud(57600);
taoqiuyang 4:37d118f2348c 138 IMU.attach(&IMU_serial_ISR);
taoqiuyang 5:451b8203ef99 139 GPS.baud(38400);
taoqiuyang 5:451b8203ef99 140 GPS.attach(&GPS_serial_ISR);
dem123456789 8:1f5a44bade4d 141 pc.baud(115200);
dem123456789 8:1f5a44bade4d 142 pc.attach(&PC_serial_ISR);
taoqiuyang 1:e7245ffb4820 143
taoqiuyang 2:afb333543af5 144 while (1) {
dem123456789 8:1f5a44bade4d 145
taoqiuyang 2:afb333543af5 146 led1 = !led1;
taoqiuyang 5:451b8203ef99 147 wait(0.2);
taoqiuyang 0:f4d390c06705 148 }
taoqiuyang 0:f4d390c06705 149 }