File content as of revision 1:3e33729a7a92:

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

///////////////////////////////////////////////////////////////////////////////////////////////
Thread *rx_handler_pointer;
Mutex command_mutex; //lock for contrlling access to command variable
Mutex sensor_mutex; //lock for controlling access to sensor variables orientation and translation
Mutex stdio_mutex;

#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);
        
        command_mutex.lock();
        vehicle_cmd->direction = command;
        vehicle_cmd->x = x;
        vehicle_cmd->y = y;
        vehicle_cmd->yaw = yaw;
        vehicle_cmd->depth = depth;
        command_mutex.unlock();
    }
}

void handle_sensors (void const *arg) {
    //run through sensor initialization
    
    while (true) {
        //read sensor values
        //...
        
        //first lock using mutex or wait to lock
        sensor_mutex.lock();
        
        //store to global variables
        //...
        
        //Unlock mutex
        sensor_mutex.unlock();           
        Thread::wait(10); // set sensor update rate    
    }
}

int main() {
    pc.attach(&callback);
    Thread service_serial(handle_packet, NULL, osPriorityHigh);
    Thread sensor_thread(handle_sensors, NULL, osPriorityHigh);
    //Thread att_cntrl_thread();
    //Thread trans_cntrl_thread();
    
    rx_handler_pointer = &service_serial;
    
    while (1) {
        led1 = !led1;
        Thread::wait(500);
    }
}