/* 
 * Принимаем данные NavData 
 */ 

import java.net.*; 
import java.util.*; 


public class Test { 

   /** 
    * @param args 
    */ 
    
    static final int NAVDATA_PORT = 5554; 
    static final int AT_PORT      = 5556; 

    //NavData offset 
    static final int NAVDATA_STATE    =  4; 
    static final int NAVDATA_BATTERY  = 24; 
    static final int NAVDATA_ALTITUDE = 40; 


    
    int seq = 1; //Send AT command with sequence number 1 will reset the counter 

    public static void main(String[] args) throws Exception { 
      // TODO Auto-generated method stub 
        InetAddress inet_addr; 
        DatagramSocket socket_at; 
        DatagramSocket socket_nav; 
        int cnt = 0; 
        
// Создаем значение для переменной ip_addr        
       String ip = "192.168.1.1"; 
       StringTokenizer st = new StringTokenizer(ip, "."); 
       byte[] ip_bytes = new byte[4]; 
       if (st.countTokens() == 4){ 
            for (int i = 0; i < 4; i++){ 
          ip_bytes[i] = (byte)Integer.parseInt(st.nextToken()); 
           } 
       } 
       inet_addr = InetAddress.getByAddress(ip_bytes); 
        
        
        
       socket_at = new DatagramSocket(AT_PORT); 
       socket_at.setSoTimeout(3000); 
        
       socket_nav = new DatagramSocket(NAVDATA_PORT); 
       socket_nav.setSoTimeout(3000); 

       try { 
// создаем содержимое пакета для отправки на NAVDATA_PORT        
       byte[] buf_snd = {0x01, 0x00, 0x00, 0x00}; 
// создаем пакет 
       DatagramPacket packet_snd = new DatagramPacket(buf_snd, buf_snd.length, inet_addr, NAVDATA_PORT); 
// отправляем пакет        
       socket_nav.send(packet_snd); 
       System.out.println("Sent trigger flag to UDP port " + NAVDATA_PORT);        


// создаем новый пакет для отправки на AT_PORT       
       String at_cmd="AT*CONFIG=" + 1 + ",\"general:navdata_demo\",\"TRUE\""; 
       byte[] buf_snd_1 = (at_cmd + "\r").getBytes(); 
      DatagramPacket packet_snd_1 = new DatagramPacket(buf_snd_1, buf_snd_1.length, inet_addr, AT_PORT); 
      socket_at.send(packet_snd_1); 
       
// Получаем сообщение с NAVDATA_PORT 
        byte[] buf_rcv = new byte[10240]; 
       DatagramPacket packet_rcv = new DatagramPacket(buf_rcv, buf_rcv.length);            

       while(true) { 
         try { 
             socket_nav.receive(packet_rcv); 
             
             cnt++; 
             if (cnt >= 2) { 
                cnt = 0; 
                System.out.println("NavData Received: " + packet_rcv.getLength() + " bytes"); 
                 

                 StringBuffer sb = new StringBuffer(); 
                 for (int i = 0; i < 4; i++) { 
                     String tmp = Integer.toHexString(((int) buf_rcv[NAVDATA_BATTERY + i]) & 0xFF); 
                     for(int t = tmp.length();t<2;t++) 
                     { 
                         sb.append("0"); 
                     } 
                     sb.append(tmp); 
                     sb.append(" "); 
                 } 
                 
                 
                 
                 
                 
                int tmp_1 = 0, n = 0; 
                System.out.println("get_int(): data = " + sb);  
                for (int i=3; i>=0; i—) {    
                    n <<= 8; 
                    tmp_1 = buf_rcv[NAVDATA_BATTERY + i] & 0xFF;    
                    n |= tmp_1;    
                } 
                 
                System.out.println("Battery: " + n); 
                 
                 
             } 
         } catch(SocketTimeoutException ex3) { 
                 System.out.println("socket_nav.receive(): Timeout"); 
         } catch(Exception ex1) { 
             ex1.printStackTrace(); 
         } 
          } 
      } catch(Exception ex2) { 
          ex2.printStackTrace(); 
      } 
   } 
    
} 

import java.awt.Frame; 
import java.awt.event.KeyEvent; 
import java.awt.event.KeyListener; 
import java.awt.event.WindowAdapter; 
import java.awt.event.WindowEvent; 
import java.net.*; 
import java.nio.FloatBuffer; 
import java.nio.IntBuffer; 
import java.util.*; 


public class Test1 extends Frame implements KeyListener{ 

   /** 
    * @param args 
    */ 
    
    static final int NAVDATA_PORT = 5554; 
    static final int AT_PORT      = 5556; 

    //NavData offset 
    static final int NAVDATA_STATE    =  4; 
    static final int NAVDATA_BATTERY  = 24; 
    static final int NAVDATA_PITCH   = 28; 
    static final int NAVDATA_ROLL    = 32; 
    static final int NAVDATA_YAW     = 36; 
    static final int NAVDATA_ALTITUDE = 40; 
    static final int NAVDATA_VX = 44; 
    static final int NAVDATA_VY = 48; 
    static final int NAVDATA_VZ = 52; 
    
    static final int MYKONOS_TRIM_COMMAND_MASK   = 1 <<  7; /*!< Trim command ACK : (0) None, (1) one received */ 
    static final int MYKONOS_TRIM_RUNNING_MASK   = 1 <<  8; /*!< Trim running : (0) none, (1) running */ 
    static final int MYKONOS_TRIM_RESULT_MASK    = 1 <<  9; /*!< Trim result : (0) failed, (1) succeeded */ 
    static final int MYKONOS_ANGLES_OUT_OF_RANGE = 1 << 19; /*!< Angles : (0) Ok, (1) out of range */ 
    static final int MYKONOS_WIND_MASK           = 1 << 20; /*!< Wind : (0) Ok, (1) too much to fly */ 
    static final int MYKONOS_ULTRASOUND_MASK     = 1 << 21; /*!< Ultrasonic sensor : (0) Ok, (1) deaf */ 
    static final int MYKONOS_CUTOUT_MASK         = 1 << 22; /*!< Cutout system detection : (0) Not detected, (1) detected */ 
    static final int MYKONOS_COM_WATCHDOG_MASK   = 1 << 30; /*!< Communication Watchdog : (1) com problem, (0) Com is ok */ 
    static final int MYKONOS_EMERGENCY_MASK      = 1 << 31; /*!< Emergency landing : (0) no emergency, (1) emergency */ 
    
    static InetAddress inet_addr; 
    
    static String at_cmd_last = ""; 
    
    static int seq = 1; //Send AT command with sequence number 1 will reset the counter 

    final static int INTERVAL = 100; 
    
    float speed = (float)0.1; 
    boolean shift = false; 
    FloatBuffer fb; 
    IntBuffer ib; 
    boolean space_bar = false; //true = Takeoff, false = Landing 
    
    static float attitude_pitch, attitude_roll, attitude_yaw; 
    
    static DatagramSocket socket_at; 
    static DatagramSocket socket_nav; 
    
    
    public Test1(String name, String args[], InetAddress inet_addr) throws Exception { 
        super(name); 
        System.out.println("Speed: " + speed); 
       addKeyListener(this);                  
       setSize(320, 160); 
       setVisible(true); 
       addWindowListener(new WindowAdapter() { 
           public void windowClosing(WindowEvent e) { 
          System.exit(0); 
           } 
         }); 
        
    } 


    public static void main(String[] args) throws Exception { 
      // TODO Auto-generated method stub 
        

        int cnt = 0; 
        
// Создаем значение для переменной ip_addr        
       String ip = "192.168.1.1"; 
       StringTokenizer st = new StringTokenizer(ip, "."); 
       byte[] ip_bytes = new byte[4]; 
       if (st.countTokens() == 4){ 
            for (int i = 0; i < 4; i++){ 
          ip_bytes[i] = (byte)Integer.parseInt(st.nextToken()); 
           } 
       } 
       inet_addr = InetAddress.getByAddress(ip_bytes); 
        
        
        
       socket_at = new DatagramSocket(AT_PORT); 
       socket_at.setSoTimeout(3000); 
        
       socket_nav = new DatagramSocket(NAVDATA_PORT); 
       socket_nav.setSoTimeout(3000); 

       try { 
// создаем содержимое пакета для отправки на NAVDATA_PORT        
       byte[] buf_snd = {0x01, 0x00, 0x00, 0x00}; 
// создаем пакет 
       DatagramPacket packet_snd = new DatagramPacket(buf_snd, buf_snd.length, inet_addr, NAVDATA_PORT); 
// отправляем пакет        
       socket_nav.send(packet_snd); 
       System.out.println("Sent trigger flag to UDP port " + NAVDATA_PORT);        

       Test1 ardrone = new Test1("ARDrone", args, inet_addr);        
       
// создаем новый пакет для отправки на AT_PORT       
       String at_cmd="AT*CONFIG=" + 1 + ",\"general:navdata_demo\",\"TRUE\""; 
       byte[] buf_snd_1 = (at_cmd + "\r").getBytes(); 
      DatagramPacket packet_snd_1 = new DatagramPacket(buf_snd_1, buf_snd_1.length, inet_addr, AT_PORT); 
      socket_at.send(packet_snd_1); 
       
// Получаем сообщение с NAVDATA_PORT 
        byte[] buf_rcv = new byte[10240]; 
       DatagramPacket packet_rcv = new DatagramPacket(buf_rcv, buf_rcv.length);            

       while(true) { 
         try { 
             socket_nav.receive(packet_rcv); 
             
             int state = get_int(buf_rcv, NAVDATA_STATE); 
              
              if ((state & MYKONOS_COM_WATCHDOG_MASK) > 0) { 
                 System.out.println("MYKONOS_COM_WATCHDOG_MASK"); 
                 send("AT*COMWDG=1"); 
                 Thread.sleep(30);    
              } 
              
                 if ((state & MYKONOS_TRIM_COMMAND_MASK) > 0) { 
                 System.out.println("MYKONOS_TRIM_COMMAND_MASK"); 
//                 send("AT*LED=1,4,1056964608,5"); 
//                 Thread.sleep(30);    
                  
              } 

              if ((state & MYKONOS_TRIM_RESULT_MASK) > 0) { 
                 System.out.println("MYKONOS_TRIM_RESULT_MASK"); 
              } 

              if ((state & MYKONOS_ANGLES_OUT_OF_RANGE) > 0) { 
                 System.out.println("MYKONOS_ANGLES_OUT_OF_RANGE"); 
              } 

              if ((state & MYKONOS_WIND_MASK) > 0) { 
                 System.out.println("MYKONOS_WIND_MASK"); 
              } 

              if ((state & MYKONOS_ULTRASOUND_MASK) > 0) { 
                 System.out.println("MYKONOS_ULTRASOUND_MASK"); 
              } 

              if ((state & MYKONOS_CUTOUT_MASK) > 0) { 
                 System.out.println("MYKONOS_CUTOUT_MASK"); 
              } 

              if ((state & MYKONOS_EMERGENCY_MASK) > 0) { 
                 System.out.println("MYKONOS_EMERGENCY_MASK"); 
              } 
              
              int battery = get_int(buf_rcv, NAVDATA_BATTERY); 
              float altitude = ((float)get_int(buf_rcv, NAVDATA_ALTITUDE)/1000); 
              attitude_pitch = get_float(buf_rcv, NAVDATA_PITCH)/1000;
              attitude_roll = get_float(buf_rcv, NAVDATA_ROLL)/1000; 
              attitude_yaw = get_float(buf_rcv, NAVDATA_YAW)/1000; 
              
             
             cnt++; 
             if (cnt >= 5) { 
                cnt = 0; 
                System.out.println("NavData Received: " + packet_rcv.getLength() + " bytes"); 
                 
                System.out.println("get_int(): data = " + byte2hex(buf_rcv, NAVDATA_BATTERY, 4));  

                System.out.println("Battery: " + battery); 
                 
                System.out.println("Altitude: " + altitude); 
                 
                 System.out.println("Pitch: " + attitude_pitch); 
                 System.out.println("Roll: " + attitude_roll); 
                 System.out.println("Yaw: " + attitude_yaw); 
                 
             } 
         } catch(SocketTimeoutException ex3) { 
                 System.out.println("socket_nav.receive(): Timeout"); 
         } catch(Exception ex1) { 
             ex1.printStackTrace(); 
         } 
          } 
      } catch(Exception ex2) { 
          ex2.printStackTrace(); 
      } 
   } 
    
    public static int get_int(byte[] data, int offset) { 
        int tmp = 0, n = 0; 
        for (int i=3; i>=0; i—) {    
            n <<= 8; 
            tmp = data[offset + i] & 0xFF;    
            n |= tmp;    
        } 
        return n;    
    } 
    
    public static void send(String msg) throws Exception { 
       byte[] bytes = (msg + "\r").getBytes(); 
       DatagramPacket ds = new DatagramPacket(bytes, bytes.length, inet_addr, AT_PORT); 
        
       socket_at.send(ds); 
    } 
    
    public static float get_float(byte[] data, int offset) { 
        return Float.intBitsToFloat(get_int(data, offset)); 
    } 
    
    public static String byte2hex(byte[] data, int offset, int len) { 
       StringBuilder sb = new StringBuilder(); 
        for (int i = 0; i < len; i++) { 
            String tmp = Integer.toHexString(((int) data[offset + i]) & 0xFF); 
            for(int t = tmp.length();t<2;t++) 
            { 
                sb.append('0'); 
            } 
            sb.append(tmp); 
            sb.append(' '); 
        } 
        return sb.toString(); 
    } 
    
    public static synchronized int get_seq() { 
       return seq++; 
    } 
    
    public static synchronized void send_at_cmd(String at_cmd) throws Exception { 
       System.out.println("AT command: " + at_cmd);    
       at_cmd_last = at_cmd; 
   byte[] buf_snd = (at_cmd + "\r").getBytes(); 
   DatagramPacket packet_snd = new DatagramPacket(buf_snd, buf_snd.length, inet_addr, AT_PORT); 
   socket_at.send(packet_snd); 

    /* AR.Drone does not send back ack message (like "OK") 
    byte[] buf_rcv = new byte[64]; 
   DatagramPacket packet_rcv = new DatagramPacket(buf_rcv, buf_rcv.length);            
   socket_at.receive(packet_rcv); 
   System.out.println(new String(packet_rcv.getData(),0,packet_rcv.getLength())); 
   */      
    } 
    
    public void keyTyped(KeyEvent e) { 
        ; 
    } 
    
    public void keyPressed(KeyEvent e) { 
        int keyCode = e.getKeyCode(); 

        try { 
            control(keyCode); 
        } catch (Exception ex) { 
            ex.printStackTrace(); 
        } 
    } 
    
    public void keyReleased(KeyEvent e) { 
        int keyCode = e.getKeyCode(); 
        //if (keyCode >= KeyEvent.VK_1 && keyCode <= KeyEvent.VK_9) speed = (float)0.1; //Reset speed 
        if (keyCode == KeyEvent.VK_SHIFT) shift = false; 
    } 
    
    //Control AR.Drone via AT commands per key code 
    public void control(int keyCode) throws Exception { 
        //System.out.println("Key: " + keyCode + " (" + KeyEvent.getKeyText(keyCode) + ")"); 
        
       switch (keyCode) { 
            case KeyEvent.VK_1: 
               speed = (float)0.05; 
              break; 
           case KeyEvent.VK_2: 
               speed = (float)0.1; 
              break; 
           case KeyEvent.VK_3: 
               speed = (float)0.15; 
              break; 
           case KeyEvent.VK_4: 
               speed = (float)0.25; 
              break; 
           case KeyEvent.VK_5: 
               speed = (float)0.35; 
              break; 
           case KeyEvent.VK_6: 
               speed = (float)0.45; 
              break; 
           case KeyEvent.VK_7: 
               speed = (float)0.6; 
              break; 
           case KeyEvent.VK_8: 
               speed = (float)0.8; 
              break; 
           case KeyEvent.VK_9: 
               speed = (float)0.99; 
              break; 
           case KeyEvent.VK_SHIFT: 
               shift = true; 
              break; 
           case KeyEvent.VK_UP: 
              if (shift) { 
              System.out.println("Go Up (gaz+)"); 
                  send_pcmd(1, 0, 0, speed, 0); 
              } else { 
                  System.out.println("Go Forward (pitch+)"); 
                  send_pcmd(1, speed, 0, 0, 0); 
              } 
              break; 
           case KeyEvent.VK_DOWN: 
              if (shift) { 
                  System.out.println("Go Down (gaz-)"); 
                  send_pcmd(1, 0, 0, -speed, 0); 
              } else { 
                  System.out.println("Go Backward (pitch-)"); 
                  send_pcmd(1, -speed, 0, 0, 0); 
              } 
                 break; 
           case KeyEvent.VK_LEFT: 
               if (shift) { 
                   System.out.println("Rotate Left (yaw-)"); 
                   send_pcmd(1, 0, 0, 0, -speed); 
      } else { 
          System.out.println("Go Left (roll-)"); 
          send_pcmd(1, 0, -speed, 0, 0); 
      } 
             break; 
           case KeyEvent.VK_RIGHT: 
          if (shift) { 
              System.out.println("Rotate Right (yaw+)"); 
              send_pcmd(1, 0, 0, 0, speed); 
      } else { 
          System.out.println("Go Right (roll+)"); 
          send_pcmd(1, 0, speed, 0, 0); 
      } 
              break; 
           case KeyEvent.VK_SPACE: 
              // Операция  Не равно "= !" 
              space_bar = !space_bar; 

             if (space_bar) { 
                  System.out.println("Takeoff"); 
                  //send_at_cmd("AT*CONFIG=" + get_seq() + ",\"leds:leds_anim\",\"3,1073741824,2\"");  
                  send_at_cmd("AT*REF=" + get_seq() + ",290718208"); 
         } else { 
                  System.out.println("Landing"); 
                  //send_at_cmd("AT*CONFIG=" + get_seq() + ",\"leds:leds_anim\",\"5,1073741824,2\"");  
                  send_at_cmd("AT*REF=" + get_seq() + ",290717696"); 
          } 

              break;                              
           case KeyEvent.VK_PAUSE: 
              System.out.println("Hovering"); 
              send_pcmd(1, 0, 0, 0, 0); 
      speed = (float)0.1; //Reset speed 
              break; 
           default: 
              break; 
       } 
        
       if (keyCode >= KeyEvent.VK_1 && keyCode <= KeyEvent.VK_9) System.out.println("Speed: " + speed); 
    } 
    
    public void send_pcmd(int enable, float pitch, float roll, float gaz, float yaw) throws Exception { 
       System.out.println("Speed: " + speed); 
   send_at_cmd("AT*PCMD=" + get_seq() + "," + enable + "," + intOfFloat(pitch) + "," + intOfFloat(roll) 
                       + "," + intOfFloat(gaz) + "," + intOfFloat(yaw)); 
    } 
    
    public int intOfFloat(float f) { 
        fb.put(0, f); 
        return ib.get(0); 
    } 

    
    
    
} 

send("AT*PCMD=" + (seq++) + ",0,0,0,0,0");
Thread.sleep(INTERVAL);
send("AT*REF=" + (seq++) + ",290717696");
send("AT*REF=" + (seq++) + ",290717696");
Thread.sleep(INTERVAL);

    				if (angle_current < angle_final & angle_final > 0 & angle_current < 0 ){
    					
    					yaw=false;
    					
    					send("AT*PCMD=" + (seq++) + ",0,0,0,0,0");
    					Thread.sleep(INTERVAL);
    					
    				}

    				if (angle_final > 0 & angle_current < 0 & angle_current < angle_start){
    					
    					yaw=false;
    					
    					send("AT*PCMD=" + (seq++) + ",0,0,0,0,0");
    					Thread.sleep(INTERVAL);
    					
    				}