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?
User | Revision | Line number | New 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 | } |