/*
* 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.nio.ByteBuffer;
import java.util.ArrayList;
import java.util.HashMap;
import java.util.Hashtable;
import java.util.LinkedList;
import java.util.Map;
import java.util.concurrent.BlockingDeque;
import java.util.concurrent.LinkedBlockingDeque;
import java.util.concurrent.TimeUnit;
import org.apache.commons.collections4.map.MultiValueMap;
import org.apache.commons.lang3.ArrayUtils;
import org.joou.UByte;
import org.joou.UInteger;
import org.joou.UShort;
import org.slf4j.Logger;
import org.slf4j.LoggerFactory;
import se.bitcraze.crazyflie.crtp.Crtp;
import se.bitcraze.crazyflie.crtp.Crtp.LogBlockResponse;
import se.bitcraze.crazyflie.crtp.Crtp.Response;
import se.bitcraze.crazyflie.crtp.CrtpDriver;
import se.bitcraze.crazyflie.crtp.DataListener;
/**
*
* @author Andreas Huber
*
*/
public class Logging extends ToC<LogVariable> implements DataListener, Runnable {
private static final String LISTENER_ALL = "all";
private static final Logger log = LoggerFactory.getLogger(Logging.class);
private MultiValueMap<String, LogListener> listeners = new MultiValueMap<String, LogListener>();
private Map<String, LogConfig> logConfigs = new Hashtable<String, LogConfig>();
private LogConfig[] logs = null;
private final BlockingDeque<ValueChange> logQueue = new LinkedBlockingDeque<ValueChange>();
private Thread queueThread;
private static int blockId = 0;
public static final synchronized int getBlockId() {
return blockId++;
}
/**
*
*/
public Logging() {
super();
}
protected int init() {
blockId = 0;
driver.addListener((DataListener) this);
Crtp.LogTOCResponse response = driver.send(new Crtp.LogTOCRequest());
log.info("Got Response: {}", response);
if (response == null) {
return 0;
}
for (byte x = 0, y = (byte) (response.getCount()); x < y; x++) {
Crtp.LogTOCItemResponse item = driver
.send(new Crtp.LogTOCRequest(x));
if (item == null)
continue;
LogVariable variable = createVariable(item);
if (variable == null)
continue;
log.info("Register Logging Variable: {}", variable);
add(variable);
}
log.info("Reset Logging");
driver.send(new Crtp.LogResetRequest());
if (queueThread == null) {
queueThread = new Thread(this);
queueThread.start();
}
for (LogConfig group : logConfigs.values()) {
log.info("Configure Log Group: {}", group);
sendGroup(group);
if (group.isActive()) {
log.info("Activate Group: {}", group.getName());
driver.send(new Crtp.LogStartBlockRequest(group.getGroupId(),
group.getInterval()));
}
}
return response.getCount();
}
protected void release() {
driver.removeListener((DataListener) this);
if (queueThread != null) {
queueThread.interrupt();
if (queueThread.isAlive())
try {
queueThread.join(1000);
} catch (InterruptedException e) {
}
queueThread = null;
}
logQueue.clear();
}
/**
* Add a Listener for a Logging Block
*
* @param l
* @param names
*/
public void addListener(LogListener l, String... names) {
synchronized (listeners) {
if (ArrayUtils.isEmpty(names)) {
listeners.put(LISTENER_ALL, l);
return;
}
for (String name : names)
listeners.put(name, l);
}
}
/**
* Remove a Listener for a Logging Block
*
* @param l
* @param names
*/
public void removeListener(LogListener l, String... names) {
synchronized (listeners) {
if (ArrayUtils.isEmpty(names)) {
listeners.removeMapping(LISTENER_ALL, l);
return;
}
for (String name : names)
listeners.removeMapping(name, l);
}
}
/**
* Add a Logging Block
*
* @param name
* @param interval
* @param variables
*/
public void addGroup(String name, int interval, String... variables) {
if (logConfigs.containsKey(name))
throw new IllegalArgumentException("Group " + name
+ " already exists");
LogConfig config = new LogConfig(name, interval, variables);
logConfigs.put(config.getName(), config);
logs = ArrayUtils.add(logs, config.getGroupId(), config);
sendGroup(config);
}
private void sendGroup(LogConfig group) {
if (driver != null && driver.isConnected()) {
ArrayList<LogVariable> ids = new ArrayList<LogVariable>();
for (String name : group.getVariableNames()) {
LogVariable v = get(name);
if (v == null) {
log.warn("No Variable found for {}", name);
continue;
}
ids.add(v);
}
group.setVariables(ids.toArray(new LogVariable[ids.size()]));
driver.send(new Crtp.LogSetBlockRequest(
Crtp.LogSetBlockRequest.CONTROL_CREATE_BLOCK, group
.getGroupId(), group.getVariables()));
}
}
/**
* Start a configured Logging Block
*
* @param name
*/
public void start(String name) {
if (!logConfigs.containsKey(name))
throw new IllegalArgumentException("Group " + name + " not exists");
log.debug("Start Group: {}", name);
LogConfig block = logConfigs.get(name);
sendStart(block);
}
private void sendStart(LogConfig block) {
if (driver != null && driver.isConnected()) {
log.debug("Start Log Block: {}", block.getName());
driver.send(new Crtp.LogStartBlockRequest(block.getGroupId(), block
.getInterval()));
} else {
log.info("Not Connected!");
}
block.setActive(true);
}
/**
* Stop a configured Logging Block
*
* @param name
*/
public void stop(String name) {
if (!logConfigs.containsKey(name)) {
log.info("Group {} not exists!", name);
return;
}
LogConfig config = logConfigs.get(name);
sendStop(config);
}
private void sendStop(LogConfig block) {
if (driver != null && driver.isConnected())
driver.send(new Crtp.LogStopBlockRequest(block.getGroupId()));
block.setActive(false);
}
/**
* Start all configured Logging Blocks
*/
public void startAll() {
for (LogConfig block : logs)
sendStart(block);
}
/**
* Stop all configured Logging Blocks
*/
public void stopAll() {
for (LogConfig block : logs)
sendStop(block);
}
/**
* Remove a Logging Block
*
* @param name
*/
public void removeGroup(String name) {
if (!logConfigs.containsKey(name)) {
log.info("Group {} not exists!", name);
return;
}
LogConfig config = logConfigs.get(name);
if (driver != null && driver.isConnected())
driver.send(new Crtp.LogDeleteBlockRequest(config.getGroupId()));
logConfigs.remove(name);
logs = ArrayUtils.remove(logs, config.getGroupId());
}
/*
* (non-Javadoc)
*
* @see
* se.bitcraze.crazyflie.crtp.DataListener#dataReceived(se.bitcraze.crazyflie
* .crtp.CrtpDriver, se.bitcraze.crazyflie.crtp.Crtp.Response)
*/
@Override
public void dataReceived(CrtpDriver driver, Response response) {
if (!(response instanceof LogBlockResponse)) {
return;
}
LogBlockResponse logBlock = (LogBlockResponse) response;
if (ArrayUtils.getLength(logs) <= logBlock.getBlockId())
return;
LogConfig group = logs[logBlock.getBlockId()];
setValues(group, logBlock.getData());
}
private void setValues(LogConfig group, byte[] data) {
ByteBuffer buf = ByteBuffer.wrap(data).order(Crtp.BYTE_ORDER);
Map<String, Object> values = new HashMap<String, Object>();
for (LogVariable v : group.getVariables()) {
switch (v.getType()) {
case 1:
v.setValue(UByte.valueOf(buf.get()));
break;
case 2:
v.setValue(UShort.valueOf(buf.getShort()));
break;
case 3:
v.setValue(UInteger.valueOf(buf.getInt()));
break;
case 4:
v.setValue(buf.get());
break;
case 5:
v.setValue(buf.getShort());
break;
case 6:
v.setValue(buf.getInt());
break;
case 7:
v.setValue(buf.getFloat());
break;
case 8:
v.setValue(buf.getFloat());
break;
default:
break;
}
values.put(v.getName(), v.getValue());
}
logQueue.add(new ValueChange(group.getName(), values));
}
/*
* (non-Javadoc)
*
* @see java.lang.Runnable#run()
*/
@Override
public void run() {
while (!Thread.currentThread().isInterrupted()) {
try {
ValueChange change = logQueue.pollFirst(5,
TimeUnit.MILLISECONDS);
if (change == null)
continue;
notifyDataReceived(change);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
break;
}
}
}
protected void notifyDataReceived(ValueChange change) {
LinkedList<LogListener> temp = new LinkedList<LogListener>();
synchronized (listeners) {
if (listeners.containsKey(change.name))
temp.addAll(listeners.getCollection(change.name));
if (listeners.containsKey(LISTENER_ALL))
temp.addAll(listeners.getCollection(LISTENER_ALL));
}
for (LogListener cl : temp) {
try {
cl.valuesReceived(change.name, change.values);
} catch (Exception e) {
log.error("Error in {}", cl.getClass().getName(), e);
}
}
}
private static class ValueChange {
final String name;
final Map<String, Object> values;
/**
* @param name
* @param value
*/
public ValueChange(String name, Map<String, Object> values) {
super();
this.name = name;
this.values = values;
}
}
private static final LogVariable createVariable(Crtp.LogTOCItemResponse item) {
switch (item.getType()) {
case 1:
return new LogVariable(item.getItem(), item.getType(),
item.getGroup(), item.getName());
case 2:
return new LogVariable(item.getItem(), item.getType(),
item.getGroup(), item.getName());
case 3:
return new LogVariable(item.getItem(), item.getType(),
item.getGroup(), item.getName());
case 4:
return new LogVariable(item.getItem(), item.getType(),
item.getGroup(), item.getName());
case 5:
return new LogVariable(item.getItem(), item.getType(),
item.getGroup(), item.getName());
case 6:
return new LogVariable(item.getItem(), item.getType(),
item.getGroup(), item.getName());
case 7:
return new LogVariable(item.getItem(), item.getType(),
item.getGroup(), item.getName());
case 8:
return new LogVariable(item.getItem(), item.getType(),
item.getGroup(), item.getName());
default:
break;
}
return null;
}
}