/*
* 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;
import java.net.URI;
import java.util.Collections;
import java.util.HashMap;
import java.util.LinkedList;
import java.util.List;
import java.util.Map;
import org.joou.UByte;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import se.bitcraze.crazyflie.crtp.CrazyradioDriver;
import se.bitcraze.crazyflie.crtp.Crtp;
import se.bitcraze.crazyflie.crtp.Crtp.ConsoleResponse;
import se.bitcraze.crazyflie.crtp.Crtp.Response;
import se.bitcraze.crazyflie.crtp.CrtpDriver;
import se.bitcraze.crazyflie.crtp.DataListener;
/**
* @author Andreas Huber
*
*/
public class Crazyflie implements ConnectionListener, DataListener {
private static final Logger log = LoggerFactory.getLogger(Crazyflie.class);
private static Map<String, CrtpDriver> links = new HashMap<String, CrtpDriver>();
public static final String[] scanInterfaces() {
return CrazyradioDriver.scanChannels();
}
private static synchronized final CrtpDriver create(String uri)
throws Exception {
if (links.containsKey(uri))
return links.get(uri);
URI u = new URI(uri);
CrazyradioDriver link = new CrazyradioDriver(u);
links.put(uri, link);
return link;
}
private List<ConnectionListener> connectionListeners = Collections
.synchronizedList(new LinkedList<ConnectionListener>());
private List<ConsoleListener> consoleListeners = Collections
.synchronizedList(new LinkedList<ConsoleListener>());
private CrtpDriver driver;
private boolean xmode;
private final Logging logging;
private final Parameters parameters;
private volatile StringBuffer consoleBuffer = new StringBuffer();
/**
*
*/
public Crazyflie() {
parameters = new Parameters();
connectionListeners.add(parameters);
logging = new Logging();
connectionListeners.add(logging);
}
/**
* Connect to Crazyflie
*
* @param uri
* @throws Exception
*/
public void connect(String uri) throws Exception {
driver = Crazyflie.create(uri);
driver.addListener((ConnectionListener) this);
driver.addListener((DataListener) this);
driver.connect();
}
/**
* Disconnect from Crazyflie
*
* @throws Exception
*/
public void disconnect() throws Exception {
if (driver == null)
return;
driver.disconnect();
driver.removeListener((DataListener) this);
driver.removeListener((ConnectionListener) this);
}
/**
*
* @return
*/
public boolean isConnected() {
if (driver == null)
return false;
return driver.isConnected();
}
public void sendSetpoint(float roll, float pitch, float yaw, char thrust) {
if (driver != null)
driver.sendAsync(new Crtp.CommandPacket(roll, pitch, yaw, thrust,
xmode));
}
/**
* Set Altitude hold mode on/off
*
* @param hold
* @return
*/
public boolean setAltHold(boolean hold) {
if (isConnected())
return getParameters().set("flightmode.althold",
UByte.valueOf(hold ? 1 : 0));
return false;
}
/**
* Return Crazyflie Parameter list
*
* @return the parameters
*/
public Parameters getParameters() {
return parameters;
}
/**
* Return Crazyflie Loggin list
*
* @return the variables
*/
public Logging getLogging() {
return logging;
}
/**
* @return the xmode
*/
public boolean isXmode() {
return xmode;
}
/**
* @param xmode
* the xmode to set
*/
public void setXmode(boolean xmode) {
this.xmode = xmode;
}
public void addListener(ConnectionListener listener) {
this.connectionListeners.add(listener);
}
public void removeListener(ConnectionListener listener) {
this.connectionListeners.remove(listener);
}
public void addListener(ConsoleListener listener) {
this.consoleListeners.add(listener);
}
public void removeListener(ConsoleListener listener) {
this.consoleListeners.remove(listener);
}
/**
* Add a LogListener
*
* @param l
* @param name
* Log Group name
*/
public void addListener(LogListener l, String... name) {
getLogging().addListener(l, name);
}
public void removeListener(LogListener l, String... name) {
getLogging().removeListener(l, name);
}
/*
* (non-Javadoc)
*
* @see
* se.bitcraze.crazyflie.ConnectionListener#connectionInitiated(se.bitcraze
* .crazyflie.crtp.CrtpDriver)
*/
@Override
public void connectionInitiated(CrtpDriver l) {
for (ConnectionListener cl : this.connectionListeners) {
cl.connectionInitiated(l);
}
}
/*
* (non-Javadoc)
*
* @see
* se.bitcraze.crazyflie.ConnectionListener#connectionSetupFinished(se.bitcraze
* .crazyflie.crtp.CrtpDriver)
*/
@Override
public void connectionSetupFinished(CrtpDriver l) {
for (ConnectionListener cl : this.connectionListeners) {
cl.connectionSetupFinished(l);
}
}
/*
* (non-Javadoc)
*
* @see
* se.bitcraze.crazyflie.ConnectionListener#disconnected(se.bitcraze.crazyflie
* .crtp.CrtpDriver)
*/
@Override
public void disconnected(CrtpDriver l) {
for (ConnectionListener cl : this.connectionListeners) {
cl.disconnected(l);
}
}
/*
* (non-Javadoc)
*
* @see
* se.bitcraze.crazyflie.ConnectionListener#connectionLost(se.bitcraze.crazyflie
* .crtp.CrtpDriver)
*/
@Override
public void connectionLost(CrtpDriver l) {
for (ConnectionListener cl : this.connectionListeners) {
cl.connectionLost(l);
}
}
/*
* (non-Javadoc)
*
* @see
* se.bitcraze.crazyflie.ConnectionListener#connectionFailed(se.bitcraze
* .crazyflie.crtp.CrtpDriver)
*/
@Override
public void connectionFailed(CrtpDriver l) {
for (ConnectionListener cl : this.connectionListeners) {
cl.connectionFailed(l);
}
}
/*
* (non-Javadoc)
*
* @see
* se.bitcraze.crazyflie.ConnectionListener#linkQualityUpdate(se.bitcraze
* .crazyflie.crtp.CrtpDriver, int)
*/
@Override
public void linkQualityUpdate(CrtpDriver l, int quality) {
for (ConnectionListener cl : this.connectionListeners) {
cl.linkQualityUpdate(l, quality);
}
}
@Override
public void dataReceived(CrtpDriver driver, Response response) {
if (response instanceof ConsoleResponse) {
ConsoleResponse r = (ConsoleResponse) response;
consoleBuffer.append(r.getText());
int n = consoleBuffer.indexOf("\n");
if (n >= 0) {
String msg = consoleBuffer.substring(0, n);
consoleBuffer = consoleBuffer.delete(0, n + 1);
for (ConsoleListener l : this.consoleListeners)
l.messageReceived(driver, msg);
}
}
}
}