Andrew Olguin
/
Nucleo_serial_interrupt
Serial interrupt and parsing
main.cpp@0:d33d2745b11b, 2016-07-24 (annotated)
- Committer:
- aolgu003
- Date:
- Sun Jul 24 23:58:30 2016 +0000
- Revision:
- 0:d33d2745b11b
Serial interrupt and parsing
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aolgu003 | 0:d33d2745b11b | 1 | #include "mbed.h" |
aolgu003 | 0:d33d2745b11b | 2 | #include <string> |
aolgu003 | 0:d33d2745b11b | 3 | #include <vector> |
aolgu003 | 0:d33d2745b11b | 4 | // Need to create sensor library to handle all the sensor readings |
aolgu003 | 0:d33d2745b11b | 5 | |
aolgu003 | 0:d33d2745b11b | 6 | DigitalOut led1(LED1); |
aolgu003 | 0:d33d2745b11b | 7 | DigitalOut led2(LED2); |
aolgu003 | 0:d33d2745b11b | 8 | |
aolgu003 | 0:d33d2745b11b | 9 | Serial pc(USBTX, USBRX); |
aolgu003 | 0:d33d2745b11b | 10 | |
aolgu003 | 0:d33d2745b11b | 11 | typedef struct { |
aolgu003 | 0:d33d2745b11b | 12 | string direction; //Forward or Downward facing gives context for the values of x and y |
aolgu003 | 0:d33d2745b11b | 13 | float x; |
aolgu003 | 0:d33d2745b11b | 14 | float y; |
aolgu003 | 0:d33d2745b11b | 15 | float yaw; |
aolgu003 | 0:d33d2745b11b | 16 | float depth; |
aolgu003 | 0:d33d2745b11b | 17 | } message; |
aolgu003 | 0:d33d2745b11b | 18 | |
aolgu003 | 0:d33d2745b11b | 19 | typedef struct { |
aolgu003 | 0:d33d2745b11b | 20 | float w_yaw; |
aolgu003 | 0:d33d2745b11b | 21 | float w_pitch; |
aolgu003 | 0:d33d2745b11b | 22 | float w_roll; |
aolgu003 | 0:d33d2745b11b | 23 | float yaw; |
aolgu003 | 0:d33d2745b11b | 24 | float pitch; |
aolgu003 | 0:d33d2745b11b | 25 | float roll; |
aolgu003 | 0:d33d2745b11b | 26 | } orientation; |
aolgu003 | 0:d33d2745b11b | 27 | |
aolgu003 | 0:d33d2745b11b | 28 | typedef struct { |
aolgu003 | 0:d33d2745b11b | 29 | float depth; //From pressure sensor |
aolgu003 | 0:d33d2745b11b | 30 | float velsurge; //from flow meter |
aolgu003 | 0:d33d2745b11b | 31 | float accelsurge; //forward |
aolgu003 | 0:d33d2745b11b | 32 | float accelsway; //side to side |
aolgu003 | 0:d33d2745b11b | 33 | float accelheave; //downwards |
aolgu003 | 0:d33d2745b11b | 34 | } translation; |
aolgu003 | 0:d33d2745b11b | 35 | |
aolgu003 | 0:d33d2745b11b | 36 | |
aolgu003 | 0:d33d2745b11b | 37 | orientation vehicle_att; |
aolgu003 | 0:d33d2745b11b | 38 | translation vehicle_trans; |
aolgu003 | 0:d33d2745b11b | 39 | message vehicle_cmd; |
aolgu003 | 0:d33d2745b11b | 40 | |
aolgu003 | 0:d33d2745b11b | 41 | #define BUFFER_SIZE 255 |
aolgu003 | 0:d33d2745b11b | 42 | |
aolgu003 | 0:d33d2745b11b | 43 | char buffer[BUFFER_SIZE] = {' '}; |
aolgu003 | 0:d33d2745b11b | 44 | int buffer_iter = 0; |
aolgu003 | 0:d33d2745b11b | 45 | |
aolgu003 | 0:d33d2745b11b | 46 | void callback() { |
aolgu003 | 0:d33d2745b11b | 47 | char c = 0; |
aolgu003 | 0:d33d2745b11b | 48 | string packet; |
aolgu003 | 0:d33d2745b11b | 49 | string command; |
aolgu003 | 0:d33d2745b11b | 50 | float x, y, yaw, depth; |
aolgu003 | 0:d33d2745b11b | 51 | |
aolgu003 | 0:d33d2745b11b | 52 | // Note: you need to actually read from the serial to clear the RX interrupt |
aolgu003 | 0:d33d2745b11b | 53 | if (pc.readable()) { |
aolgu003 | 0:d33d2745b11b | 54 | do { |
aolgu003 | 0:d33d2745b11b | 55 | c = pc.getc(); |
aolgu003 | 0:d33d2745b11b | 56 | buffer[buffer_iter] = c; |
aolgu003 | 0:d33d2745b11b | 57 | buffer_iter++; |
aolgu003 | 0:d33d2745b11b | 58 | } |
aolgu003 | 0:d33d2745b11b | 59 | while (buffer_iter < BUFFER_SIZE && c != '\n'); |
aolgu003 | 0:d33d2745b11b | 60 | |
aolgu003 | 0:d33d2745b11b | 61 | command = strtok (buffer," ,\n"); |
aolgu003 | 0:d33d2745b11b | 62 | x = atof(strtok (NULL, " ,\n")); |
aolgu003 | 0:d33d2745b11b | 63 | y = atof(strtok (NULL, " ,\n")); |
aolgu003 | 0:d33d2745b11b | 64 | yaw = atof(strtok (NULL, " ,\n")); |
aolgu003 | 0:d33d2745b11b | 65 | depth = atof(strtok (NULL, " ,\n")); |
aolgu003 | 0:d33d2745b11b | 66 | pc.printf("Received Command: %s, X: %f, Y: %f, Yaw: %f, Depth: %f\n", command, x, y, yaw); |
aolgu003 | 0:d33d2745b11b | 67 | |
aolgu003 | 0:d33d2745b11b | 68 | memset(buffer, ' ', sizeof(buffer)); |
aolgu003 | 0:d33d2745b11b | 69 | buffer_iter = 0; |
aolgu003 | 0:d33d2745b11b | 70 | fflush(stdout); |
aolgu003 | 0:d33d2745b11b | 71 | |
aolgu003 | 0:d33d2745b11b | 72 | vehicle_cmd.direction = command; |
aolgu003 | 0:d33d2745b11b | 73 | vehicle_cmd.x = x; |
aolgu003 | 0:d33d2745b11b | 74 | vehicle_cmd.y = y; |
aolgu003 | 0:d33d2745b11b | 75 | vehicle_cmd.yaw = yaw; |
aolgu003 | 0:d33d2745b11b | 76 | vehicle_cmd.depth = depth; |
aolgu003 | 0:d33d2745b11b | 77 | } |
aolgu003 | 0:d33d2745b11b | 78 | } |
aolgu003 | 0:d33d2745b11b | 79 | |
aolgu003 | 0:d33d2745b11b | 80 | |
aolgu003 | 0:d33d2745b11b | 81 | int main() { |
aolgu003 | 0:d33d2745b11b | 82 | pc.attach(&callback); |
aolgu003 | 0:d33d2745b11b | 83 | |
aolgu003 | 0:d33d2745b11b | 84 | while (1) { |
aolgu003 | 0:d33d2745b11b | 85 | led1 = !led1; |
aolgu003 | 0:d33d2745b11b | 86 | pc.printf("Current Command: %s, X: %f, Y: %f, Yaw: %f, Depth: %f\n", vehicle_cmd.direction, vehicle_cmd.x, vehicle_cmd.y, |
aolgu003 | 0:d33d2745b11b | 87 | vehicle_cmd.yaw, vehicle_cmd.depth); |
aolgu003 | 0:d33d2745b11b | 88 | wait_ms(500); |
aolgu003 | 0:d33d2745b11b | 89 | } |
aolgu003 | 0:d33d2745b11b | 90 | } |