/*
* Copyright (C) 2014 Andreas Huber
*
* This program 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.
*
* This program 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 this program; if not, write to the Free Software Foundation, Inc.,
* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
*/
package se.bitcraze.crazyflie.crtp;
import java.io.IOException;
import java.net.URI;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
import java.util.concurrent.BlockingDeque;
import java.util.concurrent.LinkedBlockingDeque;
import java.util.concurrent.TimeUnit;
import javax.usb.UsbAbortException;
import javax.usb.UsbConfiguration;
import javax.usb.UsbConfigurationDescriptor;
import javax.usb.UsbConst;
import javax.usb.UsbControlIrp;
import javax.usb.UsbDevice;
import javax.usb.UsbDeviceDescriptor;
import javax.usb.UsbEndpoint;
import javax.usb.UsbHostManager;
import javax.usb.UsbHub;
import javax.usb.UsbInterface;
import javax.usb.UsbInterfaceDescriptor;
import javax.usb.UsbPipe;
import javax.usb.UsbServices;
import org.apache.commons.lang3.ArrayUtils;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import se.bitcraze.crazyflie.crtp.Crtp.Request;
import se.bitcraze.crazyflie.util.BytePrinter;
public class CrazyradioDriver extends AbstractDriver {
/**
* Vendor ID of the CrazyRadio USB dongle.
*/
public static final int VENDOR_ID = 0x1915;
/**
* Product ID of the CrazyRadio USB dongle.
*/
public static final int PRODUCT_ID = 0x7777;
/**
* Number of packets without acknowledgment before marking the connection as
* broken and disconnecting.
*/
public static final int RETRYCOUNT_BEFORE_DISCONNECT = 10;
/**
* This number of packets should be processed between reports of the link
* quality.
*/
public static final int PACKETS_BETWEEN_LINK_QUALITY_UPDATE = 5;
// Dongle configuration requests
// See http://wiki.bitcraze.se/projects:crazyradio:protocol for
// documentation
public static final byte REQUEST_SET_RADIO_CHANNEL = 0x01;
public static final byte REQUEST_SET_RADIO_ADDRESS = 0x02;
public static final byte REQUEST_SET_DATA_RATE = 0x03;
public static final byte REQUEST_SET_RADIO_POWER = 0x04;
public static final byte REQUEST_SET_RADIO_ARD = 0x05;
public static final byte REQUEST_SET_RADIO_ARC = 0x06;
public static final byte REQUEST_ACK_ENABLE = 0x10;
public static final byte REQUEST_SET_CONT_CARRIER = 0x20;
public static final byte REQUEST_START_SCAN_CHANNELS = 0x21;
public static final byte REQUEST_GET_SCAN_CHANNELS = 0x21;
public static final byte REQUEST_LAUNCH_BOOTLOADER = (byte) 0xFF;
private static final String URI_PREFIX = "radio://";
private static final String[] DATA_RATES = new String[] { "250K", "1M",
"2M" };
private static final Logger log = LoggerFactory
.getLogger(CrazyradioDriver.class);
private final UsbDevice mUsbDevice;
private UsbConfiguration mUsbConfiguration;
private UsbInterface mIntf;
private UsbEndpoint mEpIn;
private UsbEndpoint mEpOut;
private UsbPipe mPIn;
private UsbPipe mPOut;
private volatile Thread writeThread;
private volatile Thread readThread;
private final BlockingDeque<Crtp.Request> mSendQueue;
private final Object responseSync = new Object();
private volatile Crtp.Response response = null;
/**
* Create a new link using the Crazyradio.
*
* @param usbManager
* @param usbDevice
* @param connectionData
* connection data to initialize the link
* @throws IllegalArgumentException
* if usbManager or usbDevice is <code>null</code>
* @throws IOException
* if the device cannot be opened
*/
public CrazyradioDriver(URI uri) throws Exception {
this.mUsbDevice = CrazyradioDriver.findDevice();
if (mUsbDevice == null) {
throw new IllegalArgumentException(
"USB manager and device must not be null");
}
initDevice();
setRadioChannel(Integer.parseInt(uri.getAuthority()));
String rate = uri.getPath().substring(1);
setDataRate(ArrayUtils.indexOf(DATA_RATES, rate));
this.mSendQueue = new LinkedBlockingDeque<Crtp.Request>();
}
/**
* Initialize the USB device. Determines endpoints and prepares
* communication.
*
* @param usbManager
* @throws IOException
* if the device cannot be opened
*/
private void initDevice() throws IOException {
if (log.isDebugEnabled())
log.debug("setDevice " + this.mUsbDevice);
// find interface
mUsbConfiguration = mUsbDevice.getActiveUsbConfiguration();
UsbConfigurationDescriptor desc = mUsbConfiguration
.getUsbConfigurationDescriptor();
if (desc.bNumInterfaces() != 1) {
log.error("Could not find interface");
return;
}
mIntf = this.mUsbConfiguration.getUsbInterface((byte) 0);
// device should have two endpoints
UsbInterfaceDescriptor iDesc = mIntf.getUsbInterfaceDescriptor();
if (iDesc.bNumEndpoints() != 2) {
log.error("Could not find endpoints");
return;
}
for (Object o : mIntf.getUsbEndpoints()) {
UsbEndpoint e = (UsbEndpoint) o;
if (e.getDirection() == UsbConst.ENDPOINT_DIRECTION_IN)
mEpIn = e;
else
mEpOut = e;
}
}
/**
* Scan for available channels.
*
* @return array containing the found channels and bandwidths.
* @throws Exception
*/
public static String[] scanChannels() {
List<String> result = new ArrayList<String>();
try {
UsbDevice usbDevice = findDevice();
if (usbDevice == null) {
if (log.isDebugEnabled())
log.debug("usbDevice is null");
throw new IllegalStateException("CrazyRadio not attached");
}
// null packet
final byte[] packet = Crtp.NULL_PACKET.toByteArray();
final byte[] rdata = new byte[64];
if (log.isDebugEnabled())
log.debug("Scanning...");
// scan for all 3 data rates
for (int b = 0; b < 3; b++) {
// set data rate
UsbControlIrp irp = usbDevice.createUsbControlIrp((byte) 0x40,
REQUEST_SET_DATA_RATE, (short) b, (short) 0);
usbDevice.syncSubmit(irp);
irp = usbDevice.createUsbControlIrp((byte) 0x40,
REQUEST_START_SCAN_CHANNELS, (short) 0, (short) 125);
irp.setData(packet);
// irp.setLength(packet.length);
usbDevice.syncSubmit(irp);
irp = usbDevice.createUsbControlIrp((byte) 0xc0,
REQUEST_GET_SCAN_CHANNELS, (short) 0, (short) 0);
irp.setData(rdata);
usbDevice.syncSubmit(irp);
for (int i = 0; i < irp.getActualLength(); i++) {
String uri = URI_PREFIX + rdata[i] + "/" + DATA_RATES[b];
result.add(uri);
if (log.isDebugEnabled())
log.debug("Channel found: " + rdata[i] + " Data rate: "
+ b);
}
}
} catch (Exception e) {
log.error("Failed to scan channels", e);
}
return result.toArray(new String[result.size()]);
}
/**
* Connect to the Crazyflie.
*
* @throws IllegalStateException
* if the CrazyRadio is not attached
*/
@Override
public void connect() throws Exception {
mIntf.claim();
mPIn = mEpIn.getUsbPipe();
mPOut = mEpOut.getUsbPipe();
mPIn.open();
mPOut.open();
if (log.isDebugEnabled())
log.debug("RadioLink start()");
if (readThread == null) {
readThread = new Thread(responseReader);
readThread.start();
}
if (writeThread == null) {
writeThread = new Thread(requestWriter);
writeThread.start();
}
notifyConnectionInitiated();
notifyConnectionSetupFinished();
}
@Override
public void disconnect() throws Exception {
if (log.isDebugEnabled())
log.debug("CrazyradioDriver disconnect()");
if (readThread != null) {
readThread.interrupt();
readThread = null;
}
if (writeThread != null) {
writeThread.interrupt();
writeThread = null;
}
if (mPOut.isOpen()) {
mPOut.abortAllSubmissions();
mPOut.close();
mPOut = null;
}
if (mPIn.isOpen()) {
mPIn.abortAllSubmissions();
mPIn.close();
mPIn = null;
}
if (mIntf != null)
mIntf.release();
notifyDisconnected();
}
@Override
public boolean isConnected() {
return readThread != null;
}
@Override
public void sendAsync(Crtp.Request request) {
this.mSendQueue.addLast(request);
}
/*
* (non-Javadoc)
*
* @see
* se.bitcraze.crazyflie.crtp.CrtpDriver#send(se.bitcraze.crazyflie.crtp
* .Crtp.Request, se.bitcraze.crazyflie.crtp.DataListener)
*/
@SuppressWarnings("unchecked")
@Override
public <T extends Crtp.Response> T send(Request request) {
synchronized (responseSync) {
this.mSendQueue.addLast(request);
try {
responseSync.wait(1000);
} catch (InterruptedException e) {
}
Crtp.Response r = response;
response = null;
return (T) r;
}
}
/**
* Set the radio channel.
*
* @param channel
* the new channel. Must be in range 0-125.
*/
public void setRadioChannel(int channel) {
sendControlTransfer((byte) 0x40, REQUEST_SET_RADIO_CHANNEL,
(short) channel, (short) 0, null);
}
/**
* Set the data rate.
*
* @param rate
* new data rate. Possible values are in range 0-2.
*/
public void setDataRate(int rate) {
sendControlTransfer((byte) 0x40, REQUEST_SET_DATA_RATE, (short) rate,
(short) 0, null);
}
/**
* Set the radio address. The same address must be configured in the
* receiver for the communication to work.
*
* @param address
* the new address with a length of 5 byte.
* @throws IllegalArgumentException
* if the length of the address doesn't equal 5 bytes
*/
public void setRadioAddress(byte[] address) {
if (address.length != 5) {
throw new IllegalArgumentException(
"radio address must be 5 bytes long");
}
sendControlTransfer((byte) 0x40, REQUEST_SET_RADIO_ADDRESS, (short) 0,
(short) 0, address);
}
/**
* Set the continuous carrier mode. When enabled the radio chip provides a
* test mode in which a continuous non-modulated sine wave is emitted. When
* this mode is activated the radio dongle does not transmit any packets.
*
* @param continuous
* <code>true</code> to enable the continuous carrier mode
*/
public void setContinuousCarrier(boolean continuous) {
sendControlTransfer((byte) 0x40, REQUEST_SET_CONT_CARRIER,
(short) (continuous ? 1 : 0), (short) 0, null);
}
/**
* Configure the time the radio waits for the acknowledge.
*
* @param us
* microseconds to wait. Will be rounded to the closest possible
* value supported by the radio.
*/
public void setAutoRetryADRTime(int us) {
int param = (int) Math.round(us / 250.0) - 1;
if (param < 0) {
param = 0;
} else if (param > 0xF) {
param = 0xF;
}
sendControlTransfer((byte) 0x40, REQUEST_SET_RADIO_ARD, (short) param,
(short) 0, null);
}
/**
* Set the length of the ACK payload.
*
* @param bytes
* number of bytes in the payload.
* @throws IllegalArgumentException
* if the payload length is not in range 0-32.
*/
public void setAutoRetryADRBytes(int bytes) {
if (bytes < 0 || bytes > 32) {
throw new IllegalArgumentException(
"payload length must be in range 0-32");
}
sendControlTransfer((byte) 0x40, REQUEST_SET_RADIO_ARD,
(short) (0x80 | bytes), (short) 0, null);
}
/**
* Set how often the radio will retry a transfer if the ACK has not been
* received.
*
* @param count
* the number of retries.
* @throws IllegalArgumentException
* if the number of retries is not in range 0-15.
*/
public void setAutoRetryARC(int count) {
if (count < 0 || count > 15) {
throw new IllegalArgumentException("count must be in range 0-15");
}
sendControlTransfer((byte) 0x40, REQUEST_SET_RADIO_ARC, (short) count,
(short) 0, null);
}
/**
* Handles communication with the dongle to send packets
*/
private final Runnable requestWriter = new Runnable() {
@Override
public void run() {
while (!Thread.currentThread().isInterrupted() && mPOut.isOpen()) {
try {
Request p = mSendQueue.pollFirst(10, TimeUnit.MILLISECONDS);
// if no packet was available in the send queue
if (p == null) {
p = Crtp.NULL_PACKET;
} else {
if (log.isTraceEnabled())
log.trace("Send Packet {}: {} ", p.getClass()
.getName(), BytePrinter.printHex(p
.toByteArray()));
}
final byte[] sendData = p.toByteArray();
for (int x = RETRYCOUNT_BEFORE_DISCONNECT; x >= 0; x--) {
try {
mPOut.syncSubmit(sendData);
break;
} catch (Exception e) {
log.error("Failed to send data", e);
if (x == 0)
throw e;
}
}
} catch (InterruptedException ie) {
// InterruptedException should only be when disconnect
Thread.currentThread().interrupt();
return;
} catch (Exception e) {
Thread.currentThread().interrupt();
log.error("Communication error", e);
break;
}
}
notifyConnectionLost();
try {
disconnect();
} catch (Exception e) {
log.info("Disconnect failed");
}
log.warn("Communication stopped");
}
};
/**
* Handles communication with the dongle to receive packets
*/
private final Runnable responseReader = new Runnable() {
@Override
public void run() {
int nextLinkQualityUpdate = PACKETS_BETWEEN_LINK_QUALITY_UPDATE;
while (!Thread.currentThread().isInterrupted() && mPIn != null
&& mPIn.isOpen()) {
byte[] receiveData = new byte[33];
try {
Thread.sleep(0);
if (mPIn == null)
break;
int receivedByteCount = mPIn.syncSubmit(receiveData);
if (receivedByteCount > 0) {
// update link quality status
if (nextLinkQualityUpdate <= 0) {
final int retransmission = receiveData[0] >> 4;
notifyLinkQuality(Math.max(0,
(10 - retransmission) * 10));
nextLinkQualityUpdate = PACKETS_BETWEEN_LINK_QUALITY_UPDATE;
} else {
nextLinkQualityUpdate--;
}
if ((receiveData[0] & 1) != 0) { // check if ack
// received
Crtp.Response resp = handleResponse(Arrays
.copyOfRange(receiveData, 1,
1 + (receivedByteCount - 1)));
if (resp != null) {
response = resp;
synchronized (responseSync) {
responseSync.notify();
}
}
}
} else {
log.warn("CrazyradioLink comm error - didn't receive answer");
}
} catch (InterruptedException ie) {
// InterruptedException should only be when disconnect
Thread.currentThread().interrupt();
break;
} catch (UsbAbortException uae) {
// USB Communication is aborted
Thread.currentThread().interrupt();
break;
} catch (Exception e) {
log.error("Communication error", e);
}
}
log.warn("Communication stopped");
}
};
// #Utility functions
private int sendControlTransfer(byte requestType, byte request,
short value, short index, byte[] data) {
UsbControlIrp irp = mUsbDevice.createUsbControlIrp(requestType,
request, value, index);
if (data != null)
irp.setData(data);
try {
mUsbDevice.syncSubmit(irp);
} catch (Exception e) {
log.error("Failed to send Usb Control", e);
return -1;
}
return irp.getActualLength();
}
public static boolean isCrazyRadio(UsbDevice device) {
UsbDeviceDescriptor desc = device.getUsbDeviceDescriptor();
return desc.idVendor() == CrazyradioDriver.VENDOR_ID
&& desc.idProduct() == CrazyradioDriver.PRODUCT_ID;
}
@SuppressWarnings("unchecked")
private static final UsbDevice findDevice() throws Exception {
UsbServices services = UsbHostManager.getUsbServices();
UsbHub rootHub = services.getRootUsbHub();
return CrazyradioDriver.findDevice(rootHub);
}
@SuppressWarnings("unchecked")
private static final UsbDevice findDevice(UsbHub hub) {
for (UsbDevice device : (List<UsbDevice>) hub.getAttachedUsbDevices()) {
if (isCrazyRadio(device))
return device;
if (device.isUsbHub()) {
device = findDevice((UsbHub) device);
if (device != null)
return device;
}
}
return null;
}
}