wellton/javadrone
1
0
Fork 0
Código Issues Pull Requests Ações Pacotes Projetos Versões Wiki Atividade

Refactored ARdrone NanvData and Video read decode. Separated ARDrone 1.0 decoders. Added LightUDPReader. Added support of external NavData and video decoders

Ver Código-Fonte
Esse commit está contido em:
Denis Shmyga
2013-01-23 17:39:11 +02:00
pai f31e79426e
commit 003e8ccb43
46 arquivos alterados com 1516 adições e 1055 exclusões
Baixar Arquivo de Patch Baixar Arquivo de Diff Expandir todos os arquivos Colapsar todos os arquivos
controltower/pom.xml
+3 -3
Ver Arquivo
@@ -134,9 +134,9 @@
<version>3.9.2</version>
</dependency>
<dependency>
<groupId>org.slf4j</groupId>
<artifactId>slf4j-simple</artifactId>
<version>1.6.6</version>
<groupId>org.slf4j</groupId>
<artifactId>slf4j-simple</artifactId>
<version>1.6.6</version>
</dependency>
<dependency>
<groupId>org.slf4j</groupId>
controltower/src/main/java/com/codeminders/controltower/ControlTower.java
+5 -2
Ver Arquivo
@@ -44,7 +44,7 @@ public class ControlTower extends javax.swing.JFrame implements DroneStatusChang
{
private static final long READ_UPDATE_DELAY_MS = 5L;
private static final long CONNECT_TIMEOUT = 8000L;
private static final long CONNECT_TIMEOUT = 10000L;
private static float CONTROL_THRESHOLD = 0.5f;
private final ImageIcon droneOn = new ImageIcon(
getClass().getResource("/com/codeminders/controltower/images/drone_on.gif"));
@@ -76,7 +76,6 @@ public class ControlTower extends javax.swing.JFrame implements DroneStatusChang
static
{
isHIDLibLoaded = ClassPathLibraryLoader.loadNativeHIDLibrary();
SLF4JBridgeHandler.install();
}
/**
@@ -706,6 +705,10 @@ public class ControlTower extends javax.swing.JFrame implements DroneStatusChang
*/
public static void main(String args[])
{
if (!SLF4JBridgeHandler.isInstalled()) {
SLF4JBridgeHandler.install();
}
final ControlTower tower = new ControlTower();
java.awt.EventQueue.invokeLater(new Runnable()
{
controltower/src/resources/log4j.properties
+1 -1
Ver Arquivo
@@ -1,5 +1,5 @@
# Set root logger level to DEBUG and its only appender to A1.
log4j.rootLogger=DEBUG, A1
log4j.rootLogger=ALL, A1
# A1 is set to be a ConsoleAppender.
log4j.appender.A1=org.apache.log4j.ConsoleAppender
javadrone-api/src/main/java/com/codeminders/ardrone/ARDrone.java
+73 -63
Ver Arquivo
@@ -9,14 +9,17 @@ import java.util.List;
import java.util.logging.Level;
import java.util.logging.Logger;
import com.codeminders.ardrone.NavData.FlyingState;
import com.codeminders.ardrone.data.ARDroneDataReader;
import com.codeminders.ardrone.data.ChannelProcessor;
import com.codeminders.ardrone.data.navdata.FlyingState;
import com.codeminders.ardrone.data.navdata.Mode;
import com.codeminders.ardrone.data.reader.LigthUDPDataReader;
import com.codeminders.ardrone.data.reader.UDPDataReader;
import com.codeminders.ardrone.commands.*;
import com.codeminders.ardrone.data.decoder.DataDecoder;
import com.codeminders.ardrone.data.decoder.NavDataDecoder;
import com.codeminders.ardrone.data.decoder.VideoDataDecoder;
import com.codeminders.ardrone.data.logger.ChannelDataLogger;
import com.codeminders.ardrone.data.logger.file.AsyncFileChannelDataLogger;
import com.codeminders.ardrone.data.reader.UDPDataReaderAndDecoder;
import com.codeminders.ardrone.data.decoder.ardrone10.ARDrone10NavDataDecoder;
import com.codeminders.ardrone.data.decoder.ardrone10.ARDrone10VideoDataDecoder;
import com.codeminders.ardrone.data.logger.ARDroneDataReaderAndLogWrapper;
import com.codeminders.ardrone.data.logger.DataLogger;
public class ARDrone
{
@@ -119,14 +122,13 @@ public class ARDrone
private CommandQueue cmd_queue = new CommandQueue(CMD_QUEUE_SIZE);
private UDPDataReaderAndDecoder nav_data_reader;
private UDPDataReaderAndDecoder video_reader;
private DataDecoder ext_video_data_decoder;
private ChannelProcessor drone_nav_channel_processor;
private ChannelProcessor drone_video_channel_processor;
private CommandSender cmd_sender;
private Thread nav_data_reader_thread;
private Thread cmd_sending_thread;
private Thread video_reader_thread;
private boolean combinedYawMode = true;
@@ -140,6 +142,9 @@ public class ARDrone
private int navDataReconnectTimeout = 1000; // 1 second
private int videoReconnectTimeout = 1000; // 1 second
private VideoDataDecoder ext_video_data_decoder;
private NavDataDecoder ext_nav_data_decoder;
public ARDrone() throws UnknownHostException
{
this(InetAddress.getByAddress(DEFAULT_DRONE_IP), 1000, 1000);
@@ -280,17 +285,6 @@ public class ARDrone
cmd_queue.add(new EmergencyCommand());
}
}
/**
* Initiate drone connection procedure.
* With input data channel logging
*
* @throws IOException
*/
public void connectWithChannelLoging(String logDirectory) throws IOException
{
connect(new AsyncFileChannelDataLogger(logDirectory, "video"), new AsyncFileChannelDataLogger(logDirectory, "navdata"));
}
/**
* Initiate drone connection procedure.
@@ -302,7 +296,7 @@ public class ARDrone
connect(null, null);
}
private void connect(ChannelDataLogger videoLogger, ChannelDataLogger navdataLogger) throws IOException
public void connect(DataLogger videoLogger, DataLogger navdataLogger) throws IOException
{
try
{
@@ -312,28 +306,33 @@ public class ARDrone
cmd_sending_thread = new Thread(cmd_sender);
cmd_sending_thread.setName("Command Sender");
cmd_sending_thread.start();
NavDataDecoder nav_data_decoder = new NavDataDecoder(this, NAVDATA_BUFFER_SIZE);
nav_data_decoder.start();
nav_data_reader = new UDPDataReaderAndDecoder(this, drone_addr, NAVDATA_PORT, NAVDATA_BUFFER_SIZE, navDataReconnectTimeout, nav_data_decoder, navdataLogger);
nav_data_reader_thread = new Thread(nav_data_reader);
nav_data_reader_thread.setName("NavData Reader");
nav_data_reader_thread.start();
enableVideo();
enableAutomaticVideoBitrate();
DataDecoder video_data_decoder;
if (null != ext_video_data_decoder) {
video_data_decoder = ext_video_data_decoder;
} else {
VideoDataDecoder video_decoder = new VideoDataDecoder(this, VIDEO_BUFFER_SIZE);
video_decoder.start();
video_data_decoder = video_decoder;
}
NavDataDecoder nav_data_decoder = (null == ext_nav_data_decoder) ?
new ARDrone10NavDataDecoder(this, NAVDATA_BUFFER_SIZE)
:
ext_nav_data_decoder;
ARDroneDataReader nav_data_reader = (null == navdataLogger) ?
new LigthUDPDataReader(drone_addr, NAVDATA_PORT, navDataReconnectTimeout)
:
new ARDroneDataReaderAndLogWrapper(new LigthUDPDataReader(drone_addr, NAVDATA_PORT, navDataReconnectTimeout), navdataLogger);
drone_nav_channel_processor = new ChannelProcessor(nav_data_reader, nav_data_decoder);
video_reader = new UDPDataReaderAndDecoder(this, drone_addr, VIDEO_PORT, VIDEO_BUFFER_SIZE, videoReconnectTimeout, video_data_decoder, videoLogger);
video_reader_thread = new Thread(video_reader);
video_reader_thread.setName("Video Reader");
video_reader_thread.start();
VideoDataDecoder video_data_decoder = (null == ext_video_data_decoder) ?
new ARDrone10VideoDataDecoder(this, VIDEO_BUFFER_SIZE)
:
ext_video_data_decoder;
ARDroneDataReader video_data_reader = (null == videoLogger) ?
new UDPDataReader(drone_addr, VIDEO_PORT, videoReconnectTimeout)
:
new ARDroneDataReaderAndLogWrapper(new UDPDataReader(drone_addr, VIDEO_PORT, videoReconnectTimeout), videoLogger);
drone_video_channel_processor = new ChannelProcessor(video_data_reader, video_data_decoder);
changeState(State.CONNECTING);
@@ -365,11 +364,11 @@ public class ARDrone
if(cmd_queue != null)
cmd_queue.add(new QuitCommand());
if(nav_data_reader != null)
nav_data_reader.finish();
if(drone_nav_channel_processor != null)
drone_nav_channel_processor.finish();
if(video_reader != null)
video_reader.finish();
if(drone_video_channel_processor != null)
drone_video_channel_processor.finish();
if(cmd_socket != null)
cmd_socket.close();
@@ -393,6 +392,15 @@ public class ARDrone
{
setConfigOption("video:bitrate_control_mode", "1");
}
public void enableVideo() throws IOException
{
setConfigOption("general:video_enable", "TRUE");
}
public void disableVideo() throws IOException
{
setConfigOption("general:video_enable", "FALSE");
}
public void hover() throws IOException
{
@@ -446,12 +454,11 @@ public class ARDrone
public void move(float left_right_tilt, float front_back_tilt, float vertical_speed, float angular_speed)
throws IOException
{
cmd_queue
.add(new MoveCommand(combinedYawMode, left_right_tilt, front_back_tilt, vertical_speed, angular_speed));
cmd_queue.add(new MoveCommand(combinedYawMode, left_right_tilt, front_back_tilt, vertical_speed, angular_speed));
}
// Callback used by receiver
public void navDataReceived(NavData nd)
protected void navDataReceived(NavData nd)
{
if(nd.isBatteryTooLow() || nd.isNotEnoughPower())
{
@@ -487,10 +494,10 @@ public class ARDrone
// LAND/TAKEOFF comand
// instead of nuking the whole queue?
changeState(State.DEMO);
} else if(state != State.BOOTSTRAP && nd.getMode() == NavData.Mode.BOOTSTRAP)
} else if(state != State.BOOTSTRAP && nd.getMode() == Mode.BOOTSTRAP)
{
changeState(State.BOOTSTRAP);
} else if(state == State.BOOTSTRAP && nd.getMode() == NavData.Mode.DEMO)
} else if(state == State.BOOTSTRAP && nd.getMode() == Mode.DEMO)
{
changeState(State.DEMO);
}
@@ -623,7 +630,7 @@ public class ARDrone
}
// Callback used by VideoReciver
public void videoFrameReceived(int startX, int startY, int w, int h, int[] rgbArray, int offset, int scansize)
protected void videoFrameReceived(int startX, int startY, int w, int h, int[] rgbArray, int offset, int scansize)
{
synchronized(image_listeners)
{
@@ -682,28 +689,27 @@ public class ARDrone
}
}
}
public void pauseNavData() {
if (null != nav_data_reader) {
nav_data_reader.pauseReading();
if (null != drone_nav_channel_processor) {
drone_nav_channel_processor.pause();
}
}
public void resumeNavData() {
if (null != nav_data_reader) {
nav_data_reader.resumeReading();
if (null != drone_nav_channel_processor) {
drone_nav_channel_processor.resume();
}
}
public void pauseVideo() {
if (null != video_reader_thread) {
video_reader.pauseReading();
if (null != drone_video_channel_processor) {
drone_video_channel_processor.pause();
}
}
public void resumeVideo() {
if (null != video_reader_thread) {
video_reader.resumeReading();
if (null != drone_video_channel_processor) {
drone_video_channel_processor.resume();
}
}
@@ -711,9 +717,13 @@ public class ARDrone
return state;
}
public void setExternalVideoDataDecoder(DataDecoder ext_video_data_decoder) {
public void setExternalVideoDataDecoder(VideoDataDecoder ext_video_data_decoder) {
this.ext_video_data_decoder = ext_video_data_decoder;
}
public void setExternalVideoDataDecoder(NavDataDecoder ext_nav_data_decoder) {
this.ext_nav_data_decoder = ext_nav_data_decoder;
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/NavData.java
+51 -550
Ver Arquivo
@@ -1,616 +1,117 @@
package com.codeminders.ardrone;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.List;
import java.util.logging.Logger;
import com.codeminders.ardrone.data.navdata.ControlAlgorithm;
import com.codeminders.ardrone.data.navdata.CtrlState;
import com.codeminders.ardrone.data.navdata.FlyingState;
import com.codeminders.ardrone.data.navdata.Mode;
import com.codeminders.ardrone.data.navdata.vision.VisionTag;
import com.codeminders.ardrone.VisionTag.VisionTagType;
public class NavData
{
public static enum ControlAlgorithm
{
EULER_ANGELS_CONTROL, ANGULAR_SPEED_CONTROL
}
public static enum CtrlState
{
DEFAULT, INIT, LANDED, FLYING, HOVERING, TEST, TRANS_TAKEOFF, TRANS_GOTOFIX, TRANS_LANDING;
public static CtrlState fromInt(int v) throws NavDataFormatException
{
switch(v)
{
case 0:
return DEFAULT;
case 1:
return INIT;
case 2:
return LANDED;
case 3:
return FLYING;
case 4:
return HOVERING;
case 5:
return TEST;
case 6:
return TRANS_TAKEOFF;
case 7:
return TRANS_GOTOFIX;
case 8:
return TRANS_LANDING;
default:
throw new NavDataFormatException("Invalid control state " + v);
}
}
}
public static enum FlyingState
{
FLYING, TAKING_OFF, LANDING, LANDED;
public static FlyingState fromControlState(CtrlState state)
{
switch(state)
{
case FLYING:
case HOVERING:
case TRANS_GOTOFIX:
return FlyingState.FLYING;
case TRANS_TAKEOFF:
return FlyingState.TAKING_OFF;
case TRANS_LANDING:
return FlyingState.LANDING;
default:
return FlyingState.LANDED;
}
}
}
public static enum Mode
{
BOOTSTRAP, DEMO
}
public static enum NavDataTag
{
NAVDATA_DEMO_TAG(0), NAVDATA_TIME_TAG(1), NAVDATA_RAW_MEASURES_TAG(2), NAVDATA_PHYS_MEASURES_TAG(3), NAVDATA_GYROS_OFFSETS_TAG(
4), NAVDATA_EULER_ANGLES_TAG(5), NAVDATA_REFERENCES_TAG(6), NAVDATA_TRIMS_TAG(7), NAVDATA_RC_REFERENCES_TAG(
8), NAVDATA_PWM_TAG(9), NAVDATA_ALTITUDE_TAG(10), NAVDATA_VISION_RAW_TAG(11), NAVDATA_VISION_OF_TAG(12), NAVDATA_VISION_TAG(
13), NAVDATA_VISION_PERF_TAG(14), NAVDATA_TRACKERS_SEND_TAG(15), NAVDATA_VISION_DETECT_TAG(16), NAVDATA_WATCHDOG_TAG(
17), NAVDATA_ADC_DATA_FRAME_TAG(18), NAVDATA_VIDEO_STREAM_TAG(19), NAVDATA_CKS_TAG(0xFFFF);
private int value;
private NavDataTag(int value)
{
this.value = value;
}
public int getValue()
{
return value;
}
}
private static final Logger log = Logger.getLogger(NavData.class.getName());
public static NavData createFromData(ByteBuffer buf, int len) throws NavDataFormatException
{
log.fine("Parsing navdata len=" + len);
if (ByteOrder.LITTLE_ENDIAN != buf.order()) {
buf.order(ByteOrder.LITTLE_ENDIAN);
}
NavData data = new NavData();
data.mode = NavData.Mode.BOOTSTRAP; // Assume we are in bootstrap
//int offset = 0;
int header = buf.getInt(0);
if(header != 0x55667788)
throw new NavDataFormatException("Error parsing NavData");
// offset += 4;
int state = buf.getInt(4);
//offset += 4;
parseState(data, state);
data.sequence = buf.getInt(8);
//offset += 4;
// int vision_flag = buf.getInt(offset);
//offset += 4;
int offset = 16;
// Read options
while(offset < len)
{
int option_tag = buf.getShort(offset);
offset += 2;
int option_len = buf.getShort(offset);
offset += 2;
if(option_len == 0)
throw new NavDataFormatException("Zero-len option with tag " + option_tag);
// log.fine("At offset " + (offset - 4) + " found option " +
// option_tag + " with len=" + option_len);
if(option_tag == NavDataTag.NAVDATA_DEMO_TAG.getValue())
{
parseDemoNavData(data, buf, offset);
data.mode = NavData.Mode.DEMO;
} else if(option_tag == NavDataTag.NAVDATA_CKS_TAG.getValue())
{
// this is last tag. We do not unpack it yet, but we gracefully
// exit if it has been encountered.
break;
} else if(option_tag == NavDataTag.NAVDATA_VISION_DETECT_TAG.getValue())
{
List<VisionTag> vtags = parseVisionTags(data, buf, offset);
if(vtags != null)
data.setVisionTags(vtags);
} else
{
log.warning("Skipping unknown NavData option with tag=" + option_tag);
}
offset = offset + option_len - 4;
}
// TODO: calculate checksum
log.fine("Got Nav data. mode " + data.mode);
return data;
}
private static List<VisionTag> parseVisionTags(NavData data, ByteBuffer buf, int offset) throws NavDataFormatException
{
int nb_detected = buf.getInt(offset);
offset += 4;
if(nb_detected!=0)
log.fine("" + nb_detected + " vision tags detected");
if(nb_detected == 0)
return null;
assert (nb_detected > 0);
List<VisionTag> res = new ArrayList<VisionTag>(nb_detected);
for(int i = 0; i < nb_detected; i++)
{
int type = buf.getInt(offset + 4 * i);
int xc = buf.getInt(offset + 4 * i + 1 * 4 * 4);
int yc = buf.getInt(offset + 4 * i + 2 * 4 * 4);
int width = buf.getInt(offset + 4 * i + 3 * 4 * 4);
int height = buf.getInt(offset + 4 * i + 4 * 4 * 4);
int dist = buf.getInt(offset + 4 * i + 5 * 4 * 4);
VisionTag vt = new VisionTag(VisionTagType.fromInt(type), new Point(xc, yc), new Dimension(width, height),
dist);
log.fine("Vision#" + i + " " + vt.toString());
res.add(vt);
}
return res;
}
private static void parseDemoNavData(NavData data, ByteBuffer buf, int offset) throws NavDataFormatException
{
data.ctrl_state = CtrlState.fromInt(buf.getInt(offset) >> 16);
log.fine("Ctrl State " + data.ctrl_state);
offset += 4;
data.battery = buf.getInt(offset);
offset += 4;
data.pitch = buf.getFloat(offset) / 1000;
offset += 4;
data.roll = buf.getFloat(offset) / 1000;
offset += 4;
data.yaw = buf.getFloat(offset) / 1000;
offset += 4;
data.altitude = ((float) buf.getInt(offset)) / 1000;
offset += 4;
data.vx = buf.getFloat(offset);
offset += 4;
data.vy = buf.getFloat(offset);
offset += 4;
data.vz = buf.getFloat(offset);
offset += 4;
}
private static void parseState(NavData data, int state)
{
data.flying = (state & 1) != 0;
data.videoEnabled = (state & (1 << 1)) != 0;
data.visionEnabled = (state & (1 << 2)) != 0;
data.controlAlgorithm = (state & (1 << 3)) != 0 ? ControlAlgorithm.ANGULAR_SPEED_CONTROL
: ControlAlgorithm.EULER_ANGELS_CONTROL;
data.altitudeControlActive = (state & (1 << 4)) != 0;
data.userFeedbackOn = (state & (1 << 5)) != 0;
data.controlReceived = (state & (1 << 6)) != 0;
data.trimReceived = (state & (1 << 7)) != 0;
data.trimRunning = (state & (1 << 8)) != 0;
data.trimSucceeded = (state & (1 << 9)) != 0;
data.navDataDemoOnly = (state & (1 << 10)) != 0;
data.navDataBootstrap = (state & (1 << 11)) != 0;
data.motorsDown = (state & (1 << 12)) != 0;
//ARDRONE_COM_LOST_MASK = 1U << 13, /*!< Communication Lost : (1) com problem, (0) Com is ok */
data.gyrometersDown = (state & (1 << 14)) != 0;
data.batteryTooLow = (state & (1 << 15)) != 0;
data.batteryTooHigh = (state & (1 << 16)) != 0;
data.timerElapsed = (state & (1 << 17)) != 0;
data.notEnoughPower = (state & (1 << 18)) != 0;
data.angelsOutOufRange = (state & (1 << 19)) != 0;
data.tooMuchWind = (state & (1 << 20)) != 0;
data.ultrasonicSensorDeaf = (state & (1 << 21)) != 0;
data.cutoutSystemDetected = (state & (1 << 22)) != 0;
data.PICVersionNumberOK = (state & (1 << 23)) != 0;
data.ATCodedThreadOn = (state & (1 << 24)) != 0;
data.navDataThreadOn = (state & (1 << 25)) != 0;
data.videoThreadOn = (state & (1 << 26)) != 0;
data.acquisitionThreadOn = (state & (1 << 27)) != 0;
data.controlWatchdogDelayed = (state & (1 << 28)) != 0;
data.ADCWatchdogDelayed = (state & (1 << 29)) != 0;
data.communicationProblemOccurred = (state & (1 << 30)) != 0;
data.emergency = (state & (1 << 31)) != 0;
}
@SuppressWarnings("unused")
private static void printState(NavData data)
{
StringBuffer sb = new StringBuffer();
sb.append("IsFlying: " + data.isFlying() + "\n");
sb.append("IsVideoEnabled: " + data.isVideoEnabled() + "\n");
sb.append("IsVisionEnabled: " + data.isVisionEnabled() + "\n");
sb.append("controlAlgo: " + data.getControlAlgorithm() + "\n");
sb.append("AltitudeControlActive: " + data.isAltitudeControlActive() + "\n");
sb.append("IsUserFeedbackOn: " + data.isUserFeedbackOn() + "\n");
sb.append("ControlReceived: " + data.isVideoEnabled() + "\n");
sb.append("IsTrimReceived: " + data.isTrimReceived() + "\n");
sb.append("IsTrimRunning: " + data.isTrimRunning() + "\n");
sb.append("IsTrimSucceeded: " + data.isTrimSucceeded() + "\n");
sb.append("IsNavDataDemoOnly: " + data.isNavDataDemoOnly() + "\n");
sb.append("IsNavDataBootstrap: " + data.isNavDataBootstrap() + "\n");
sb.append("IsMotorsDown: " + data.isMotorsDown() + "\n");
sb.append("IsGyrometersDown: " + data.isGyrometersDown() + "\n");
sb.append("IsBatteryLow: " + data.isBatteryTooLow() + "\n");
sb.append("IsBatteryHigh: " + data.isBatteryTooHigh() + "\n");
sb.append("IsTimerElapsed: " + data.isTimerElapsed() + "\n");
sb.append("isNotEnoughPower: " + data.isNotEnoughPower() + "\n");
sb.append("isAngelsOutOufRange: " + data.isAngelsOutOufRange() + "\n");
sb.append("isTooMuchWind: " + data.isTooMuchWind() + "\n");
sb.append("isUltrasonicSensorDeaf: " + data.isUltrasonicSensorDeaf() + "\n");
sb.append("isCutoutSystemDetected: " + data.isCutoutSystemDetected() + "\n");
sb.append("isPICVersionNumberOK: " + data.isPICVersionNumberOK() + "\n");
sb.append("isATCodedThreadOn: " + data.isATCodedThreadOn() + "\n");
sb.append("isNavDataThreadOn: " + data.isNavDataThreadOn() + "\n");
sb.append("isVideoThreadOn: " + data.isVideoThreadOn() + "\n");
sb.append("isAcquisitionThreadOn: " + data.isAcquisitionThreadOn() + "\n");
sb.append("isControlWatchdogDelayed: " + data.isControlWatchdogDelayed() + "\n");
sb.append("isADCWatchdogDelayed: " + data.isADCWatchdogDelayed() + "\n");
sb.append("isCommunicationProblemOccurred: " + data.isCommunicationProblemOccurred() + "\n");
sb.append("IsEmergency: " + data.isEmergency() + "\n");
sb.append("CtrlState: " + data.getControlState() + "\n");
sb.append("Battery: " + data.getBattery() + "\n");
sb.append("Altidtude: " + data.getAltitude() + "\n");
sb.append("Pitch: " + data.getPitch() + "\n");
sb.append("Roll: " + data.getRoll() + "\n");
sb.append("Yaw: " + data.getYaw() + "\n");
sb.append("X velocity: " + data.getVx() + "\n");
sb.append("Y velocity: " + data.getLongitude() + "\n");
sb.append("Z velocity: " + data.getVz() + "\n");
sb.append("Vision Tags: " + data.getVisionTags() + "\n");
log.fine("State: " + sb.toString());
}
protected Mode mode;
// state flags
protected boolean flying;
protected boolean videoEnabled;
protected boolean visionEnabled;
protected ControlAlgorithm controlAlgorithm;
protected boolean altitudeControlActive;
protected boolean userFeedbackOn; // /TODO better
// name
protected boolean controlReceived;
protected boolean trimReceived;
protected boolean trimRunning;
protected boolean trimSucceeded;
protected boolean navDataDemoOnly;
protected boolean navDataBootstrap;
protected boolean motorsDown;
protected boolean gyrometersDown;
protected boolean batteryTooLow;
protected boolean batteryTooHigh;
protected boolean timerElapsed;
protected boolean notEnoughPower;
protected boolean angelsOutOufRange;
protected boolean tooMuchWind;
protected boolean ultrasonicSensorDeaf;
protected boolean cutoutSystemDetected;
protected boolean PICVersionNumberOK;
protected boolean ATCodedThreadOn;
protected boolean navDataThreadOn;
protected boolean videoThreadOn;
protected boolean acquisitionThreadOn;
protected boolean controlWatchdogDelayed;
protected boolean ADCWatchdogDelayed;
protected boolean communicationProblemOccurred;
protected boolean emergency;
// Common nav data
protected int sequence;
// Demo nav data
protected CtrlState ctrl_state;
protected int battery;
protected float altitude;
protected float pitch;
protected float roll;
protected float yaw;
protected float vx;
protected float vy;
protected float vz;
// Vision tags data
protected List<VisionTag> vision_tags;
public interface NavData {
/**
*
* @return value in meters
*/
public float getAltitude()
{
return altitude;
}
public abstract float getAltitude();
public int getBattery()
{
return battery;
}
public abstract int getBattery();
public ControlAlgorithm getControlAlgorithm()
{
return controlAlgorithm;
}
public abstract ControlAlgorithm getControlAlgorithm();
public CtrlState getControlState()
{
return ctrl_state;
}
public abstract CtrlState getControlState();
public float getLongitude()
{
return vy;
}
public abstract float getLongitude();
public Mode getMode()
{
return mode;
}
public abstract Mode getMode();
/**
*
* @return value in degrees
*/
public float getPitch()
{
return pitch;
}
public abstract float getPitch();
/**
*
* @return value in degrees
*/
public float getRoll()
{
return roll;
}
public abstract float getRoll();
public int getSequence()
{
return sequence;
}
public abstract int getSequence();
public float getVx()
{
return vx;
}
public abstract float getVx();
public float getVz()
{
return vz;
}
public abstract float getVz();
/**
*
* @return value in degrees
*/
public float getYaw()
{
return yaw;
}
public abstract float getYaw();
public boolean isAcquisitionThreadOn()
{
return acquisitionThreadOn;
}
public abstract boolean isAcquisitionThreadOn();
public boolean isADCWatchdogDelayed()
{
return ADCWatchdogDelayed;
}
public abstract boolean isADCWatchdogDelayed();
public boolean isAltitudeControlActive()
{
return altitudeControlActive;
}
public abstract boolean isAltitudeControlActive();
public boolean isAngelsOutOufRange()
{
return angelsOutOufRange;
}
public abstract boolean isAngelsOutOufRange();
public boolean isATCodedThreadOn()
{
return ATCodedThreadOn;
}
public abstract boolean isATCodedThreadOn();
public boolean isBatteryTooHigh()
{
return batteryTooHigh;
}
public abstract boolean isBatteryTooHigh();
public boolean isBatteryTooLow()
{
return batteryTooLow;
}
public abstract boolean isBatteryTooLow();
public boolean isCommunicationProblemOccurred()
{
return communicationProblemOccurred;
}
public abstract boolean isCommunicationProblemOccurred();
public boolean isControlReceived()
{
return controlReceived;
}
public abstract boolean isControlReceived();
public boolean isControlWatchdogDelayed()
{
return controlWatchdogDelayed;
}
public abstract boolean isControlWatchdogDelayed();
public boolean isCutoutSystemDetected()
{
return cutoutSystemDetected;
}
public abstract boolean isCutoutSystemDetected();
public boolean isEmergency()
{
return emergency;
}
public abstract boolean isEmergency();
public boolean isFlying()
{
return flying;
}
public abstract boolean isFlying();
public boolean isGyrometersDown()
{
return gyrometersDown;
}
public abstract boolean isGyrometersDown();
public boolean isMotorsDown()
{
return motorsDown;
}
public abstract boolean isMotorsDown();
public boolean isNavDataBootstrap()
{
return navDataBootstrap;
}
public abstract boolean isNavDataBootstrap();
public boolean isNavDataDemoOnly()
{
return navDataDemoOnly;
}
public abstract boolean isNavDataDemoOnly();
public boolean isNavDataThreadOn()
{
return navDataThreadOn;
}
public abstract boolean isNavDataThreadOn();
public boolean isNotEnoughPower()
{
return notEnoughPower;
}
public abstract boolean isNotEnoughPower();
public boolean isPICVersionNumberOK()
{
return PICVersionNumberOK;
}
public abstract boolean isPICVersionNumberOK();
public boolean isTimerElapsed()
{
return timerElapsed;
}
public abstract boolean isTimerElapsed();
public boolean isTooMuchWind()
{
return tooMuchWind;
}
public abstract boolean isTooMuchWind();
public boolean isTrimReceived()
{
return trimReceived;
}
public abstract boolean isTrimReceived();
public boolean isTrimRunning()
{
return trimRunning;
}
public abstract boolean isTrimRunning();
public boolean isTrimSucceeded()
{
return trimSucceeded;
}
public abstract boolean isTrimSucceeded();
public boolean isUltrasonicSensorDeaf()
{
return ultrasonicSensorDeaf;
}
public abstract boolean isUltrasonicSensorDeaf();
public boolean isUserFeedbackOn()
{
return userFeedbackOn;
}
public abstract boolean isUserFeedbackOn();
public boolean isVideoEnabled()
{
return videoEnabled;
}
public abstract boolean isVideoEnabled();
public boolean isVideoThreadOn()
{
return videoThreadOn;
}
public abstract boolean isVideoThreadOn();
public boolean isVisionEnabled()
{
return visionEnabled;
}
public abstract boolean isVisionEnabled();
public FlyingState getFlyingState()
{
return FlyingState.fromControlState(ctrl_state);
}
public abstract FlyingState getFlyingState();
public List<VisionTag> getVisionTags()
{
return vision_tags;
}
public abstract List<VisionTag> getVisionTags();
public void setVisionTags(List<VisionTag> vision_tags)
{
this.vision_tags = vision_tags;
}
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/NavDataDecoder.java
+17
Ver Arquivo
@@ -0,0 +1,17 @@
package com.codeminders.ardrone;
import com.codeminders.ardrone.data.DataDecoder;
public abstract class NavDataDecoder extends DataDecoder {
private ARDrone drone;
public NavDataDecoder(ARDrone drone) {
this.drone = drone;
}
public void notifyDroneWithDecodedNavdata(NavData navdata) {
drone.navDataReceived(navdata);
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/VideoDataDecoder.java
+16
Ver Arquivo
@@ -0,0 +1,16 @@
package com.codeminders.ardrone;
import com.codeminders.ardrone.data.DataDecoder;
public abstract class VideoDataDecoder extends DataDecoder {
private ARDrone drone;
public VideoDataDecoder(ARDrone drone) {
this.drone = drone;
}
public void notifyDroneWithDecodedFrame(int startX, int startY, int width, int height, int[] rgbArray, int offset, int scansize) {
drone.videoFrameReceived(startX, startY, width, height, rgbArray, offset, scansize);
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/ARDroneDataReader.java
+21
Ver Arquivo
@@ -0,0 +1,21 @@
package com.codeminders.ardrone.data;
import java.io.IOException;
import java.io.InputStream;
public interface ARDroneDataReader {
/**
* @param buf input data buffer to read;
* @return length of data that is obtained from ardrone
* @throws IOException
*/
public int readDataBlock(byte[] buf) throws IOException;
public InputStream getDataStream();
public boolean isStreamSupported();
public void reconnect() throws IOException;
public void finish();
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/ChannelProcessor.java
+41
Ver Arquivo
@@ -0,0 +1,41 @@
package com.codeminders.ardrone.data;
import java.util.logging.Level;
import java.util.logging.Logger;
public class ChannelProcessor {
private Logger log = Logger.getLogger(getClass().getName());
ARDroneDataReader reader;
DataDecoder decoder;
private static int STOP_TIMEOUT = 3000; // 3 sec.
public ChannelProcessor(ARDroneDataReader reader, DataDecoder decoder) {
super();
this.reader = reader;
this.decoder = decoder;
decoder.setDataReader(reader); // decoder and reader is now linked
decoder.start();
}
public void finish() {
decoder.finish();
try {
decoder.join(STOP_TIMEOUT);
} catch (InterruptedException e) {
log.log(Level.FINEST, "Waiting till decoder is stopped is interrupted", e);
}
reader.finish();
}
public void pause() {
decoder.pauseDecoding();
}
public void resume() {
decoder.pauseDecoding();
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/DataDecoder.java
+59
Ver Arquivo
@@ -0,0 +1,59 @@
package com.codeminders.ardrone.data;
import java.util.logging.Level;
import java.util.logging.Logger;
public abstract class DataDecoder extends Thread {
private Logger log = Logger.getLogger(this.getClass().getName());
private ARDroneDataReader datareader;
private boolean pauseFlag;
@Override
public synchronized void start() {
super.start();
if (null == datareader) {
throw new RuntimeException("No reading thread is arrached");
}
}
protected void pauseCheck(){
if (pauseFlag) {
synchronized(this) {
if (pauseFlag) {
try {
wait();
} catch (InterruptedException e) {
log.log(Level.SEVERE, "Pause is interrupted", e);
}
}
}
}
}
protected void setDataReader(ARDroneDataReader datareader) {
if (this.isAlive()) {
throw new RuntimeException("Rading Thrad already started. You can't change stream on fly");
}
this.datareader = datareader;
}
public ARDroneDataReader getDataReader() {
return datareader;
}
public synchronized void pauseDecoding() {
pauseFlag = true;
}
public synchronized void resumeDecoding() {
pauseFlag = false;
notify();
}
public abstract void finish();
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/decoder/DataDecoder.java
-10
Ver Arquivo
@@ -1,10 +0,0 @@
package com.codeminders.ardrone.data.decoder;
import java.nio.ByteBuffer;
public interface DataDecoder {
void decodeData(ByteBuffer inbuf, int len);
void finish();
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/decoder/NavDataDecoder.java
-29
Ver Arquivo
@@ -1,29 +0,0 @@
package com.codeminders.ardrone.data.decoder;
import java.nio.ByteBuffer;
import java.util.logging.Level;
import java.util.logging.Logger;
import com.codeminders.ardrone.ARDrone;
import com.codeminders.ardrone.NavData;
import com.codeminders.ardrone.NavDataFormatException;
public class NavDataDecoder extends OnlyActualDataDecoder {
private Logger log = Logger.getLogger(this.getClass().getName());
public NavDataDecoder(ARDrone drone, int buffer_size) {
super(drone, buffer_size);
setName("NavData decodding thread");
}
@Override
public void decodeActualData(ByteBuffer inbuf, int len) {
try {
drone.navDataReceived(NavData.createFromData(inbuf, len));
} catch (NavDataFormatException ex) {
log.log(Level.FINE ,"Failed to decode received navdata. Reason unsupported format", ex);
}
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/decoder/OnlyActualDataDecoder.java
-97
Ver Arquivo
@@ -1,97 +0,0 @@
package com.codeminders.ardrone.data.decoder;
import java.nio.ByteBuffer;
import com.codeminders.ardrone.ARDrone;
/**
* Data processing is taking some time. We constantly receive income data.
* If system is under decoding of current data packet and we receive two more
* current realization of data processing mechanism will process only last data packet that was received.
*
* This class makes an assumption - that we receive a complete amount of data in one chunk.
*/
public abstract class OnlyActualDataDecoder extends Thread implements DataDecoder {
ARDrone drone;
ByteBuffer worckInbuf;
int worckInDataBufferLength;
ByteBuffer nextBuffer;
int nextDataBufferLength;
boolean nextEmpty = true;
boolean done = false;
Object lock = new Object();
public OnlyActualDataDecoder(ARDrone drone, int buffer_size) {
super();
this.drone = drone;
worckInbuf = ByteBuffer.allocate(buffer_size);
nextBuffer = ByteBuffer.allocate(buffer_size);
}
@Override
public void run() {
while(!done) {
if (nextEmpty) {
synchronized (lock) {
try {
if (nextEmpty) {
lock.wait();
}
} catch (InterruptedException e) {
done = true;
}
}
}
if (done) {
break;
}
synchronized (nextBuffer) {
if (!nextEmpty) {
worckInbuf.clear();
worckInbuf.put(nextBuffer);
worckInbuf.flip();
worckInDataBufferLength = nextDataBufferLength;
nextEmpty = true;
}
}
decodeActualData(worckInbuf, worckInDataBufferLength);
}
}
abstract void decodeActualData(ByteBuffer infBuffer, int len);
public void decodeData(ByteBuffer infBuffer, int len) {
synchronized (nextBuffer) {
nextBuffer.clear();
nextBuffer.put(infBuffer);
nextBuffer.flip();
nextDataBufferLength = len;
nextEmpty = false;
}
synchronized (lock) {
lock.notify();
}
}
public void finish()
{
done = true;
synchronized (lock) {
lock.notify();
}
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/decoder/VideoDataDecoder.java
-21
Ver Arquivo
@@ -1,21 +0,0 @@
package com.codeminders.ardrone.data.decoder;
import java.nio.ByteBuffer;
import com.codeminders.ardrone.ARDrone;
import com.codeminders.ardrone.video.BufferedVideoImage;
public class VideoDataDecoder extends OnlyActualDataDecoder {
final BufferedVideoImage vi = new BufferedVideoImage();
public VideoDataDecoder(ARDrone drone, int buffer_size) {
super(drone, buffer_size);
setName("Video decoding thread");
}
public void decodeActualData(ByteBuffer infbuf, int len) {
vi.addImageStream(infbuf);
drone.videoFrameReceived(0, 0, vi.getWidth(), vi.getHeight(), vi.getJavaPixelData(), 0, vi.getWidth());
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/decoder/ardrone10/ARDrone10NavDataDecoder.java
+65
Ver Arquivo
@@ -0,0 +1,65 @@
package com.codeminders.ardrone.data.decoder.ardrone10;
import java.io.IOException;
import java.nio.ByteBuffer;
import java.util.logging.Level;
import java.util.logging.Logger;
import com.codeminders.ardrone.ARDrone;
import com.codeminders.ardrone.NavDataDecoder;
import com.codeminders.ardrone.data.ARDroneDataReader;
import com.codeminders.ardrone.data.decoder.ardrone10.navdata.ARDrone10NavData;
import com.codeminders.ardrone.data.navdata.NavDataFormatException;
public class ARDrone10NavDataDecoder extends NavDataDecoder {
private Logger log = Logger.getLogger(this.getClass().getName());
private boolean done = false;
byte[] buffer;
public ARDrone10NavDataDecoder(ARDrone drone, int buffer_size) {
super(drone);
setName("ARDrone 1.0 NavData decodding thread");
buffer = new byte[buffer_size];
}
@Override
public void run() {
super.run();
ARDroneDataReader reader = getDataReader();
while (!done) {
try {
pauseCheck();
int len = reader.readDataBlock(buffer);
if (len > 0) {
try {
notifyDroneWithDecodedNavdata(ARDrone10NavData.createFromData(ByteBuffer.wrap(buffer), len));
} catch (NavDataFormatException e) {
log.log(Level.SEVERE, "Failed to decode receivd navdata information", e);
} catch (Exception ex) {
log.log(Level.SEVERE, "Failed to decode receivd navdata information", ex);
}
}
} catch (IOException e) {
log.log(Level.SEVERE, " Error reading data from data input stream. Stopping decoding thread", e);
try {
reader.reconnect();
} catch (IOException e1) {
log.log(Level.SEVERE, " Error reconnecting data reader", e);
}
}
}
log.fine("Decodding thread is stopped");
}
@Override
public void finish() {
done = true;
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/decoder/ardrone10/ARDrone10VideoDataDecoder.java
+60
Ver Arquivo
@@ -0,0 +1,60 @@
package com.codeminders.ardrone.data.decoder.ardrone10;
import java.io.IOException;
import java.util.logging.Level;
import java.util.logging.Logger;
import com.codeminders.ardrone.ARDrone;
import com.codeminders.ardrone.VideoDataDecoder;
import com.codeminders.ardrone.data.ARDroneDataReader;
import com.codeminders.ardrone.data.decoder.ardrone10.video.BufferedVideoImage;
public class ARDrone10VideoDataDecoder extends VideoDataDecoder {
private Logger log = Logger.getLogger(this.getClass().getName());
final BufferedVideoImage vi = new BufferedVideoImage();
private boolean done = false;
byte[] buffer;
public ARDrone10VideoDataDecoder(ARDrone drone, int buffer_size) {
super(drone);
buffer = new byte[buffer_size];
setName("ARDrone 1.0 Video decoding thread");
}
@Override
public void run() {
super.run();
ARDroneDataReader reader = getDataReader();
int len = 0;
while (!done) {
try {
pauseCheck();
len = reader.readDataBlock(buffer);
if (len > 0) {
vi.addImageStream(buffer, len);
notifyDroneWithDecodedFrame(0, 0, vi.getWidth(), vi.getHeight(), vi.getJavaPixelData(), 0, vi.getWidth());
}
} catch (IOException e) {
log.log(Level.SEVERE, " Error reading data from data input stream. Stopping decoding thread", e);
try {
reader.reconnect();
} catch (IOException e1) {
log.log(Level.SEVERE, " Error reconnecting data reader", e);
}
}
}
log.fine("Video Decodding thread is stopped");
}
@Override
public void finish() {
done = true;
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/decoder/ardrone10/navdata/ARDrone10NavData.java
+563
Ver Arquivo
@@ -0,0 +1,563 @@
package com.codeminders.ardrone.data.decoder.ardrone10.navdata;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.ArrayList;
import java.util.List;
import java.util.logging.Logger;
import com.codeminders.ardrone.NavData;
import com.codeminders.ardrone.data.navdata.ControlAlgorithm;
import com.codeminders.ardrone.data.navdata.CtrlState;
import com.codeminders.ardrone.data.navdata.FlyingState;
import com.codeminders.ardrone.data.navdata.Mode;
import com.codeminders.ardrone.data.navdata.NavDataFormatException;
import com.codeminders.ardrone.data.navdata.NavDataTag;
import com.codeminders.ardrone.data.navdata.vision.Dimension;
import com.codeminders.ardrone.data.navdata.vision.Point;
import com.codeminders.ardrone.data.navdata.vision.VisionTag;
import com.codeminders.ardrone.data.navdata.vision.VisionTag.VisionTagType;
public class ARDrone10NavData implements NavData
{
private static final Logger log = Logger.getLogger(ARDrone10NavData.class.getName());
protected Mode mode;
// state flags
protected boolean flying;
protected boolean videoEnabled;
protected boolean visionEnabled;
protected ControlAlgorithm controlAlgorithm;
protected boolean altitudeControlActive;
protected boolean userFeedbackOn; // /TODO better
// name
protected boolean controlReceived;
protected boolean trimReceived;
protected boolean trimRunning;
protected boolean trimSucceeded;
protected boolean navDataDemoOnly;
protected boolean navDataBootstrap;
protected boolean motorsDown;
protected boolean gyrometersDown;
protected boolean batteryTooLow;
protected boolean batteryTooHigh;
protected boolean timerElapsed;
protected boolean notEnoughPower;
protected boolean angelsOutOufRange;
protected boolean tooMuchWind;
protected boolean ultrasonicSensorDeaf;
protected boolean cutoutSystemDetected;
protected boolean PICVersionNumberOK;
protected boolean ATCodedThreadOn;
protected boolean navDataThreadOn;
protected boolean videoThreadOn;
protected boolean acquisitionThreadOn;
protected boolean controlWatchdogDelayed;
protected boolean ADCWatchdogDelayed;
protected boolean communicationProblemOccurred;
protected boolean emergency;
// Common nav data
protected int sequence;
// Demo nav data
protected CtrlState ctrl_state;
protected int battery;
protected float altitude;
protected float pitch;
protected float roll;
protected float yaw;
protected float vx;
protected float vy;
protected float vz;
// Vision tags data
protected List<VisionTag> vision_tags;
@Override
public float getAltitude()
{
return altitude;
}
@Override
public int getBattery()
{
return battery;
}
@Override
public ControlAlgorithm getControlAlgorithm()
{
return controlAlgorithm;
}
@Override
public CtrlState getControlState()
{
return ctrl_state;
}
@Override
public float getLongitude()
{
return vy;
}
@Override
public Mode getMode()
{
return mode;
}
@Override
public float getPitch()
{
return pitch;
}
@Override
public float getRoll()
{
return roll;
}
@Override
public int getSequence()
{
return sequence;
}
@Override
public float getVx()
{
return vx;
}
@Override
public float getVz()
{
return vz;
}
@Override
public float getYaw()
{
return yaw;
}
@Override
public boolean isAcquisitionThreadOn()
{
return acquisitionThreadOn;
}
@Override
public boolean isADCWatchdogDelayed()
{
return ADCWatchdogDelayed;
}
@Override
public boolean isAltitudeControlActive()
{
return altitudeControlActive;
}
@Override
public boolean isAngelsOutOufRange()
{
return angelsOutOufRange;
}
@Override
public boolean isATCodedThreadOn()
{
return ATCodedThreadOn;
}
@Override
public boolean isBatteryTooHigh()
{
return batteryTooHigh;
}
@Override
public boolean isBatteryTooLow()
{
return batteryTooLow;
}
@Override
public boolean isCommunicationProblemOccurred()
{
return communicationProblemOccurred;
}
@Override
public boolean isControlReceived()
{
return controlReceived;
}
@Override
public boolean isControlWatchdogDelayed()
{
return controlWatchdogDelayed;
}
@Override
public boolean isCutoutSystemDetected()
{
return cutoutSystemDetected;
}
@Override
public boolean isEmergency()
{
return emergency;
}
@Override
public boolean isFlying()
{
return flying;
}
@Override
public boolean isGyrometersDown()
{
return gyrometersDown;
}
@Override
public boolean isMotorsDown()
{
return motorsDown;
}
@Override
public boolean isNavDataBootstrap()
{
return navDataBootstrap;
}
@Override
public boolean isNavDataDemoOnly()
{
return navDataDemoOnly;
}
@Override
public boolean isNavDataThreadOn()
{
return navDataThreadOn;
}
@Override
public boolean isNotEnoughPower()
{
return notEnoughPower;
}
@Override
public boolean isPICVersionNumberOK()
{
return PICVersionNumberOK;
}
@Override
public boolean isTimerElapsed()
{
return timerElapsed;
}
@Override
public boolean isTooMuchWind()
{
return tooMuchWind;
}
@Override
public boolean isTrimReceived()
{
return trimReceived;
}
@Override
public boolean isTrimRunning()
{
return trimRunning;
}
@Override
public boolean isTrimSucceeded()
{
return trimSucceeded;
}
@Override
public boolean isUltrasonicSensorDeaf()
{
return ultrasonicSensorDeaf;
}
@Override
public boolean isUserFeedbackOn()
{
return userFeedbackOn;
}
@Override
public boolean isVideoEnabled()
{
return videoEnabled;
}
@Override
public boolean isVideoThreadOn()
{
return videoThreadOn;
}
@Override
public boolean isVisionEnabled()
{
return visionEnabled;
}
@Override
public FlyingState getFlyingState()
{
return FlyingState.fromControlState(ctrl_state);
}
@Override
public List<VisionTag> getVisionTags()
{
return vision_tags;
}
public void setVisionTags(List<VisionTag> vision_tags)
{
this.vision_tags = vision_tags;
}
public static NavData createFromData(ByteBuffer buf, int len) throws NavDataFormatException
{
log.fine("Parsing navdata len=" + len);
if (ByteOrder.LITTLE_ENDIAN != buf.order()) {
buf.order(ByteOrder.LITTLE_ENDIAN);
}
ARDrone10NavData data = new ARDrone10NavData();
data.mode = Mode.BOOTSTRAP; // Assume we are in bootstrap
//int offset = 0;
int header = buf.getInt(0);
if(header != 0x55667788)
throw new NavDataFormatException("Error parsing NavData");
// offset += 4;
int state = buf.getInt(4);
//offset += 4;
parseState(data, state);
data.sequence = buf.getInt(8);
//offset += 4;
// int vision_flag = buf.getInt(offset);
//offset += 4;
int offset = 16;
// Read options
while(offset < len)
{
int option_tag = buf.getShort(offset);
offset += 2;
int option_len = buf.getShort(offset);
offset += 2;
if(option_len == 0)
throw new NavDataFormatException("Zero-len option with tag " + option_tag);
// log.fine("At offset " + (offset - 4) + " found option " +
// option_tag + " with len=" + option_len);
if(option_tag == NavDataTag.NAVDATA_DEMO_TAG.getValue())
{
parseDemoNavData(data, buf, offset);
data.mode = Mode.DEMO;
} else if(option_tag == NavDataTag.NAVDATA_CKS_TAG.getValue())
{
// this is last tag. We do not unpack it yet, but we gracefully
// exit if it has been encountered.
break;
} else if(option_tag == NavDataTag.NAVDATA_VISION_DETECT_TAG.getValue())
{
List<VisionTag> vtags = parseVisionTags(data, buf, offset);
if(vtags != null)
data.setVisionTags(vtags);
} else
{
//log.warning("Skipping unknown NavData option with tag=" + option_tag);
}
offset = offset + option_len - 4;
}
// TODO: calculate checksum
log.fine("Got Nav data. mode " + data.mode);
return data;
}
private static List<VisionTag> parseVisionTags(NavData data, ByteBuffer buf, int offset) throws NavDataFormatException
{
int nb_detected = buf.getInt(offset);
offset += 4;
if(nb_detected!=0)
log.fine("" + nb_detected + " vision tags detected");
if(nb_detected == 0)
return null;
assert (nb_detected > 0);
List<VisionTag> res = new ArrayList<VisionTag>(nb_detected);
for(int i = 0; i < nb_detected; i++)
{
int type = buf.getInt(offset + 4 * i);
int xc = buf.getInt(offset + 4 * i + 1 * 4 * 4);
int yc = buf.getInt(offset + 4 * i + 2 * 4 * 4);
int width = buf.getInt(offset + 4 * i + 3 * 4 * 4);
int height = buf.getInt(offset + 4 * i + 4 * 4 * 4);
int dist = buf.getInt(offset + 4 * i + 5 * 4 * 4);
VisionTag vt = new VisionTag(VisionTagType.fromInt(type), new Point(xc, yc), new Dimension(width, height),
dist);
log.fine("Vision#" + i + " " + vt.toString());
res.add(vt);
}
return res;
}
private static void parseDemoNavData(ARDrone10NavData data, ByteBuffer buf, int offset) throws NavDataFormatException
{
data.ctrl_state = CtrlState.fromInt(buf.getInt(offset) >> 16);
log.fine("Ctrl State " + data.ctrl_state);
offset += 4;
data.battery = buf.getInt(offset);
offset += 4;
data.pitch = buf.getFloat(offset) / 1000;
offset += 4;
data.roll = buf.getFloat(offset) / 1000;
offset += 4;
data.yaw = buf.getFloat(offset) / 1000;
offset += 4;
data.altitude = ((float) buf.getInt(offset)) / 1000;
offset += 4;
data.vx = buf.getFloat(offset);
offset += 4;
data.vy = buf.getFloat(offset);
offset += 4;
data.vz = buf.getFloat(offset);
offset += 4;
}
private static void parseState(ARDrone10NavData data, int state)
{
data.flying = (state & 1) != 0;
data.videoEnabled = (state & (1 << 1)) != 0;
data.visionEnabled = (state & (1 << 2)) != 0;
data.controlAlgorithm = (state & (1 << 3)) != 0 ? ControlAlgorithm.ANGULAR_SPEED_CONTROL
: ControlAlgorithm.EULER_ANGELS_CONTROL;
data.altitudeControlActive = (state & (1 << 4)) != 0;
data.userFeedbackOn = (state & (1 << 5)) != 0;
data.controlReceived = (state & (1 << 6)) != 0;
data.trimReceived = (state & (1 << 7)) != 0;
data.trimRunning = (state & (1 << 8)) != 0;
data.trimSucceeded = (state & (1 << 9)) != 0;
data.navDataDemoOnly = (state & (1 << 10)) != 0;
data.navDataBootstrap = (state & (1 << 11)) != 0;
data.motorsDown = (state & (1 << 12)) != 0;
//ARDRONE_COM_LOST_MASK = 1U << 13, /*!< Communication Lost : (1) com problem, (0) Com is ok */
data.gyrometersDown = (state & (1 << 14)) != 0;
data.batteryTooLow = (state & (1 << 15)) != 0;
data.batteryTooHigh = (state & (1 << 16)) != 0;
data.timerElapsed = (state & (1 << 17)) != 0;
data.notEnoughPower = (state & (1 << 18)) != 0;
data.angelsOutOufRange = (state & (1 << 19)) != 0;
data.tooMuchWind = (state & (1 << 20)) != 0;
data.ultrasonicSensorDeaf = (state & (1 << 21)) != 0;
data.cutoutSystemDetected = (state & (1 << 22)) != 0;
data.PICVersionNumberOK = (state & (1 << 23)) != 0;
data.ATCodedThreadOn = (state & (1 << 24)) != 0;
data.navDataThreadOn = (state & (1 << 25)) != 0;
data.videoThreadOn = (state & (1 << 26)) != 0;
data.acquisitionThreadOn = (state & (1 << 27)) != 0;
data.controlWatchdogDelayed = (state & (1 << 28)) != 0;
data.ADCWatchdogDelayed = (state & (1 << 29)) != 0;
data.communicationProblemOccurred = (state & (1 << 30)) != 0;
data.emergency = (state & (1 << 31)) != 0;
}
public void printState()
{
StringBuffer sb = new StringBuffer();
sb.append("IsFlying: " + this.isFlying() + "\n");
sb.append("IsVideoEnabled: " + this.isVideoEnabled() + "\n");
sb.append("IsVisionEnabled: " + this.isVisionEnabled() + "\n");
sb.append("controlAlgo: " + this.getControlAlgorithm() + "\n");
sb.append("AltitudeControlActive: " + this.isAltitudeControlActive() + "\n");
sb.append("IsUserFeedbackOn: " + this.isUserFeedbackOn() + "\n");
sb.append("ControlReceived: " + this.isVideoEnabled() + "\n");
sb.append("IsTrimReceived: " + this.isTrimReceived() + "\n");
sb.append("IsTrimRunning: " + this.isTrimRunning() + "\n");
sb.append("IsTrimSucceeded: " + this.isTrimSucceeded() + "\n");
sb.append("IsNavthisDemoOnly: " + this.isNavDataDemoOnly() + "\n");
sb.append("IsNavthisBootstrap: " + this.isNavDataBootstrap() + "\n");
sb.append("IsMotorsDown: " + this.isMotorsDown() + "\n");
sb.append("IsGyrometersDown: " + this.isGyrometersDown() + "\n");
sb.append("IsBatteryLow: " + this.isBatteryTooLow() + "\n");
sb.append("IsBatteryHigh: " + this.isBatteryTooHigh() + "\n");
sb.append("IsTimerElapsed: " + this.isTimerElapsed() + "\n");
sb.append("isNotEnoughPower: " + this.isNotEnoughPower() + "\n");
sb.append("isAngelsOutOufRange: " + this.isAngelsOutOufRange() + "\n");
sb.append("isTooMuchWind: " + this.isTooMuchWind() + "\n");
sb.append("isUltrasonicSensorDeaf: " + this.isUltrasonicSensorDeaf() + "\n");
sb.append("isCutoutSystemDetected: " + this.isCutoutSystemDetected() + "\n");
sb.append("isPICVersionNumberOK: " + this.isPICVersionNumberOK() + "\n");
sb.append("isATCodedThreadOn: " + this.isATCodedThreadOn() + "\n");
sb.append("isNavthisThreadOn: " + this.isNavDataThreadOn() + "\n");
sb.append("isVideoThreadOn: " + this.isVideoThreadOn() + "\n");
sb.append("isAcquisitionThreadOn: " + this.isAcquisitionThreadOn() + "\n");
sb.append("isControlWatchdogDelayed: " + this.isControlWatchdogDelayed() + "\n");
sb.append("isADCWatchdogDelayed: " + this.isADCWatchdogDelayed() + "\n");
sb.append("isCommunicationProblemOccurred: " + this.isCommunicationProblemOccurred() + "\n");
sb.append("IsEmergency: " + this.isEmergency() + "\n");
sb.append("CtrlState: " + this.getControlState() + "\n");
sb.append("Battery: " + this.getBattery() + "\n");
sb.append("Altidtude: " + this.getAltitude() + "\n");
sb.append("Pitch: " + this.getPitch() + "\n");
sb.append("Roll: " + this.getRoll() + "\n");
sb.append("Yaw: " + this.getYaw() + "\n");
sb.append("X velocity: " + this.getVx() + "\n");
sb.append("Y velocity: " + this.getLongitude() + "\n");
sb.append("Z velocity: " + this.getVz() + "\n");
sb.append("Vision Tags: " + this.getVisionTags() + "\n");
log.fine("State: " + sb.toString());
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/video/BufferedVideoImage.java → javadrone-api/src/main/java/com/codeminders/ardrone/data/decoder/ardrone10/video/BufferedVideoImage.java
+4 -7
Ver Arquivo
@@ -1,6 +1,5 @@
package com.codeminders.ardrone.video;
package com.codeminders.ardrone.data.decoder.ardrone10.video;
import java.nio.ByteBuffer;
// Copyright (C) 2007-2011, PARROT SA, all rights reserved.
@@ -105,7 +104,6 @@ public class BufferedVideoImage
* Length of one row of pixels in the destination image in bytes.
*/
private int pixelRowSize;
private ByteBuffer imageStream;
private byte[] imageStreamByteArray;
private int imageStreamCapacity;
private ImageSlice imageSlice;
@@ -127,11 +125,10 @@ public class BufferedVideoImage
* @param ByteBuffer stream
* A ByteBuffer full of the bytes that represent the image to be decoded.
*/
public void addImageStream(ByteBuffer stream)
public void addImageStream(byte[] imageStreamByteArray, int actualDatalength)
{
imageStream = stream;
imageStreamByteArray = imageStream.array();
imageStreamCapacity = imageStream.capacity();
this.imageStreamByteArray = imageStreamByteArray;
imageStreamCapacity = actualDatalength;
processStream();
}
javadrone-api/src/main/java/com/codeminders/ardrone/video/ImageSlice.java → javadrone-api/src/main/java/com/codeminders/ardrone/data/decoder/ardrone10/video/ImageSlice.java
+1 -1
Ver Arquivo
@@ -1,5 +1,5 @@
package com.codeminders.ardrone.video;
package com.codeminders.ardrone.data.decoder.ardrone10.video;
// Copyright � 2007-2011, PARROT SA, all rights reserved.
javadrone-api/src/main/java/com/codeminders/ardrone/video/MacroBlock.java → javadrone-api/src/main/java/com/codeminders/ardrone/data/decoder/ardrone10/video/MacroBlock.java
+1 -1
Ver Arquivo
@@ -1,5 +1,5 @@
package com.codeminders.ardrone.video;
package com.codeminders.ardrone.data.decoder.ardrone10.video;
// Copyright � 2007-2011, PARROT SA, all rights reserved.
javadrone-api/src/main/java/com/codeminders/ardrone/data/logger/ARDroneDataReaderAndLogWrapper.java
+49
Ver Arquivo
@@ -0,0 +1,49 @@
package com.codeminders.ardrone.data.logger;
import java.io.IOException;
import java.io.InputStream;
import com.codeminders.ardrone.data.ARDroneDataReader;
public class ARDroneDataReaderAndLogWrapper implements ARDroneDataReader {
ARDroneDataReader reader;
DataLogger logger;
public ARDroneDataReaderAndLogWrapper(ARDroneDataReader reader, DataLogger logger) {
super();
this.reader = reader;
this.logger = logger;
}
@Override
public int readDataBlock(byte[] buf) throws IOException {
int len = reader.readDataBlock(buf);
if (len > 0) {
byte[] data = new byte[len];
System.arraycopy(buf,0, data, 0, len);
logger.log(new ChannelDataChunk(data, System.currentTimeMillis()));
}
return len;
}
@Override
public InputStream getDataStream() {
return new LogStreamWrapper(reader.getDataStream(), logger);
}
@Override
public boolean isStreamSupported() {
return reader.isStreamSupported();
}
@Override
public void reconnect() throws IOException {
reader.reconnect();
}
@Override
public void finish() {
reader.finish();
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/logger/ChannelDataChunk.java
+5 -5
Ver Arquivo
@@ -7,23 +7,23 @@ import java.io.IOException;
public class ChannelDataChunk {
byte[] data;
long ioDelay;
long timemark;
public ChannelDataChunk(byte[] data, long ioDelay) {
public ChannelDataChunk(byte[] data, long timemark) {
super();
this.data = data;
this.ioDelay = ioDelay;
this.timemark = timemark;
}
public byte[] getData() {
return data;
}
public long getIoDelay() {
return ioDelay;
return timemark;
}
public void writeToStream(DataOutputStream out) throws IOException {
out.writeLong(ioDelay);
out.writeLong(timemark);
out.writeInt(data.length);
out.write(data, 0, data.length);
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/logger/ChannelDataLogger.java
-6
Ver Arquivo
@@ -1,6 +0,0 @@
package com.codeminders.ardrone.data.logger;
public interface ChannelDataLogger {
void log(ChannelDataChunk data);
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/logger/DataLogger.java
+10
Ver Arquivo
@@ -0,0 +1,10 @@
package com.codeminders.ardrone.data.logger;
public interface DataLogger {
void log(ChannelDataChunk data);
void logStreamContent(int data);
public void finish();
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/logger/LogStreamWrapper.java
+23
Ver Arquivo
@@ -0,0 +1,23 @@
package com.codeminders.ardrone.data.logger;
import java.io.IOException;
import java.io.InputStream;
public class LogStreamWrapper extends InputStream {
InputStream dataStream;
DataLogger logger;
public LogStreamWrapper(InputStream dataStream, DataLogger logger) {
this.dataStream = dataStream;
this.logger = logger;
}
@Override
public int read() throws IOException {
int data = dataStream.read();
logger.logStreamContent(data);
return data;
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/navdata/ControlAlgorithm.java
+6
Ver Arquivo
@@ -0,0 +1,6 @@
package com.codeminders.ardrone.data.navdata;
public enum ControlAlgorithm
{
EULER_ANGELS_CONTROL, ANGULAR_SPEED_CONTROL
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/navdata/CtrlState.java
+34
Ver Arquivo
@@ -0,0 +1,34 @@
package com.codeminders.ardrone.data.navdata;
public enum CtrlState
{
DEFAULT, INIT, LANDED, FLYING, HOVERING, TEST, TRANS_TAKEOFF, TRANS_GOTOFIX, TRANS_LANDING;
public static CtrlState fromInt(int v) throws NavDataFormatException
{
switch(v)
{
case 0:
return DEFAULT;
case 1:
return INIT;
case 2:
return LANDED;
case 3:
return FLYING;
case 4:
return HOVERING;
case 5:
return TEST;
case 6:
return TRANS_TAKEOFF;
case 7:
return TRANS_GOTOFIX;
case 8:
return TRANS_LANDING;
default:
throw new NavDataFormatException("Invalid control state " + v);
}
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/navdata/FlyingState.java
+26
Ver Arquivo
@@ -0,0 +1,26 @@
package com.codeminders.ardrone.data.navdata;
public enum FlyingState
{
FLYING, TAKING_OFF, LANDING, LANDED;
public static FlyingState fromControlState(CtrlState state)
{
switch(state)
{
case FLYING:
case HOVERING:
case TRANS_GOTOFIX:
return FlyingState.FLYING;
case TRANS_TAKEOFF:
return FlyingState.TAKING_OFF;
case TRANS_LANDING:
return FlyingState.LANDING;
default:
return FlyingState.LANDED;
}
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/navdata/Mode.java
+6
Ver Arquivo
@@ -0,0 +1,6 @@
package com.codeminders.ardrone.data.navdata;
public enum Mode
{
BOOTSTRAP, DEMO
}
javadrone-api/src/main/java/com/codeminders/ardrone/NavDataFormatException.java → javadrone-api/src/main/java/com/codeminders/ardrone/data/navdata/NavDataFormatException.java
+1 -1
Ver Arquivo
@@ -1,5 +1,5 @@
package com.codeminders.ardrone;
package com.codeminders.ardrone.data.navdata;
@SuppressWarnings("serial")
public class NavDataFormatException extends Exception
javadrone-api/src/main/java/com/codeminders/ardrone/data/navdata/NavDataTag.java
+22
Ver Arquivo
@@ -0,0 +1,22 @@
package com.codeminders.ardrone.data.navdata;
public enum NavDataTag
{
NAVDATA_DEMO_TAG(0), NAVDATA_TIME_TAG(1), NAVDATA_RAW_MEASURES_TAG(2), NAVDATA_PHYS_MEASURES_TAG(3), NAVDATA_GYROS_OFFSETS_TAG(
4), NAVDATA_EULER_ANGLES_TAG(5), NAVDATA_REFERENCES_TAG(6), NAVDATA_TRIMS_TAG(7), NAVDATA_RC_REFERENCES_TAG(
8), NAVDATA_PWM_TAG(9), NAVDATA_ALTITUDE_TAG(10), NAVDATA_VISION_RAW_TAG(11), NAVDATA_VISION_OF_TAG(12), NAVDATA_VISION_TAG(
13), NAVDATA_VISION_PERF_TAG(14), NAVDATA_TRACKERS_SEND_TAG(15), NAVDATA_VISION_DETECT_TAG(16), NAVDATA_WATCHDOG_TAG(
17), NAVDATA_ADC_DATA_FRAME_TAG(18), NAVDATA_VIDEO_STREAM_TAG(19), NAVDATA_CKS_TAG(0xFFFF);
private int value;
private NavDataTag(int value)
{
this.value = value;
}
public int getValue()
{
return value;
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/Dimension.java → javadrone-api/src/main/java/com/codeminders/ardrone/data/navdata/vision/Dimension.java
+1 -12
Ver Arquivo
@@ -1,5 +1,5 @@
package com.codeminders.ardrone;
package com.codeminders.ardrone.data.navdata.vision;
public class Dimension
{
@@ -18,19 +18,8 @@ public class Dimension
return width;
}
public void setWidth(int width)
{
this.width = width;
}
public int getHeight()
{
return height;
}
public void setHeight(int height)
{
this.height = height;
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/Point.java → javadrone-api/src/main/java/com/codeminders/ardrone/data/navdata/vision/Point.java
+2 -13
Ver Arquivo
@@ -1,5 +1,5 @@
package com.codeminders.ardrone;
package com.codeminders.ardrone.data.navdata.vision;
public class Point
{
@@ -17,20 +17,9 @@ public class Point
{
return x;
}
public void setX(int x)
{
this.x = x;
}
public int getY()
{
return y;
}
public void setY(int y)
{
this.y = y;
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/VisionTag.java → javadrone-api/src/main/java/com/codeminders/ardrone/data/navdata/vision/VisionTag.java
+3 -1
Ver Arquivo
@@ -1,5 +1,7 @@
package com.codeminders.ardrone;
package com.codeminders.ardrone.data.navdata.vision;
import com.codeminders.ardrone.data.navdata.NavDataFormatException;
public class VisionTag
{
javadrone-api/src/main/java/com/codeminders/ardrone/data/reader/LigthUDPDataReader.java
+83
Ver Arquivo
@@ -0,0 +1,83 @@
package com.codeminders.ardrone.data.reader;
import java.io.IOException;
import java.io.InputStream;
import java.net.DatagramPacket;
import java.net.DatagramSocket;
import java.net.InetAddress;
import com.codeminders.ardrone.data.ARDroneDataReader;
public class LigthUDPDataReader implements ARDroneDataReader {
private int timeout;
private int data_port;
static final byte[] TRIGGER_BYTES = { 0x01, 0x00, 0x00, 0x00 };
protected DatagramSocket socket;
private DatagramPacket trigger_packet;
private InetAddress drone_addr;
public LigthUDPDataReader(InetAddress drone_addr, int data_port, int timeout) throws IOException {
super();
this.data_port = data_port;
this.timeout = timeout;
this.drone_addr = drone_addr;
trigger_packet = new DatagramPacket(TRIGGER_BYTES, TRIGGER_BYTES.length, drone_addr, data_port);
connect();
}
public InputStream getDataStream() {
return null;
}
public void connect() throws IOException {
disconnect();
socket = new DatagramSocket(data_port);
socket.setSoTimeout(timeout);
}
private void disconnect() {
if (null != socket && socket.isConnected()) {
socket.disconnect();
}
if (null != socket && !socket.isClosed()) {
socket.close();
}
}
public int readDataBlock(byte[] buf) throws IOException {
//send trigger data
socket.send(trigger_packet);
//receive data
DatagramPacket packet = new DatagramPacket(buf, buf.length, drone_addr, data_port);
socket.receive(packet);
return packet.getLength();
}
public synchronized void finish()
{
disconnect();
}
@Override
public boolean isStreamSupported() {
return false;
}
@Override
public void reconnect() throws IOException {
disconnect();
connect();
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/reader/TCPDataRader.java
+77
Ver Arquivo
@@ -0,0 +1,77 @@
package com.codeminders.ardrone.data.reader;
import java.io.IOException;
import java.io.InputStream;
import java.net.InetAddress;
import java.net.Socket;
import java.util.logging.Level;
import java.util.logging.Logger;
import com.codeminders.ardrone.data.ARDroneDataReader;
public class TCPDataRader implements ARDroneDataReader {
Logger log = Logger.getLogger(this.getClass().getName());
private InetAddress drone_addr;
private int data_port;
private int timeout;
private Socket socket;
private InputStream socketInput;
public TCPDataRader(InetAddress drone_addr, int data_port, int buffer_size, int timeout) throws IOException {
super();
this.drone_addr = drone_addr;
this.data_port = data_port;
this.timeout = timeout;
connect();
}
private void connect() throws IOException {
socket = new Socket(drone_addr, data_port);
socket.setSoTimeout(timeout);
socketInput = socket.getInputStream();
}
@Override
public InputStream getDataStream() {
return socketInput;
}
@Override
public void finish() {
disconnect();
}
private void disconnect() {
if (socket.isClosed()) {
try {
socket.close();
} catch (IOException e) {
log.log(Level.FINER, "Excepton on stopping TCP reading", e);
}
}
}
@Override
public int readDataBlock(byte[] buf) throws IOException {
return socketInput.read(buf);
}
@Override
public boolean isStreamSupported() {
return true;
}
@Override
public void reconnect() throws IOException {
disconnect();
connect();
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/reader/UDPDataReader.java
+46 -98
Ver Arquivo
@@ -1,6 +1,7 @@
package com.codeminders.ardrone.data.reader;
import java.io.IOException;
import java.io.InputStream;
import java.net.InetAddress;
import java.net.InetSocketAddress;
import java.nio.ByteBuffer;
@@ -10,40 +11,32 @@ import java.nio.channels.SelectionKey;
import java.nio.channels.Selector;
import java.util.Iterator;
import java.util.Set;
import java.util.logging.Level;
import java.util.logging.Logger;
import com.codeminders.ardrone.ARDrone;
import com.codeminders.ardrone.data.ARDroneDataReader;
public abstract class UDPDataReader implements Runnable {
public class UDPDataReader implements ARDroneDataReader {
private int reconnect_timeout;
private static final int MAX_TMEOUT = 500;
private Logger log = Logger.getLogger(this.getClass().getName());
private int timeout;
protected DatagramChannel channel;
ARDrone drone;
protected Selector selector;
private boolean done;
private boolean pauseFlag;
private InetAddress drone_addr;
private int data_port;
private long timeOfLastMessage = 0;
private int buffer_size;
static final byte[] TRIGGER_BYTES = { 0x01, 0x00, 0x00, 0x00 };
static final byte[] TRIGGER_BYTES = { 0x01, 0x00, 0x00, 0x00 };
ByteBuffer trigger_buffer = ByteBuffer.allocate(TRIGGER_BYTES.length);
ByteBuffer trigger_buffer = ByteBuffer.allocate(TRIGGER_BYTES.length);
ByteBuffer inbuf = ByteBuffer.allocate(buffer_size);
public UDPDataReader(ARDrone drone, InetAddress drone_addr, int data_port, int buffer_size, int reconnect_timeout) throws ClosedChannelException, IOException {
public UDPDataReader(InetAddress drone_addr, int data_port, int timeout) throws ClosedChannelException, IOException {
super();
this.drone = drone;
this.drone_addr = drone_addr;
this.data_port = data_port;
this.buffer_size = buffer_size;
this.reconnect_timeout = reconnect_timeout;
this.timeout = timeout;
trigger_buffer.put(TRIGGER_BYTES);
trigger_buffer.flip();
@@ -92,97 +85,52 @@ public abstract class UDPDataReader implements Runnable {
}
@Override
public void run()
public int readDataBlock(byte[] buf) throws IOException
{
try
{
ByteBuffer inbuf = ByteBuffer.allocate(buffer_size);
done = false;
timeOfLastMessage = System.currentTimeMillis();
while(!done)
{
if (pauseFlag) {
synchronized(this) {
if (pauseFlag) {
wait();
timeOfLastMessage = 1; // will automatically reconnect channel
}
}
}
selector.select(MAX_TMEOUT);
if(done)
{
disconnect();
break;
}
Set<SelectionKey> readyKeys = selector.selectedKeys();
Iterator<SelectionKey> iterator = readyKeys.iterator();
if (!iterator.hasNext()) {
if (timeOfLastMessage > 0 && System.currentTimeMillis() - timeOfLastMessage > reconnect_timeout ) {
log.fine("Data Timeout in " + reconnect_timeout + "ms. reached. Attemting to reconnect" );
disconnect();
try {
connect();
} catch (Exception e) {
log.log(Level.FINE, "Failed to re-connect", e);
}
timeOfLastMessage = System.currentTimeMillis();
}
}
while(iterator.hasNext())
{
timeOfLastMessage = System.currentTimeMillis();
SelectionKey key = (SelectionKey) iterator.next();
iterator.remove();
if(key.isWritable())
{
channel.write(trigger_buffer);
channel.register(selector, SelectionKey.OP_READ);
// prepare buffer for new reconnection attempt
trigger_buffer.clear();
trigger_buffer.put(TRIGGER_BYTES);
trigger_buffer.flip();
}
else if(key.isReadable())
{
inbuf.clear();
int len = channel.read(inbuf);
inbuf.flip();
handleData(inbuf, len);
}
}
}
} catch(Exception e)
{
if (!done) {
drone.changeToErrorState(e);
int len = 0;
selector.select(timeout);
Set<SelectionKey> readyKeys = selector.selectedKeys();
Iterator<SelectionKey> iterator = readyKeys.iterator();
while (iterator.hasNext()) {
SelectionKey key = (SelectionKey) iterator.next();
iterator.remove();
if (key.isWritable()) {
channel.write(trigger_buffer);
channel.register(selector, SelectionKey.OP_READ);
// prepare buffer for new reconnection attempt
trigger_buffer.clear();
trigger_buffer.put(TRIGGER_BYTES);
trigger_buffer.flip();
} else if (key.isReadable()) {
return channel.read(ByteBuffer.wrap(buf));
}
}
return len;
}
public abstract void handleData(ByteBuffer buf, int len) throws Exception;
public synchronized void finish()
{
if (pauseFlag) {
resumeReading();
}
done = true;
if (null != selector) {
selector.wakeup();
}
}
public synchronized void pauseReading() {
pauseFlag = true;
@Override
public InputStream getDataStream() {
return null;
}
public synchronized void resumeReading() {
pauseFlag = false;
notify();
@Override
public boolean isStreamSupported() {
return false;
}
@Override
public void reconnect() throws IOException {
disconnect();
connect();
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/reader/UDPDataReaderAndDecoder.java
-49
Ver Arquivo
@@ -1,49 +0,0 @@
package com.codeminders.ardrone.data.reader;
import java.io.IOException;
import java.net.InetAddress;
import java.nio.ByteBuffer;
import com.codeminders.ardrone.ARDrone;
import com.codeminders.ardrone.data.decoder.DataDecoder;
import com.codeminders.ardrone.data.logger.ChannelDataLogger;
import com.codeminders.ardrone.data.logger.ChannelDataChunk;
public class UDPDataReaderAndDecoder extends UDPDataReader {
DataDecoder dataDecoder;
private ChannelDataLogger logger;
long lastDataTime = 0l;
public UDPDataReaderAndDecoder(ARDrone drone, InetAddress drone_addr, int navdata_port, int bufferSize, int reconnect_timeout, DataDecoder dataDecoder, ChannelDataLogger logger) throws IOException
{
super(drone, drone_addr, navdata_port, bufferSize, reconnect_timeout);
this.dataDecoder = dataDecoder;
this.logger = logger;
}
@Override
public void handleData(ByteBuffer inbuf, int len)
{
if (null != logger) {
byte[] data = inbuf.array();
logger.log(new ChannelDataChunk(data, lastDataTime));
lastDataTime = System.currentTimeMillis();
inbuf.clear();
inbuf.put(data);
inbuf.flip();
}
dataDecoder.decodeData(inbuf, len);
}
public void finish()
{
super.finish();
dataDecoder.finish();
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/reader/VideoReader.java
-40
Ver Arquivo
@@ -1,40 +0,0 @@
package com.codeminders.ardrone.data.reader;
import java.io.IOException;
import java.net.InetAddress;
import java.nio.ByteBuffer;
import com.codeminders.ardrone.ARDrone;
import com.codeminders.ardrone.data.decoder.DataDecoder;
public class VideoReader extends UDPDataReader {
/**
* Image data buffer. It should be big enough to hold single full frame
* (encoded).
*/
private static final int BUFSIZE = 100 * 1024;
DataDecoder dataDecoder;
public VideoReader(ARDrone drone, InetAddress drone_addr, int video_port, int reconnect_timeout, DataDecoder dataDecoder) throws IOException
{
super(drone, drone_addr, video_port, BUFSIZE, reconnect_timeout);
this.dataDecoder = dataDecoder;
}
@Override
public void handleData(ByteBuffer inbuf, int len)
throws Exception {
if (len > 0) {
dataDecoder.decodeData(inbuf, len);
}
}
public void finish()
{
super.finish();
dataDecoder.finish();
}
}
javadrone-utils/pom.xml
+1 -1
Ver Arquivo
@@ -77,7 +77,7 @@
<dependency>
<groupId>com.codeminders</groupId>
<artifactId>hidapi</artifactId>
<version>1.1</version>
<version>1.2</version>
</dependency>
<dependency>
<groupId>com.twilight</groupId>
javadrone-api/src/main/java/com/codeminders/ardrone/data/logger/file/AsyncFileChannelDataLogger.java → javadrone-utils/src/main/java/com/codeminders/ardrone/data/logger/file/AsyncFileChannelDataLogger.java
+23 -5
Ver Arquivo
@@ -8,13 +8,14 @@ import java.util.Date;
import java.util.logging.Logger;
import com.codeminders.ardrone.data.logger.ChannelDataLogger;
import com.codeminders.ardrone.data.logger.DataLogger;
import com.codeminders.ardrone.data.logger.ChannelDataChunk;
public class AsyncFileChannelDataLogger implements ChannelDataLogger {
public class AsyncFileChannelDataLogger implements DataLogger {
private static final int DAFAULT_QUEUE_CAPACITY = 128;
private static final int DAFAULT_STREAM_QUEUE_CAPACITY = 32 * 1024; // 32 Kb;
private Logger log = Logger.getLogger(getClass().getName());
@@ -23,6 +24,7 @@ public class AsyncFileChannelDataLogger implements ChannelDataLogger {
private static char indexSeparator = '_';
SaveToFile toFile;
SaveStreamToFile streamFile;
public AsyncFileChannelDataLogger(String logDirPath, String filePrefix) throws ClosedChannelException,
IOException {
@@ -30,16 +32,24 @@ public class AsyncFileChannelDataLogger implements ChannelDataLogger {
validate(logDirPath, filePrefix);
shiftIndexOfPreviousLogFiles(logDirPath, filePrefix);
toFile = new SaveToFile(new File(logDirPath + File.separatorChar + getFileName(filePrefix)+ getFileExt()) , DAFAULT_QUEUE_CAPACITY);
toFile = new SaveToFile(new File(logDirPath + File.separatorChar + getFileName(filePrefix) + "-chunk" + getFileExt()) , DAFAULT_QUEUE_CAPACITY);
toFile.start();
streamFile = new SaveStreamToFile(new File(logDirPath + File.separatorChar + getFileName(filePrefix)+ "-stream" + getFileExt()) , DAFAULT_STREAM_QUEUE_CAPACITY);
toFile.start();
}
@Override
public void log(ChannelDataChunk data) {
toFile.toFile(data);
}
@Override
public void logStreamContent(int data) {
streamFile.toFile(data);
}
private void validate(String logDirPath, String filePrefix) {
if (null == logDirPath || logDirPath.trim().isEmpty()) {
log.severe("Error. Please scpecify correct value of logging derectory");
@@ -100,7 +110,15 @@ public class AsyncFileChannelDataLogger implements ChannelDataLogger {
try {
toFile.join(1000);
} catch (InterruptedException e) {
log.severe("Error. Faild to wat till file logger is topped is aborded");
log.severe("Error. Faild to wait till file logger is topped or aborded");
}
streamFile.finish();
try {
streamFile.join(1000);
} catch (InterruptedException e) {
log.severe("Error. Faild to wait till stream file logger is topped or aborded");
}
}
javadrone-utils/src/main/java/com/codeminders/ardrone/data/logger/file/SaveStreamToFile.java
+75
Ver Arquivo
@@ -0,0 +1,75 @@
package com.codeminders.ardrone.data.logger.file;
import java.io.DataOutputStream;
import java.io.File;
import java.io.FileNotFoundException;
import java.io.FileOutputStream;
import java.io.IOException;
import java.util.concurrent.LinkedBlockingQueue;
import java.util.logging.Level;
import java.util.logging.Logger;
public class SaveStreamToFile extends Thread {
private Logger log = Logger.getLogger(getClass().getName());
FileOutputStream fout;
DataOutputStream dout;
LinkedBlockingQueue<Integer> queue;
boolean done = false;
public SaveStreamToFile(File file, int queueCapacity) throws FileNotFoundException {
super();
fout = new FileOutputStream(file);
dout = new DataOutputStream(fout);
queue = new LinkedBlockingQueue<Integer>(queueCapacity);
setName("File " + file.getName() + " writer");
}
@Override
public void run() {
Integer toSave;
while (!done) {
toSave = null;
try {
toSave = queue.take();
} catch (InterruptedException e) {
break;
}
if (null != toSave) {
try {
dout.writeInt(toSave);
} catch (IOException e) {
done = true;
log.log(Level.SEVERE, "Failed to store data", e);
}
}
}
try {
fout.flush();
fout.close();
} catch (IOException e) {}
}
public void toFile(int data) {
try {
if (!done) {
queue.put(data);
} /**else {
// throw
} **/
} catch (InterruptedException e) {
// external interrupt
}
}
public synchronized void finish()
{
done = true;
try {
queue.put(null);
} catch (InterruptedException e) {}
}
}
javadrone-api/src/main/java/com/codeminders/ardrone/data/logger/file/SaveToFile.java → javadrone-utils/src/main/java/com/codeminders/ardrone/data/logger/file/SaveToFile.java
Ver Arquivo
javadrone-utils/src/main/java/com/codeminders/ardrone/decoder/FileDataReaderAndDecoder.java
+2 -2
Ver Arquivo
@@ -4,7 +4,7 @@ import java.io.File;
import java.io.FileNotFoundException;
import java.nio.ByteBuffer;
import com.codeminders.ardrone.data.decoder.DataDecoder;
import com.codeminders.ardrone.data.DataDecoder;
import com.codeminders.ardrone.data.reader.FileDataReader;
public class FileDataReaderAndDecoder extends FileDataReader {
@@ -18,7 +18,7 @@ public class FileDataReaderAndDecoder extends FileDataReader {
@Override
public void handleData(ByteBuffer inbuf, int len) throws Exception {
dataDecoder.decodeData(inbuf, len);
// dataDecoder.decodeData(inbuf, len);
}
}
javadrone-utils/src/main/java/com/codeminders/ardrone/decoder/TestH264DataDecoder.java
+16 -15
Ver Arquivo
@@ -1,21 +1,21 @@
package com.codeminders.ardrone.decoder;
import java.nio.ByteBuffer;
import java.io.InputStream;
import java.util.Arrays;
import com.codeminders.ardrone.ARDrone;
import com.codeminders.ardrone.data.decoder.DataDecoder;
import com.codeminders.ardrone.VideoDataDecoder;
import com.twilight.h264.decoder.AVFrame;
import com.twilight.h264.decoder.AVPacket;
import com.twilight.h264.decoder.H264Decoder;
import com.twilight.h264.decoder.MpegEncContext;
import com.twilight.h264.player.FrameUtils;
public class TestH264DataDecoder implements DataDecoder {
public class TestH264DataDecoder extends VideoDataDecoder {
ARDrone drone;
int buffer_size;
//public static final int INBUF_SIZE = 65535;
public int INBUF_SIZE;
H264Decoder codec;
@@ -36,7 +36,7 @@ public class TestH264DataDecoder implements DataDecoder {
public TestH264DataDecoder(ARDrone drone, int buffer_size) {
super();
super(drone);
this.drone = drone;
this.buffer_size = buffer_size;
INBUF_SIZE = buffer_size;
@@ -65,16 +65,17 @@ public class TestH264DataDecoder implements DataDecoder {
}
@Override
public void decodeData(ByteBuffer fin, int len) {
public void run() {
InputStream fin = getDataReader().getDataStream();
try {
// avpkt must contain exactly 1 NAL Unit in order for decoder to decode correctly.
// thus we must read until we get next NAL header before sending it to decoder.
// Find 1st NAL
int[] cacheRead = new int[3];
cacheRead[0] = fin.getInt();
cacheRead[1] = fin.getInt();
cacheRead[2] = fin.getInt();
cacheRead[0] = fin.read();
cacheRead[1] = fin.read();
cacheRead[2] = fin.read();
while(!(
cacheRead[0] == 0x00 &&
@@ -83,7 +84,7 @@ public class TestH264DataDecoder implements DataDecoder {
)) {
cacheRead[0] = cacheRead[1];
cacheRead[1] = cacheRead[2];
cacheRead[2] = fin.getInt();
cacheRead[2] = fin.read();
} // while
boolean hasMoreNAL = true;
@@ -95,11 +96,11 @@ public class TestH264DataDecoder implements DataDecoder {
while(hasMoreNAL) { // TODO: Possible error because we use not file
dataPointer = 4;
// Find next NAL
cacheRead[0] = fin.getInt();
cacheRead[0] = fin.read();
if(cacheRead[0]==-1) hasMoreNAL = false;
cacheRead[1] = fin.getInt();
cacheRead[1] = fin.read();
if(cacheRead[1]==-1) hasMoreNAL = false;
cacheRead[2] = fin.getInt();
cacheRead[2] = fin.read();
if(cacheRead[2]==-1) hasMoreNAL = false;
while(!(
cacheRead[0] == 0x00 &&
@@ -109,7 +110,7 @@ public class TestH264DataDecoder implements DataDecoder {
inbuf_int[dataPointer++] = cacheRead[0];
cacheRead[0] = cacheRead[1];
cacheRead[1] = cacheRead[2];
cacheRead[2] = fin.getInt();
cacheRead[2] = fin.read();
if(cacheRead[2]==-1) hasMoreNAL = false;
} // while
@@ -135,7 +136,7 @@ public class TestH264DataDecoder implements DataDecoder {
}
FrameUtils.YUV2RGB(picture, buffer);
drone.videoFrameReceived(0, 0, picture.imageWidth ,picture.imageHeight, buffer, 0, picture.imageWidth);
notifyDroneWithDecodedFrame(0, 0, picture.imageWidth ,picture.imageHeight, buffer, 0, picture.imageWidth);
}
avpkt.size -= len;
avpkt.data_offset += len;
javadrone-utils/src/main/java/com/codeminders/ardrone/logs/video/VideoPlayer.java
+24 -22
Ver Arquivo
@@ -14,7 +14,7 @@ import javax.swing.JFrame;
import com.codeminders.ardrone.ARDrone;
import com.codeminders.ardrone.DroneVideoListener;
import com.codeminders.ardrone.data.decoder.VideoDataDecoder;
import com.codeminders.ardrone.VideoDataDecoder;
import com.codeminders.ardrone.decoder.FileDataReaderAndDecoder;
public class VideoPlayer implements Runnable, DroneVideoListener {
@@ -64,29 +64,31 @@ public class VideoPlayer implements Runnable, DroneVideoListener {
@Override
public void run() {
//TODO: play decoded content;
try {
File in = new File(fileName);
// try {
// File in = new File(fileName);
//
// VideoDataDecoder video_decoder = new VideoDataDecoder(drone, 100 * 1024);
//
// FileDataReaderAndDecoder video_reader = new FileDataReaderAndDecoder(in, video_decoder);
//
// video_decoder.start();
//
// Thread video_reader_thread = new Thread(video_reader);
// video_reader_thread.setName("Video Reader");
// video_reader_thread.start();
//
// video_reader_thread.join();
//
// video_decoder.finish();
VideoDataDecoder video_decoder = new VideoDataDecoder(drone, 100 * 1024);
FileDataReaderAndDecoder video_reader = new FileDataReaderAndDecoder(in, video_decoder);
video_decoder.start();
Thread video_reader_thread = new Thread(video_reader);
video_reader_thread.setName("Video Reader");
video_reader_thread.start();
video_reader_thread.join();
video_decoder.finish();
} catch (FileNotFoundException e) {
e.printStackTrace();
} catch (InterruptedException e) {
e.printStackTrace();
}
// } catch (FileNotFoundException e) {
// e.printStackTrace();
// } catch (InterruptedException e) {
// e.printStackTrace();
// }
}
@Override