Diff: main.cpp

Revision:

1:3e33729a7a92

Parent:

0:b7d6e190f091

--- a/main.cpp	Sat Jul 09 10:44:08 2016 +0000
+++ b/main.cpp	Fri Jul 22 22:49:27 2016 +0000
@@ -2,6 +2,7 @@
 #include "rtos.h"
 #include <string>
 #include <vector>
+// Need to create sensor library to handle all the sensor readings
  
 DigitalOut led1(LED1);
 DigitalOut led2(LED2);
@@ -15,7 +16,7 @@
     float yaw;
     float depth;
 
-} packet;
+} message;
 
 typedef struct {
     float w_yaw;
@@ -24,7 +25,7 @@
     float yaw;
     float pitch;
     float roll;
-} orrientation;
+} orientation;
 
 typedef struct {
     float depth; //From pressure sensor
@@ -34,7 +35,16 @@
     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
 
@@ -75,16 +85,42 @@
         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);
-    service_serial.set_priority(osPriorityHigh);
+    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;