using concurrent::ActorPool
using concurrent::Actor
using concurrent::AtomicBool
using concurrent::AtomicRef
using afConcurrent::Synchronized
using afConcurrent::AtomicMap
** The main interface to the Parrot AR Drone 2.0.
**
** Once the drone has been disconnected, this class instance can not be re-connected.
** Instead create a new 'Drone' instance.
const class Drone {
private const Log log := Drone#.pod.log
private const AtomicRef navDataRef := AtomicRef()
private const AtomicRef exitStrategyRef := AtomicRef(ExitStrategy.land)
private const AtomicRef onNavDataRef := AtomicRef()
private const AtomicRef onStateChangeRef := AtomicRef()
private const AtomicRef onBatterDrainRef := AtomicRef()
private const AtomicRef onEmergencyRef := AtomicRef()
private const AtomicRef onBatteryLowRef := AtomicRef()
private const AtomicRef onDisconnectRef := AtomicRef()
private const AtomicRef onVideoFrameRef := AtomicRef()
private const AtomicRef onVideoErrorRef := AtomicRef()
private const AtomicRef onRecordFrameRef := AtomicRef()
private const AtomicRef onRecordErrorRef := AtomicRef()
private const AtomicRef droneVersionRef := AtomicRef()
private const AtomicBool absoluteModeRef := AtomicBool()
private const AtomicRef absoluteAngleRef := AtomicRef()
private const AtomicRef absoluteAccuracyRef := AtomicRef(0.1f)
private const AtomicRef oldVideoCodecRef := AtomicRef()
private const AtomicBool connectedRef := AtomicBool(false)
private const AtomicMap configMapRef := AtomicMap { it.keyType = Str#; it.valType = Str#; it.caseInsensitive = true }
private const DroneConfig configRef := DroneConfig(this)
private const Synchronized eventThread
private const CmdSender cmdSender
private const NavDataReader navDataReader
private const ControlReader controlReader
private const VideoReader videoReader
private const VideoReader recordReader
private const |->| shutdownHook
// FIXME Stream recording video
// FIXME test userBox cmds
// FIXME try roundel hovering
// FIXME drone.turnTo() cmd ???
** The 'ActorPool' responsible for controlling all the threads handled by this drone.
@NoDoc const ActorPool actorPool
** The version of the drone, as reported by an FTP of 'version.txt'.
Version droneVersion {
get { droneVersionRef.val ?: Version.defVal }
private set { droneVersionRef.val = it }
}
** The network configuration as passed to the ctor.
const NetworkConfig networkConfig
** Returns the latest Nav Data or 'null' if not connected.
** Note that this instance always contains a culmination of all the latest nav options received.
NavData? navData {
get { navDataRef.val }
private set { navDataRef.val = it }
}
** Returns the current flight state of the Drone.
FlightState? flightState {
get { navData?.demoData?.flightState }
private set { }
}
** Should this 'Drone' class instance disconnect from the drone whilst it is flying (or the VM
** otherwise exits) then this strategy governs what last commands should be sent to the drone
** before it exits.
**
** (It's a useful feature I wish I'd implemented before the time my program crashed and I
** watched my drone sail up, up, and away, over the tree line!)
**
** Defaults to 'land'.
**
** Note this can not guard against a forced process kill or a 'kill -9' command.
ExitStrategy exitStrategy {
get { exitStrategyRef.val }
set { exitStrategyRef.val = it }
}
** Event hook that's notified when the drone sends new NavData.
** Expect the function to be called many times a second.
**
** The nav data is raw from the drone so does **not** contain a culmination of all option data.
** Note 'Drone.navData' is updated with the new contents before the hook is called.
**
** Throws 'NotImmutableErr' if the function is not immutable.
** Note this hook is called from a different Actor / thread to the one that sets it.
|NavData, Drone|? onNavData {
get { onNavDataRef.val }
set { onNavDataRef.val = it?.toImmutable }
}
** Event hook that's called when the drone's state is changed.
**
** Throws 'NotImmutableErr' if the function is not immutable.
** Note this hook is called from a different Actor / thread to the one that sets it.
|FlightState state, Drone|? onStateChange {
get { onStateChangeRef.val }
set { onStateChangeRef.val = it?.toImmutable }
}
** Event hook that's called when the drone's battery loses a percentage of charge.
** The function is called with the new battery percentage level (0 - 100).
**
** Throws 'NotImmutableErr' if the function is not immutable.
** Note this hook is called from a different Actor / thread to the one that sets it.
|Int newPercentage, Drone|? onBatteryDrain {
get { onBatterDrainRef.val }
set { onBatterDrainRef.val = it?.toImmutable }
}
** Event hook that's called when the drone enters emergency mode.
** When this happens, the engines are cut and the drone will not respond to commands until
** emergency mode is cleared.
**
** Note this hook is only called when the drone is flying.
**
** Throws 'NotImmutableErr' if the function is not immutable.
** Note this hook is called from a different Actor / thread to the one that sets it.
|Drone|? onEmergency {
get { onEmergencyRef.val }
set { onEmergencyRef.val = it?.toImmutable }
}
** Event hook that's called when the drone's battery reaches a critical level ~ 20%.
**
** Throws 'NotImmutableErr' if the function is not immutable.
** Note this hook is called from a different Actor / thread to the one that sets it.
|Drone|? onBatteryLow {
get { onBatteryLowRef.val }
set { onBatteryLowRef.val = it?.toImmutable }
}
** Event hook that's called when the drone is disconnected.
** The 'abnormal' boolean is set to 'true' if the drone is disconnected due to a communication
** / socket error. This may happen if the battery drains too low or the drone is switched off.
**
** Throws 'NotImmutableErr' if the function is not immutable.
** Note this hook is called from a different Actor / thread to the one that sets it.
|Bool abnormal, Drone|? onDisconnect {
get { onDisconnectRef.val }
set { onDisconnectRef.val = it?.toImmutable }
}
** Event hook that's called when a video frame is received. (On Drone TCP port 5555.)
**
** The payload is a raw frame from the H.264 codec.
**
** Setting this hook instructs the drone to start streaming live video.
** Setting to 'null' instructs the drone to stop streaming video.
**
** Throws 'NotImmutableErr' if the function is not immutable.
** Note this hook is called from a different Actor / thread to the one that sets it.
|Buf, PaveHeader, Drone|? onVideoFrame {
get { onVideoFrameRef.val }
set { onVideoFrameRef.val = it?.toImmutable
if (it == null)
videoReader.disconnect
if (it != null && !videoReader.isConnected)
videoReader.connect
}
}
** Event hook that's called when there's an error decoding the video stream.
**
** Given the nature of streaming chunks, errors may be un-recoverable and future video frames
** may not be decoded. Therefore on error, it is advised to restart the video comms with the
** drone.
|Err, Drone|? onVideoErr {
get { onVideoErrorRef.val }
set { onVideoErrorRef.val = it?.toImmutable }
}
** Event hook that's called when video record frame is received. (On Drone TCP port 5553.)
** Video recordings are generally not streamed live but do have better quality.
**
** Call [startRecording()]`startRecording` before setting this.
**
** The payload is a raw frame from the H.264 codec.
**
** Setting this hook instructs the drone to start streaming video that it's recording.
** Setting to 'null' instructs the drone to stop streaming the video recording.
**
** Throws 'NotImmutableErr' if the function is not immutable.
** Note this hook is called from a different Actor / thread to the one that sets it.
@NoDoc
|Buf, PaveHeader, Drone|? onRecordFrame {
get { onRecordFrameRef.val }
set { onRecordFrameRef.val = it?.toImmutable
if (it == null)
recordReader.disconnect
if (it != null && !recordReader.isConnected)
recordReader.connect
}
}
** Event hook that's called when there's an error decoding the video record stream.
**
** Given the nature of streaming chunks, errors may be un-recoverable and future video frames
** may not be decoded. Therefore on error, it is advised to restart the video comms with the
** drone.
|Err, Drone|? onRecordErr {
get { onRecordErrorRef.val }
set { onRecordErrorRef.val = it?.toImmutable }
}
** Creates a 'Drone' instance, optionally passing in network configuration.
new make(NetworkConfig networkConfig := NetworkConfig(), |This|? f := null) {
this.actorPool = ActorPool() { it.name = "Parrot Drone" }
this.networkConfig = networkConfig
this.navDataReader = NavDataReader(this, actorPool)
this.controlReader = ControlReader(this)
this.videoReader = VideoReader(this, actorPool, networkConfig.videoPort)
this.recordReader = VideoReader(this, actorPool, networkConfig.videoRecPort)
this.cmdSender = CmdSender(this, actorPool)
this.eventThread = Synchronized(actorPool)
this.shutdownHook = #onShutdown.func.bind([this])
f?.call(this)
}
** Sets up the socket connections to the drone.
** Your device must already be connected to the drone's wifi hot spot.
**
** Whilst there is no real concept of being *connected* to a drone,
** this method blocks until nav data is being received and all initiating commands have been acknowledged.
This connect() {
// we could re-use if we didn't stop the actor pool...?
// but nah, we created it, we should destroy it
if (actorPool.isStopped)
throw Err("Can not re-use Drone() instances")
navDataReader.addListener(#processNavData.func.bind([this]))
videoReader .setFrameListener(#processVidData.func.bind([this]))
videoReader .setErrorListener(#processVidErr .func.bind([this]))
recordReader .setFrameListener(#processRecData.func.bind([this]))
recordReader .setErrorListener(#processRecErr .func.bind([this]))
cmdSender.connect
navData := navDataReader.connect
if (navData == null)
throw IOErr("Drone did not respond to NavData initialisation")
if (navData.flags.bootstrapMode) {
// send me nav data please!
cmdSender.send(Cmd.makeConfig("general:navdata_demo", "TRUE"))
if (actorPool.isStopped)
throw IOErr("Drone not sending NavData")
try {
NavDataLoop.waitForAckClear (this, networkConfig.configCmdAckClearTimeout, true)
NavDataLoop.waitUntilReady (this, networkConfig.actionTimeout)
} catch (TimeoutErr err)
throw IOErr(err.msg)
// this is a bit of a no-op, not really needed but mentioned in the docs
// see p40, 7.1.2 Initiating the reception of Navigation data
cmdSender.send(Cmd("CTRL", [0]))
}
try droneVersion = BareBonesFtp().readVersion(networkConfig)
catch (Err err)
log.warn("Could not FTP version.txt from drone\n ${err.msg}")
// grab some config
configMapRef.map = controlReader.read
Env.cur.addShutdownHook(shutdownHook)
connectedRef.val = true
log.info("Connected to AR Drone ${droneVersion}")
return this
}
** Disconnects all comms to the drone.
** Performs the 'crashLandOnExit' strategy should the drone still be flying.
**
** This method blocks until it's finished.
This disconnect(Duration timeout := 2sec) {
_doDisconnect(false, timeout)
}
** Returns 'true' if connected to the drone.
Bool isConnected() {
connectedRef.val && !actorPool.isStopped
}
** Returns a read only map of the drone's raw configuration data, as read from the control
** (TCP 5559) port.
**
** All config data is cached, see [configRefresh()]`Drone.configRefresh` obtain fresh data from the
** drone.
Str:Str configMap() {
configMapRef.map
}
** Reloads the 'configMap' with fresh data from the drone.
Void configRefresh() {
if (isConnected)
configMapRef.map = controlReader.read
}
** Returns config for the drone. Note all data is backed by the raw 'configMap'.
DroneConfig config() {
configRef
}
// ---- Video Recording ----
** Instructs the drone to start recording video. Video will be automatically saved to any USB
** drive attached to the drone.
**
** Use the 'VideoStreamer' class to have the drone send the record stream to your computer:
**
** pre>
** syntax: fantom
** drone.config.session("Video Test")
** drone.startRecording
** vs := VideoStreamer.toMp4File(`vid.mp4`).attachToRecordStream(drone)
**
** // ... wait for bit ...
**
** drone.stopRecording
** <pre
**
** Note this is not the same as *streaming live video* - to enable live video just set the 'onVideoFrame' hook.
**
** Can only be called with a non-default session ID. Throws an Err if this is not the case.
@NoDoc
Void startRecording(VideoResolution res := VideoResolution._720p) {
if (isRecording) return
config.session._checkId("start recording")
// MP4_360P_H264_360P_CODEC = 0x82 // Live stream with MPEG4.2 soft encoder. Record stream with 360p H264 hardware encoder.
// MP4_360P_H264_720P_CODEC = 0x88 // Live stream with MPEG4.2 soft encoder. Record stream with 720p H264 hardware encoder.
newCodec := (res == VideoResolution._720p) ? 0x88 : 0x82
oldCodec := configMap["VIDEO:video_codec"].toInt
oldHook := onVideoFrame
// onVideoFrame = null
config.sendConfig("VIDEO:video_codec", newCodec)
oldVideoCodecRef.val = oldCodec
// onVideoFrame = oldHook
}
** Stops the video recording.
**
** Can only be called with a non-default session ID. Throws an Err if this is not the case.
@NoDoc
Void stopRecording() {
if (!isRecording) return
config.session._checkId("stop recording")
oldCodec := oldVideoCodecRef.val
if (oldCodec != null) {
config.sendConfig("VIDEO:video_codec", oldCodec.toStr)
// onVideoFrame = oldVideoCodecRef.val
oldVideoCodecRef.val = null
}
}
** Returns 'true' if video is being recorded.
@NoDoc
Bool isRecording() {
codec := configMap["VIDEO:video_codec"].toInt
return (codec == 0x82 || codec == 0x88) && oldVideoCodecRef.val != null
}
// ---- Misc Commands ----
** (Advanced) Sends the given Cmd to the drone.
** If a second Cmd is given, it is sent in the same UDP data packet.
**
** This method does not block.
Void sendCmd(Cmd cmd, Cmd? cmd2 := null) {
if (!isConnected) return // needed to connect
cmdSender.send(cmd, cmd2)
}
internal Void _sendCmd(Cmd cmd, Cmd? cmd2 := null) {
cmdSender.send(cmd, cmd2)
}
** (Advanced)
** Sends a config cmd to the drone, and blocks until it's been acknowledged.
**
** 'val' may be a Bool, Int, Float, Str, or a List of said types.
**
** For multi-config support, pass in the appropriate IDs.
Void sendConfig(Str key, Obj val, Int? sessionId := null, Int? userId := null, Int? appId := null) {
if (!isConnected) return
// see http://stackoverflow.com/questions/3466452/xor-of-three-values
diff := (sessionId != null ? 1 : 0) + (userId != null ? 1 : 0) + (appId != null ? 1 : 0)
if (diff != 0 && diff != 3)
throw ArgErr("For multi-config support, either ALL IDs must be set or NONE - ${sessionId} : ${userId} : ${appId}")
block := true
NavDataLoop.waitForAckClear (this, networkConfig.configCmdAckClearTimeout, block)
if (sessionId != null)
cmdSender.send(Cmd.makeConfigIds(sessionId, userId, appId), Cmd.makeConfig(key, val))
else
cmdSender.send(Cmd.makeConfig(key, val))
NavDataLoop.waitForAck (this, networkConfig.configCmdAckTimeout, block)
NavDataLoop.waitForAckClear (this, networkConfig.configCmdAckClearTimeout, block)
}
** Clears the emergency landing and the user emergency flags on the drone.
**
** 'timeout' is how long it should wait for an acknowledgement from the drone before throwing
** a 'TimeoutErr'. If 'null' then it defaults to 'NetworkConfig.configCmdAckTimeout'.
Void clearEmergency(Duration? timeout := null) {
if (!isConnected) return
flags := navData?.flags
if (flags?.emergencyLanding == true || flags?.userEmergencyLanding == true) {
cmdSender.send(Cmd.makeEmergency)
NavDataLoop.clearEmergency(this, timeout ?: networkConfig.configCmdAckTimeout)
}
}
** Sets the drone's emergency landing flag which cuts off the motors, causing a crash landing.
**
** 'timeout' is how long it should wait for an acknowledgement from the drone before throwing
** a 'TimeoutErr'. If 'null' then it defaults to 'NetworkConfig.configCmdAckTimeout'.
Void setUserEmergency(Duration? timeout := null) {
if (!isConnected) return
if (navData?.flags?.emergencyLanding == false) {
cmdSender.send(Cmd.makeLand, Cmd.makeEmergency)
NavDataLoop.setEmergency(this, timeout ?: networkConfig.configCmdAckTimeout)
}
}
** Sets a horizontal plane reference for the drone's internal control system.
**
** Call before each flight, while making sure the drone is sitting horizontally on the ground.
** Not doing so will result in the drone not being unstable.
**
** This method does not block.
Void flatTrim() {
if (!isConnected) return
if (flightState != FlightState.def && flightState != FlightState.init && flightState != FlightState.landed) {
log.warn("Can not flat trim when state is ${flightState}")
return
}
cmdSender.send(Cmd.makeFlatTrim)
}
** Tell the drone to calibrate its magnetometer.
**
** The drone calibrates its magnetometer by spinning around itself a few times, hence can
** only be performed when flying.
**
** This method does not block.
Void calibrate(Int deviceNum) {
if (!isConnected) return
if (flightState != FlightState.flying && flightState != FlightState.hovering) {
log.warn("Can not calibrate magnetometer when state is ${flightState}")
return
}
sendCmd(Cmd.makeCalib(deviceNum))
}
** Plays one of the pre-configured LED animation sequences. Example:
**
** syntax: fantom
** drone.animateLeds(LedAnimation.snakeRed, 3sec)
**
** If 'freqInHz' is 'null' then the default frequency of the enum is used.
**
** Corresponds to the 'leds:leds_anim' config cmd.
**
** This method does not block.
Void animateLeds(LedAnimation anim, Duration duration, Float? freqInHz := null) {
if (!isConnected) return
params := [anim.ordinal, (freqInHz ?: anim.defaultFrequency), duration.toSec]
config.sendConfig("leds:leds_anim", params)
}
** Performs one of the pre-configured flight sequences.
**
** syntax: fantom
** drone.animateFlight(FlightAnimation.phiDance)
**
** If duration is 'null' then the default duration of the enum is used.
**
** Corresponds to the 'control:flight_anim' config cmd.
Void animateFlight(FlightAnimation anim, Duration? duration := null, Bool block := true) {
if (!isConnected) return
params := [anim.ordinal, (duration ?: anim.defaultDuration).toMillis]
config.sendConfig("control:flight_anim", params)
if (block)
Actor.sleep(duration ?: anim.defaultDuration)
}
// ---- Stabilising Commands ----
** Repeatedly sends a take off command to the drone until it reports its state as either 'hovering' or 'flying'.
**
** If 'block' is 'true' then this method blocks until a stable hover has been achieved;
** which usually takes ~ 6 seconds.
**
** 'timeout' is how long it should wait for the drone to reach the desired state before
** throwing a 'TimeoutErr'. If 'null' then it defaults to 'NetworkConfig.actionTimeout'.
Void takeOff(Bool block := true, Duration? timeout := null) {
if (!isConnected) return
if (flightState == null || ![FlightState.def, FlightState.init, FlightState.landed].contains(flightState)) {
log.warn("Can not take off - flight state is already ${flightState}")
return
}
NavDataLoop.takeOff(this, block, timeout ?: networkConfig.actionTimeout)
}
** Repeatedly sends a land command to the drone until it reports its state as 'landed'.
**
** If 'block' is 'true' then this method blocks until the drone has landed.
**
** 'timeout' is how long it should wait for the drone to reach the desired state before
** throwing a 'TimeoutErr'. If 'null' then it defaults to 'NetworkConfig.actionTimeout'.
Void land(Bool block := true, Duration? timeout := null) {
if (!isConnected) return
if (flightState == null || [FlightState.def, FlightState.init, FlightState.landed, FlightState.transLanding].contains(flightState)) {
log.warn("Can not land - flight state is already ${flightState}")
return
}
NavDataLoop.land(this, block, timeout ?: networkConfig.actionTimeout)
}
** Repeatedly sends a hover command to the drone until it reports its state as 'hovering'.
**
** If 'block' is 'true' then this method blocks until the drone hovers.
**
** 'timeout' is how long it should wait for the drone to reach the desired state before
** throwing a 'TimeoutErr'. If 'null' then it defaults to 'NetworkConfig.actionTimeout'.
Void hover(Bool block := true, Duration? timeout := null) {
if (!isConnected) return
if (flightState == null || [FlightState.hovering, FlightState.landed, FlightState.transLanding].contains(flightState)) {
log.warn("Can not hover - flight state is already ${flightState}")
return
}
// the best way to prevent this from interfering with an emergency land cmd, is to set the
// emergency flag (to kill off any existing cmds), clear it, then hover
NavDataLoop.hover(this, block, timeout ?: networkConfig.actionTimeout)
}
// ---- Movement Commands ----
** Turns the drone's absolute movement mode on and off.
** Absolute mode is where the drone's left / right / forward / backward movement is fixed to a
** set compass position and is not relative to where the drone is facing.
**
** The compass direction is set when absolute mode is turned on. If you make sure the drone is
** facing away from you when this happens, then it should make for easier drone control.
Bool absoluteMode {
get { absoluteModeRef.val }
set {
absoluteAngleRef.val = it ? (navData.demoData.psi / -360f) : null
absoluteModeRef.val = it
}
}
** How accurate absolute mode is. Should be a number between 0 - 1.
**
** Defaults to '0.1f'.
Float absoluteAccuracy {
get { absoluteAccuracyRef.val }
set { absoluteAccuracyRef.val = it }
}
private Bool combinedYawMode() {
if (!configMap.containsKey("CONTROL:control_level")) return false
return configMap["CONTROL:control_level"].toInt.and(0x02) > 0
}
** A combined move method encapsulating:
** - 'moveRight()'
** - 'moveBackward()'
** - 'moveUp()'
** - 'spinClickwise()'
|->|? move(Float tiltRight, Float tiltBackward, Float verticalSpeed, Float clockwiseSpin, Duration? duration := null, Bool? block := true) {
if (tiltRight < -1f || tiltRight > 1f)
throw ArgErr("tiltRight must be between -1 and 1 : ${tiltRight}")
if (tiltBackward < -1f || tiltBackward > 1f)
throw ArgErr("tiltBackward must be between -1 and 1 : ${tiltBackward}")
if (verticalSpeed < -1f || verticalSpeed > 1f)
throw ArgErr("verticalSpeed must be between -1 and 1 : ${verticalSpeed}")
if (clockwiseSpin < -1f || clockwiseSpin > 1f)
throw ArgErr("clockwiseSpin must be between -1 and 1 : ${clockwiseSpin}")
return doMove(Cmd.makeMove(tiltRight, tiltBackward, verticalSpeed, clockwiseSpin, combinedYawMode, absoluteMode, absoluteAngleRef.val, absoluteAccuracyRef.val), tiltRight, duration, block)
}
** Moves the drone vertically upwards.
**
** 'verticalSpeed' is a percentage of the maximum vertical speed and should be a value between -1 and 1.
** A positive value makes the drone rise in the air, a negative value makes it go down.
**
** 'duration' is how long the drone should move for, during which the move command is resent every 'config.cmdInterval'.
** Movement is cancelled if 'land()' is called or an emergency flag is set.
**
** Should 'block' be 'false', this method returns immediately and movement commands are sent in the background.
** Call the returned function to cancel movement before the 'duration' interval is reached.
** Calling the function after 'duration' does nothing.
**
** pre>
** syntax: fantom
** // move the drone for 5secs
** cancel := drone.moveUp(0.5f, 5sec, false)
**
** // wait a bit
** Actor.sleep(2sec)
**
** // cancel the move prematurely
** cancel()
** <pre
**
** If 'duration' is 'null' then the movement command is sent just the once.
**
** See config commands 'CONTROL:altitude_max', 'CONTROL:control_vz_max'.
|->|? moveUp(Float verticalSpeed, Duration? duration := null, Bool? block := true) {
doMove(Cmd.makeMove(0f, 0f, verticalSpeed, 0f, combinedYawMode, absoluteMode, absoluteAngleRef.val, absoluteAccuracyRef.val), verticalSpeed, duration, block)
}
** Moves the drone vertically downwards.
**
** 'verticalSpeed' is a percentage of the maximum vertical speed and should be a value between -1 and 1.
** A positive value makes the drone descend in the air, a negative value makes it go up.
**
** 'duration' is how long the drone should move for, during which the move command is resent every 'config.cmdInterval'.
** Movement is cancelled if 'land()' is called or an emergency flag is set.
**
** Should 'block' be 'false', this method returns immediately and movement commands are sent in the background.
** Call the returned function to cancel movement before the 'duration' interval is reached.
** Calling the function after 'duration' does nothing.
**
** pre>
** syntax: fantom
** // move the drone for 5secs
** cancel := drone.moveDown(0.5f, 5sec, false)
**
** // wait a bit
** Actor.sleep(2sec)
**
** // cancel the move prematurely
** cancel()
** <pre
**
** If 'duration' is 'null' then the movement command is sent just the once.
**
** See config commands 'CONTROL:altitude_min', 'CONTROL:control_vz_max'.
|->|? moveDown(Float verticalSpeed, Duration? duration := null, Bool? block := true) {
doMove(Cmd.makeMove(0f, 0f, -verticalSpeed, 0f, combinedYawMode, absoluteMode, absoluteAngleRef.val, absoluteAccuracyRef.val), verticalSpeed, duration, block)
}
** Moves the drone to the left.
**
** 'tilt' (aka *roll* or *phi*) is a percentage of the maximum inclination and should be a value between -1 and 1.
** A positive value makes the drone tilt to its left, thus flying leftward. A negative value makes the drone tilt to its right.
**
** 'duration' is how long the drone should move for, during which the move command is resent every 'config.cmdInterval'.
** Movement is cancelled if 'land()' is called or an emergency flag is set.
**
** Should 'block' be 'false', this method returns immediately and movement commands are sent in the background.
** Call the returned function to cancel movement before the 'duration' interval is reached.
** Calling the function after 'duration' does nothing.
**
** pre>
** syntax: fantom
** // move the drone for 5secs
** cancel := drone.moveLeft(0.5f, 5sec, false)
**
** // wait a bit
** Actor.sleep(2sec)
**
** // cancel the move prematurely
** cancel()
** <pre
**
** If 'duration' is 'null' then the movement command is sent just the once.
**
** See config command 'CONTROL:euler_angle_max'.
|->|? moveLeft(Float tilt, Duration? duration := null, Bool? block := true) {
doMove(Cmd.makeMove(-tilt, 0f, 0f, 0f, combinedYawMode, absoluteMode, absoluteAngleRef.val, absoluteAccuracyRef.val), tilt, duration, block)
}
** Moves the drone to the right.
**
** The 'tilt' (aka *roll* or *phi*) is a percentage of the maximum inclination and should be a value between -1 and 1.
** A positive value makes the drone tilt to its right, thus flying right. A negative value makes the drone tilt to its left.
**
** 'duration' is how long the drone should move for, during which the move command is resent every 'config.cmdInterval'.
** Movement is cancelled if 'land()' is called or an emergency flag is set.
**
** Should 'block' be 'false', this method returns immediately and movement commands are sent in the background.
** Call the returned function to cancel movement before the 'duration' interval is reached.
** Calling the function after 'duration' does nothing.
**
** pre>
** syntax: fantom
** // move the drone for 5secs
** cancel := drone.moveRight(0.5f, 5sec, false)
**
** // wait a bit
** Actor.sleep(2sec)
**
** // cancel the move prematurely
** cancel()
** <pre
**
** If 'duration' is 'null' then the movement command is sent just the once.
**
** See config command 'CONTROL:euler_angle_max'.
|->|? moveRight(Float tilt, Duration? duration := null, Bool? block := true) {
doMove(Cmd.makeMove(tilt, 0f, 0f, 0f, combinedYawMode, absoluteMode, absoluteAngleRef.val, absoluteAccuracyRef.val), tilt, duration, block)
}
** Moves the drone forward.
**
** The 'tilt' (aka *pitch* or *theta*) is a percentage of the maximum inclination and should be a value between -1 and 1.
** A positive value makes the drone drop its nose, thus flying forward. A negative value makes the drone tilt back.
**
** 'duration' is how long the drone should move for, during which the move command is resent every 'config.cmdInterval'.
** Movement is cancelled if 'land()' is called or an emergency flag is set.
**
** Should 'block' be 'false', this method returns immediately and movement commands are sent in the background.
** Call the returned function to cancel movement before the 'duration' interval is reached.
** Calling the function after 'duration' does nothing.
**
** pre>
** syntax: fantom
** // move the drone for 5secs
** cancel := drone.moveForward(0.5f, 5sec, false)
**
** // wait a bit
** Actor.sleep(2sec)
**
** // cancel the move prematurely
** cancel()
** <pre
**
** If 'duration' is 'null' then the movement command is sent just the once.
**
** See config command 'CONTROL:euler_angle_max'.
|->|? moveForward(Float tilt, Duration? duration := null, Bool? block := true) {
doMove(Cmd.makeMove(0f, -tilt, 0f, 0f, combinedYawMode, absoluteMode, absoluteAngleRef.val, absoluteAccuracyRef.val), tilt, duration, block)
}
** Moves the drone backward.
**
** The 'tilt' (aka *pitch* or *theta*) is a percentage of the maximum inclination and should be a value between -1 and 1.
** A positive value makes the drone raise its nose, thus flying forward. A negative value makes the drone tilt forward.
**
** 'duration' is how long the drone should move for, during which the move command is resent every 'config.cmdInterval'.
** Movement is cancelled if 'land()' is called or an emergency flag is set.
**
** Should 'block' be 'false', this method returns immediately and movement commands are sent in the background.
** Call the returned function to cancel movement before the 'duration' interval is reached.
** Calling the function after 'duration' does nothing.
**
** pre>
** syntax: fantom
** // move the drone for 5secs
** cancel := drone.moveBackward(0.5f, 5sec, false)
**
** // wait a bit
** Actor.sleep(2sec)
**
** // cancel the move prematurely
** cancel()
** <pre
**
** If 'duration' is 'null' then the movement command is sent just the once.
**
** See config command 'CONTROL:euler_angle_max'.
|->|? moveBackward(Float tilt, Duration? duration := null, Bool? block := true) {
doMove(Cmd.makeMove(0f, -tilt, 0f, 0f, combinedYawMode, absoluteMode, absoluteAngleRef.val, absoluteAccuracyRef.val), tilt, duration, block)
}
** Spins the drone clockwise.
**
** The 'angularSpeed' (aka *yaw*) is a percentage of the maximum angular speed and should be a value between -1 and 1.
** A positive value makes the drone spin clockwise; a negative value makes it spin anti-clockwise.
**
** 'duration' is how long the drone should move for, during which the move command is resent every 'config.cmdInterval'.
** Movement is cancelled if 'land()' is called or an emergency flag is set.
**
** Should 'block' be 'false', this method returns immediately and movement commands are sent in the background.
** Call the returned function to cancel movement before the 'duration' interval is reached.
** Calling the function after 'duration' does nothing.
**
** pre>
** syntax: fantom
** // move the drone for 5secs
** cancel := drone.spinClockwise(0.5f, 5sec, false)
**
** // wait a bit
** Actor.sleep(2sec)
**
** // cancel the move prematurely
** cancel()
** <pre
**
** If 'duration' is 'null' then the movement command is sent just the once.
**
** See config command 'CONTROL:control_yaw'.
|->|? spinClockwise(Float angularSpeed, Duration? duration := null, Bool? block := true) {
doMove(Cmd.makeMove(0f, 0f, 0f, angularSpeed, combinedYawMode, absoluteMode, absoluteAngleRef.val, absoluteAccuracyRef.val), angularSpeed, duration, block)
}
** Spins the drone anti-clockwise.
**
** The 'angularSpeed' (aka *yaw*) is a percentage of the maximum angular speed and should be a value between -1 and 1.
** A positive value makes the drone spin anti-clockwise; a negative value makes it spin clockwise.
**
** 'duration' is how long the drone should move for, during which the move command is resent every 'config.cmdInterval'.
** Movement is cancelled if 'land()' is called or an emergency flag is set.
**
** Should 'block' be 'false', this method returns immediately and movement commands are sent in the background.
** Call the returned function to cancel movement before the 'duration' interval is reached.
** Calling the function after 'duration' does nothing.
**
** pre>
** syntax: fantom
** // move the drone for 5secs
** cancel := drone.spinAntiClockwise(0.5f, 5sec, false)
**
** // wait a bit
** Actor.sleep(2sec)
**
** // cancel the move prematurely
** cancel()
** <pre
**
** If 'duration' is 'null' then the movement command is sent just the once.
**
** See config command 'CONTROL:control_yaw'.
|->|? spinAntiClockwise(Float angularSpeed, Duration? duration := null, Bool? block := true) {
doMove(Cmd.makeMove(0f, 0f, 0f, -angularSpeed, combinedYawMode, absoluteMode, absoluteAngleRef.val, absoluteAccuracyRef.val), angularSpeed, duration, block)
}
// ---- Private Stuff ----
internal This _doDisconnect(Bool abnormal, Duration timeout := 2sec) {
Env.cur.removeShutdownHook(shutdownHook)
if (actorPool.isStopped) return this
shutdownHook.call()
// set connectedRef AFTER we've called the shutdown hook
connectedRef.val = false
videoReader.disconnect
recordReader.disconnect
navDataReader.disconnect
cmdSender.disconnect
// call handlers from a different thread so they don't block
eventThread.async |->| {
callSafe(onDisconnect, [abnormal, this])
}
actorPool.stop
if (!abnormal) // don't block internal threads
actorPool.join(timeout)
log.info("Disconnected from Drone")
return this
}
internal Void _addNavDataListener(|NavData| f) {
navDataReader.addListener(f)
}
internal Void _removeNavDataListener(|NavData| f) {
navDataReader.removeListener(f)
}
internal Void _updateConfig(Str key, Obj val) {
// selectively update config (i.e. MY code!) 'cos we don't trust the user not to add random shite!
if (configMapRef.containsKey(key))
configMapRef[key] = Cmd.encodeConfigParams(val)
}
private Void processNavData(NavData navData) {
if (actorPool.isStopped) return
oldNavData := (NavData?) navDataRef.val
// we don't need a mutex 'cos we're only ever called from the single-threaded NavDataReader actor
newOpts := oldNavData?._lazyOpts?.rw ?: NavOption:LazyNavOptData[:]
newOpts.setAll(navData._lazyOpts)
newNav := NavData {
it.flags = navData.flags
it.seqNum = navData.seqNum
it.visionFlag = navData.visionFlag
it._lazyOpts = newOpts
}
navDataRef.val = newNav
// ---- perform standard feedback ----
// hey! I'm still here!
if (navData.flags.comWatchdogProblem)
cmdSender.send(Cmd.makeKeepAlive)
// ---- call event handlers ----
// call handlers from a different thread so they don't block the NavDataReader
eventThread.async |->| {
callSafe(onNavData, [navData, this])
if (navData.flags.emergencyLanding && oldNavData?.flags?.emergencyLanding != true && oldNavData?.flags?.flying == true)
callSafe(onEmergency, [this])
if (navData.flags.batteryTooLow && oldNavData?.flags?.batteryTooLow != true )
callSafe(onBatteryLow, [this])
demoData := navData.demoData
if (demoData != null) {
if (demoData.flightState != oldNavData?.demoData?.flightState)
callSafe(onStateChange, [demoData.flightState, this])
if (oldNavData?.demoData?.batteryPercentage == null || demoData.batteryPercentage < oldNavData.demoData.batteryPercentage)
// sometimes the percentage jumps up a bit, then drains backdown: 46% -> 47% -> 46% so we get 2 x battery events @ 46%
callSafe(onBatteryDrain, [demoData.batteryPercentage, this])
}
}
}
private Void processVidData(Buf payload, PaveHeader pave) {
if (actorPool.isStopped) return
// ---- call event handlers ----
// call handlers from a different thread so they don't block the VidReader
// should I use a different thread for vid data?
eventThread.async |->| {
callSafe(onVideoFrame, [payload, pave, this])
}
}
private Void processVidErr(Err err) {
if (actorPool.isStopped) return
// ---- call event handlers ----
// call handlers from a different thread so they don't block the VidReader
// should I use a different thread for vid data?
if (onVideoErr == null)
log.err("Could not decode Video data on port $networkConfig.videoPort", err)
else
eventThread.async |->| {
callSafe(onVideoErr, [err, this])
}
}
private Void processRecData(Buf payload, PaveHeader pave) {
if (actorPool.isStopped) return
// ---- call event handlers ----
// call handlers from a different thread so they don't block the VidReader
// should I use a different thread for vid data?
eventThread.async |->| {
callSafe(onRecordFrame, [payload, pave, this])
}
}
private Void processRecErr(Err err) {
if (actorPool.isStopped) return
// ---- call event handlers ----
// call handlers from a different thread so they don't block the VidReader
// should I use a different thread for vid data?
if (onRecordErr == null)
log.err("Could not decode Video data on port $networkConfig.videoRecPort", err)
else
eventThread.async |->| {
callSafe(onRecordErr, [err, this])
}
}
private Void onShutdown() {
if (navData?.flags?.flying == true || (flightState != null && flightState != FlightState.landed && flightState != FlightState.def)) {
switch (exitStrategy) {
case ExitStrategy.nothing:
log.warn("Enforcing Exit Strategy --> Doing Nothing!")
case ExitStrategy.hover:
log.warn("Enforcing Exit Strategy --> Hovering Drone")
hover(false)
case ExitStrategy.land:
log.warn("Enforcing Exit Strategy --> Landing Drone")
land(false)
case ExitStrategy.crashLand:
log.warn("Enforcing Exit Strategy --> Crash Landing Drone")
setUserEmergency
default:
throw Err("WTF is a '${exitStrategy}' exit strategy ???")
}
}
}
private |->|? doMove(Cmd cmd, Float speed, Duration? duration, Bool? block) {
if (!isConnected) return null
if (speed < -1f || speed > 1f)
throw ArgErr("Speed must be between -1 and 1 : ${speed}")
if (duration == null) {
cmdSender.send(cmd)
return null
}
future := TimedLoop(this, duration, cmd).future
if (block) {
future.get
return null
}
return |->| {
if (!future.state.isComplete)
try { future.cancel } catch { /* meh - race condition */ }
}
}
private Void callSafe(Func? f, Obj[]? args) {
try f?.callList(args)
catch (Err err)
err.trace
}
@NoDoc
override Str toStr() {
(configMapRef["GENERAL:ardrone_name"] ?: "AR Drone").toStr + " v${droneVersion}"
}
}
** Pre-configured LED animation sequences.
enum class LedAnimation {
** Alternates all LEDs between red and green.
blinkGreenRed(2f),
** Alternates all LEDs between green and off.
blinkGreen(2f),
** Alternates all LEDs between red and off.
blinkRed(2f),
** Alternates all LEDs between orange (red and green at the same time) and off.
blinkOrange(2f),
** Rotates the green and red LEDs around the drone.
snakeGreenRed(0.75f),
** Flashes the front LEDs orange (red and green at the same time).
fire(9f),
** Turns the front LEDs green and the rear LEDs red.
standard(1f),
** Turns all LEDs red.
red(1f),
** Turns all LEDs green.
green(1f),
** Rotates the red LED around the drone.
snakeRed(1.5f),
** Turns all LEDs off.
off(1f),
** Flashes the front LED orange and the rear LED red - right side only.
rightMissile(6f),
** Flashes the front LED orange and the rear LED red - left side only.
leftMissile(6f),
** Flashes the front LEDs orange and the rear LEDs red - both sides.
doubleMissile(6f),
** Turns the front left LED green and the others red.
frontLeftGreenOthersRed(1f),
** Turns the front right LED green and the others red.
frontRightGreenOthersRed(1f),
** Turns the rear right LED green and the others red.
rearRightGreenOthersRed(1f),
** Turns the rear left LED green and the others red.
rearLeftGreenOthersRed(1f),
** Turns the left LEDs green and the right LEDs red.
leftGreenRightRed(1f),
** Turns the left LEDs red and the right LEDs green.
leftRedRightGreen(1f),
** Alternates all LEDs between standard (front LEDs green and rear LEDs red) and off.
blinkStandard(2f);
** A default frequency for the animation.
const Float defaultFrequency
private new make(Float defaultFrequency) {
this.defaultFrequency = defaultFrequency
}
}
** Pre-configured flight paths.
enum class FlightAnimation {
phiM30Deg(1sec), phi30Deg(1sec), thetaM30Deg(1sec), theta30Deg(1sec), theta20degYaw200Deg(1sec), theta20degYawM200Deg(1sec), turnaround(5sec), turnaroundGoDown(5sec), yawShake(2sec), yawDance(5sec), phiDance(5sec), thetaDance(5sec), vzDance(5sec), wave(5sec), phiThetaMixed(5sec), doublePhiThetaMixed(5sec), flipForward(15ms), flipBackward(15ms), flipLeft(15ms), flipRight(15ms);
** How long the manoeuvre should take.
const Duration defaultDuration
private new make(Duration defaultDuration) {
this.defaultDuration = defaultDuration
}
}
** Governs what the drone should do if the program exists whilst flying.
** Default is 'land'.
enum class ExitStrategy {
** Do nothing, let the drone continue doing whatever it was last told to do.
nothing,
** Sends a 'stop()' command.
hover,
** Sends a 'land()' command.
land,
** Cuts the drone's engines, forcing a crash landing.
crashLand;
}
