Diff: RefRX.cpp

Revision:

3:9eb188e979ba

Parent:

2:f34ca2c57ed0

Child:

4:87d3a005c4de

--- a/RefRX.cpp	Fri Jul 19 06:43:17 2013 +0000
+++ b/RefRX.cpp	Sat Jul 20 11:22:12 2013 +0000
@@ -1,39 +1,103 @@
 #include "RefRX.h"
-
+#define REFRX_THREAD_START 1
 
 // /////static members
 
-SharedObject<uint32_t>* RefRX::_ref1_ptr = 0;
-SharedObject<uint32_t>* RefRX::_ref2_ptr = 0;
-SharedObject<uint32_t>* RefRX::_ref3_ptr = 0;
-SharedObject<uint32_t>* RefRX::_ref4_ptr = 0;
+bool RefRX::_initialized = false;
 
-unsigned short RefRX::_listen_port = 0;
-unsigned short RefRX::_destination_control_port = 0;
+Mutex RefRX::_refs_access = Mutex();
+float RefRX::_ref1 = 0;
+float RefRX::_ref2 = 0;
+float RefRX::_ref3 = 0;
+float RefRX::_ref4 = 0;
 
-Thread RefRX::_RX_thread = 0;
+int RefRX::_listen_port = 0;
+int RefRX::_destination_control_port = 0;
+
+Thread RefRX::_RX_thread = Thread(RefRX::_RX_thread_cycle);
 
 // /////end of static members
 
 
-void RefRX::init (const unsigned short listen_port,
-        const unsigned short destination_control_port,
-        SharedObject<uint32_t>* ref1,
-        SharedObject<uint32_t>* ref2,
-        SharedObject<uint32_t>* ref3,
-        SharedObject<uint32_t>* ref4 )
+bool RefRX::get (float &ref1, float &ref2, float &ref3, float &ref4)
 {
+    bool lock = _refs_access.trylock();
+    if(!lock)
+        return false;
+    
+    ref1 = _ref1;
+    ref2 = _ref2;
+    ref3 = _ref3;
+    ref4 = _ref4;
+    
+    _refs_access.unlock();
+    return true;
+}
+
+
+void RefRX::init (const int listen_port, const int destination_control_port)
+{
+    if(_initialized)
+        return;
+    
     _listen_port = listen_port;
     _destination_control_port = destination_control_port;
-    _ref1_ptr = ref1;
-    _ref2_ptr = ref2;
-    _ref3_ptr = ref3;
-    _ref4_ptr = ref4;
+    
+    _initialized = true;
     
-    _RX_thread = Thread(&_RX_thread_cycle);
+    _RX_thread.signal_set(REFRX_THREAD_START);
 }
 
+
 void RefRX::_RX_thread_cycle (void const *args)
 {
-    Thread::yield();
+    _RX_thread.signal_wait(REFRX_THREAD_START);
+
+    //Setup RX socket
+    UDPSocket sock;
+    sock.bind(_listen_port);
+    Endpoint remote_endpoint;
+
+    //Start listeing loop
+    while(1)
+    {
+        const float one_over_uint32_max = 2.3283064365386963e-10;
+        uint32_t refs[] =  { 0, 0, 0, 0};
+        const unsigned int refs_size = 4 * sizeof(refs[0]);
+        
+        //listen for the values
+        char in_buffer[256];
+        int result = sock.receiveFrom(remote_endpoint, in_buffer, sizeof(in_buffer));
+        
+        switch (result) {
+          case -1:
+            //TODO: memorize error without the following message.
+            printf("Failed to read from UDP Socket\n\r");
+            break;
+          case 0:
+            // error ?
+            break;
+          default:
+            //socket.getRemoteEndpoint().getAddress().toString()
+            //socket.getRemoteEndpoint().getPort());
+            
+            //retrive and share refs (convert to float)
+            std::memcpy(refs,in_buffer,refs_size);
+            _refs_access.lock();
+            _ref1 = one_over_uint32_max * refs[0];
+            _ref2 = one_over_uint32_max * refs[1];
+            _ref3 = one_over_uint32_max * refs[2];
+            _ref4 = one_over_uint32_max * refs[3];
+            _refs_access.unlock();
+            
+            //respond
+            /*
+            if (!socket.getRemoteEndpoint().getAddress().isEmpty()) {
+                buffer.write("OK\r\n",4);
+                socket.send(buffer, socket.getRemoteEndpoint());
+            }*/
+       }
+    
+        Thread::yield();
+    }
 }
\ No newline at end of file