root/ossiedev/branches/drdepoy/USRP_UHD_0.8.2/src/USRP_UHD.cpp @ 10608

/****************************************************************************

Copyright 2005,2006 Virginia Polytechnic Institute and State University

This file is part of the OSSIE USRP_UHD Device.

OSSIE USRP_UHD Device is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.

OSSIE USRP_UHD Device is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with OSSIE USRP_UHD Device; if not, write to the Free Software
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA


****************************************************************************/

#include <byteswap.h>
#include <iostream>
#include <vector>
#include <complex>

#include "ossie/cf.h"
#include "ossie/PortTypes.h"

#include "ossie/ossieSupport.h"
#include "ossie/debug.h"

#include "USRP_UHD.h"

USRP_UHD_i::USRP_UHD_i(char *id, char *label, char *profile) :
        Device_impl(id, label, profile),
        set_rx_packet_count(-1),
        rx_packet_count(1024),
        rx_packet_size(256),
        rx_data_size(2),
        number_of_channels(1),
        complex(true),
        rx_overruns(0),
        rx_active(false),
        tx_active(false),
        full_duplex(false),
        did_rx(false)
{
// Create USRP_UHD Control ports for TX and RX
    rx_control_port = new USRP_UHD_RX_Control_i(this, "U2_RX_Control", "DomainName1");
    tx_control_port = new USRP_UHD_TX_Control_i(this, "U2_TX_Control", "DomainName1");

// Create the ports for TX data
    tx_data_port = new standardInterfaces_i::complexShort_p("U2_TX_Data", "DomainName1");

// Create the ports for RX Data
    rx_data_1_port = new standardInterfaces_i::complexShort_u("U2_RX_Data_1", "DomainName1");
    rx_data_2_port = new standardInterfaces_i::complexShort_u("U2_RX_Data_2", "DomainName1");
}

void USRP_UHD_i::start() throw (CF::Resource::StartError, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "Start USRP_UHD called")
}

void USRP_UHD_i::stop() throw (CF::Resource::StopError, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "Stop USRP_UHD called")
    (rx_active)?std::cout<<"Active": std::cout<<"NOT Active";
}

CORBA::Object_ptr USRP_UHD_i::getPort(const char* portName)
throw(CF::PortSupplier::UnknownPort, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "USRP_UHD getPort called with : " << portName)

    CORBA::Object_var p;

    if (!CORBA::is_nil(p = rx_control_port->getPort(portName)))
        return p._retn();
    else if (!CORBA::is_nil(p = rx_data_1_port->getPort(portName)))
        return p._retn();
    else if (!CORBA::is_nil(p = rx_data_1_port->getPort(portName))) {
        return p._retn();
    } else if (!CORBA::is_nil(p = tx_control_port->getPort(portName))) {
        return p._retn();
    } else if (!CORBA::is_nil(p = tx_data_port->getPort(portName))) {
        return p._retn();
    }
    std::cerr << "Couldn't find port " << portName << "Throwing exception" << std::endl;
    throw CF::PortSupplier::UnknownPort();
}

void USRP_UHD_i::initialize() throw (CF::LifeCycle::InitializeError, CORBA::SystemException)
{
    DEBUG(3, USRP_UHD, "USRP_UHD Initialize called")
    std::string args("addr=192.168.10.2");
    DEBUG(1, USRP_UHD, "Creating the usrp device with: "<< args)
    sdev = uhd::usrp::single_usrp::make(args);
    dev = sdev->get_device();
    sdev->set_rx_antenna("RX2");
    sdev->set_rx_antenna("TX/RX");
    DEBUG(1, USRP_UHD, "Using Device: "<< sdev->get_pp_string())
}

void USRP_UHD_i::configure(const CF::Properties &configProperties)
throw (CORBA::SystemException, CF::PropertySet::InvalidConfiguration,
       CF::PropertySet::PartialConfiguration)
{

}

void USRP_UHD_i::query(CF::Properties &configProperties)
throw (CORBA::SystemException, CF::UnknownProperties)
{

}


void USRP_UHD_i::releaseObject()
throw (CF::LifeCycle::ReleaseError, CORBA::SystemException)
{

    DEBUG(3, USRP_UHD, "USRP_UHD releaseObject called")
}


void USRP_UHD_i::rx_data_process()
{

///\TODO assumes one channel operation.  Set full_deplex to false to prevent Rx buffering during Tx
///Full duplex mode is still broken. we still cannot transmit and receive simultaneously using WBX board

    //tells the device to stream samples indefinitely
    uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);

    //Use with STREAM_MODE_NUM_SAMPS_AND_DONE/_MORE     
    //stream_cmd.num_samps = 65536;
    //stream_cmd.time_spec = uhd::time_spec_t((time_t)0);
    
    //When true, the device will begin streaming ASAP
    stream_cmd.stream_now = true;

    sdev->issue_stream_cmd(stream_cmd);

    PortTypes::ShortSequence I;
    PortTypes::ShortSequence Q;
    
///\TODO Make sure rx_packet_size is >= than USRP_UHD_MIN_RX_SAMPLES!!!!!
    rx_buff.resize(rx_packet_size);
    I.length(rx_packet_size);
    Q.length(rx_packet_size);

    DEBUG(3, USRP_UHD, "RX packet count " << rx_packet_count)

    while (rx_active && ((rx_packet_count == -1) || (rx_packet_count > 0))) {
        uhd::rx_metadata_t md;
//if we are running half duplex mode we will need to tell start streaming at the beginning of the loop
        if (full_duplex == false && did_rx == false)  { 
                uhd::stream_cmd_t stream_cmd(uhd::stream_cmd_t::STREAM_MODE_START_CONTINUOUS);
                sdev->issue_stream_cmd(stream_cmd);
                did_rx = true;
        }

        
//TODO get rid of num_rx_samps variable
        size_t num_rx_samps = dev->recv( &rx_buff.front(), rx_packet_size, md,
                                        uhd::io_type_t::COMPLEX_INT16,
                                        uhd::device::RECV_MODE_FULL_BUFF);
        
        //Handle UHD error codes
        switch(md.error_code){
            case uhd::rx_metadata_t::ERROR_CODE_NONE:
                break;
            case uhd::rx_metadata_t::ERROR_CODE_TIMEOUT:
                DEBUG(3, USRP_UHD, "Got timeout before all samples received, possible packet loss")
                break;
            default:
                DEBUG(3, USRP_UHD, "Got error code 0x"<<md.error_code)
        }

        for (unsigned int i = 0; i < num_rx_samps; i++) {
            I[i] = (CORBA::Short) rx_buff[i].real();
            Q[i] = (CORBA::Short) rx_buff[i].imag();
        }
        rx_data_1_port->pushPacket(I, Q);
       
        if (rx_packet_count != -1)
            --rx_packet_count;
    }

    DEBUG(3, USRP_UHD, "Exiting rx_data_process thread")
    //tells the device to stop streaming samples
    stream_cmd = uhd::stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS;
    sdev->issue_stream_cmd(stream_cmd);
    rx_active = false;
    rx_thread->exit();
}



// USRP_UHD Expects data in little endian format
static inline short convertToLE(short s)
{
#ifdef __powerpc__
    s = bswap_16(s);
#endif

    return s;
}

void USRP_UHD_i::tx_data_process()
{
    PortTypes::ShortSequence *I_in, *Q_in;

    const unsigned int tx_buf_len(USRP_UHD_MAX_TX_SAMPLES);

    uhd::tx_metadata_t md3;

    //Set start and end of burst flags to help with packet fragmentation if needed
    //Host will send ACK packets when an EOB flag is received
    //SOB true -> first packet in sequence gets SOB flag
    //EOB true -> last packet in sequence gets EOB flag
    md3.start_of_burst = false;  
    md3.end_of_burst   = false;     

    //We can send packets in the future if needed
    //md.has_time_spec  = 0;     //no time
    //md.time_spec = uhd::time_spec_t((time_t)0 );

    std::vector< std::complex<short int> > tx_buff(tx_buf_len);

    DEBUG(3, USRP_UHD, "Starting tx_data_process thread.")
    
    tx_active = true; // Cleared to stop TX in USRP_UHD_TX_Control->stop()

    while (tx_active) {
        //if running half duplex mode we want to stop Rx streaming while transmitting
        if (full_duplex == false && did_rx == true)  {
                did_rx = false;         
                uhd::rx_metadata_t md;
                size_t num_rx_samps = dev->recv( &rx_buff.front(), rx_packet_size, md,
                                                uhd::io_type_t::COMPLEX_INT16,
                                                uhd::device::RECV_MODE_FULL_BUFF);              
                sdev->issue_stream_cmd(uhd::stream_cmd_t::STREAM_MODE_STOP_CONTINUOUS);
                
        }


        tx_data_port->getData(I_in, Q_in);
        size_t samps_to_send = I_in->length();
        
        if(samps_to_send > tx_buf_len){
            std::cerr << "USRP_UHD Tx Buffer Overflow. Throwing Exception"<<std::endl;
            throw std::runtime_error("USRP_UHD buffer overflow");
        }

        for (unsigned int i = 0; i < samps_to_send; ++i) {
            tx_buff[i] = std::complex<short int> (convertToLE((*I_in)[i]),
                                                 convertToLE((*Q_in)[i]));
        }

        //send the entire packet (driver fragments internally)
        size_t num_tx_samps = dev->send( &tx_buff.front(), samps_to_send, md3,
                                        uhd::io_type_t::COMPLEX_INT16,
                                        uhd::device::SEND_MODE_FULL_BUFF);
        tx_data_port->bufferEmptied();
                
    }
    
    DEBUG(3, USRP_UHD, "Exiting tx_data_process thread.")
    tx_thread->exit();
}