@Override
public synchronized void newNavdata(NavdataChannel channel) {
if (channel.getLastDemoData() != null) {
float[] sVector = channel.getLastDemoData().velocity;
speedVector[0] = sVector[0];
speedVector[1] = sVector[1];
speedVector[2] = sVector[2];
// System.out.println(speedVector[0] + " " + speedVector[1] + " " + speedVector[2]);
float[] rotationVector = channel.getLastDemoData().euler;
if (lastRotationVector != null) {
double[] rotationDelta = {0, 0, 0};
for (int i = 0; i < 3; i++) {
rotationDelta[i] = (rotationVector[i] - lastRotationVector[i]) / 1000.0;
if (rotationDelta[i] < -270) {
rotationDelta[i] += 360;
}
if (rotationDelta[i] > 270) {
rotationDelta[i] -= 360;
}
}
integratedRotationVector[0] += rotationDelta[0] / 180 * Math.PI;
integratedRotationVector[1] += rotationDelta[1] / 180 * Math.PI;
integratedRotationVector[2] += rotationDelta[2] / 180 * Math.PI;
}
lastRotationVector = rotationVector;
}
}
public void connect(String remoteAddress) {
disconnect();
try {
InetAddress remoteInetAddress = InetAddress.getByName(remoteAddress);
controlCommandInterface.connect(remoteInetAddress);
try {
navdataChannel.connect(remoteInetAddress);
try {
vision.connect(remoteInetAddress, this);
} catch (IOException e) {
navdataChannel.disconnect();
throw e;
}
controlAbstractionThread = new Thread(this);
controlAbstractionThread.start();
} catch (IOException e) {
controlCommandInterface.disconnect();
throw e;
}
} catch (IOException e) {
e.printStackTrace();
}
}
public DroneGroundStation() throws SocketException {
controlCommandInterface = new ATControlCommandInterface();
droneConfigurationChannel = new DroneConfigurationChannel(controlCommandInterface);
vision = new VisionStandin();
navdataChannel = new NavdataChannel(droneConfigurationChannel);
navdataChannel.addNavdataChannelObserver(this);
}
@Override
public synchronized FlyingState getFlyingState() {
if (navdataChannel.getLastNavdataTimestamp() < 0) {
return FlyingState.DISCONNECTED;
}
if ((navdataChannel.getLastDroneStatus() & NavdataPacket.ARDRONE_VBAT_LOW) != 0) {
return FlyingState.BATTERY_LOW;
}
if ((navdataChannel.getLastDroneStatus() & NavdataPacket.ARDRONE_EMERGENCY_MASK) != 0) {
return FlyingState.EMERGENCY;
}
if ((navdataChannel.getLastDroneStatus() & NavdataPacket.ARDRONE_FLY_MASK) != 0) {
return FlyingState.FLYING;
}
return FlyingState.LANDED;
}
public void disconnect() {
if (controlAbstractionThread != null) {
controlAbstractionThread.interrupt();
try {
controlAbstractionThread.join();
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
controlAbstractionThread = null;
}
vision.disconnect();
navdataChannel.disconnect();
controlCommandInterface.disconnect();
lastRotationVector = null;
integratedRotationVector[0] = 0;
integratedRotationVector[1] = 0;
integratedRotationVector[2] = 0;
speedVector[0] = 0;
speedVector[1] = 0;
speedVector[2] = 0;
}
@Override
public void run() {
boolean lDoHover, lDoStart, lEmergency;
double lHeight;
double lPitch, lRoll, lYaw;
double currentHeight;
double startHeight = 0;
long lastEmergencyStrobe = -1;
long lastNavdataTimeStamp = navdataChannel.getLastNavdataTimestamp();
long lastNewNavdataTimeStamp = System.currentTimeMillis();
controlCommandInterface.land();
while (!Thread.currentThread().isInterrupted()) {
synchronized (this) {
lDoStart = doStart;
lEmergency = setEmergency;
}
if (lEmergency != (getFlyingState() == FlyingState.EMERGENCY)) {
if (System.currentTimeMillis() - lastEmergencyStrobe > 250) {
controlCommandInterface.strobeEmergency();
lastEmergencyStrobe = System.currentTimeMillis();
}
} else {
if (getFlyingState() == FlyingState.LANDED) {
if (lDoStart) {
controlCommandInterface.takeoff();
}
}
if (getFlyingState() == FlyingState.FLYING) {
if (!lDoStart) {
controlCommandInterface.land();
}
synchronized (this) {
lDoHover = doHover;
lHeight = setHeight * (MAX_HEIGHT - MIN_HEIGHT) + MIN_HEIGHT;
lPitch = setPitch;
lRoll = setRoll;
lYaw = setYaw;
if (navdataChannel.getLastNavdataTimestamp() > lastNavdataTimeStamp) {
lastNavdataTimeStamp = navdataChannel.getLastNavdataTimestamp();
lastNewNavdataTimeStamp = System.currentTimeMillis();
}
currentHeight = lHeight;
if ((navdataChannel.getLastDemoData() != null)
&& (System.currentTimeMillis() - lastNewNavdataTimeStamp < 500)) {
currentHeight = navdataChannel.getLastDemoData().altitude;
}
}
if (Math.abs(currentHeight - lHeight) > MAX_HEIGHT_DIFFERENCE) {
if ((Math.signum(startHeight - lHeight) != Math.signum(currentHeight - lHeight))
|| (Math.abs(startHeight - lHeight) < MAX_HEIGHT_DIFFERENCE)) {
startHeight = currentHeight;
}
}
if (Math.abs(startHeight - lHeight) > MAX_HEIGHT_DIFFERENCE) {
double upSpeed =
Math.max(0.05, Math.min(1, 1.0 * Math.abs(lHeight - currentHeight) / 1000.0));
if (startHeight > lHeight) {
upSpeed = -upSpeed;
if (currentHeight < lHeight) {
startHeight = lHeight;
}
} else {
if (currentHeight > lHeight) {
startHeight = lHeight;
}
}
if (lDoHover) {
controlCommandInterface.flyForward(0);
controlCommandInterface.flySideways(0);
controlCommandInterface.spin(0);
} else {
controlCommandInterface.flyForward(lPitch);
controlCommandInterface.flySideways(lRoll);
controlCommandInterface.spin(lYaw);
}
controlCommandInterface.setUpSpeed(upSpeed);
} else {
if (lDoHover) {
controlCommandInterface.hover();
} else {
controlCommandInterface.flyForward(lPitch);
controlCommandInterface.flySideways(lRoll);
controlCommandInterface.spin(lYaw);
controlCommandInterface.setUpSpeed(0);
}
}
}
}
try {
Thread.sleep(10);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
}
@Override
public synchronized long getLastSensoryDataTimeStamp() {
return navdataChannel.getLastNavdataTimestamp();
}
