File content as of revision 0:d33d2745b11b:

#include "mbed.h"
#include <string>
#include <vector>
// Need to create sensor library to handle all the sensor readings
 
DigitalOut led1(LED1);
DigitalOut led2(LED2);
 
Serial pc(USBTX, USBRX);

typedef struct { 
    string direction; //Forward or Downward facing gives context for the values of x and y
    float x;
    float y;
    float yaw;
    float depth;
} message;

typedef struct {
    float w_yaw;
    float w_pitch;
    float w_roll;
    float yaw;
    float pitch;
    float roll;
} orientation;

typedef struct {
    float depth; //From pressure sensor
    float velsurge; //from flow meter
    float accelsurge; //forward
    float accelsway; //side to side
    float accelheave; //downwards
} translation;


orientation vehicle_att;
translation vehicle_trans;
message vehicle_cmd;

#define BUFFER_SIZE 255

char buffer[BUFFER_SIZE] = {' '};
int buffer_iter = 0;
 
void callback() {
    char c = 0;
    string packet;
    string command;
    float x, y, yaw, depth;
    
    // Note: you need to actually read from the serial to clear the RX interrupt
    if (pc.readable()) {
       do {
            c = pc.getc();
            buffer[buffer_iter] = c;
            buffer_iter++;
        }
        while (buffer_iter < BUFFER_SIZE && c != '\n');
        
        command = strtok (buffer," ,\n");
        x = atof(strtok (NULL, " ,\n"));
        y = atof(strtok (NULL, " ,\n"));
        yaw = atof(strtok (NULL, " ,\n"));
        depth = atof(strtok (NULL, " ,\n"));
        pc.printf("Received Command: %s, X: %f, Y: %f, Yaw: %f, Depth: %f\n", command, x, y, yaw);
        
        memset(buffer, ' ', sizeof(buffer));
        buffer_iter = 0;
        fflush(stdout);
        
        vehicle_cmd.direction = command;
        vehicle_cmd.x = x;
        vehicle_cmd.y = y;
        vehicle_cmd.yaw = yaw;
        vehicle_cmd.depth = depth;
    }
}


int main() {
    pc.attach(&callback);
    
    while (1) {
        led1 = !led1;
        pc.printf("Current Command: %s, X: %f, Y: %f, Yaw: %f, Depth: %f\n", vehicle_cmd.direction, vehicle_cmd.x, vehicle_cmd.y, 
            vehicle_cmd.yaw, vehicle_cmd.depth);
            wait_ms(500);
    }
}