CC3000 WiFi Shield Connected to a Parrot AR Drone 2.0 UDP

For other supported Arduino products from Adafruit: Shields, accessories, etc.

Moderators: adafruit_support_bill, adafruit

Please be positive and constructive with your questions and comments.
Locked
User avatar
PBathula
 
Posts: 3
Joined: Wed Aug 20, 2014 11:00 am

CC3000 WiFi Shield Connected to a Parrot AR Drone 2.0 UDP

Post by PBathula »

Hi folks,

So I've been tasked with building a gesture recognition and control system for controlling a UAV, in my case there were a couple of Parrot AR drones in the office so I figured I'd use a WiFi shield with my arduino and try and control these. The Good news is that I've managed to build my gesture recognition glove and that works fine however the sending and receiving of data to and from the drone has proven to be infuriatingly hard :p

Connecting to the Drone's wireless AP is easy enough, connecting to the command and communication ports via UDP (5556, 5554) is easy enough as well however the sending of commands, specifically the AT* commands is very difficult to troubleshoot as I'm not too sure if the problem is with the arduino and CC3000 not being able to connect properly or whether my text string command format is flawed.

I'm not really a Noob with arduino but this is my first time trying to communicate using wireless, any help anyone can provide would be greatly appreciated
Cheers

Here's my code for starters :)

Code: Select all

//Libraries
#include <Adafruit_CC3000.h>
#include <ccspi.h>
#include <Wire.h>
#include <SPI.h>
#include <String.h>
#include <HMC58X3.h>
#include <ITG3200.h>
#include <ADXL345.h>
#include <FreeIMU.h>
#include "utility/debug.h"

#define ADAFRUIT_CC3000_IRQ    3
#define ADAFRUIT_CC3000_VBAT   5
#define ADAFRUIT_CC3000_CS     10
#define WLAN_SSID              "ardrone_000000"  /*"MagelliumLtd"*/
#define WLAN_PASS              ""  /*"mag2013uk;"*/
#define WLAN_SECURITY          WLAN_SEC_UNSEC /*WLAN_SEC_WPA2*/
#define ARDRONE_NAVDATA_PORT                5554
#define ARDRONE_VIDEO_PORT                  5555
#define ARDRONE_AT_PORT                     5556
#define ARDRONE_CONFIGDATA_PORT             5559
#define ARDRONE_STATE_CONNECTING               0
#define ARDRONE_STATE_INITIALISING             1
#define ARDRONE_STATE_INITIALISED              2
// Flags for AtPCmd
//#define ARDRONE_PCMD_FLAG_PROGRESSIVE        0x1
//#define ARDRONE_PCMD_FLAG_COMBINED_YAW       0x2
// Flags for AtRefCmd
//#define ARDRONE_REF_FLAG_EMERGENCY         0x100
//#define ARDRONE_REF_FLAG_START             0x200
//#define ARDRONE_REF_FLAG_BASIC        0x11540000
// Milliseconds between AT commands:
#define ARDRONE_AT_CMD_INTERVAL               30
// Max length of an accumulated AT command
// Caution there is no protection against buffer overruns
#define ARDRONE_MAX_AT_COMMAND_LENGTH        300


Adafruit_CC3000 cc3000 = Adafruit_CC3000(ADAFRUIT_CC3000_CS, ADAFRUIT_CC3000_IRQ, ADAFRUIT_CC3000_VBAT, SPI_CLOCK_DIVIDER);
uint32_t ipAddress, netmask, gateway, dhcpserv, dnsserv;

//Variables
float val[11];
//float q[4];
float ang[3];
int index_val = 0;
int middle_val = 0;
int ring_val = 0;
int pinky_val = 0;
char cmd[ARDRONE_MAX_AT_COMMAND_LENGTH];
int sequence = 0;
int run_count = 0;
//float high = 1;

//Set d'Object
FreeIMU IMU = FreeIMU();

//Hardware Configuration
const int flex_index = A0;
const int flex_middle = A1;
const int flex_ring = A2;
const int flex_pinky = A3;

void setup() {
  Serial.begin(115200);
  Serial.println(F("Hello, CC3000!\n")); 

  displayDriverMode();
  Serial.print(F("Free RAM: ")); Serial.println(getFreeRam(), DEC);
  
  /* Initialise the module */
  Serial.println(F("\nInitialising the CC3000 ..."));
  if (!cc3000.begin())
  {
    Serial.println(F("Unable to initialise the CC3000! Check your wiring?"));
    while(1);
  }

  /* Optional: Update the Mac Address to a known value */
/*
  uint8_t macAddress[6] = { 0x08, 0x00, 0x28, 0x01, 0x79, 0xB7 };
   if (!cc3000.setMacAddress(macAddress))
   {
     Serial.println(F("Failed trying to update the MAC address"));
     while(1);
   }
*/
  
  uint16_t firmware = checkFirmwareVersion();
  if (firmware < 0x113) {
    Serial.println(F("Wrong firmware version!"));
    for(;;);
  } 
  
  displayMACAddress();
  
  /* Optional: Get the SSID list (not available in 'tiny' mode) */
#ifndef CC3000_TINY_DRIVER
  listSSIDResults();
#endif
  
  /* Delete any old connection data on the module */
  Serial.println(F("\nDeleting old connection profiles"));
  if (!cc3000.deleteProfiles()) {
    Serial.println(F("Failed!"));
    while(1);
  }

  /* Optional: Set a static IP address instead of using DHCP.
     Note that the setStaticIPAddress function will save its state
     in the CC3000's internal non-volatile memory and the details
     will be used the next time the CC3000 connects to a network.
     This means you only need to call the function once and the
     CC3000 will remember the connection details.  To switch back
     to using DHCP, call the setDHCP() function (again only needs
     to be called once).
  */
  /*
  uint32_t ipAddress = cc3000.IP2U32(192, 168, 1, 19);
  uint32_t netMask = cc3000.IP2U32(255, 255, 255, 0);
  uint32_t defaultGateway = cc3000.IP2U32(192, 168, 1, 1);
  uint32_t dns = cc3000.IP2U32(8, 8, 4, 4);
  if (!cc3000.setStaticIPAddress(ipAddress, netMask, defaultGateway, dns)) {
    Serial.println(F("Failed to set static IP!"));
    while(1);
  }
  */
  /* Optional: Revert back from static IP addres to use DHCP.
     See note for setStaticIPAddress above, this only needs to be
     called once and will be remembered afterwards by the CC3000.
  */
  /*
  if (!cc3000.setDHCP()) {
    Serial.println(F("Failed to set DHCP!"));
    while(1);
  }
  */

  /* Attempt to connect to an access point */
  char *ssid = WLAN_SSID;             /* Max 32 chars */
  Serial.print(F("\nAttempting to connect to ")); Serial.println(ssid);
  
  /* NOTE: Secure connections are not available in 'Tiny' mode!
     By default connectToAP will retry indefinitely, however you can pass an
     optional maximum number of retries (greater than zero) as the fourth parameter.
  */
  if (!cc3000.connectToAP(WLAN_SSID, WLAN_PASS, WLAN_SECURITY)) {
    Serial.println(F("Failed!"));
    while(1);
  }
   
  Serial.println(F("Connected!"));
  
  /* Wait for DHCP to complete */
  Serial.println(F("Request DHCP"));
  while (!cc3000.checkDHCP())
  {
    delay(100); // ToDo: Insert a DHCP timeout!
  }  

  /* Display the IP address DNS, Gateway, etc. */  
  while (! displayConnectionDetails()) {
    delay(1000);
  }
  
//#ifndef CC3000_TINY_DRIVER
  /* Try looking up www.adafruit.com */
  /*
  uint32_t ip = 0;
  Serial.print(F("www.adafruit.com -> "));
  while  (ip  ==  0)  {
    if  (!  cc3000.getHostByName("www.adafruit.com", &ip))  {
      Serial.println(F("Couldn't resolve!"));
    }
    delay(500);
  }  
  cc3000.printIPdotsRev(ip);
  
  /* Do a quick ping test on adafruit.com */  
  /*
  Serial.print(F("\n\rPinging ")); cc3000.printIPdotsRev(ip); Serial.print("...");  
  uint8_t replies = cc3000.ping(ip, 5);
  Serial.print(replies); Serial.println(F(" replies"));
  if (replies)
    Serial.println(F("Ping successful!"));
#endif

  /* You need to make sure to clean up after yourself or the CC3000 can freak out */
  /* the next time you try to connect ... */
  //Serial.println(F("\n\nClosing the connection"));
  //cc3000.disconnect();
 
  
  Wire.begin();
  
  delay(500);
  IMU.init(true);
  delay(500);
}

void loop() {
   connect_drone();
  
  //Read Flexors
  int index = analogRead(flex_index);
  int middle = analogRead(flex_middle);
  int ring = analogRead(flex_ring);
  int pinky = analogRead(flex_pinky);

  //Map Values
  int index_val = map(index, 330, 190, 0, 100);
  int middle_val = map(middle, 435, 360, 0, 100);
  int ring_val = map(ring, 400, 310, 0, 100);
  int pinky_val = map(pinky, 430, 320, 0, 100);  
  
  //Euler angles
  IMU.getEuler(ang);
  
  //drone talk
  //talkToDrone();
  
  Serial.print(F("Roll:  "));
  Serial.print(ang[0]);
  Serial.print(F(" deg"));
  Serial.print(F("\t"));
  Serial.print(F("Pitch:  "));
  Serial.print(ang[1]);
  Serial.print(F(" deg"));
  Serial.print(F("\t"));
  Serial.print(F("Yaw:  "));
  Serial.print(ang[2]);
  Serial.print(F(" deg"));
  Serial.print(F("\t"));
    
  //Quaternion values
  /*  
  IMU.getQ(q);
  Serial.print(q[0]);
  Serial.print("\t");
  Serial.print(q[1]);
  Serial.print("\t");
  Serial.print(q[2]);
  Serial.print("\t");
  Serial.println(q[3]);
  */
  
  //Somewhat Calibrated Values
  /*
  IMU.getValues(val);
  for (int i=0; i<9; i++) {
    Serial.print(val[i], 4);
    Serial.print("\t");
  }
  */
  
  
  //Flexors Output Values
  Serial.print(index_val);
  Serial.print(F("\t"));
  Serial.print(middle_val);
  Serial.print(F("\t"));
  Serial.print(ring_val);
  Serial.print(F("\t"));
  Serial.println(pinky_val);
   
  
  
  if (index_val < 50 && middle_val > 50 && ring_val > 50 && pinky_val > 50) {
    Serial.print(F("\nINDEX POINTER DETECTED\t"));
  } else if (index_val > 50 && middle_val > 50 && ring_val > 50 && pinky_val > 50) {
    Serial.print(F("\nFIST DETECTED\t"));
  } else if (index_val < 50 && middle_val > 50 && ring_val > 50 && pinky_val < 50) {
    Serial.print(F("\nROCKER DETECTED\t"));
  } else if (index_val > 50 && middle_val > 50 && ring_val > 50 && pinky_val < 50) {
    Serial.print(F("\nCOUNT 1 DETECTED\t"));
  } else if (index_val > 50 && middle_val > 50 && ring_val < 50 && pinky_val < 50) {
    Serial.print(F("\nCOUNT 2 DETECTED\t"));
  } else if (index_val > 90 && middle_val < 50 && ring_val < 50 && pinky_val < 50) {
    Serial.print(F("\nCOUNT 3 DETECTED\t"));
  } else if (index_val > 50 && middle_val < 50 && ring_val < 50 && pinky_val < 50) {
    Serial.print(F("\nOK DETECTED\t"));
  }
  
  delay(250);
}

void displayDriverMode(void)
{
  #ifdef CC3000_TINY_DRIVER
    Serial.println(F("CC3000 is configure in 'Tiny' mode"));
  #else
    Serial.print(F("RX Buffer : "));
    Serial.print(CC3000_RX_BUFFER_SIZE);
    Serial.println(F(" bytes"));
    Serial.print(F("TX Buffer : "));
    Serial.print(CC3000_TX_BUFFER_SIZE);
    Serial.println(F(" bytes"));
  #endif
}

/**************************************************************************/
/*!
    @brief  Tries to read the CC3000's internal firmware patch ID
*/
/**************************************************************************/
uint16_t checkFirmwareVersion(void)
{
  uint8_t major, minor;
  uint16_t version;
  
#ifndef CC3000_TINY_DRIVER  
  if(!cc3000.getFirmwareVersion(&major, &minor))
  {
    Serial.println(F("Unable to retrieve the firmware version!\r\n"));
    version = 0;
  }
  else
  {
    Serial.print(F("Firmware V. : "));
    Serial.print(major); Serial.print(F(".")); Serial.println(minor);
    version = major; version <<= 8; version |= minor;
  }
#endif
  return version;
}

/**************************************************************************/
/*!
    @brief  Tries to read the 6-byte MAC address of the CC3000 module
*/
/**************************************************************************/
void displayMACAddress(void)
{
  uint8_t macAddress[6];
  
  if(!cc3000.getMacAddress(macAddress))
  {
    Serial.println(F("Unable to retrieve MAC Address!\r\n"));
  }
  else
  {
    Serial.print(F("MAC Address : "));
    cc3000.printHex((byte*)&macAddress, 6);
  }
}


/**************************************************************************/
/*!
    @brief  Tries to read the IP address and other connection details
*/
/**************************************************************************/
bool displayConnectionDetails(void)
{
  uint32_t ipAddress, netmask, gateway, dhcpserv, dnsserv;
  
  if(!cc3000.getIPAddress(&ipAddress, &netmask, &gateway, &dhcpserv, &dnsserv))
  {
    Serial.println(F("Unable to retrieve the IP Address!\r\n"));
    return false;
  }
  else
  {
    Serial.print(F("\nIP Addr: ")); cc3000.printIPdotsRev(ipAddress);
    Serial.print(F("\nNetmask: ")); cc3000.printIPdotsRev(netmask);
    Serial.print(F("\nGateway: ")); cc3000.printIPdotsRev(gateway);
    Serial.print(F("\nDHCPsrv: ")); cc3000.printIPdotsRev(dhcpserv);
    Serial.print(F("\nDNSserv: ")); cc3000.printIPdotsRev(dnsserv);
    Serial.println();
    return true;
  }
}

/**************************************************************************/
/*!
    @brief  Begins an SSID scan and prints out all the visible networks
*/
/**************************************************************************/

void listSSIDResults(void)
{
  uint8_t valid, rssi, sec, index;
  char ssidname[33]; 

  index = cc3000.startSSIDscan();

  Serial.print(F("Networks found: ")); Serial.println(index);
  Serial.println(F("================================================"));

  while (index) {
    index--;

    valid = cc3000.getNextSSID(&rssi, &sec, ssidname);
    
    Serial.print(F("SSID Name    : ")); Serial.print(ssidname);
    Serial.println();
    Serial.print(F("RSSI         : "));
    Serial.println(rssi);
    Serial.print(F("Security Mode: "));
    Serial.println(sec);
    Serial.println();
  }
  Serial.println(F("================================================"));

  cc3000.stopSSIDscan();
}

void connect_drone() {
  cc3000.getIPAddress(&ipAddress, &netmask, &gateway, &dhcpserv, &dnsserv);
  Adafruit_CC3000_Client drone_control = cc3000.connectUDP(ipAddress, ARDRONE_AT_PORT);
  Adafruit_CC3000_Client drone_comms = cc3000.connectUDP(ipAddress, ARDRONE_NAVDATA_PORT);
  if (drone_comms.connected()) {
    Serial.println(F("Drone talking"));
    nav_data_init();
  } else {
    Serial.println(F("Drone not talking"));
  }
  if (drone_control.connected()) {
    Serial.println(F("Drone ready for Commands"));
    periodicTask();
  } else {
    Serial.print(F("Drone having none of it..."));
  }
  /*
  while (drone_config.connected()) {
    Serial.println(F("Drone Configged"));
    while (drone_config.available()) {
      char c = drone_config.read();
      Serial.print(c);
    }
  } 
  */
 /*
    Serial.println(F("Closing Comms"));
  drone_comms.close();
  Serial.println(F("-----------------------------------------------------------"));*/
}

void talkToDrone() {
  Adafruit_CC3000_Client drone_control = cc3000.connectUDP(ipAddress, ARDRONE_AT_PORT);
  Adafruit_CC3000_Client drone_comms = cc3000.connectUDP(ipAddress, ARDRONE_NAVDATA_PORT);
  if (drone_control.connected() && run_count == 0) {
    Serial.print(F("Sending Takeoff Sequence."));
    calib();
    takeOff();
    watchdog();
    drone_control.print(cmd);
    cmd[0] = 0;
    Serial.print(F("."));
    Serial.println(F(".Done!"));
    run_count++;
    sequence = 0;
  } else if (drone_control.connected() && run_count > 0) {
    Serial.println(F("Sending Hover Sequence."));
    hover();
    watchdog();
    drone_control.print(cmd);
    cmd[0] = 0;
    Serial.print(F("."));
    Serial.println(F(".Done!"));
    run_count++;
    sequence = 0;
  }
}

void nav_data_init() {
  Adafruit_CC3000_Client drone_comms = cc3000.connectUDP(ipAddress, ARDRONE_NAVDATA_PORT);
  if (drone_comms.connected()) {
    drone_comms.write(float(0));
  }
 
  if (drone_comms.connected()) {
    if (drone_comms.available()) {
      Serial.println(F("Receiving Telemetry Data"));
      char c = drone_comms.read();
      Serial.print(c);
    }
  }
}



void takeOff() {
  snprintf(cmd , strlen(cmd), "AT*REF=%d,290718208\r", sequence++);//add \r
}

void land() {
  snprintf(cmd , strlen(cmd), "AT*REF=%d,290717696\r", sequence++);
}

void hover() {
  snprintf(cmd , strlen(cmd), "AT*PCMD=%d,1,0,0,0,0\r", sequence++);
}

void calib() {
  snprintf(cmd, strlen(cmd), "AT*FTRIM=%d\r", sequence++);
}

void watchdog() {
  snprintf(cmd, strlen(cmd), "AT*COMWDG\r", sequence++);
}

void periodicTask() {
  unsigned long current_t = millis();
  static unsigned long next_t = 0;
  if (current_t > next_t) {
    talkToDrone();
    next_t = current_t + ARDRONE_AT_CMD_INTERVAL;
  }
}

User avatar
adafruit_support_rick
 
Posts: 35092
Joined: Tue Mar 15, 2011 11:42 am

Re: CC3000 WiFi Shield Connected to a Parrot AR Drone 2.0 UD

Post by adafruit_support_rick »

Why are you constantly opening and closing your UDP sockets? Wouldn't it be better to just open them once and be done with it?

User avatar
PBathula
 
Posts: 3
Joined: Wed Aug 20, 2014 11:00 am

Re: CC3000 WiFi Shield Connected to a Parrot AR Drone 2.0 UD

Post by PBathula »

I wasn't too sure if my debug messages would come through to the serial monitor if I just left the sockets open. I'll open them and leave them open though, see if it makes a difference. It turns out I need to send some form of command to the drone every 30ms at best or at a minimum of every 100ms to not lose the wifi connection. Is this actually possible with the CC3000? If it is then I can strike that off my list of issues and just focus on my string command construction.
Cheers

User avatar
adafruit_support_rick
 
Posts: 35092
Joined: Tue Mar 15, 2011 11:42 am

Re: CC3000 WiFi Shield Connected to a Parrot AR Drone 2.0 UD

Post by adafruit_support_rick »

PBathula wrote:It turns out I need to send some form of command to the drone every 30ms at best or at a minimum of every 100ms to not lose the wifi connection.
Not a problem.

User avatar
PBathula
 
Posts: 3
Joined: Wed Aug 20, 2014 11:00 am

Re: CC3000 WiFi Shield Connected to a Parrot AR Drone 2.0 UD

Post by PBathula »

great, I can focus on the real problem at hand :) With regards UDP connections is there a particular method that data has to be sent/ received or can I simply do client.write() etc. I've hooked up my gesture control glove to my xivley account via TCP successfully so I was inclined to use the same procedure via UDP for the drone

User avatar
adafruit_support_rick
 
Posts: 35092
Joined: Tue Mar 15, 2011 11:42 am

Re: CC3000 WiFi Shield Connected to a Parrot AR Drone 2.0 UD

Post by adafruit_support_rick »

client.write() etc. is fine.

Locked
Please be positive and constructive with your questions and comments.

Return to “Other Arduino products from Adafruit”