root/ossiedev/branches/trnewman/CIREN/components/CIREN_CE/CIREN_CE.cpp @ 7973

Revision 7973, 19.2 KB (checked in by trnewman, 5 years ago)
  • Property svn:eol-style set to native
Line 
1/****************************************************************************
2
3Copyright 2007 Virginia Polytechnic Institute and State University
4
5This file is part of the OSSIE CIREN_CE.
6
7OSSIE CIREN_CE is free software; you can redistribute it and/or modify
8it under the terms of the GNU General Public License as published by
9the Free Software Foundation; either version 2 of the License, or
10(at your option) any later version.
11
12OSSIE CIREN_CE is distributed in the hope that it will be useful,
13but WITHOUT ANY WARRANTY; without even the implied warranty of
14MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15GNU General Public License for more details.
16
17You should have received a copy of the GNU General Public License
18along with OSSIE CIREN_CE; if not, write to the Free Software
19Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
20
21****************************************************************************/
22
23
24#include <string>
25#include <iostream>
26#include "CIREN_CE.h"
27
28CIREN_CE_i::CIREN_CE_i(const char *uuid, omni_condition *condition) :
29    Resource_impl(uuid), component_running(condition)
30{
31    // Create the ports
32    portAppDataOut = new standardInterfaces_i::realChar_u("app_data_out");
33    portPacketDataOut = new standardInterfaces_i::realChar_u("packet_data_out");
34    portControlDataOut = new standardInterfaces_i::realChar_u("control_data_out");
35    portAppDataIn = new standardInterfaces_i::realChar_p("app_data_in");
36    portPacketDataIn = new standardInterfaces_i::realChar_p("packet_data_in");
37    portControlDataIn = new standardInterfaces_i::realChar_p("control_data_in");
38
39    // Initialize CORBA variables
40    app_data_in = NULL;
41    packet_data_in = NULL;
42    control_data_in = NULL;
43    tx_control_data_out.length(512);
44    rx_control_data_out.length(512);
45
46    // Initialize TX control metadata object
47    standardInterfaces::InitializeMetaData(tx_control_metadata);
48    tx_control_metadata.modulation_scheme.scheme =
49        standardInterfaces::ModulationScheme::PSK;
50    tx_control_metadata.modulation_scheme.M = 2;
51    tx_control_metadata.eom = true;
52    tx_control_metadata.app_id = 0;
53    tx_control_metadata.src_id = 0;
54
55    standardInterfaces::InitializeMetaData(tx_control_metadata_5);
56    tx_control_metadata_5.modulation_scheme.scheme =
57        standardInterfaces::ModulationScheme::PSK;
58    tx_control_metadata_5.modulation_scheme.M = 4;
59    tx_control_metadata_5.eom = true;
60    tx_control_metadata_5.app_id = 5;
61    tx_control_metadata_5.src_id = 0;
62
63    standardInterfaces::InitializeMetaData(tx_control_metadata_6);
64    tx_control_metadata_6.modulation_scheme.scheme =
65        standardInterfaces::ModulationScheme::PSK;
66    tx_control_metadata_6.modulation_scheme.M = 4;
67    tx_control_metadata_6.eom = true;
68    tx_control_metadata_6.app_id = 6;
69    tx_control_metadata_6.src_id = 0;
70
71    // Initialize other variables
72    node_id = 0;
73    newTxControlPacketFlag = false;
74
75    // Create and start the thread for the packet transmit processing function
76    packet_tx_thread = new omni_thread(run_packet_tx, (void *) this);
77    packet_tx_thread->start();
78
79    // Create and start the thread for the packet transmit processing function
80    packet_rx_thread = new omni_thread(run_packet_rx, (void *) this);
81    packet_rx_thread->start();
82
83    // Create and start the thread for the packet transmit processing function
84    control_thread = new omni_thread(run_control, (void *) this);
85    control_thread->start();
86
87}
88
89CIREN_CE_i::~CIREN_CE_i(void)
90{   
91    // delete the ports
92    delete portAppDataOut;
93    delete portPacketDataOut;
94    delete portControlDataOut;
95    delete portAppDataIn;
96    delete portPacketDataIn;
97    delete portControlDataIn;
98
99#if 0
100    // delete the threads (compiler throws error)
101    delete packet_tx_thread;
102    delete packet_rx_thread;
103    delete control_thread;
104#endif
105}
106
107// Static function for omni thread
108void CIREN_CE_i::run_packet_tx( void * data )
109{
110    ((CIREN_CE_i*)data)->ProcessPacketTx();
111}
112
113// Static function for omni thread
114void CIREN_CE_i::run_packet_rx( void * data )
115{
116    ((CIREN_CE_i*)data)->ProcessPacketRx();
117}
118
119// Static function for omni thread
120void CIREN_CE_i::run_control( void * data )
121{
122    ((CIREN_CE_i*)data)->ProcessControl();
123}
124
125CORBA::Object_ptr CIREN_CE_i::getPort( const char* portName ) throw (
126    CORBA::SystemException, CF::PortSupplier::UnknownPort)
127{
128    DEBUG(3, CIREN_CE, "getPort() invoked with " << portName)
129   
130    CORBA::Object_var p;
131
132    p = portAppDataOut->getPort(portName);
133
134    if (!CORBA::is_nil(p))
135        return p._retn();
136
137    p = portPacketDataOut->getPort(portName);
138
139    if (!CORBA::is_nil(p))
140        return p._retn();
141
142    p = portControlDataOut->getPort(portName);
143
144    if (!CORBA::is_nil(p))
145        return p._retn();
146
147    p = portAppDataIn->getPort(portName);
148
149    if (!CORBA::is_nil(p))
150        return p._retn();
151
152    p = portPacketDataIn->getPort(portName);
153
154    if (!CORBA::is_nil(p))
155        return p._retn();
156
157    p = portControlDataIn->getPort(portName);
158
159    if (!CORBA::is_nil(p))
160        return p._retn();
161
162    /*exception*/
163    throw CF::PortSupplier::UnknownPort();
164}
165
166void CIREN_CE_i::start() throw (CORBA::SystemException,
167    CF::Resource::StartError)
168{
169    DEBUG(3, CIREN_CE, "start() invoked")
170}
171
172void CIREN_CE_i::stop() throw (CORBA::SystemException, CF::Resource::StopError)
173{ 
174    DEBUG(3, CIREN_CE, "stop() invoked")
175}
176
177void CIREN_CE_i::releaseObject() throw (CORBA::SystemException,
178    CF::LifeCycle::ReleaseError)
179{
180    DEBUG(3, CIREN_CE, "releaseObject() invoked")
181   
182    component_running->signal();
183}
184
185void CIREN_CE_i::initialize() throw (CF::LifeCycle::InitializeError,
186    CORBA::SystemException)
187{
188    DEBUG(3, CIREN_CE, "initialize() invoked")
189}
190
191void CIREN_CE_i::configure(const CF::Properties& props)
192throw (CORBA::SystemException,
193    CF::PropertySet::InvalidConfiguration,
194    CF::PropertySet::PartialConfiguration)
195{
196    DEBUG(3, CIREN_CE, "configure() invoked")
197   
198    std::cout << "props length : " << props.length() << std::endl;
199
200    for (unsigned int i = 0; i <props.length(); i++)
201    {
202        std::cout << "Property id : " << props[i].id << std::endl;
203        if (strcmp(props[i].id, "DCE:88f65d5f-0619-425a-aed7-fa5cb8a658bc")==0) {
204            // node_id
205            CORBA::UShort simple_temp;
206            props[i].value >>= simple_temp;
207            node_id = simple_temp;
208            tx_control_metadata.src_id = node_id;
209            tx_control_metadata_5.src_id = node_id;
210            tx_control_metadata_6.src_id = node_id;
211        } else {
212            // unknown property
213            std::cerr << "ERROR: SymbolSyncPoly::configure() unknown property \""
214               << props[i].id << "\"" << std::endl;
215            throw CF::PropertySet::InvalidConfiguration();
216        }
217
218    }
219}
220
221void CIREN_CE_i::ProcessPacketTx()
222{
223    DEBUG(3, CIREN_CE, "ProcessPacketTx() invoked");
224
225    unsigned int i;
226    unsigned int N_in(0);
227    unsigned int N_out(0);
228    unsigned int app_type;
229
230    unsigned int nr;
231    unsigned int nw;
232
233    unsigned int buf_len = 1024;
234    unsigned char * buf1 = new unsigned char[buf_len];
235    unsigned char * buf2 = new unsigned char[buf_len];
236
237    // dummy buffer of CVSD bits (101010...) to flush FrameAssembler
238    PortTypes::CharSequence cvsd_dummy_out;
239    cvsd_dummy_out.length(512);
240    for (i=0; i <512; i++)
241        cvsd_dummy_out[i] = i & 0x01;
242    unsigned int audio_packet_counter(0);
243    unsigned int num_audio_packets_complete_frame(8);
244
245    unsigned int text_packet_counter(0);
246    unsigned int generic_packet_counter(0);
247
248    while(true) {
249        portAppDataIn->getData(app_data_in, tx_metadata);
250        N_in = app_data_in->length();
251
252        if (audio_packet_counter==0) {
253            // configure modulation scheme here
254            num_audio_packets_complete_frame = 16;
255        }
256
257        // process data here
258        app_type = tx_metadata->app_id;
259       
260        printf("\n\nSENDING PACKET WITH APP ID = %i\n\n",app_type);
261        switch (app_type) {
262        case 1:
263            // audio data: just copy data to output port
264            N_out = N_in;
265            packet_data_out.length(N_out);
266            for (i=0; i<N_in; i++)
267                packet_data_out[i] = (*app_data_in)[i];
268
269            tx_metadata->packet_id = audio_packet_counter;
270
271            tx_metadata->modulation_scheme.scheme =
272                standardInterfaces::ModulationScheme::QAM;
273            tx_metadata->modulation_scheme.M = 16;
274
275            audio_packet_counter++;
276            break;
277        case 2:
278            // text data: unpack ASCII to bits and push to output port
279            ///\todo: break up into pieces
280            if (N_in > 64) {
281                std::cerr << "ERROR! CIREN_CE::ProcessPacketTx(): "
282                          << "too many input text characters!" << std::endl;
283                portAppDataIn->bufferEmptied();
284                continue;
285            }
286
287            if (N_in / 8 > buf_len) {
288                std::cerr << "ERROR! CIREN_CE::ProcessPacketTx(): "
289                          << "input ASCII data too large!" << std::endl;
290                throw 0;
291            }
292
293            // copy ASCII data to buf1
294            for (i=0; i<N_in; i++)
295                buf1[i] = (*app_data_in)[i];
296
297            // unpack bytes and store in buf2
298            ///\todo: unpack only 7 bits?
299            SigProc::unpack_bytes(buf1, N_in, buf2, buf_len, &nw);
300            // NOTE: nw (num bits written) should be 8*N_in
301
302            // copy unpacked bits to output buffer
303            N_out = nw;
304            packet_data_out.length(N_out);
305            for (i=0; i<N_out; i++)
306                packet_data_out[i] = buf2[i];
307
308            tx_metadata->packet_id = audio_packet_counter;
309            tx_metadata->modulation_scheme.scheme =
310                standardInterfaces::ModulationScheme::PSK;
311            tx_metadata->modulation_scheme.M = 4;
312            text_packet_counter++;
313            break;
314        case 3:
315            // FILE SEND:: text data: unpack ASCII to bits and push to output port
316            ///\todo: break up into pieces
317            if (N_in > 128) {
318                std::cerr << "FILE:: ERROR! CIREN_CE::ProcessPacketTx(): "
319                          << "too many input text characters! ==" << N_in << std::endl;
320                portAppDataIn->bufferEmptied();
321                continue;
322            }
323
324            if (N_in / 8 > buf_len) {
325                std::cerr << "FILE::  ERROR! CIREN_CE::ProcessPacketTx(): "
326                          << "input ASCII data too large!" << std::endl;
327                throw 0;
328            }
329
330            // copy ASCII data to buf1
331            for (i=0; i<N_in; i++)
332                buf1[i] = (*app_data_in)[i];
333
334            // unpack bytes and store in buf2
335            ///\todo: unpack only 7 bits?
336            SigProc::unpack_bytes(buf1, N_in, buf2, buf_len, &nw);
337            // NOTE: nw (num bits written) should be 8*N_in
338
339            // copy unpacked bits to output buffer
340            N_out = nw;
341            packet_data_out.length(N_out);
342            for (i=0; i<N_out; i++)
343                packet_data_out[i] = buf2[i];
344
345            tx_metadata->packet_id = generic_packet_counter;
346            tx_metadata->modulation_scheme.scheme =
347                standardInterfaces::ModulationScheme::PSK;
348            tx_metadata->modulation_scheme.M = 4;
349            generic_packet_counter++;
350            break;
351        default:
352            std::cout << "WARNING! CIREN_CE::ProcessPacketTx(): "
353                      << "unknown/unsupported application type "
354                      << app_type << std::endl;
355        }
356       
357        // configure metadata properties
358        tx_metadata->packet_type = 1;
359        tx_metadata->src_id = node_id;
360        tx_metadata->dst_id = 65;
361        tx_metadata->port_id = 1;
362
363        portAppDataIn->bufferEmptied();
364        DEBUG(1, CIREN_CE, "pushing app data to radio...");
365        portPacketDataOut->pushPacket(packet_data_out, *tx_metadata);
366
367        // TODO: if EOM, send control packet/frame
368        if (tx_metadata->eom) {
369            // flush FrameAssembler with dummy CVSD audio packets
370            while (audio_packet_counter % num_audio_packets_complete_frame != 0) {
371                audio_packet_counter++;
372                tx_metadata->packet_id++;
373                DEBUG(1, CIREN_CE, "pushing dummy cvsd data to radio");
374                portPacketDataOut->pushPacket(cvsd_dummy_out, *tx_metadata);
375            }
376            audio_packet_counter = 0;
377        }
378
379    }
380
381    // delete buffers
382    delete [] buf1;
383    delete [] buf2;
384}
385
386
387void CIREN_CE_i::ProcessPacketRx()
388{
389    DEBUG(3, CIREN_CE, "ProcessPacketRx() invoked");
390
391    unsigned int i;
392    unsigned int N_in(0);
393    unsigned int N_out(0);
394
395    unsigned int app_type;
396
397    unsigned int nr;
398    unsigned int nw;
399
400    unsigned int buf_len = 1024;
401    unsigned char * buf1 = new unsigned char[buf_len];
402    unsigned char * buf2 = new unsigned char[buf_len];
403
404    while(true) {
405        portPacketDataIn->getData(packet_data_in, rx_metadata);
406        N_in = packet_data_in->length();
407
408        // process data here
409        app_type = rx_metadata->app_id;
410        printf("\n\nGETTING PACKET WITH APP ID = %i\n\n",app_type);
411        switch (app_type) {
412        case 0:
413            // control packet: copy data and push to CIREN_SM
414            DEBUG(1, CIREN_CE, "external control packet received!");
415           
416            printf("\n\n---- N =%i\n\n\n",N_in);
417
418          for (i=0; i<512; i++)
419                //rx_control_data_out[i] = (*packet_data_in)[i];
420                rx_control_data_out[i] = 1;
421                //printf("packet_data_in[%i] = %i",i,(*packet_data_in)[i]);
422
423            DEBUG(1, CIREN_CE, "1external control packet received!");
424            // Send received control packet to CIREN_SM
425            portControlDataOut->pushPacket(rx_control_data_out, *rx_metadata);
426            DEBUG(1, CIREN_CE, "2external control packet received!");
427
428            // Send feedback control frame
429            usleep(100 + rand() % 1000);
430            DEBUG(1, CIREN_CE, " :: pushing control frame :: ");
431            portPacketDataOut->pushPacket(rx_control_data_out, tx_control_metadata_5);
432            portPacketDataIn->bufferEmptied();
433            continue;
434        case 1:
435            // audio data: just copy data to output port
436            ///\todo: set eom flag appropriately
437            N_out = N_in;
438            DEBUG(5, CIREN_CE_RX, "audio packet [" << rx_metadata->packet_id << "]");
439            app_data_out.length(N_out);
440            for (i=0; i<N_in; i++)
441                app_data_out[i] = (*packet_data_in)[i];
442
443            break;
444        case 2:
445            // text data: pack ASCII from bits and push to output port
446            ///\todo: combine pieces into block
447           
448            // ignore zero-length packets
449            if (N_in == 0)
450                break;
451
452            if (rx_metadata->port_id==0) {
453                // did not pass CRC
454                ///\todo take appropriate action!
455                portPacketDataIn->bufferEmptied();
456                continue;
457            }
458
459            DEBUG(5, CIREN_CE_RX, "data packet [" << rx_metadata->packet_id << "]");
460            // copy bits to buf1
461            for (i=0; i<N_in; i++)
462                buf1[i] = (*packet_data_in)[i];
463
464            // pack bytes and store in buf2
465            ///\todo: pack only 7 bits?
466            SigProc::pack_bytes(buf1, N_in, buf2, buf_len, &nw);
467            // NOTE: nw (num bits written) should be N_in/8
468
469            // copy packed ASCII bytes to output buffer
470            N_out = nw;
471            app_data_out.length(N_out);
472            for (i=0; i<N_out; i++)
473                app_data_out[i] = buf2[i];
474
475            break;
476        case 5:
477            DEBUG(1, CIREN_CE_RX, "\n\ngot control frame!\n\n");
478            DEBUG(0, CIREN_CE_RX, " BEACON REPLY DETECTED!");
479
480            // Forward received control packet to CIREN_SM
481            portControlDataOut->pushPacket(rx_control_data_out, *rx_metadata);
482
483            portPacketDataIn->bufferEmptied();
484            continue;
485        case 6:
486            DEBUG(1, CIREN_CE_RX, "\n\ngot control frame!\n\n");
487            DEBUG(0, CIREN_CE_RX, " BEACON DETECTED!");
488                std::cout << "WARNING: GOT CONTROL FRAME!!!!!!! " << rx_metadata->src_id << std::endl;
489                std::cout << "WARNING: GOT CONTROL FRAME!!!!!!! " << rx_metadata->src_id << std::endl;
490                std::cout << "WARNING: GOT CONTROL FRAME!!!!!!! " << rx_metadata->src_id << std::endl;
491                std::cout << "WARNING: GOT CONTROL FRAME!!!!!!! " << rx_metadata->src_id << std::endl;
492                std::cout << "WARNING: GOT CONTROL FRAME!!!!!!! " << rx_metadata->src_id << std::endl;
493                std::cout << "WARNING: GOT CONTROL FRAME!!!!!!! " << rx_metadata->src_id << std::endl;
494
495            if ( (rx_metadata->src_id == 0) || (node_id == 0) ) {
496                // Forward received control packet to CIREN_SM
497                portControlDataOut->pushPacket(rx_control_data_out, *rx_metadata);
498            } else {
499                std::cout << "WARNING: not master node, received node id: " << rx_metadata->src_id << std::endl;
500            }
501
502            portPacketDataIn->bufferEmptied();
503            continue;
504        default:
505            std::cout << "WARNING! CIREN_CE::ProcessPacketRx(): "
506                      << "unknown/unsupported application type "
507                      << app_type << std::endl;
508            portPacketDataIn->bufferEmptied();
509            continue;
510        }
511       
512        portPacketDataIn->bufferEmptied();
513        DEBUG(7, CIREN_CE::RX, "pushing " << N_out << " samples")
514        portAppDataOut->pushPacket(app_data_out, *rx_metadata);
515    }
516
517    // delete buffers
518    delete [] buf1;
519    delete [] buf2;
520}
521
522
523void CIREN_CE_i::ProcessControl()
524{
525    DEBUG(3, CIREN_CE, "ProcessControl() invoked");
526
527    unsigned int i;
528    unsigned int N_in;
529    unsigned int N_out;
530
531    unsigned int buf_len = 512;
532    unsigned char * buf = new unsigned char[buf_len];
533   
534    unsigned int nw;
535
536    while(true) {
537        portControlDataIn->getData(control_data_in, rx_control_metadata);
538        N_in = control_data_in->length();
539        DEBUG(1, CIREN_CE, "internal control packet received!");
540        if (rx_control_metadata->app_id==6)
541            DEBUG(1, CIREN_CE_CTRL, "  control packet is BEACON signal!");
542
543        if (N_in != 512) {
544            std::cerr << "ERROR: CIREN_CE::ProcessControl()" << std::endl
545                      << "  => control input not 512 bits!" << std::endl;
546            throw 0;
547        }
548
549        // TODO: lock mutex
550        // just pass data
551        for (i=0; i<512; i++)
552            tx_control_data_out[i] = (*control_data_in)[i];
553        // TODO: unlock mutex
554
555        newTxControlPacketFlag = true;
556
557        if (rx_control_metadata->app_id==0) {
558            // end-of-frame control: BPSK
559            tx_control_metadata.modulation_scheme.scheme =
560                standardInterfaces::ModulationScheme::PSK;
561            tx_control_metadata.modulation_scheme.M = 2;
562        } else {
563            // special control frame: QPSK
564            tx_control_metadata.modulation_scheme.scheme =
565                standardInterfaces::ModulationScheme::PSK;
566            tx_control_metadata.modulation_scheme.M = 4;
567        }
568
569        tx_control_metadata.app_id = rx_control_metadata->app_id;
570        DEBUG(1, CIREN_CE, "  pushing control (app_id=" << tx_control_metadata.app_id << ") to radio...");
571        portPacketDataOut->pushPacket(tx_control_data_out, tx_control_metadata);
572        portControlDataIn->bufferEmptied();
573    }
574    delete [] buf;
575}
576
577
Note: See TracBrowser for help on using the browser.