File content as of revision 0:b7d6e190f091:

#include "mbed.h"
#include "rtos.h"
#include <string>
#include <vector>
 
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;

} packet;

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

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

Thread *rx_handler_pointer;

#define BUFFER_SIZE 255

char buffer[BUFFER_SIZE] = {' '};
int buffer_iter = 0;
 
void callback() {
    char c = 0;
    string packet;
    // Note: you need to actually read from the serial to clear the RX interrupt
    if (pc.readable()) {
       //led1 = !led1;
        
        do {
            c = pc.getc();
            buffer[buffer_iter] = c;
            buffer_iter++;
        }
        while (buffer_iter < BUFFER_SIZE && c != '\n');
        (*rx_handler_pointer).signal_set(0x8);
    }
}

void handle_packet(void const *argument)  {
    string command;
    float x,y,yaw;
    std::string packet;
    while (true) {
        Thread::signal_wait(0x8);
        
        command = strtok (buffer," ,\n");
        x = atof(strtok (NULL, " ,\n"));
        y = atof(strtok (NULL, " ,\n"));
        yaw = atof(strtok (NULL, " ,\n"));
        depth = atof(strtok (NULL, " ,\n"));
        printf("Command: %s, X: %f, Y: %f, Z: %f\n", command, x, y, yaw);
        
        memset(buffer, ' ', sizeof(buffer));
        buffer_iter = 0;
        fflush(stdout);

    }
}



int main() {
    pc.attach(&callback);
    Thread service_serial(handle_packet);
    service_serial.set_priority(osPriorityHigh);
    
    rx_handler_pointer = &service_serial;
    
    while (1) {
        led1 = !led1;
        Thread::wait(500);
    }
}