@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;
}
@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();
}
