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