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;
}
}