Skip to content
This repository has been archived by the owner on Jun 22, 2022. It is now read-only.

Commit

Permalink
Add drone events
Browse files Browse the repository at this point in the history
  • Loading branch information
@baptadn
baptadn committed Aug 19, 2013
1 parent 793dafa commit bb94c42
Show file tree
Hide file tree
Showing 9 changed files with 219 additions and 93 deletions.
25 changes: 19 additions & 6 deletions examples/control.php
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,22 +4,35 @@

$client = new \jolicode\PhpARDrone\Client();

$client->on('navdata', function($frame) {
// do something with navdata (ie log);
$client->on('lowBattery', function($frame) {
// * Houston, we've got a problem *
});

$client->on('hovering', function() {
// lala
});

$client->on('landed', function() {
// loulou
});

$client->takeoff();

$client
->after('5', function() use ($client) {
->after('7', function() use ($client) {
$client->clockwise(1);
})
->after('5', function() use ($client) {
->after('3', function() use ($client) {
$client->counterClockwise(0.5);
})
->after('5', function() use ($client) {
->after('7', function() use ($client) {
$client->stop();
$client->land();

// Landing with callback function
$client->land(function() {
echo 'Nous sommes arrivés à destination, la température extérieure est de 28°C.
Merci d\'avoir choisi AR Drone Company.';
});
});

$client->start();
6 changes: 5 additions & 1 deletion examples/repl.php
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,10 @@

$client = new \jolicode\PhpARDrone\Client();

$client->createRepl();
$client->on('navdata', function($frame) {
echo $frame;
});

//$client->createRepl();

$client->start();
16 changes: 15 additions & 1 deletion src/jolicode/PhpARDrone/Buffer/Buffer.php
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,6 @@ public function getUint32LE()
{
$value = unpack('V/', substr($this->data, $this->offset, ($this->offset + 4)));
$this->moveOffset(4);

return dechex($value[1]);
}

Expand Down Expand Up @@ -52,6 +51,10 @@ public function getInt32()
return dechex($value[1]);
}

public function getMask32($masks)
{
return $this->mask($masks, $this->getUint32LE());
}

public function getVector31() {
return array(
Expand Down Expand Up @@ -88,6 +91,17 @@ private function moveOffset($step)
$this->offset = $this->offset + $step;
}

//todo: move this function ?
private function mask($masks, $value)
{
$flags = array();

foreach($masks as $name => $mask) {
$flags[$name] = (hexdec($value) & ($mask)) ? 1 : 0;
}

return $flags;
}
/**
* @return mixed
*/
Expand Down
99 changes: 88 additions & 11 deletions src/jolicode/PhpARDrone/Client.php
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@

use Evenement\EventEmitter;
use jolicode\PhpARDrone\Control\UdpControl;
use jolicode\PhpARDrone\Navdata\Frame;
use jolicode\PhpARDrone\Navdata\UdpNavdata;
use React\EventLoop\Factory AS LoopFactory;
use Datagram\Factory AS UdpFactory;
Expand All @@ -14,31 +15,98 @@ class Client extends EventEmitter {
private $udpControl;
private $udpNavdata;
private $timerOffset;
public $disableEmergency;
public $lastState;
public $lastBattery;
public $lastAltitude;
private $loop;

public function __construct()
{
$this->loop = LoopFactory::create();
$this->loop = LoopFactory::create();

$udpFactory = new UdpFactory($this->loop);
$this->udpFactory = $udpFactory;
$this->socket = null;
$this->timerOffset = 0;
$udpFactory = new UdpFactory($this->loop);
$this->udpFactory = $udpFactory;
$this->socket = null;
$this->timerOffset = 0;
$this->lastState = 'CTRL_LANDED';
$this->lastBattery = 100;
$this->lastAltitude = 0;
$this->disableEmergency = false;

$this->startUdpNavdata();
$this->startUdpControl();
}

private function startUdpNavdata()
public function startUdpNavdata()
{
$this->udpNavdata = new UdpNavdata($this->loop);
$that = $this;
$self = $this;

$this->udpNavdata->on('navdata', function($navdata) use ($that) {
$that->emit('navdata', array($navdata));
$this->udpNavdata->on('navdata', function(Frame $navdata) use (&$self) {

if (count($navdata->getDroneState()) > 0) {
$stateData = $navdata->getDroneState();
if ($stateData['emergencyLanding'] && $self->disableEmergency) {
// this._ref.emergency = true;
} else {
// this._ref.emergency = false;
// this._disableEmergency = false;
}
}

$options = $navdata->getOptions();

if (count($navdata->getDroneState()) > 0 && isset($options['demo'])) {
// Control drone state
$optionDemo = $options['demo'];
$demoData = $optionDemo->getData();

$currentState = $demoData['controlState'];

$self->emitState('landing', 'CTRL_TRANS_LANDING', $currentState);
$self->emitState('landed', 'CTRL_LANDED', $currentState);
$self->emitState('takeoff', 'CTRL_TRANS_TAKEOFF', $currentState);
$self->emitState('hovering', 'CTRL_HOVERING', $currentState);
$self->emitState('flying', 'CTRL_FLYING', $currentState);
$self->lastState = $currentState;

$battery = $demoData['batteryPercentage'];


// battery events
$stateData = $navdata->getDroneState();

if ($stateData['lowBattery'] === 1) {
$self->emit('lowBattery', array($battery));
}

if ($battery !== $self->lastBattery) {
$self->emit('batteryChange', array($battery));
$self->lastBattery = $battery;
}

// altitude events
$altitude = $demoData['altitudeMeters'];

if ($altitude !== $self->lastAltitude) {
$self->emit('altitudeChange', array($altitude));
$self->lastAltitude = $altitude;
}
}

$self->emit('navdata', array($navdata));
});
}

public function emitState($e, $state, $currentState)
{
echo $currentState.PHP_EOL;
if ($currentState === $state && $this->lastState !== $state) {
$this->emit($e, array());
}
}

private function startUdpControl()
{
$this->udpControl = new UdpControl($this->loop);
Expand All @@ -52,7 +120,7 @@ public function createRepl()
$udpControl = $this->udpControl;

$repl->on('action', function($action) use (&$udpControl) {
$udpControl->emit($action);
$udpControl->emit(array($action));
});
}

Expand Down Expand Up @@ -88,8 +156,17 @@ public function start()
public function __call($name, $arguments)
{
if(in_array($name, Config::$commands)) {
if ($name === 'takeoff' || $name === 'land' || $name === 'stop') {
if ($name === 'takeoff' || $name === 'land') {
// process callback function
$callback = (count($arguments) == 1) ? $arguments[0] : function() {};
$eventName = ($name === 'takeoff') ? 'hovering' : 'landed';

$this->once($eventName, $callback);

$this->udpControl->emit($name);
} else if ($name === 'stop') {
$this->udpControl->emit($name);
// Control commands
} else {
if (count($arguments) > 1) {
new \Exception('There are too many arguments');
Expand Down
2 changes: 2 additions & 0 deletions src/jolicode/PhpARDrone/Config/Config.php
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,4 +22,6 @@ class Config {
'stop',
'exit',
);


}
6 changes: 4 additions & 2 deletions src/jolicode/PhpARDrone/Control/UdpControl.php
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,11 +40,13 @@ private function start()
$ref = $udpControl->ref;
$pcmd = $udpControl->pcmd;

for($j = 0; $j < 30; $j++) {
for($j = 0; $j < 5; $j++) {
$command = $commandCreator->createConfigCommand('general:navdata_demo', 'TRUE');
$client->send($command);
}

// According to tests, a satisfying control of the AR.Drone 2.0 is reached
// by sending the AT-commands every 30 ms for smooth drone movements.
$loop->addPeriodicTimer(0.03, function() use ($client, $commandCreator, &$ref, &$pcmd) {
$cmds = array();

Expand Down Expand Up @@ -76,7 +78,7 @@ private function start()
unset($pcmd['clockwise']);
});

$udpControl->on('stop', function($speed) use (&$pcmd) {
$udpControl->on('stop', function() use (&$pcmd) {
$pcmd = array();
});

Expand Down
64 changes: 48 additions & 16 deletions src/jolicode/PhpARDrone/Navdata/Frame.php
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,9 +14,46 @@ class Frame {
private $sequenceNumber;
private $visionFlag;
private $options;
private $droneStateMasks;

public function __construct($binaryFrame)
{
// from ARDrone_SDK_2_0/ARDroneLib/Soft/Common/config.h
$this->droneStateMasks = array(
'flying' => (1 << 0), /*!< FLY MASK => (0) ardrone is landed, (1) ardrone is flying */
'videoEnabled' => (1 << 1), /*!< VIDEO MASK => (0) video disable, (1) video enable */
'visionEnabled' => (1 << 2), /*!< VISION MASK => (0) vision disable, (1) vision enable */
'controlAlgorithm' => (1 << 3), /*!< CONTROL ALGO => (0) euler angles control, (1) angular speed control */
'altitudeControlAlgorithm' => (1 << 4), /*!< ALTITUDE CONTROL ALGO => (0) altitude control inactive (1) altitude control active */
'startButtonState' => (1 << 5), /*!< USER feedback => Start button state */
'controlCommandAck' => (1 << 6), /*!< Control command ACK => (0) None, (1) one received */
'cameraReady' => (1 << 7), /*!< CAMERA MASK => (0) camera not ready, (1) Camera ready */
'travellingEnabled' => (1 << 8), /*!< Travelling mask => (0) disable, (1) enable */
'usbReady' => (1 << 9), /*!< USB key => (0) usb key not ready, (1) usb key ready */
'navdataDemo' => (1 << 10), /*!< Navdata demo => (0) All navdata, (1) only navdata demo */
'navdataBootstrap' => (1 << 11), /*!< Navdata bootstrap => (0) options sent in all or demo mode, (1) no navdata options sent */
'motorProblem' => (1 << 12), /*!< Motors status => (0) Ok, (1) Motors problem */
'communicationLost' => (1 << 13), /*!< Communication Lost => (1) com problem, (0) Com is ok */
'softwareFault' => (1 << 14), /*!< Software fault detected - user should land as quick as possible (1) */
'lowBattery' => (1 << 15), /*!< VBat low => (1) too low, (0) Ok */
'userEmergencyLanding' => (1 << 16), /*!< User Emergency Landing => (1) User EL is ON, (0) User EL is OFF*/
'timerElapsed' => (1 << 17), /*!< Timer elapsed => (1) elapsed, (0) not elapsed */
'MagnometerNeedsCalibration' => (1 << 18), /*!< Magnetometer calibration state => (0) Ok, no calibration needed, (1) not ok, calibration needed */
'anglesOutOfRange' => (1 << 19), /*!< Angles => (0) Ok, (1) out of range */
'tooMuchWind' => (1 << 20), /*!< WIND MASK=> (0) ok, (1) Too much wind */
'ultrasonicSensorDeaf' => (1 << 21), /*!< Ultrasonic sensor => (0) Ok, (1) deaf */
'cutoutDetected' => (1 << 22), /*!< Cutout system detection => (0) Not detected, (1) detected */
'picVersionNumberOk' => (1 << 23), /*!< PIC Version number OK => (0) a bad version number, (1) version number is OK */
'atCodecThreadOn' => (1 << 24), /*!< ATCodec thread ON => (0) thread OFF (1) thread ON */
'navdataThreadOn' => (1 << 25), /*!< Navdata thread ON => (0) thread OFF (1) thread ON */
'videoThreadOn' => (1 << 26), /*!< Video thread ON => (0) thread OFF (1) thread ON */
'acquisitionThreadOn' => (1 << 27), /*!< Acquisition thread ON => (0) thread OFF (1) thread ON */
'controlWatchdogDelay' => (1 << 28), /*!< CTRL watchdog => (1) delay in control execution (> 5ms), (0) control is well scheduled */
'adcWatchdogDelay' => (1 << 29), /*!< ADC Watchdog => (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */
'comWatchdogProblem' => (1 << 30), /*!< Communication Watchdog => (1) com problem, (0) Com is ok */
'emergencyLanding' => (1 << 31) /*!< Emergency landing : (0) no emergency, (1) emergency */
);

$this->buffer = new Buffer($binaryFrame);
$this->options = array();

Expand All @@ -36,7 +73,7 @@ public function __construct($binaryFrame)
private function getFrameConfig()
{
// Get drone state
$this->droneState = $this->buffer->getUint32LE();
$this->droneState = $this->buffer->getMask32($this->droneStateMasks);

// Get sequence number
$this->sequenceNumber = $this->buffer->getUint32LE();
Expand All @@ -49,15 +86,17 @@ private function getFrameConfig()
public function getFrameOptions()
{
$isChecksum = false;
$i = 0;

while(!$isChecksum) {
$idOption = $this->buffer->getUint16LE();
$nameOption = Option::$optionIds[hexdec($idOption)];
$idOption = hexdec($this->buffer->getUint16LE());
$nameOption = Option::$optionIds[$idOption];
$sizeOption = $this->buffer->getUint16LE();

if ($nameOption === 'checksum') {
$isChecksum = true;
$expectedChecksum = 0;
$checksum = $this->buffer->getUint32LE();


$data = $this->buffer->getData();

Expand All @@ -66,30 +105,23 @@ public function getFrameOptions()
}
$expectedChecksum = dechex($expectedChecksum);

$checksum = $this->buffer->getUint32LE();

if ($checksum !== $expectedChecksum) {
throw new \Exception('Invalid checksum');
}
$option = new Option($idOption, $this->buffer);

} else {
$i++;
if ($nameOption !== 'demo' && $nameOption !== 'pwm' && $nameOption !== 'physMeasures' && $nameOption !== 'visionDetect') {
echo $nameOption; die();
}
// Debug demo case
$option = new Option($idOption, $this->buffer);
}

$option = new Option($idOption, $this->buffer);

array_push($this->options, $option);
$this->options[$option->getName()] = $option;
}
}

private function checkHeaderIntegrity()
{
return true;
// return($this->header == Util::NAVDATA_HEADER);
// return ($this->header === 55667788);
}

public function getHeader()
Expand Down Expand Up @@ -125,7 +157,7 @@ function __toString()
$toString = '';

$toString .= 'HEADER: ' . $this->getHeader() . PHP_EOL;
$toString .= 'DRONE STATE: ' . $this->getDroneState() . PHP_EOL;
$toString .= 'DRONE STATE: ' . print_r($this->getDroneState()) . PHP_EOL;
$toString .= 'SEQUENCE NUMBER: ' . $this->getSequenceNumber() . PHP_EOL;
$toString .= 'VISION FLAG: ' . $this->getVisionFlag() . PHP_EOL;

Expand Down
Loading

0 comments on commit bb94c42

Please sign in to comment.