Matteo Terruzzi / RefRX

work in progress

Experiment - work in progress...

RefRX.cpp@5:444939b4790a, 2013-07-20 (annotated)

Committer:
MatteoT
Date:
Sat Jul 20 12:36:22 2013 +0000
Revision:
5:444939b4790a
Parent:
4:87d3a005c4de 
test good

Who changed what in which revision?

UserRevisionLine numberNew contents of line
MatteoT 2:f34ca2c57ed0 1 #include "RefRX.h"
MatteoT 3:9eb188e979ba 2 #define REFRX_THREAD_START 1
MatteoT 2:f34ca2c57ed0 3
MatteoT 2:f34ca2c57ed0 4 // /////static members
MatteoT 2:f34ca2c57ed0 5
MatteoT 3:9eb188e979ba 6 bool RefRX::_initialized = false;
MatteoT 2:f34ca2c57ed0 7
MatteoT 5:444939b4790a 8 Mutex RefRX::_refs_access;
MatteoT 3:9eb188e979ba 9 float RefRX::_ref1 = 0;
MatteoT 3:9eb188e979ba 10 float RefRX::_ref2 = 0;
MatteoT 3:9eb188e979ba 11 float RefRX::_ref3 = 0;
MatteoT 3:9eb188e979ba 12 float RefRX::_ref4 = 0;
MatteoT 2:f34ca2c57ed0 13
MatteoT 3:9eb188e979ba 14 int RefRX::_listen_port = 0;
MatteoT 3:9eb188e979ba 15 int RefRX::_destination_control_port = 0;
MatteoT 3:9eb188e979ba 16
MatteoT 5:444939b4790a 17 Thread* RefRX::_RX_thread = NULL;
MatteoT 2:f34ca2c57ed0 18
MatteoT 2:f34ca2c57ed0 19 // /////end of static members
MatteoT 2:f34ca2c57ed0 20
MatteoT 0:ac535bde6cd9 21
MatteoT 3:9eb188e979ba 22 bool RefRX::get (float &ref1, float &ref2, float &ref3, float &ref4)
MatteoT 0:ac535bde6cd9 23 {
MatteoT 4:87d3a005c4de 24 if(!_initialized)
MatteoT 4:87d3a005c4de 25 return false;
MatteoT 3:9eb188e979ba 26 bool lock = _refs_access.trylock();
MatteoT 3:9eb188e979ba 27 if(!lock)
MatteoT 3:9eb188e979ba 28 return false;
MatteoT 3:9eb188e979ba 29
MatteoT 3:9eb188e979ba 30 ref1 = _ref1;
MatteoT 3:9eb188e979ba 31 ref2 = _ref2;
MatteoT 3:9eb188e979ba 32 ref3 = _ref3;
MatteoT 3:9eb188e979ba 33 ref4 = _ref4;
MatteoT 3:9eb188e979ba 34
MatteoT 3:9eb188e979ba 35 _refs_access.unlock();
MatteoT 3:9eb188e979ba 36 return true;
MatteoT 3:9eb188e979ba 37 }
MatteoT 3:9eb188e979ba 38
MatteoT 3:9eb188e979ba 39
MatteoT 5:444939b4790a 40 void RefRX::init (const int listen_port, const int destination_control_port, Thread & worker_thread)
MatteoT 3:9eb188e979ba 41 {
MatteoT 3:9eb188e979ba 42 if(_initialized)
MatteoT 3:9eb188e979ba 43 return;
MatteoT 3:9eb188e979ba 44
MatteoT 2:f34ca2c57ed0 45 _listen_port = listen_port;
MatteoT 2:f34ca2c57ed0 46 _destination_control_port = destination_control_port;
MatteoT 5:444939b4790a 47 _RX_thread = &worker_thread;
MatteoT 3:9eb188e979ba 48
MatteoT 3:9eb188e979ba 49 _initialized = true;
MatteoT 2:f34ca2c57ed0 50
MatteoT 5:444939b4790a 51 _RX_thread->signal_set(REFRX_THREAD_START);
MatteoT 0:ac535bde6cd9 52 }
MatteoT 0:ac535bde6cd9 53
MatteoT 3:9eb188e979ba 54
MatteoT 5:444939b4790a 55 void RefRX::worker (void const *args)
MatteoT 0:ac535bde6cd9 56 {
MatteoT 5:444939b4790a 57 _RX_thread->signal_wait(REFRX_THREAD_START);
MatteoT 3:9eb188e979ba 58
MatteoT 3:9eb188e979ba 59 //Setup RX socket
MatteoT 3:9eb188e979ba 60 UDPSocket sock;
MatteoT 3:9eb188e979ba 61 sock.bind(_listen_port);
MatteoT 3:9eb188e979ba 62 Endpoint remote_endpoint;
MatteoT 5:444939b4790a 63
MatteoT 5:444939b4790a 64 DigitalOut led3 (LED3);
MatteoT 5:444939b4790a 65 led3 = 1;
MatteoT 5:444939b4790a 66 Thread::wait(1000);
MatteoT 3:9eb188e979ba 67
MatteoT 3:9eb188e979ba 68 //Start listeing loop
MatteoT 3:9eb188e979ba 69 while(1)
MatteoT 3:9eb188e979ba 70 {
MatteoT 3:9eb188e979ba 71 const float one_over_uint32_max = 2.3283064365386963e-10;
MatteoT 3:9eb188e979ba 72 uint32_t refs[] = { 0, 0, 0, 0};
MatteoT 3:9eb188e979ba 73 const unsigned int refs_size = 4 * sizeof(refs[0]);
MatteoT 3:9eb188e979ba 74
MatteoT 3:9eb188e979ba 75 //listen for the values
MatteoT 3:9eb188e979ba 76 char in_buffer[256];
MatteoT 3:9eb188e979ba 77 int result = sock.receiveFrom(remote_endpoint, in_buffer, sizeof(in_buffer));
MatteoT 3:9eb188e979ba 78
MatteoT 3:9eb188e979ba 79 switch (result) {
MatteoT 3:9eb188e979ba 80 case -1:
MatteoT 3:9eb188e979ba 81 //TODO: memorize error without the following message.
MatteoT 5:444939b4790a 82 //printf("Failed to read from UDP Socket\n\r");
MatteoT 5:444939b4790a 83 led3 = 0;
MatteoT 3:9eb188e979ba 84 break;
MatteoT 3:9eb188e979ba 85 case 0:
MatteoT 3:9eb188e979ba 86 // error ?
MatteoT 5:444939b4790a 87 led3 = 0;
MatteoT 3:9eb188e979ba 88 break;
MatteoT 3:9eb188e979ba 89 default:
MatteoT 3:9eb188e979ba 90 //socket.getRemoteEndpoint().getAddress().toString()
MatteoT 3:9eb188e979ba 91 //socket.getRemoteEndpoint().getPort());
MatteoT 3:9eb188e979ba 92
MatteoT 3:9eb188e979ba 93 //retrive and share refs (convert to float)
MatteoT 3:9eb188e979ba 94 std::memcpy(refs,in_buffer,refs_size);
MatteoT 3:9eb188e979ba 95 _refs_access.lock();
MatteoT 3:9eb188e979ba 96 _ref1 = one_over_uint32_max * refs[0];
MatteoT 3:9eb188e979ba 97 _ref2 = one_over_uint32_max * refs[1];
MatteoT 3:9eb188e979ba 98 _ref3 = one_over_uint32_max * refs[2];
MatteoT 3:9eb188e979ba 99 _ref4 = one_over_uint32_max * refs[3];
MatteoT 3:9eb188e979ba 100 _refs_access.unlock();
MatteoT 5:444939b4790a 101 led3 = 0;
MatteoT 5:444939b4790a 102 Thread::wait(100);
MatteoT 5:444939b4790a 103 led3 = 1;
MatteoT 3:9eb188e979ba 104
MatteoT 3:9eb188e979ba 105 //respond
MatteoT 3:9eb188e979ba 106 /*
MatteoT 3:9eb188e979ba 107 if (!socket.getRemoteEndpoint().getAddress().isEmpty()) {
MatteoT 3:9eb188e979ba 108 buffer.write("OK\r\n",4);
MatteoT 3:9eb188e979ba 109 socket.send(buffer, socket.getRemoteEndpoint());
MatteoT 3:9eb188e979ba 110 }*/
MatteoT 3:9eb188e979ba 111 }
MatteoT 3:9eb188e979ba 112
MatteoT 3:9eb188e979ba 113 Thread::yield();
MatteoT 3:9eb188e979ba 114 }
MatteoT 2:f34ca2c57ed0 115 }