root/experimental/components/CIREN_SM/trunk/CIREN_SM/CIREN_SM.cpp @ 5677

Revision 5677, 23.5 KB (checked in by hvolos, 6 years ago)

decreasing verbosity
Line 
1/****************************************************************************
2
3Copyright 2007 Virginia Polytechnic Institute and State University
4
5This file is part of the OSSIE CIREN_SM.
6
7OSSIE CIREN_SM 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_SM 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_SM; 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_SM.h"
27
28
29// hard control
30#define CONTROL_POWER_STABLE    0
31#define CONTROL_POWER_INCREASE  1
32#define CONTROL_POWER_DECREASE  2
33
34const char* CIREN_SM_i::SSR_STR[5] = {
35    "none",
36    "no signal",
37    "FRS",
38    "CIREN",
39    "UNKNOWN"};
40
41const float CIREN_SM_i::frs_channels[14] = {
42    462.5625e+06f,
43    462.5875e+06f,
44    462.6125e+06f,
45    462.6375e+06f,
46    462.6625e+06f,
47    462.6875e+06f,
48    462.7125e+06f,
49    467.5625e+06f,
50    467.5875e+06f,
51    467.6125e+06f,
52    467.6375e+06f,
53    467.6625e+06f,
54    467.6875e+06f,
55    467.7125e+06f};
56
57CIREN_SM_i::CIREN_SM_i(const char *uuid, omni_condition *condition) :
58    Resource_impl(uuid),
59    component_running(condition)
60{
61    // Create the ports
62    portTxDataOut = new standardInterfaces_i::complexShort_u("tx_data_out");
63    portRxDataOut = new standardInterfaces_i::complexShort_u("rx_data_out");
64    portControlDataOut = new standardInterfaces_i::realChar_u("control_data_out");
65    portTxDataIn = new standardInterfaces_i::complexShort_p("tx_data_in");
66    portRxDataIn = new standardInterfaces_i::complexShort_p("rx_data_in");
67    portControlDataIn = new standardInterfaces_i::realChar_p("control_data_in");
68    TXControl = new standardInterfaces_i::TX_Control_u("TX_Control");
69    RXControl = new standardInterfaces_i::RX_Control_u("RX_Control");
70    data_control = new standardInterfaces_i::Resource_u("Data_Control");
71
72    // Initialize control block
73    rx_control = new unsigned char[64];
74
75    tx_control = new unsigned char[64];
76    InitializeControlPacket(tx_control);
77    tx_control[3] = 2; // power control
78    control_data_out.length(512);
79
80    tx_beacon = new unsigned char[64];
81    InitializeControlPacket(tx_beacon);
82    tx_beacon[0] = 0;
83    beacon_data_out.length(512);
84    standardInterfaces::InitializeMetaData(beacon_metadata);
85    beacon_metadata.packet_id = 5;
86
87    // Initialize RF properties
88    current_frs_channel = 3;
89    tx_interp = 512;
90    tx_freq = frs_channels[current_frs_channel-1];
91    tx_gain = 1;                // USRP tx gain, not CIREN_SM gain_tx
92    tx_start = true;
93    rx_decim = 256;
94    rx_gain_max = 1;
95    rx_freq = frs_channels[current_frs_channel-1];
96    rx_gain = 1;
97    rx_size = 2048;
98    rx_start = true;
99
100    // CIREN_SM RF properties
101    gain_tx = 1.0f;             // CIREN_SM gain_tx, not USRP tx_gain
102    rx_power_threshold_lo = 20.0f;
103    rx_power_threshold_hi = 35.0f;
104    spectrumSensingFlag = false;
105    switch_scan = 50;
106    switch_linger = 100;
107    switch_dwell = 10000;
108    opMode = SCAN;
109    dwell_packet_counter = 0;
110    dwell_packet_switch = switch_scan;
111
112    channel_score = new int[14];
113    for (unsigned int i=0; i<14; i++)
114        channel_score[i] = 0;
115    scan_results = SSR_NONE;
116    scan_0 = SSR_UNKNOWN;
117    scan_1 = SSR_NONE;
118    scan_2 = SSR_NONE;
119    sensing_counter = 0;
120
121    // Design IIR pre-filter
122    // 10th-order Butterworth IIR filter, wc = 0.25*pi rad,
123    pf_order = 10;
124    b_pf = new float[pf_order+1];
125    a_pf = new float[pf_order+1];
126    SigProc::design_butter_lowpass_filter(pf_order, 0.25, b_pf, a_pf);
127    lpf_i = new SigProc::iir_filter(a_pf, pf_order+1, b_pf, pf_order+1);
128    lpf_q = new SigProc::iir_filter(a_pf, pf_order+1, b_pf, pf_order+1);
129
130    // Design IIR classifier filter
131    // 2nd-order Butterworth IIR filter, wc = 0.1*pi rad,
132    cf_order = 2;
133    b_cf = new float[cf_order+1];
134    a_cf = new float[cf_order+1];
135    SigProc::design_butter_lowpass_filter(cf_order, 0.15, b_cf, a_cf);
136    cf_i = new SigProc::iir_filter(a_cf, cf_order+1, b_cf, cf_order+1);
137    cf_q = new SigProc::iir_filter(a_cf, cf_order+1, b_cf, cf_order+1);
138
139    for (unsigned int i=0; i<11; i++) {
140        printf("  a[%d] = %E\n", i, a_cf[i]);
141        printf("  b[%d] = %E\n", i, b_cf[i]);
142    }
143
144    // Create and start the thread for the packet transmit processing function
145    tx_thread = new omni_thread(run_packet_tx, (void *) this);
146    tx_thread->start();
147
148    // Create and start the thread for the packet transmit processing function
149    rx_thread = new omni_thread(run_packet_rx, (void *) this);
150    rx_thread->start();
151
152    // Create and start the thread for the packet transmit processing function
153    control_thread = new omni_thread(run_control, (void *) this);
154    control_thread->start();
155
156}
157
158CIREN_SM_i::~CIREN_SM_i(void)
159{   
160    // delete the ports
161    delete portTxDataOut;
162    delete portRxDataOut;
163    delete portControlDataOut;
164    delete portTxDataIn;
165    delete portRxDataIn;
166    delete portControlDataIn;
167    delete TXControl;
168    delete RXControl;
169    delete data_control;
170
171    delete [] rx_control;
172    delete [] tx_control;
173
174    delete [] b_pf;
175    delete [] a_pf;
176    delete [] lpf_i;
177    delete [] lpf_q;
178
179#if 0
180    // delete the threads (compiler throws error)
181    delete tx_thread;
182    delete rx_thread;
183    delete control_thread;
184#endif
185}
186
187// Static function for omni thread
188void CIREN_SM_i::run_packet_tx( void * data )
189{
190    ((CIREN_SM_i*)data)->ProcessTx();
191}
192
193// Static function for omni thread
194void CIREN_SM_i::run_packet_rx( void * data )
195{
196    ((CIREN_SM_i*)data)->ProcessRx();
197}
198
199// Static function for omni thread
200void CIREN_SM_i::run_control( void * data )
201{
202    ((CIREN_SM_i*)data)->ProcessControl();
203}
204
205CORBA::Object_ptr CIREN_SM_i::getPort( const char* portName ) throw (
206    CORBA::SystemException, CF::PortSupplier::UnknownPort)
207{
208    DEBUG(3, CIREN_SM, "getPort() invoked with " << portName)
209   
210    CORBA::Object_var p;
211
212    p = portTxDataOut->getPort(portName);
213    if (!CORBA::is_nil(p))
214        return p._retn();
215
216    p = portRxDataOut->getPort(portName);
217    if (!CORBA::is_nil(p))
218        return p._retn();
219
220    p = portControlDataOut->getPort(portName);
221    if (!CORBA::is_nil(p))
222        return p._retn();
223
224    p = portTxDataIn->getPort(portName);
225    if (!CORBA::is_nil(p))
226        return p._retn();
227
228    p = portRxDataIn->getPort(portName);
229    if (!CORBA::is_nil(p))
230        return p._retn();
231
232    p = portControlDataIn->getPort(portName);
233    if (!CORBA::is_nil(p))
234        return p._retn();
235    //
236    p = TXControl->getPort(portName);
237    if (!CORBA::is_nil(p))
238        return p._retn();
239
240    p = RXControl->getPort(portName);
241    if (!CORBA::is_nil(p))
242        return p._retn();
243
244    p = data_control->getPort(portName);
245    if (!CORBA::is_nil(p))
246        return p._retn();
247
248    // Unknown port
249    std::cerr << "ERROR! CIREN_SM Unknown port: " << portName << std::endl;
250    throw CF::PortSupplier::UnknownPort();
251}
252
253void CIREN_SM_i::start() throw (CORBA::SystemException,
254    CF::Resource::StartError)
255{
256    DEBUG(3, CIREN_SM, "start() invoked")
257    printf("\n\n---------- starting CIREN SM --------------\n\n\n");
258
259    // Call start on remote resources
260    CF::Resource_var r = data_control->getRef();
261    if (!CORBA::is_nil(r))
262        r->start();
263
264    //-----------------------------------------------------------------------------
265    //
266    // Set TX properties
267    //
268    //-----------------------------------------------------------------------------
269
270    // Initialize to default TX values
271    TXControl->set_number_of_channels(1);
272    TXControl->set_gain(DEFAULT_USRP_TX_CHANNEL, tx_gain);
273    TXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, tx_freq);
274    TXControl->set_interpolation_rate(DEFAULT_USRP_TX_CHANNEL, tx_interp);
275
276    // Set transmit configurable properties not included in Radio_Control idl
277    CF::Properties tx_config;
278    tx_config.length(1);
279
280    // Set automatic transmit/receive mode on
281    tx_config[0].id = CORBA::string_dup("SET_AUTO_TR_1");
282    tx_config[0].value <<= (CORBA::ULong) 1;
283
284    TXControl->set_values(tx_config);
285    //-----------------------------------------------------------------------------
286    //
287    // Set RX properties
288    //
289    //-----------------------------------------------------------------------------
290
291    // Initialize to default RX values
292    RXControl->set_number_of_channels(1);
293    RXControl->set_gain(DEFAULT_USRP_RX_CHANNEL, rx_gain);
294    RXControl->set_frequency(DEFAULT_USRP_RX_CHANNEL, rx_freq);
295    RXControl->set_decimation_rate(DEFAULT_USRP_RX_CHANNEL, rx_decim);
296    RXControl->set_data_packet_size(DEFAULT_USRP_RX_CHANNEL, rx_size);
297
298    // Set transmit configurable properties not included in Radio_Control idl
299    CF::Properties rx_config;
300    rx_config.length(1);
301
302    // Set rx antenna
303    rx_config[0].id = CORBA::string_dup("SET_RX_ANT_1");
304    rx_config[0].value <<= (CORBA::ULong) 0;
305
306    RXControl->set_values(rx_config);
307
308    if (tx_start) {
309        DEBUG(3, CIREN_SM, "starting USRP transmit process...");
310        TXControl->start(DEFAULT_USRP_TX_CHANNEL);
311    }
312
313    if (rx_start) {
314        RXControl->start(DEFAULT_USRP_RX_CHANNEL);
315        DEBUG(3, CIREN_SM, "starting USRP receive process...");
316    }
317   
318    unsigned char buf1[512];
319    unsigned int nw(0);
320
321    DEBUG(2, CIREN_SM, "Sending control packet to CIREN_CE");
322    SigProc::unpack_bytes(tx_control, 64, buf1, 512, &nw);
323    for (unsigned int i=0; i<512; i++)
324        control_data_out[i] = buf1[i];
325    portControlDataOut->pushPacket(control_data_out);
326
327}
328
329void CIREN_SM_i::stop() throw (CORBA::SystemException, CF::Resource::StopError)
330{ 
331    DEBUG(3, CIREN_SM, "stop() invoked")
332}
333
334void CIREN_SM_i::releaseObject() throw (CORBA::SystemException,
335    CF::LifeCycle::ReleaseError)
336{
337    DEBUG(3, CIREN_SM, "releaseObject() invoked")
338   
339    component_running->signal();
340}
341
342void CIREN_SM_i::initialize() throw (CF::LifeCycle::InitializeError,
343    CORBA::SystemException)
344{
345    DEBUG(3, CIREN_SM, "initialize() invoked")
346}
347
348void CIREN_SM_i::configure(const CF::Properties& props)
349throw (CORBA::SystemException,
350    CF::PropertySet::InvalidConfiguration,
351    CF::PropertySet::PartialConfiguration)
352{
353    DEBUG(3, CIREN_SM, "configure() invoked")
354   
355    std::cout << "props length : " << props.length() << std::endl;
356
357    for (unsigned int i = 0; i <props.length(); i++)
358    {
359        std::cout << "Property id : " << props[i].id << std::endl;
360
361    }
362}
363
364void CIREN_SM_i::InitializeControlPacket(unsigned char *c)
365{
366    // initialize with blank values
367    memset(c, 0, 64);
368
369    c[0] = 0;           // reply
370    c[1] = 211;         // network id
371    c[2] = 0;           // switch channels (ch_id, 0: no action)
372    c[3] = 0;           // change power: 0: no change, 1: inc, 2: dec
373    c[4] = 0;           // end-of-message
374
375    // spectrum sensing results
376    for (unsigned int i=0; i<14; i++)
377        c[5+i] = (int) SSR_NONE;
378
379}
380
381void CIREN_SM_i::PrintControlPacket(unsigned char *c)
382{
383    std::cout << "  control:" << std::endl;
384    std::cout << "    network id: " << (int) (c[1]) << std::endl;
385    std::cout << "    switch channels: " << (int) (c[2]) << std::endl;
386    std::cout << "    change power: " << (int) (c[3]) << std::endl;
387    std::cout << "    eom: " << (int) (c[4]) << std::endl;
388    std::cout << "    spectrum sensing results: " << std::endl;
389    for (unsigned int i=0; i<14; i++)
390        std::cout << "      ch[" << i+1 << "]: " << SSR_STR[c[5+i]] << std::endl;
391}
392void CIREN_SM_i::ScrambleControlPacket(unsigned char *c)
393{
394    for (unsigned int i=0; i<64; i++)
395        c[i] ^= 0xD2;   // 1101 0010
396}
397
398void CIREN_SM_i::SwitchOperationalMode(CIREN_SM_MODE m)
399{
400    if (m==opMode)
401        return;
402
403    switch (m) {
404    case SCAN:
405        DEBUG(2, CIREN_SM, "switching mode to scan");
406        dwell_packet_switch = switch_scan;
407        break;
408    case LINGER:
409        DEBUG(2, CIREN_SM, "switching mode to linger");
410        dwell_packet_switch = switch_linger;
411        break;
412    case DWELL:
413        DEBUG(2, CIREN_SM, "switching mode to dwell");
414        dwell_packet_switch = switch_dwell;
415        break;
416    default:;
417    }
418    opMode = m;
419    dwell_packet_counter = 0;
420}
421
422void CIREN_SM_i::ProcessTx()
423{
424    DEBUG(3, CIREN_SM, "ProcessTx() invoked");
425
426    unsigned int i;
427    unsigned int N_in(0);
428    unsigned int N_out(0);
429
430    while(true) {
431        portTxDataIn->getData(I_in_tx, Q_in_tx, tx_metadata);
432        N_in = I_in_tx->length();
433        if (opMode == SCAN)
434           SwitchOperationalMode(LINGER);
435
436        // process data here
437        // just push data through
438        N_out = N_in;
439        I_out_tx.length(N_out);
440        Q_out_tx.length(N_out);
441        for (i=0; i<N_in; i++) {
442            I_out_tx[i] = (short) (gain_tx * (*I_in_tx)[i]);
443            Q_out_tx[i] = (short) (gain_tx * (*Q_in_tx)[i]);
444        }
445
446        portTxDataIn->bufferEmptied();
447        portTxDataOut->pushPacket(I_out_tx, Q_out_tx, *tx_metadata);
448    }
449
450}
451
452SSR_CLASS CIREN_SM_i::ClassifySignal(float e1, float e2)
453{
454    if (e1 < 2.2f) {
455        return SSR_NO_SIGNAL;
456    } else if ( (e1-e2) < 0.9f && e1 > 35.0f) {
457        return SSR_FRS;
458    } else if ( (e1-e2) < 7.2 && e1 < 15.0f) {
459        return SSR_UNKNOWN;
460    //} else if ( (e1-e2) < 7.2 ) {
461        //return SSR_CIREN;
462    } else {
463        return SSR_UNKNOWN;
464    }
465}
466   
467
468void CIREN_SM_i::ProcessRx()
469{
470    DEBUG(3, CIREN_SM, "ProcessRx() invoked");
471
472    unsigned int i;
473    unsigned int N_in(0);
474    unsigned int N_out(0);
475    unsigned int nw;
476
477    unsigned int buf_len = 2048;
478    short * I_buf = new short[buf_len];
479    short * Q_buf = new short[buf_len];
480
481    short * I_buf2 = new short[buf_len];
482    short * Q_buf2 = new short[buf_len];
483
484    unsigned char tx_control_buf[512];
485
486    float scan_energy;
487    float classifier_energy;
488
489    while(true) {
490        portRxDataIn->getData(I_in_rx, Q_in_rx, rx_metadata);
491        N_in = I_in_rx->length();
492       
493        dwell_packet_counter++;
494
495        // Configure meta data
496        rx_metadata->carrier_frequency = rx_freq;
497        rx_metadata->sampling_frequency = 64e6f / rx_decim;
498
499        //standardInterfaces::PrintMetaData(*tx_metadata);
500
501        // process data here
502        if (N_in > buf_len) {
503            delete [] I_buf;
504            delete [] Q_buf;
505            delete [] I_buf2;
506            delete [] Q_buf2;
507            buf_len = N_in;
508            I_buf = new short[buf_len];
509            Q_buf = new short[buf_len];
510            I_buf2 = new short[buf_len];
511            Q_buf2 = new short[buf_len];
512        }
513
514        // filter input, copy data to input buffer
515        N_out = N_in/4;
516        I_out_rx.length(N_out);
517        Q_out_rx.length(N_out);
518        for (i=0; i<N_in; i++) {
519            // apply IIR pre-filter
520            lpf_i->do_work((*I_in_rx)[i], I_buf[i]);
521            lpf_q->do_work((*Q_in_rx)[i], Q_buf[i]);
522        }
523
524        // perform further analysis here (AGC?)
525        sensing_counter++;
526        scan_energy = 0.01f;
527        if (opMode != DWELL || (opMode == DWELL && sensing_counter >= 100) ) {
528            sensing_counter = 0;
529            // measure energy
530            for (i=256; i<N_in; i++) {
531                scan_energy += fabs(float(I_buf[i])) + fabs(float(Q_buf[i]));
532            }
533
534            scan_energy = 20*log10f(scan_energy/float(N_in));
535            DEBUG(7, CIREN_SM, "scan energy: " << scan_energy << " dB");
536            if (scan_energy < 3.0) {
537                // check if in scan mode and dwell_packet_counter is right
538                if (opMode==SCAN) {
539                    if (dwell_packet_counter == 20 || dwell_packet_counter == 60) {
540                        // send beacon signal
541                        DEBUG(2, CIREN_SM, "sending beacon signal");
542                        beacon_metadata.app_id=6; // specify packet should be sent as frame
543                        tx_beacon[0] = 1; // request reply
544                        SigProc::unpack_bytes(tx_beacon, 64, tx_control_buf, 512, &nw);
545                        for (unsigned int i=0; i<512; i++)
546                            beacon_data_out[i] = tx_control_buf[i];
547                        portControlDataOut->pushPacket(beacon_data_out, beacon_metadata);
548                    }
549                }
550            } else {
551                for (i=0; i<N_in; i++) {
552                    cf_i->do_work(I_buf[i], I_buf2[i]);
553                    cf_q->do_work(Q_buf[i], Q_buf2[i]);
554                }
555
556                // compute energy
557                classifier_energy = 0.01f;
558                for (i=256; i<N_in; i++) {
559                    classifier_energy += fabs(float(I_buf2[i])) + fabs(float(Q_buf2[i]));
560                }
561                classifier_energy = 20*log10f(classifier_energy/float(N_in));
562
563                scan_energy_ratio = scan_energy - classifier_energy;
564                DEBUG(7, CIREN_SM, "scan energy ratio: " << scan_energy_ratio << " dB");
565                DEBUG(7, CIREN_SM, "  scan energy bw1: " << scan_energy << " dB");
566                DEBUG(7, CIREN_SM, "  scan energy bw2: " << classifier_energy << " dB");
567
568                scan_2 = scan_1;
569                scan_1 = scan_0;
570                scan_0 = ClassifySignal(scan_energy, classifier_energy);
571
572                if (scan_0 == scan_1 && scan_1 == scan_2)
573                    scan_results = scan_0;
574                else
575                    scan_results = SSR_NONE;
576
577                switch (scan_results) {
578                case SSR_FRS:
579                    std::cout << "FRS user detected on channel " << current_frs_channel << std::endl;
580                    ///todo update database
581                    dwell_packet_counter += 1000;
582                    break;
583                case SSR_UNKNOWN:
584                    //std::cout << "Unknown detected on channel " << current_frs_channel << std::endl;
585                    //dwell_packet_counter += 20;
586                    break;
587                case SSR_CIREN:
588                    std::cout << "Possible CIREN user detected on channel " << current_frs_channel << std::endl;
589                    //SwitchOperationalMode(DWELL);
590                    //dwell_packet_counter--;
591                    break;
592                default:;
593                }
594
595            } // if scan_energy < 3.0
596        }
597
598       
599        // decimate by 4
600        for (i=0; i<N_out; i++) {
601            I_out_rx[i] = I_buf[4*i];
602            Q_out_rx[i] = Q_buf[4*i];
603        }
604        portRxDataIn->bufferEmptied();
605        portRxDataOut->pushPacket(I_out_rx, Q_out_rx, *rx_metadata);
606
607        // switch channels if necessary
608        if (dwell_packet_counter >= dwell_packet_switch) {
609            SwitchOperationalMode(SCAN);
610            //current_frs_channel++;
611            //if (current_frs_channel > 7)
612            //    current_frs_channel = 1;
613            current_frs_channel = (rand() % 7) + 1;
614            DEBUG(0, CIREN_SM, "switching to frs_channel " << current_frs_channel);
615            scan_0 = SSR_UNKNOWN;
616            scan_1 = SSR_NONE;
617            scan_1 = SSR_NONE;
618
619            ///todo: set channel based on other criteria
620            RXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[current_frs_channel-1]);
621            TXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[current_frs_channel-1]);
622
623            dwell_packet_counter = 0;
624        }
625
626    }
627
628    delete [] I_buf;
629    delete [] Q_buf;
630
631    delete [] I_buf2;
632    delete [] Q_buf2;
633
634}
635
636
637void CIREN_SM_i::ProcessControl()
638{
639    DEBUG(3, CIREN_SM, "ProcessControl() invoked");
640
641    unsigned int i;
642    unsigned int nw;
643    unsigned int N_in(0);
644
645    unsigned int buf_len = 512;
646    unsigned char * buf1 = new unsigned char[buf_len];
647
648    while(true) {
649        portControlDataIn->getData(control_data_in, control_metadata);
650        N_in = control_data_in->length();
651        DEBUG(1, CIREN_SM, "CONTROL MESSAGE RECEIVED!");
652        DEBUG(1, CIREN_SM, "  => rx power: " << control_metadata->signal_strength << "dB");
653        if (control_metadata->app_id==0) {
654            DEBUG(0, CIREN_SM, "  control type:  frame footer");
655        } else if (control_metadata->app_id==5) {
656            DEBUG(0, CIREN_SM, "  control type:  control frame");
657        } else if (control_metadata->app_id==6) {
658            DEBUG(0, CIREN_SM, "  control type:  BEACON frame");
659        } else {
660            std::cerr << "ERROR!: CIREN_SM control type:  unknown" << std::endl;;
661            throw 0;
662        }
663        dwell_packet_counter = 0;
664        SwitchOperationalMode(DWELL);
665        unsigned int frs_channel_detected_signal;
666        for (i=0; i<14; i++) {
667            if ( fabs(frs_channels[i] - control_metadata->carrier_frequency) < 1000.0f) {
668                frs_channel_detected_signal = i+1;
669                std::cout << "detected on frs channel: " << frs_channel_detected_signal << std::endl;
670                RXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[frs_channel_detected_signal-1]);
671                TXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[frs_channel_detected_signal-1]);
672                current_frs_channel = frs_channel_detected_signal;
673            }
674        }
675
676        // process data here
677        if (N_in != 512) {
678            std::cerr << "ERROR! CIREN_SM got control packet of wrong length!" << std::endl;
679            throw 0;
680        }
681
682        // copy data to buf1
683        for (i=0; i<N_in; i++)
684            buf1[i] = (*control_data_in)[i];
685
686        // convert to 8-bit symbols
687        SigProc::pack_bytes(buf1, N_in, rx_control, 64, &nw);
688
689#if 0
690        // Execute external control
691        switch (rx_control[3]) {
692        case CONTROL_POWER_INCREASE:
693            DEBUG(2, CIREN_SM, "increasing tx power");
694            gain_tx *= 1.05;
695            break;
696        case CONTROL_POWER_DECREASE:
697            DEBUG(2, CIREN_SM, "decreasing tx power");
698            gain_tx *= 0.95;
699            break;
700        default:;
701        }
702#endif
703        if (rx_control[0] == 1) {
704            DEBUG(0, CIREN_SM, "received control is requesting reply");
705        }
706
707        if (rx_control[2] != 0 && rx_control[2] != current_frs_channel ) {
708            DEBUG(0, CIREN_SM, "\n\nchanging to FRS channel " << rx_control[2]+1 << "\n\n");
709            RXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[rx_control[2]]);
710            TXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[rx_control[2]]);
711            tx_control[2] = rx_control[2];
712        }
713
714        // Make internal decision
715        if (control_metadata->signal_strength < rx_power_threshold_lo) {
716            // send command to increase power
717            //tx_control[3] = CONTROL_POWER_INCREASE;
718        } else if (control_metadata->signal_strength > rx_power_threshold_hi) {
719            // send command to decrease power
720            //tx_control[3] = CONTROL_POWER_DECREASE;
721        } else {
722            // don't change power setting
723            //tx_control[3] = CONTROL_POWER_STABLE;
724        }
725
726        // clear input buffer
727        portControlDataIn->bufferEmptied();
728#if 0
729        // push reply control
730        if (control_metadata->app_id==0) {
731            control_metadata->app_id=5;
732            tx_control[0] = 0; // set type of control message, 0: noreply, 1: reply
733            DEBUG(2, CIREN_SM, "Sending control REPLY packet to CIREN_CE");
734            SigProc::unpack_bytes(tx_control, 64, buf1, 512, &nw);
735            for (unsigned int i=0; i<512; i++)
736                control_data_out[i] = buf1[i];
737            portControlDataOut->pushPacket(control_data_out, *control_metadata);
738        } else {
739            // control came from special control frame; do not reply
740        }
741#endif
742    }
743    delete [] buf1;
744}
745
746
Note: See TracBrowser for help on using the browser.