Commit 443d123f authored by Ian McInerney's avatar Ian McInerney

Refactored radio into multiple files

parent 15b97156
// Copyright (c) 2013, Jan Winkler <winkler@cs.uni-bremen.de>
// All rights reserved.
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright
// notice, this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
// * Neither the name of Universität Bremen nor the names of its
// contributors may be used to endorse or promote products derived from
// this software without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
#include "CCrazyRadio.h"
#include "CCrazyflie.h"
CCrazyRadio::CCrazyRadio(int dongleNumber) {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
//m_strRadioIdentifier = strRadioIdentifier; -- multi flie support
m_enumPower = P_M18DBM;
m_nDongleNumber = dongleNumber;
m_ctxContext = NULL;
m_hndlDevice = NULL;
m_bAckReceived = false;
/*int nReturn = */libusb_init(&m_ctxContext);
// Do error checking here.
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
CCrazyRadio::~CCrazyRadio() {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
this->closeDevice();
if (m_ctxContext) {
libusb_exit(m_ctxContext);
}
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
// returns true if successfully started, else false on error
bool CCrazyRadio::startRadio() {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
if (this->openUSBDongle()) {
std::stringstream sts;
// Read device version
libusb_device_descriptor ddDescriptor;
libusb_get_device_descriptor(m_devDevice, &ddDescriptor);
sts.clear();
sts.str(std::string());
sts << (ddDescriptor.bcdDevice >> 8);
sts << ".";
sts << (ddDescriptor.bcdDevice & 0x0ff);
std::sscanf(sts.str().c_str(), "%f", &m_fDeviceVersion);
std::cout << "Got device version " << m_fDeviceVersion << std::endl;
if (m_fDeviceVersion < 0.3) {
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return false;
}
// Set active configuration to 1
libusb_set_configuration(m_hndlDevice, 1);
std::cout << "Attempt Interface Claim" << std::endl;
// Claim interface
if (this->claimInterface(0)) {
// Set power-up settings for dongle (>= v0.4)
std::cout << "Interface Claim Success" << std::endl;
if( ALL_THE_DEBUG ) printf( "setting channel to 2\n" );
this->setDataRate(XFER_2M);
this->setChannel(2);
if (m_fDeviceVersion >= 0.4) {
std::cout << "Device Version > 0.4" << std::endl;
this->setContCarrier(false);
char cAddress[5];
cAddress[0] = 0xe7;
cAddress[1] = 0xe7;
cAddress[2] = 0xe7;
cAddress[3] = 0xe7;
cAddress[4] = 0xe7;
this->setAddress(cAddress);
this->setPower(P_0DBM);
this->setARC(3);
this->setARDBytes(32);
}
// Initialize device
if (m_fDeviceVersion >= 0.4) {
this->setARC(10);
}
if( ALL_THE_DEBUG ) printf( "*HACK* hardcoding channel to 60\n" );
this->setChannel(60);
this->setDataRate(XFER_2M);
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return true;
}
}
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return false;
}
bool CCrazyRadio::ackReceived() {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
return m_bAckReceived;
}
#include "CCrazyRadio.h"
#include "CCrazyflie.h"
void CCrazyRadio::closeDevice() {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
if (m_hndlDevice) {
libusb_close(m_hndlDevice);
libusb_unref_device(m_devDevice);
m_hndlDevice = NULL;
m_devDevice = NULL;
}
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
std::list<libusb_device*> CCrazyRadio::listDevices(int nVendorID,
int nProductID) {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
std::list<libusb_device*> lstDevices;
ssize_t szCount;
libusb_device **ptDevices;
szCount = libusb_get_device_list(m_ctxContext, &ptDevices);
for (unsigned int unI = 0; unI < szCount; unI++) {
libusb_device *devCurrent = ptDevices[unI];
libusb_device_descriptor ddDescriptor;
libusb_get_device_descriptor(devCurrent, &ddDescriptor);
if (ddDescriptor.idVendor == nVendorID
&& ddDescriptor.idProduct == nProductID) {
libusb_ref_device(devCurrent);
lstDevices.push_back(devCurrent);
}
}
if (szCount > 0) {
libusb_free_device_list(ptDevices, 1);
}
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return lstDevices;
}
bool CCrazyRadio::openUSBDongle() {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
this->closeDevice(); // JRB: This seems dangerous if we haven't opened yet
int popIndex = 0;
libusb_device *devFirst;
std::list<libusb_device*> lstDevices = this->listDevices(0x1915, 0x7777);
if (lstDevices.size() > 0) {
// For now, just take the first device. Give it a second to
// initialize the system permissions.
sleep(1.0);
// Iterate through the list of dongles that were returned and save off
// the one we're interested
while (popIndex <= m_nDongleNumber ) {
devFirst = lstDevices.front(); // this will keep getting overwritten until we reach the one we're interested in
lstDevices.pop_front();
popIndex++;
}
// devFirst now contains the one we're interested in
int nError = libusb_open(devFirst, &m_hndlDevice);
if (nError == 0) {
// Opening device OK. Don't free the first device just yet.
m_devDevice = devFirst;
}
for (std::list<libusb_device*>::iterator itDevice = lstDevices.begin();
itDevice != lstDevices.end(); itDevice++) {
libusb_device *devCurrent = *itDevice;
libusb_unref_device(devCurrent); // JRB: Not sure why we are unreferencing all devices... must be a reason?
}
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return !nError;
}
std::cout << "No USB Devices Found" << std::endl;
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return false;
}
CCRTPPacket *CCrazyRadio::writeData(void *vdData, int nLength) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(vdData:0x%016lx, nLength:%d)\n", __FUNCTION__, (unsigned long)vdData, nLength );
CCRTPPacket *crtpPacket = NULL;
int nActuallyWritten;
int nReturn = libusb_bulk_transfer(m_hndlDevice,
(0x01 | LIBUSB_ENDPOINT_OUT), (unsigned char*) vdData, nLength,
&nActuallyWritten, 1000);
if (nReturn == 0 && nActuallyWritten == nLength) {
crtpPacket = this->readACK();
}
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return crtpPacket;
}
bool CCrazyRadio::readData(void *vdData, int &nMaxLength) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(vdData:0x%016lx, nMaxLength:%d)\n", __FUNCTION__, (unsigned long)vdData, nMaxLength );
int nActuallyRead;
int nReturn = libusb_bulk_transfer(m_hndlDevice,
(0x81 | LIBUSB_ENDPOINT_IN), (unsigned char*) vdData, nMaxLength,
&nActuallyRead, 50);
if (nReturn == 0) {
nMaxLength = nActuallyRead;
return true;
} else {
switch (nReturn) {
case LIBUSB_ERROR_TIMEOUT:
std::cout << "USB timeout" << std::endl;
break;
default:
break;
}
}
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return false;
}
bool CCrazyRadio::writeControl(void *vdData, int nLength, uint8_t u8Request,
uint16_t u16Value, uint16_t u16Index) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(vdData:0x%016lx, nLength:%d, u8Request:0x%02x, u16Value:0x%04x, u16Index:0x%04x)\n",
__FUNCTION__, (unsigned long)vdData, nLength, u8Request, u16Value, u16Index );
int nTimeout = 1000;
/*int nReturn = */libusb_control_transfer(m_hndlDevice,
LIBUSB_REQUEST_TYPE_VENDOR, u8Request, u16Value, u16Index,
(unsigned char*) vdData, nLength, nTimeout);
// Hack. // JRB: Why was this hacked in?
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
return true;
}
bool CCrazyRadio::claimInterface(int nInterface) {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
return libusb_claim_interface(m_hndlDevice, nInterface) == 0;
}
bool CCrazyRadio::usbOK() {
if( ALL_THE_DEBUG ) printf( "Enter: %s\n", __FUNCTION__ );
libusb_device_descriptor ddDescriptor;
return (libusb_get_device_descriptor(m_devDevice, &ddDescriptor) == 0);
}
\ No newline at end of file
#include "CCrazyRadio.h"
#include "CCrazyflie.h"
void CCrazyRadio::setARC(int nARC) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(nARC:%d)\n", __FUNCTION__, nARC );
m_nARC = nARC;
this->writeControl(NULL, 0, 0x06, nARC, 0);
}
void CCrazyRadio::setChannel(int nChannel) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(nChannel:%d)\n", __FUNCTION__, nChannel );
//m_nChannel = nChannel; -- removing for multi flie support
this->writeControl(NULL, 0, 0x01, nChannel, 0);
}
void CCrazyRadio::setDataRate(XferRate rate) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(rate:%d)\n", __FUNCTION__, rate );
this->writeControl(NULL, 0, 0x03, (int)rate, 0);
}
void CCrazyRadio::setARDTime(int nARDTime) { // in uSec
if( ALL_THE_DEBUG ) printf( "Enter: %s(nARDTime:%d)\n", __FUNCTION__, nARDTime );
m_nARDTime = nARDTime;
int nT = int((nARDTime / 250) - 1);
if (nT < 0) {
nT = 0;
} else if (nT > 0xf) {
nT = 0xf;
}
this->writeControl(NULL, 0, 0x05, nT, 0);
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
void CCrazyRadio::setARDBytes(int nARDBytes) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(nARDBytes:%d)\n", __FUNCTION__, nARDBytes );
m_nARDBytes = nARDBytes;
this->writeControl(NULL, 0, 0x05, 0x80 | nARDBytes, 0);
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
enum Power CCrazyRadio::power() {
return m_enumPower;
}
void CCrazyRadio::setPower(enum Power enumPower) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(enumPower:%d)\n", __FUNCTION__, enumPower );
m_enumPower = enumPower;
this->writeControl(NULL, 0, 0x04, enumPower, 0);
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
void CCrazyRadio::setAddress(char *cAddress) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(cAddress:0x%016lx)\n", __FUNCTION__, (unsigned long)cAddress );
m_cAddress = cAddress;
this->writeControl(cAddress, 5, 0x02, 0, 0);
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
void CCrazyRadio::setContCarrier(bool bContCarrier) {
if( ALL_THE_DEBUG ) printf( "Enter: %s(bContCarrier:%d)\n", __FUNCTION__, bContCarrier );
m_bContCarrier = bContCarrier;
this->writeControl(NULL, 0, 0x20, (bContCarrier ? 1 : 0), 0);
if( ALL_THE_DEBUG ) printf( "Exit: %s\n", __FUNCTION__ );
}
\ No newline at end of file
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment