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

Revision 5681, 23.6 KB (checked in by jgaeddert, 6 years ago)

hacking port id to scan properly
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 = 100;
106    switch_linger = 500;
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(0, CIREN_SM, "switching mode to scan");
406        dwell_packet_switch = switch_scan;
407        break;
408    case LINGER:
409        DEBUG(0, CIREN_SM, "switching mode to linger");
410        dwell_packet_switch = switch_linger;
411        break;
412    case DWELL:
413        DEBUG(0, 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 (tx_metadata->port_id == 127) {
434            dwell_packet_counter = 0;
435            SwitchOperationalMode(LINGER);
436        }
437
438        // process data here
439        // just push data through
440        N_out = N_in;
441        I_out_tx.length(N_out);
442        Q_out_tx.length(N_out);
443        for (i=0; i<N_in; i++) {
444            I_out_tx[i] = (short) (gain_tx * (*I_in_tx)[i]);
445            Q_out_tx[i] = (short) (gain_tx * (*Q_in_tx)[i]);
446        }
447
448        portTxDataIn->bufferEmptied();
449        portTxDataOut->pushPacket(I_out_tx, Q_out_tx, *tx_metadata);
450    }
451
452}
453
454SSR_CLASS CIREN_SM_i::ClassifySignal(float e1, float e2)
455{
456    if (e1 < 2.2f) {
457        return SSR_NO_SIGNAL;
458    } else if ( (e1-e2) < 0.9f && e1 > 35.0f) {
459        return SSR_FRS;
460    } else if ( (e1-e2) < 7.2 && e1 < 15.0f) {
461        return SSR_UNKNOWN;
462    //} else if ( (e1-e2) < 7.2 ) {
463        //return SSR_CIREN;
464    } else {
465        return SSR_UNKNOWN;
466    }
467}
468   
469
470void CIREN_SM_i::ProcessRx()
471{
472    DEBUG(3, CIREN_SM, "ProcessRx() invoked");
473
474    unsigned int i;
475    unsigned int N_in(0);
476    unsigned int N_out(0);
477    unsigned int nw;
478
479    unsigned int buf_len = 2048;
480    short * I_buf = new short[buf_len];
481    short * Q_buf = new short[buf_len];
482
483    short * I_buf2 = new short[buf_len];
484    short * Q_buf2 = new short[buf_len];
485
486    unsigned char tx_control_buf[512];
487
488    float scan_energy;
489    float classifier_energy;
490
491    while(true) {
492        portRxDataIn->getData(I_in_rx, Q_in_rx, rx_metadata);
493        N_in = I_in_rx->length();
494       
495        dwell_packet_counter++;
496
497        // Configure meta data
498        rx_metadata->carrier_frequency = rx_freq;
499        rx_metadata->sampling_frequency = 64e6f / rx_decim;
500
501        //standardInterfaces::PrintMetaData(*tx_metadata);
502
503        // process data here
504        if (N_in > buf_len) {
505            delete [] I_buf;
506            delete [] Q_buf;
507            delete [] I_buf2;
508            delete [] Q_buf2;
509            buf_len = N_in;
510            I_buf = new short[buf_len];
511            Q_buf = new short[buf_len];
512            I_buf2 = new short[buf_len];
513            Q_buf2 = new short[buf_len];
514        }
515
516        // filter input, copy data to input buffer
517        N_out = N_in/4;
518        I_out_rx.length(N_out);
519        Q_out_rx.length(N_out);
520        for (i=0; i<N_in; i++) {
521            // apply IIR pre-filter
522            lpf_i->do_work((*I_in_rx)[i], I_buf[i]);
523            lpf_q->do_work((*Q_in_rx)[i], Q_buf[i]);
524        }
525
526        // perform further analysis here (AGC?)
527        sensing_counter++;
528        scan_energy = 0.01f;
529        if (opMode == SCAN || (opMode == DWELL && sensing_counter >= 100) ) {
530            sensing_counter = 0;
531            // measure energy
532            for (i=256; i<N_in; i++) {
533                scan_energy += fabs(float(I_buf[i])) + fabs(float(Q_buf[i]));
534            }
535
536            scan_energy = 20*log10f(scan_energy/float(N_in));
537            DEBUG(7, CIREN_SM, "scan energy: " << scan_energy << " dB");
538            if (scan_energy < 3.0) {
539                // check if in scan mode and dwell_packet_counter is right
540                if (opMode==SCAN) {
541                    if (dwell_packet_counter == 20 || dwell_packet_counter == 60) {
542                        // send beacon signal
543                        DEBUG(2, CIREN_SM, "sending beacon signal");
544                        beacon_metadata.app_id=6; // specify packet should be sent as frame
545                        tx_beacon[0] = 1; // request reply
546                        SigProc::unpack_bytes(tx_beacon, 64, tx_control_buf, 512, &nw);
547                        for (unsigned int i=0; i<512; i++)
548                            beacon_data_out[i] = tx_control_buf[i];
549                        portControlDataOut->pushPacket(beacon_data_out, beacon_metadata);
550                    }
551                }
552            } else {
553                for (i=0; i<N_in; i++) {
554                    cf_i->do_work(I_buf[i], I_buf2[i]);
555                    cf_q->do_work(Q_buf[i], Q_buf2[i]);
556                }
557
558                // compute energy
559                classifier_energy = 0.01f;
560                for (i=256; i<N_in; i++) {
561                    classifier_energy += fabs(float(I_buf2[i])) + fabs(float(Q_buf2[i]));
562                }
563                classifier_energy = 20*log10f(classifier_energy/float(N_in));
564
565                scan_energy_ratio = scan_energy - classifier_energy;
566                DEBUG(7, CIREN_SM, "scan energy ratio: " << scan_energy_ratio << " dB");
567                DEBUG(7, CIREN_SM, "  scan energy bw1: " << scan_energy << " dB");
568                DEBUG(7, CIREN_SM, "  scan energy bw2: " << classifier_energy << " dB");
569
570                scan_2 = scan_1;
571                scan_1 = scan_0;
572                scan_0 = ClassifySignal(scan_energy, classifier_energy);
573
574                if (scan_0 == scan_1 && scan_1 == scan_2)
575                    scan_results = scan_0;
576                else
577                    scan_results = SSR_NONE;
578
579                switch (scan_results) {
580                case SSR_FRS:
581                    std::cout << "FRS user detected on channel " << current_frs_channel << std::endl;
582                    ///todo update database
583                    dwell_packet_counter += 1000;
584                    break;
585                case SSR_UNKNOWN:
586                    //std::cout << "Unknown detected on channel " << current_frs_channel << std::endl;
587                    //dwell_packet_counter += 20;
588                    break;
589                case SSR_CIREN:
590                    std::cout << "Possible CIREN user detected on channel " << current_frs_channel << std::endl;
591                    //SwitchOperationalMode(DWELL);
592                    //dwell_packet_counter--;
593                    break;
594                default:;
595                }
596
597            } // if scan_energy < 3.0
598        }
599
600       
601        // decimate by 4
602        for (i=0; i<N_out; i++) {
603            I_out_rx[i] = I_buf[4*i];
604            Q_out_rx[i] = Q_buf[4*i];
605        }
606        portRxDataIn->bufferEmptied();
607        portRxDataOut->pushPacket(I_out_rx, Q_out_rx, *rx_metadata);
608
609        // switch channels if necessary
610        if (dwell_packet_counter >= dwell_packet_switch) {
611            SwitchOperationalMode(SCAN);
612            //current_frs_channel++;
613            //if (current_frs_channel > 7)
614            //    current_frs_channel = 1;
615            current_frs_channel = (rand() % 7) + 1;
616            DEBUG(0, CIREN_SM, "switching to frs_channel " << current_frs_channel);
617            scan_0 = SSR_UNKNOWN;
618            scan_1 = SSR_NONE;
619            scan_1 = SSR_NONE;
620
621            ///todo: set channel based on other criteria
622            RXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[current_frs_channel-1]);
623            TXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[current_frs_channel-1]);
624
625            dwell_packet_counter = 0;
626        }
627
628    }
629
630    delete [] I_buf;
631    delete [] Q_buf;
632
633    delete [] I_buf2;
634    delete [] Q_buf2;
635
636}
637
638
639void CIREN_SM_i::ProcessControl()
640{
641    DEBUG(3, CIREN_SM, "ProcessControl() invoked");
642
643    unsigned int i;
644    unsigned int nw;
645    unsigned int N_in(0);
646
647    unsigned int buf_len = 512;
648    unsigned char * buf1 = new unsigned char[buf_len];
649
650    while(true) {
651        portControlDataIn->getData(control_data_in, control_metadata);
652        N_in = control_data_in->length();
653        DEBUG(1, CIREN_SM, "CONTROL MESSAGE RECEIVED!");
654        DEBUG(1, CIREN_SM, "  => rx power: " << control_metadata->signal_strength << "dB");
655        if (control_metadata->app_id==0) {
656            DEBUG(0, CIREN_SM, "  control type:  frame footer");
657        } else if (control_metadata->app_id==5) {
658            DEBUG(0, CIREN_SM, "  control type:  control frame");
659        } else if (control_metadata->app_id==6) {
660            DEBUG(0, CIREN_SM, "  control type:  BEACON frame");
661        } else {
662            std::cerr << "ERROR!: CIREN_SM control type:  unknown" << std::endl;;
663            throw 0;
664        }
665        dwell_packet_counter = 0;
666        SwitchOperationalMode(DWELL);
667        unsigned int frs_channel_detected_signal;
668        for (i=0; i<14; i++) {
669            if ( fabs(frs_channels[i] - control_metadata->carrier_frequency) < 1000.0f) {
670                frs_channel_detected_signal = i+1;
671                std::cout << "detected on frs channel: " << frs_channel_detected_signal << std::endl;
672                RXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[frs_channel_detected_signal-1]);
673                TXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[frs_channel_detected_signal-1]);
674                current_frs_channel = frs_channel_detected_signal;
675            }
676        }
677
678        // process data here
679        if (N_in != 512) {
680            std::cerr << "ERROR! CIREN_SM got control packet of wrong length!" << std::endl;
681            throw 0;
682        }
683
684        // copy data to buf1
685        for (i=0; i<N_in; i++)
686            buf1[i] = (*control_data_in)[i];
687
688        // convert to 8-bit symbols
689        SigProc::pack_bytes(buf1, N_in, rx_control, 64, &nw);
690
691#if 0
692        // Execute external control
693        switch (rx_control[3]) {
694        case CONTROL_POWER_INCREASE:
695            DEBUG(2, CIREN_SM, "increasing tx power");
696            gain_tx *= 1.05;
697            break;
698        case CONTROL_POWER_DECREASE:
699            DEBUG(2, CIREN_SM, "decreasing tx power");
700            gain_tx *= 0.95;
701            break;
702        default:;
703        }
704#endif
705        if (rx_control[0] == 1) {
706            DEBUG(0, CIREN_SM, "received control is requesting reply");
707        }
708
709        if (rx_control[2] != 0 && rx_control[2] != current_frs_channel ) {
710            DEBUG(0, CIREN_SM, "\n\nchanging to FRS channel " << rx_control[2]+1 << "\n\n");
711            RXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[rx_control[2]]);
712            TXControl->set_frequency(DEFAULT_USRP_TX_CHANNEL, frs_channels[rx_control[2]]);
713            tx_control[2] = rx_control[2];
714        }
715
716        // Make internal decision
717        if (control_metadata->signal_strength < rx_power_threshold_lo) {
718            // send command to increase power
719            //tx_control[3] = CONTROL_POWER_INCREASE;
720        } else if (control_metadata->signal_strength > rx_power_threshold_hi) {
721            // send command to decrease power
722            //tx_control[3] = CONTROL_POWER_DECREASE;
723        } else {
724            // don't change power setting
725            //tx_control[3] = CONTROL_POWER_STABLE;
726        }
727
728        // clear input buffer
729        portControlDataIn->bufferEmptied();
730#if 0
731        // push reply control
732        if (control_metadata->app_id==0) {
733            control_metadata->app_id=5;
734            tx_control[0] = 0; // set type of control message, 0: noreply, 1: reply
735            DEBUG(2, CIREN_SM, "Sending control REPLY packet to CIREN_CE");
736            SigProc::unpack_bytes(tx_control, 64, buf1, 512, &nw);
737            for (unsigned int i=0; i<512; i++)
738                control_data_out[i] = buf1[i];
739            portControlDataOut->pushPacket(control_data_out, *control_metadata);
740        } else {
741            // control came from special control frame; do not reply
742        }
743#endif
744    }
745    delete [] buf1;
746}
747
748
Note: See TracBrowser for help on using the browser.