AR Drone control system implementation. Developed with data from http://parrotcontact.emencia.net/website/user-guides/download-user-guides.php?pdf=ar-drone-2/AR-Drone-2_User-guide_Android_UK.pdf See final report for diagram of implementation. More...

Inheritance diagram for ArDrone:

[legend]

Collaboration diagram for ArDrone:

[legend]

Public Attributes
float	maxLateralSpeed
	The maximum lateral speed allowed by the control system. More...

Pid	lateralSpeedController
	The lateral speed controller. More...

Pid	tiltController
	The tilt controller. More...

float	verticalSpeed
	The maximum speed that the vertical target location can change. More...

float	maxThrottle
	The max throttle of each propeller force. More...

Pid	throttleController
	The throttle controller. More...

float	maxTilt
	The maximum roll and pitch permitted by the control system. (measured in degrees) More...

bool	rotateYaw
	If `true` then AR Drone will rotate to face the command direction vector. More...

float	maxYawVelocity
	The max yaw rotational velocity. (measured in degrees per second) More...

float	yawKp
	Gain parameter in controlling the yaw torque. More...

Pid	rollController
	The roll controller. More...

Pid	pitchController
	The pitch controller. More...

Transform	FL
	Reference to the front-left propeller position. More...

Transform	FR
	Reference to the front-right propeller position. More...

Transform	BL
	Reference to the back-left propeller position. More...

Transform	BR
	Reference to the back-right propeller position. More...

Private Member Functions
void	Awake ()
	Awake this instance. More...

void	Start ()
	Start this instance. More...

void	Reset ()
	Reset the PID controllers. More...

void	Update ()

void	FixedUpdate ()

Private Attributes
float	_actualPitch

float	_actualRoll

float	_targetSpeed

float	_targetTilt

float	_targetRoll

float	_targetPitch

float	_targetHeight

Robot	_robot

Vector3	_command

Vector3	_target2d

Vector3	_holdRotation

Vector3	_holdPosition

Vector3	_controlTorque

Vector3	_flControlThrust

Vector3	_frControlThrust

Vector3	_blControlThrust

Vector3	_brControlThrust

Detailed Description

AR Drone control system implementation. Developed with data from http://parrotcontact.emencia.net/website/user-guides/download-user-guides.php?pdf=ar-drone-2/AR-Drone-2_User-guide_Android_UK.pdf See final report for diagram of implementation.

Definition at line 10 of file ArDrone.cs.

Member Function Documentation

void ArDrone.Awake ( )

private

Awake this instance.

Definition at line 122 of file ArDrone.cs.

                  {
         _robot = GetComponent<Robot>();
     }

void ArDrone.FixedUpdate ( )

private

Definition at line 223 of file ArDrone.cs.

                        {
         if(!_robot.moveEnabled)  return;
         
         float displacement2d = _target2d.magnitude;
 
         _targetSpeed = lateralSpeedController.output(0f, - displacement2d);
         _targetSpeed = Mathf.Clamp(_targetSpeed, 0f, maxLateralSpeed);
         _targetTilt = tiltController.output(_targetSpeed, rigidbody.velocity.magnitude);
         _targetTilt = Mathf.Clamp(_targetTilt, 0f, maxTilt);
         
         Vector3 forward2d = new Vector3(transform.forward.x, 0f, transform.forward.z).normalized;
         Vector3 right2d = new Vector3(transform.right.x, 0f, transform.right.z).normalized;
         
         // PITCH
         float forwardAxisAngle = Vector3.Angle(forward2d, _target2d);
         float forwardCosine = Mathf.Cos(forwardAxisAngle * Mathf.Deg2Rad);
         _actualPitch = Vector3.Angle(transform.forward, Vector3.up) - 90f; // avoids the 0-360 boundary
         _targetPitch = forwardCosine*_targetTilt;
         
         // ROLL
         float rightAxisAngle = Vector3.Angle(right2d, _target2d);
         float rightCosine = Mathf.Cos(rightAxisAngle * Mathf.Deg2Rad);
         _actualRoll = Vector3.Angle(transform.right, Vector3.up) - 90f; // avoids the 0-360 boundary
         _targetRoll = rightCosine*_targetTilt;
         
         
         float pitchOutput = pitchController.output(_targetPitch, _actualPitch);
         float rollOutput = rollController.output(_targetRoll, _actualRoll);
         
         // THROTTLE
         float throttle = throttleController.output(_targetHeight, transform.position.y);
         
         // adapted from here
         // https://ghowen.me/build-your-own-quadcopter-autopilot/
         float flForce = Mathf.Clamp(throttle + rollOutput - pitchOutput, 0f, maxThrottle);
         float blForce = Mathf.Clamp(throttle + rollOutput + pitchOutput, 0f, maxThrottle);
         float frForce = Mathf.Clamp(throttle - rollOutput - pitchOutput, 0f, maxThrottle);
         float brForce = Mathf.Clamp(throttle - rollOutput + pitchOutput, 0f, maxThrottle);
         
         
         _flControlThrust = FL.up * flForce;
         _blControlThrust = BL.up * blForce;
         _frControlThrust = FR.up * frForce;
         _brControlThrust = BR.up * brForce;
         
         rigidbody.AddForceAtPosition(_flControlThrust, FL.position);
         rigidbody.AddForceAtPosition(_frControlThrust, FR.position);
         rigidbody.AddForceAtPosition(_blControlThrust, BL.position);
         rigidbody.AddForceAtPosition(_brControlThrust, BR.position);
         rigidbody.AddTorque(_controlTorque, ForceMode.Acceleration);
         
     }

Here is the call graph for this function:

void ArDrone.Reset ( )

private

Reset the PID controllers.

Definition at line 137 of file ArDrone.cs.

                  {
         lateralSpeedController.Reset();
         pitchController.Reset();
         rollController.Reset();
         throttleController.Reset();
         tiltController.Reset();
     }

Here is the call graph for this function:

Here is the caller graph for this function:

void ArDrone.Start ( )

private

Start this instance.

Definition at line 129 of file ArDrone.cs.

                  {
         _holdPosition = transform.position;
         _targetHeight = transform.position.y;
     }

void ArDrone.Update ( )

private

Definition at line 146 of file ArDrone.cs.

                   {
         
         if (!_robot.moveEnabled) {
             Reset();
             _holdPosition = transform.position;
             return;
         }
         
         _command = _robot.navigationCommand;
         
         Draw.Instance.Bearing(transform.position, _command, Color.green);
         Debug.DrawRay(transform.position, _command, Color.green);
         
         _targetHeight += _command.y * Time.deltaTime * verticalSpeed;
         
         Vector3 forward2d = new Vector3(transform.forward.x, 0f, transform.forward.z).normalized;
         Vector3 right2d = new Vector3(transform.right.x, 0f, transform.right.z).normalized;
         Vector3 yawTarget;  // set to vector3.forward when there is no command
         
 
         
         // if no command
         if (_command.magnitude < 0.1f) {        // no command, replace command vector with hold values
             
             
             _target2d = _holdPosition - transform.position;
             _target2d.y = 0f;
             yawTarget = Vector3.forward;
             
         } else {    // deal with command vector
             
             // X/Z plane target direction
             _target2d = new Vector3(_command.x, 0f, _command.z);
             yawTarget = _target2d;
             // update hold previous command 
             _holdRotation = _command;
             // keep hold position
             _holdPosition = transform.position;
         }
         
 
         
         
         Draw.Instance.Bearing(transform.position, _target2d, Color.red);
         Debug.DrawRay(transform.position, _target2d, Color.red);
             
         if (rotateYaw) {
             // control the local yaw velocity so that ArDrone faces command direction
             float yawAxisAngle = Vector3.Angle(right2d, yawTarget);
             float yawCosine = Mathf.Cos(yawAxisAngle * Mathf.Deg2Rad); // 1:RotateRight, -1:RotateLeft
             float localYawVelocity = transform.InverseTransformDirection(rigidbody.angularVelocity).y;
             float yawForce = (yawCosine*maxYawVelocity - localYawVelocity) * yawKp;
             _controlTorque = transform.rotation * new Vector3(0f, yawForce, 0f);
         }
 
 
     
 
         
 
         Draw.Instance.Bearing(transform.position, throttleController.error * Vector3.up, Color.red);
         Debug.DrawRay(transform.position, throttleController.error * Vector3.up, Color.red);
         
         
         Draw.Instance.Bearing(FL.position, _flControlThrust, Color.magenta);
         Draw.Instance.Bearing(BR.position, _brControlThrust, Color.magenta);
         Draw.Instance.Bearing(FR.position, _frControlThrust, Color.magenta);
         Draw.Instance.Bearing(BL.position, _blControlThrust, Color.magenta);
         
         Debug.DrawRay(FL.position, _flControlThrust, Color.magenta);
         Debug.DrawRay(BR.position, _brControlThrust, Color.magenta);
         Debug.DrawRay(FR.position, _frControlThrust, Color.magenta);
         Debug.DrawRay(BL.position, _blControlThrust, Color.magenta);
         
     }

Here is the call graph for this function:

Member Data Documentation

float ArDrone._actualPitch

private

Definition at line 96 of file ArDrone.cs.

float ArDrone._actualRoll

private

Definition at line 97 of file ArDrone.cs.

Vector3 ArDrone._blControlThrust

private

Definition at line 116 of file ArDrone.cs.

Vector3 ArDrone._brControlThrust

private

Definition at line 117 of file ArDrone.cs.

Vector3 ArDrone._command

private

Definition at line 107 of file ArDrone.cs.

Vector3 ArDrone._controlTorque

private

Definition at line 113 of file ArDrone.cs.

Vector3 ArDrone._flControlThrust

private

Definition at line 114 of file ArDrone.cs.

Vector3 ArDrone._frControlThrust

private

Definition at line 115 of file ArDrone.cs.

Vector3 ArDrone._holdPosition

private

Definition at line 110 of file ArDrone.cs.

Vector3 ArDrone._holdRotation

private

Definition at line 109 of file ArDrone.cs.

Robot ArDrone._robot

private

Definition at line 105 of file ArDrone.cs.

Vector3 ArDrone._target2d

private

Definition at line 108 of file ArDrone.cs.

float ArDrone._targetHeight

private

Definition at line 103 of file ArDrone.cs.

float ArDrone._targetPitch

private

Definition at line 102 of file ArDrone.cs.

float ArDrone._targetRoll

private

Definition at line 101 of file ArDrone.cs.

float ArDrone._targetSpeed

private

Definition at line 99 of file ArDrone.cs.

float ArDrone._targetTilt

private

Definition at line 100 of file ArDrone.cs.

Transform ArDrone.BL

Reference to the back-left propeller position.

Definition at line 89 of file ArDrone.cs.

Transform ArDrone.BR

Reference to the back-right propeller position.

Definition at line 94 of file ArDrone.cs.

Transform ArDrone.FL

Reference to the front-left propeller position.

Definition at line 79 of file ArDrone.cs.

Transform ArDrone.FR

Reference to the front-right propeller position.

Definition at line 84 of file ArDrone.cs.

Pid ArDrone.lateralSpeedController

The lateral speed controller.

Definition at line 23 of file ArDrone.cs.

float ArDrone.maxLateralSpeed

The maximum lateral speed allowed by the control system.

Definition at line 18 of file ArDrone.cs.

float ArDrone.maxThrottle

The max throttle of each propeller force.

Definition at line 38 of file ArDrone.cs.

float ArDrone.maxTilt

The maximum roll and pitch permitted by the control system. (measured in degrees)

Definition at line 49 of file ArDrone.cs.

float ArDrone.maxYawVelocity

The max yaw rotational velocity. (measured in degrees per second)

Definition at line 60 of file ArDrone.cs.

Pid ArDrone.pitchController

The pitch controller.

Definition at line 74 of file ArDrone.cs.

Pid ArDrone.rollController

The roll controller.

Definition at line 70 of file ArDrone.cs.

bool ArDrone.rotateYaw

If true then AR Drone will rotate to face the command direction vector.

Definition at line 54 of file ArDrone.cs.

Pid ArDrone.throttleController

The throttle controller.

Definition at line 43 of file ArDrone.cs.

Pid ArDrone.tiltController

The tilt controller.

Definition at line 28 of file ArDrone.cs.

float ArDrone.verticalSpeed

The maximum speed that the vertical target location can change.

Definition at line 33 of file ArDrone.cs.

float ArDrone.yawKp

Gain parameter in controlling the yaw torque.

Definition at line 65 of file ArDrone.cs.

The documentation for this class was generated from the following file:

Assets/botnavsim/Scripts/Robots/ArDrone.cs

Public Attributes

Private Member Functions

Private Attributes

Detailed Description

Member Function Documentation

Member Data Documentation