image thumbnail
from Controller for AR.Drone by Phillip
Shows basic control functionality of a Parrot AR.Drone and displays navdata returned by the drone.

ARDrone
classdef ARDrone < handle
    % ARDrone Class
    % Controls AR Parrot Drone

    
    properties
        seq = 3;
        ARc;
        ARn;

        % NavData Properties
        Control_State = [];
        Battery_Voltage = 0;
        Pitch = 0;
        Roll = 0;
        Yaw = 0;
        Altitude = 0;
        X_Velocity = 0;
        Y_Velocity = 0;
        NavData = 0;
    end
    
    methods

        function [obj] = ARDrone()
            instrreset
            clc

           global fh;

           %Create Control connection
           obj.ARc = udp('192.168.1.1', 5556, 'LocalPort', 5556);
           fopen(obj.ARc);

           fh = 0;

           %Create NavData connection
           obj.ARn = udp('192.168.1.1', 5554, 'LocalPort', 5554, ...
            'ByteOrder', 'littleEndian', ...
            'InputBufferSize', 500, ...
            'BytesAvailableFcn', {@navPacketRxCallback,obj}, ...
            'DatagramTerminateMode', 'on', ...
            'BytesAvailableFcnMode', 'byte', ...
            'BytesAvailableFcnCount', 24);
            fopen(obj.ARn);

            % Byte to NavData port for initialization
            fwrite(obj.ARn, 1);

            % NavData command to command port
            AR_NAV_CONFIG = sprintf('AT*CONFIG=2,\"general:navdata_demo\",\"TRUE\"\r');
            fprintf(obj.ARc, AR_NAV_CONFIG);

            % Flicker LEDs
            obj.command('LED', '1,5,5,5,5')
          function navPacketRxCallback(obj, event, drone)
            if (get(obj, 'BytesAvailable') ~= 0)
                data = fread(obj);
                data = uint8(data);
                if ~isempty(data);

                    % Drone Control State Information
                        drone_state = [data(5) data(6) data(7) data(8)];
                        drone_state = typecast(drone_state, 'uint32');
                        for i = 1:32
                           drone.Control_State(i) = bitget(drone_state, i);
                        end

                    % Battery Voltage Percentage
                        vBattery = [data(25) data(26) data(27) data(28)];
                        drone.Battery_Voltage = typecast(vBattery, 'uint32');

                    % Theta (Pitch) Values
                        tPitch = [data(29) data(30) data(31) data(32)];
                        drone.Pitch = typecast(tPitch, 'single') / 1000;

                    % Phi (Roll) Values
                        tRoll = [data(33) data(34) data(35) data(36)];
                        drone.Roll = typecast(tRoll, 'single') / 1000;

                    % Psi (Yaw) Values
                        tYaw = [data(37) data(38) data(39) data(40)];
                        drone.Yaw = typecast(tYaw, 'single') / 1000;

                    % Altitude
                        tAltitude = [data(41) data(42) data(43) data(44)];
                        drone.Altitude = single(typecast(tAltitude, 'int32'))/ 1000;

                    % X Velocity
                        Vx = [data(45) data(46) data(47) data(48)];
                        drone.X_Velocity = typecast(Vx, 'single') / 1000;

                    % Y Velocity
                        Vy = [data(49) data(50) data(51) data(52)];
                        drone.Y_Velocity = typecast(Vy, 'single') / 1000;

                    % Entire NavData Packet
                        drone.NavData = data;
                end

                % Reset Drone Watchdog Bit
                AR_WDG = strcat('AT*COMWDG=',num2str(drone.seq),',');
                fprintf(drone.ARc, AR_WDG);
                drone.seq = drone.seq + 1;

        %         if fh ~= 0
        %             if strcmp(get(fh, 'Selected'), 'off')
        %                 set(fh, 'Selected', 'on')
        %             end
        %         end
        %         
            end
          end 
     end
        
        function command(obj, command_type, code)
            AR_CMD = sprintf( 'AT*%s=%i,%s\r', command_type, obj.seq, code);
            fprintf(obj.ARc, AR_CMD);
            obj.seq = obj.seq + 1;
        end
      
        function takeoff(obj)
            obj.command('FTRIM','')
            obj.command('CONFIG','\"control:altitude_max\", \"20000\"')
            obj.command('REF','290718208')
        end
        
        function hover(obj)
            obj.drive([0,0,0,0])
        end

        function land(obj)
          obj.command('REF','290717696')
        end
        
        function emergency(obj)
            obj.command('REF','290717952')        
            obj.command('REF','290717696')
        end

        function drive(obj, speed) 
            % Controls the robot movement directly using motor speeds
            fvel = typecast(single(speed(1)/2), 'int32');
            lvel = typecast(single(speed(2)/2), 'int32');
            uvel = typecast(single(speed(3)/2), 'int32');
            rvel = typecast(single(speed(4)/2), 'int32');
            fvel_str = strcat(num2str(fvel),',');
            lvel_str = strcat(num2str(lvel),',');
            uvel_str = strcat(num2str(uvel),',');
            rvel_str = num2str(rvel);
            obj.command('PCMD',strcat('1,',  lvel_str,  fvel_str,  uvel_str,  rvel_str))
        end
        
        function moveUp(obj, speed)
        obj.drive([0,0,speed,0])
        end
        
        function moveDown(obj, speed)
        obj.drive([0,0,-speed,0])
        end

        function moveLeft(obj, speed)
        % Moves the robot left at a specific speed
        obj.drive([0,-speed,0,0])
        end
        
        function moveRight(obj, speed)
        % Moves the robot right at a specific speed
        obj.drive([0,speed,0,0])
        end
        
        function moveForward(obj, speed)
        % Moves the robot forward at a specific speed
        obj.drive([-speed,0,0,0])
        end
        
        function moveReverse(obj, speed)
        % Moves the robot forward at a specific speed
        obj.drive([speed,0,0,0])
        end
        
        function rotateLeft(obj, omega)
        % Rotates robot left
            obj.drive([0,0,0, -omega])
         end
        
        function rotateRight(obj, omega)
            obj.drive([0,0,0, omega]) 
        end
        
        function stop(obj)
        % Stop all Robot motion and close communications.
            obj.land
            fclose(obj.ARn);
            fclose(obj.ARc);
        end        

        function control(obj, varargin)
            clc
            disp('Controls');
            disp('arrow keys = up,down,rotate_left,rotate_right');
            disp( 'w,a,s,d = move:forward,backward,left,right');
            disp('enter,spacebar,shift = takeoff,land,hover');
            disp('q = quit program');

        % Manually control robot using W/S (forward/reverse), A/D
        % (strafe left/right), I/K (vertical up/down), J/L (turn left/right)
        % E (emergency stop), and Q (quit) keys.
            global fh;
            if nargin == 2
                vel = varargin{1};
            else
                vel = .5;
            end

            left = 0;
            bottom = 0;
            width = 100;
            height = 150;
            fh = figure(...
                'name','ARDrone Controller', ...
                'keypressfcn',@keyPress, ...
                'windowstyle','modal',...
                'numbertitle','off', ...
                'Position',[left bottom width*3 height],...
                'userdata','timeout',...
                'Color','white') ;
            
            t = timer('TimerFcn',{@display_Nav,obj},'ExecutionMode','fixedRate');
            start(t)
            function keyPress(~, event)
                    %obj.display_Nav();
                    if (strcmp(event.Character, 'q'))
                        obj.command('REF','290717952')
                        stop(t)
                        delete(t)
                        delete(fh)
                        fh = 0;
                    end
                    cmd = event.Key; 

                    switch (cmd)

                        case 'return' % Take off
                            obj.takeoff()
                        case 'space' % Land
                            obj.land()
                        case 'e' % Emergency
                            obj.emergency()
                        case 'a' % Rotate Left
                            obj.rotateLeft(vel)
                        case 'd' % Rotate Right
                            obj.rotateRight(vel)
                        case 'uparrow' % Up
                            obj.moveUp(vel)
                        case 'downarrow' % Down
                            obj.moveDown(vel)
                        case 'shift' % Hover
                            obj.hover()
                        case 'leftarrow' % Left
                            obj.moveLeft(vel)
                        case 'rightarrow' % Right
                            obj.moveRight(vel)
                        case 'w' % Forward
                            obj.moveForward(vel)
                        case 's' % Backward
                            obj.moveReverse(vel)
                        case 'q'
                            disp('Quitting...')
                        otherwise
                            disp('Unknown Command')
                    end                   
            end
                
        function display_Nav(value,event,obj)
            left = 0;
            bottom = 0;
            width = 100;
            height = 150;
            labels = uicontrol('Style','text','Position',[left bottom width height],'HorizontalAlignment', 'left', 'FontSize', 14,'BackgroundColor','white');
            str1 = 'Battery';
            str2 = 'Pitch';
            str3 = 'Yaw';
            str4 = 'Roll';
            str5 = 'Altitude';
            str6 = 'X_Velocity';
            str7 = 'Y_Velocity';
            set(labels,'String',{str1,str2,str3,str4,str5,str6,str7});

            values = uicontrol('Style','text','Position',[left+width bottom width height],'HorizontalAlignment', 'right', 'FontSize', 14,'BackgroundColor','white');
            str1 = num2str(obj.Battery_Voltage);
            str2 = num2str(obj.Pitch);
            str3 = num2str(obj.Yaw);
            str4 = num2str(obj.Roll);
            str5 = num2str(obj.Altitude);
            str6 = num2str(obj.X_Velocity);
            str7 = num2str(obj.Y_Velocity);
            set(values,'String',{str1,str2,str3,str4,str5,str6,str7});

            units = uicontrol('Style','text','Position',[left+width+width+10 bottom width height],'HorizontalAlignment', 'left', 'FontSize', 14,'BackgroundColor','white');
            str1 = num2str('volts');
            str2 = num2str('degrees');
            str3 = num2str('degrees');
            str4 = num2str('degrees');
            str5 = num2str('meters');
            str6 = num2str('meters/sec');
            str7 = num2str('meters/sec');
            set(units,'String',{str1,str2,str3,str4,str5,str6,str7});

    end
                
        end
    end
end

Contact us