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@9:bf5939466e86, 2015-08-24 (annotated)

Committer:
dem123456789
Date:
Mon Aug 24 05:08:35 2015 +0000
Revision:
9:bf5939466e86
Parent:
8:1f5a44bade4d
Child:
10:12ba6ed2d6f0
IMU signal processing finished

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
taoqiuyang 3:ab9f94d112c0 6
taoqiuyang 2:afb333543af5 7 DigitalOut led1(LED1);
taoqiuyang 2:afb333543af5 8 DigitalOut led2(LED2);
taoqiuyang 5:451b8203ef99 9 DigitalOut led3(LED3);
dem123456789 8:1f5a44bade4d 10 DigitalOut led4(LED4);
taoqiuyang 2:afb333543af5 11
taoqiuyang 2:afb333543af5 12 Serial pc(USBTX, USBRX);
taoqiuyang 2:afb333543af5 13 Serial IMU(p28, p27); // tx, rx
taoqiuyang 5:451b8203ef99 14 Serial GPS(p13, p14); // tx, rx
taoqiuyang 2:afb333543af5 15
taoqiuyang 2:afb333543af5 16 char IMU_message[256];
taoqiuyang 3:ab9f94d112c0 17 int IMU_message_counter=0;
taoqiuyang 5:451b8203ef99 18 char GPS_message[256];
taoqiuyang 5:451b8203ef99 19 int GPS_message_counter=0;
dem123456789 9:bf5939466e86 20 string IMU_Y;
dem123456789 9:bf5939466e86 21 string IMU_P;
dem123456789 9:bf5939466e86 22 string IMU_R;
dem123456789 9:bf5939466e86 23
dem123456789 9:bf5939466e86 24 vector<string> split(const string &s, char delim) {
dem123456789 9:bf5939466e86 25 stringstream ss(s);
dem123456789 9:bf5939466e86 26 string item;
dem123456789 9:bf5939466e86 27 vector<string> tokens;
dem123456789 9:bf5939466e86 28 while (getline(ss, item, delim)) {
dem123456789 9:bf5939466e86 29 tokens.push_back(item);
dem123456789 9:bf5939466e86 30 }
dem123456789 9:bf5939466e86 31 return tokens;
dem123456789 9:bf5939466e86 32 }
dem123456789 9:bf5939466e86 33
dem123456789 9:bf5939466e86 34 void updateIMU(string IMU_data) {
dem123456789 9:bf5939466e86 35 string IMU_data_string(IMU_data);
dem123456789 9:bf5939466e86 36 if (IMU_data_string.substr(0,4) == "#YPR" and IMU_data_string.size() <= MAX_IMU_SIZE) {
dem123456789 9:bf5939466e86 37 IMU_data_string = IMU_data_string.substr(5);
dem123456789 9:bf5939466e86 38 vector<string> result = split(IMU_data_string, ',');
dem123456789 9:bf5939466e86 39 IMU_Y = result.at(0);
dem123456789 9:bf5939466e86 40 IMU_P = result.at(1);
dem123456789 9:bf5939466e86 41 IMU_R = result.at(2).substr(0, result.at(2).size()-1);;
dem123456789 9:bf5939466e86 42 }
dem123456789 9:bf5939466e86 43 }
dem123456789 9:bf5939466e86 44
dem123456789 9:bf5939466e86 45 void printState() {
dem123456789 9:bf5939466e86 46 pc.printf("IMU_Y: %s, IMU_P: %s, IMU_R: %s\n",IMU_Y,IMU_P,IMU_R);
dem123456789 9:bf5939466e86 47 }
dem123456789 9:bf5939466e86 48 //#YPR=-183.-174,-134.27,-114.39
taoqiuyang 4:37d118f2348c 49 void IMU_serial_ISR() {
taoqiuyang 3:ab9f94d112c0 50 char buf;
taoqiuyang 3:ab9f94d112c0 51
taoqiuyang 3:ab9f94d112c0 52 while (IMU.readable()) {
taoqiuyang 3:ab9f94d112c0 53 buf = IMU.getc();
taoqiuyang 3:ab9f94d112c0 54
dem123456789 9:bf5939466e86 55
taoqiuyang 3:ab9f94d112c0 56 IMU_message[IMU_message_counter]=buf;
dem123456789 9:bf5939466e86 57 IMU_message_counter+=1;
taoqiuyang 3:ab9f94d112c0 58
taoqiuyang 3:ab9f94d112c0 59 if (buf=='\n'){
dem123456789 9:bf5939466e86 60 string IMU_copy(IMU_message, IMU_message_counter);
dem123456789 9:bf5939466e86 61 //pc.printf("%s\n", IMU_copy);
dem123456789 9:bf5939466e86 62 updateIMU(IMU_copy);
dem123456789 9:bf5939466e86 63 printState();
dem123456789 9:bf5939466e86 64 IMU_message_counter=0;
dem123456789 9:bf5939466e86 65 IMU_copy[0] = '\0';
dem123456789 9:bf5939466e86 66 }
dem123456789 9:bf5939466e86 67
taoqiuyang 3:ab9f94d112c0 68 }
taoqiuyang 2:afb333543af5 69 led2 = !led2;
taoqiuyang 2:afb333543af5 70 }
dem123456789 9:bf5939466e86 71
dem123456789 9:bf5939466e86 72
dem123456789 8:1f5a44bade4d 73
taoqiuyang 5:451b8203ef99 74 void GPS_serial_ISR() {
taoqiuyang 5:451b8203ef99 75 char buf;
taoqiuyang 5:451b8203ef99 76
taoqiuyang 5:451b8203ef99 77 while (GPS.readable()) {
taoqiuyang 5:451b8203ef99 78 buf = GPS.getc();
taoqiuyang 5:451b8203ef99 79 //pc.putc(buf);
taoqiuyang 5:451b8203ef99 80 }
taoqiuyang 5:451b8203ef99 81
taoqiuyang 5:451b8203ef99 82 led3 = !led3;
taoqiuyang 5:451b8203ef99 83 }
taoqiuyang 3:ab9f94d112c0 84
dem123456789 8:1f5a44bade4d 85 void PC_serial_ISR() {
dem123456789 8:1f5a44bade4d 86 char buf;
dem123456789 8:1f5a44bade4d 87
dem123456789 8:1f5a44bade4d 88 while (pc.readable()) {
dem123456789 8:1f5a44bade4d 89 buf = pc.getc();
dem123456789 9:bf5939466e86 90 //pc.putc(buf);
dem123456789 8:1f5a44bade4d 91 }
dem123456789 8:1f5a44bade4d 92
dem123456789 8:1f5a44bade4d 93 led4= !led4;
dem123456789 8:1f5a44bade4d 94 }
taoqiuyang 3:ab9f94d112c0 95
taoqiuyang 0:f4d390c06705 96 int main() {
taoqiuyang 2:afb333543af5 97 IMU.baud(57600);
taoqiuyang 4:37d118f2348c 98 IMU.attach(&IMU_serial_ISR);
taoqiuyang 5:451b8203ef99 99 GPS.baud(38400);
taoqiuyang 5:451b8203ef99 100 GPS.attach(&GPS_serial_ISR);
dem123456789 8:1f5a44bade4d 101 pc.baud(115200);
dem123456789 8:1f5a44bade4d 102 pc.attach(&PC_serial_ISR);
taoqiuyang 1:e7245ffb4820 103
taoqiuyang 2:afb333543af5 104 while (1) {
dem123456789 8:1f5a44bade4d 105
taoqiuyang 2:afb333543af5 106 led1 = !led1;
taoqiuyang 5:451b8203ef99 107 wait(0.2);
taoqiuyang 0:f4d390c06705 108 }
taoqiuyang 0:f4d390c06705 109 }