Permalink
Browse files

Initial commit of support for communicating with the ARDrone

  • Loading branch information...
1 parent 097bdc4 commit c93a8e6d5d2fc778cdb9d6e4708001f47324b50d @bmcdorman bmcdorman committed Mar 3, 2013
View
207 include/kovan/ardrone.h
@@ -0,0 +1,207 @@
+/**************************************************************************
+ * Copyright 2013 KISS Institute for Practical Robotics *
+ * *
+ * This file is part of libkovan. *
+ * *
+ * libkovan is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * libkovan is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with libkovan. Check the LICENSE file in the project root. *
+ * If not, see <http://www.gnu.org/licenses/>. *
+ **************************************************************************/
+
+#ifndef _ARDRONE_H_
+#define _ARDRONE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * \brief This function will establish a connection between the drone and the CBC for transmitting information.
+ */
+void drone_connect();
+
+/*
+ * \brief This function will be used to disconnect the drone from the CBC.
+ */
+void drone_disconnect();
+
+/**
+ * \brief This function will be used to make the drone takeoff and stabilize itself.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \post The drone should reach its normal operating height
+ */
+void drone_takeoff();
+
+/**
+ * \brief This function will be used to land the drone at its current position.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \post The drone should slowly descend to the ground from its current height.
+ */
+void drone_land();
+
+/**
+ * \brief This function will be used to immediately turn off the drone rotors or reset the emergency toggle
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \post The drone will stop moving and fall to the ground.
+ */
+void drone_emergency();
+
+/**
+ * \brief retrieves the cached battery value
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \return An integer representing the current battery level
+ * \TODO Specify the range in the above comment
+ */
+int drone_get_battery();
+
+/**
+ * \brief Retrieves the x value relative to the drones starting position. Negative values indicate
+ * the drone has moved to the left of it's starting position.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \return x value away from the drones starting position in milimeters
+ * \TODO verify it is in fact milimeters
+ */
+float drone_get_x();
+
+/**
+ * \brief Retrieves the y value relative to the drones starting position. Negative values indicate
+ * the drone has moved backwards from it's starting position.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \return y value away from the drones starting position in milimeters
+ * \TODO verify it is in fact milimeters
+ **/
+float drone_get_y();
+
+/**
+ * \brief Retrieves the y value relative to the drones starting position. Negative values indicate
+ * the drone has moved down from it's starting position.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \return z value away from the drones starting position in milimeters
+ * \TODO verify it is in fact milimeters also verify that the z can be negative is it negative from the ground or its air starting position
+ */
+float drone_get_z();
+
+/**
+ * \brief Retrieves the current velocity in the right or left direction.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \return A float indicating the millimeters per second
+ */
+float drone_get_x_velocity();
+
+/**
+ * \brief Retrieves the current velocity in the forward or backwards direction.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \return A float indicating the velocity in millimeters per second
+ */
+float drone_get_y_velocity();
+
+/**
+ * \brief Retrieves the current velocity in the upward or downwards direction.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \return A float indicating the velocity in millimeters per second
+ */
+float drone_get_z_velocity();
+
+/**
+ * \brief Retrieves the current rotation in the clockwise (positive) or counterclockwise (negative) direction.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \return A float indicating the degrees rotated from the original orientation
+ */
+float drone_get_yaw();
+
+/**
+ * \brief Switches the drone vision feed to the front facing camera
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ */
+void drone_front_camera();
+
+/**
+ * \brief Switches the drone vision feed to the downward facing camera
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ */
+void drone_down_camera();
+
+/**
+ * \brief Starts sending video data to the cbc for vision tracking.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ */
+void enable_drone_vision();
+
+/**
+ * \brief Stops the drone from sending data to the cbc for vision tracking.
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ */
+void disable_drone_vision();
+
+/**
+ * \brief Sets the Drone's Mac Address Pair to be the given string
+ * \param macAddress A string representing the Mac Address of your CBC
+ */
+void set_drone_Mac_Address(char * macAddress);
+
+/**
+ * \brief Tells the drone to move with the given parameters
+ * \param enable A value indicating if movement is enabled. 0 - True 1 - False
+ * \param x_tilt A value from zero to one indicating the percentage of maximum tilt in the left or right direction
+ * negative values are left and positive values are right. Ex: -.5 means Half of the total tilt left
+ * \param y_tilt A value from zero to one indicating the percentage of maximum tilt in the forward or backward direction
+ * negative values are left and positive values are right. Ex: -.5 means Half of the total tilt backwards.
+ * \param yaw_vel A value indicating the rotational velocity of the dronein milieters per second
+ * \param z_vel A value indicating the change in altitude in milimeters per second
+ */
+void drone_move(float x_tilt, float y_tilt, float yaw_vel, float z_vel);
+
+/**
+ * \brief Tells the drone that it should stop moving and hover at its current location
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ */
+void drone_hover();
+
+/**
+ * \brief Tells the drone to hover over the Roundel that it has detected
+ * \param shouldHover A integer representing if the drone should hover or not
+ * 1 == True meaning the drone will attempt to hover on a roundel
+ * 0 == False meaning the drone will resume normal flight
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ * \pre A roundel must have been detected in the drones vision tags
+ */
+void drone_hover_on_roundel(int shouldHover);
+
+/**
+ * \brief Switches the channel the drone's altitude meter operates on so that interferance does not occur between two drones
+ * \param channel A integer representing which channel to operate on
+ * 1 == True turns the drone to channel A
+ * 0 == False turns the drone to channel B
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ */
+void drone_set_ultrasound_channel(int channel);
+
+/**
+ * \brief Initializes the drone's onboard video detection
+ * \param channel A integer representing which channel to operate on
+ * 0 == Turns the drones detection off
+ * 1 == Detects green enemies via the front camera
+ * 2 == Detects yellow enemies via the front camera
+ * 3 == Detects blue enemies via the front camera
+ * 4 == Detects orange/green ground stripes via the bottom camera
+ * 5 == Detects yellow/blue ground stripes via the bottom camera
+ * 6 == Detects roundels via the bottom camera
+ * \pre drone_connect must have been previously called to establish a connection to the drone.
+ */
+void drone_set_detection(int detectType);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
View
62 include/kovan/ardrone.hpp
@@ -0,0 +1,62 @@
+/**************************************************************************
+ * Copyright 2013 KISS Institute for Practical Robotics *
+ * *
+ * This file is part of libkovan. *
+ * *
+ * libkovan is free software: you can redistribute it and/or modify *
+ * it under the terms of the GNU General Public License as published by *
+ * the Free Software Foundation, either version 2 of the License, or *
+ * (at your option) any later version. *
+ * *
+ * libkovan is distributed in the hope that it will be useful, *
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of *
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
+ * GNU General Public License for more details. *
+ * *
+ * You should have received a copy of the GNU General Public License *
+ * along with libkovan. Check the LICENSE file in the project root. *
+ * If not, see <http://www.gnu.org/licenses/>. *
+ **************************************************************************/
+
+#ifndef _ARDRONE_HPP_
+#define _ARDRONE_HPP_
+
+class DroneController;
+
+class ARDrone
+{
+public:
+ enum State
+ {
+ Disconnected = 0,
+ Landed,
+ TakenOff
+ };
+
+ ~ARDrone();
+
+ bool connect(const char *ip = "192.168.1.1");
+ void disconnect();
+
+ void flatTrim();
+ void takeoff();
+ void land();
+
+ void hover();
+ void move(const float x, const float y, const float z, const float yaw);
+
+ ARDrone::State state() const;
+
+ static ARDrone *instance();
+
+private:
+ ARDrone();
+
+ DroneController *m_controller;
+
+ int m_controlFd;
+};
+
+
+
+#endif
View
1 include/kovan/kovan.h
@@ -27,6 +27,7 @@
#ifndef _KOVAN_H_
#define _KOVAN_H_
+#include "ardrone.h"
#include "audio.h"
#include "motors.h"
#include "servo.h"
View
1 include/kovan/kovan.hpp
@@ -41,6 +41,7 @@
#define _KOVAN_HPP_
#include "motors.hpp"
+#include "ardrone.hpp"
#include "servo.hpp"
#include "analog.hpp"
#include "digital.hpp"
View
311 src/ardrone.cpp
@@ -0,0 +1,311 @@
+#include "kovan/ardrone.hpp"
+#include "kovan/thread.hpp"
+#include "kovan/util.h"
+#include "ardrone_constants_p.hpp"
+
+#include <arpa/inet.h>
+#include <netinet/in.h>
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <cstdlib>
+#include <cstdio>
+#include <cstring>
+
+#include <stack>
+
+class DroneSequencer
+{
+public:
+ typedef unsigned sequence_t;
+
+ DroneSequencer();
+ void wait(const unsigned times = 1);
+ DroneSequencer::sequence_t next();
+ void reset();
+
+private:
+ sequence_t m_current;
+};
+
+DroneSequencer::DroneSequencer()
+{
+ reset();
+}
+
+void DroneSequencer::wait(const unsigned times)
+{
+ sequence_t current = m_current;
+ while(current + times < m_current) msleep(10);
+}
+
+DroneSequencer::sequence_t DroneSequencer::next()
+{
+ return ++m_current;
+}
+
+void DroneSequencer::reset()
+{
+ m_current = 0;
+}
+
+struct DroneCommand
+{
+ DroneCommand();
+
+ char data[ARDRONE_MAX_CMD_LENGTH];
+};
+
+DroneCommand::DroneCommand()
+{
+ memset(data, 0, ARDRONE_MAX_CMD_LENGTH);
+}
+
+class DroneController : public Thread
+{
+public:
+ DroneController();
+ ~DroneController();
+
+ void setup(const sockaddr_in &addr, int controlFd);
+ void invalidate();
+
+ void flatTrim();
+
+ void land(const bool emergency = false);
+ void takeoff();
+
+ void move(const float x, const float y, const float z, const float yaw);
+
+ void run();
+
+ void stop();
+ bool isStopped() const;
+
+private:
+ void pushCommand(const char *const command);
+ void popCommand();
+
+ void oneTimeCommand(const char *const command);
+
+ bool update();
+
+ sockaddr_in m_addr;
+ int m_controlFd;
+ Mutex m_mutex;
+ std::stack<DroneCommand> m_commandStack;
+ DroneSequencer m_seq;
+ bool m_stop;
+};
+
+DroneController::DroneController()
+ : m_controlFd(-1)
+{
+}
+
+DroneController::~DroneController()
+{
+}
+
+void DroneController::setup(const sockaddr_in &addr, int controlFd)
+{
+ m_addr = addr;
+ m_controlFd = controlFd;
+}
+
+void DroneController::invalidate()
+{
+ m_mutex.lock();
+ m_controlFd = -1;
+ m_mutex.unlock();
+}
+
+void DroneController::flatTrim()
+{
+ char command[ARDRONE_MAX_CMD_LENGTH];
+ sprintf(command, "%s%%u\r", ARDRONE_AT_FTRIM);
+ oneTimeCommand(command);
+}
+
+void DroneController::land(const bool emergency)
+{
+ popCommand();
+ char command[ARDRONE_MAX_CMD_LENGTH];
+ sprintf(command, "%s%%u,%u\r", ARDRONE_AT_REF, 0x11540000 | ((emergency ? 1 : 0) << 8));
+ pushCommand(command);
+}
+
+void DroneController::takeoff()
+{
+ popCommand();
+ char command[ARDRONE_MAX_CMD_LENGTH];
+ sprintf(command, "%s%%u,%u\r", ARDRONE_AT_REF, 0x11540200);
+ pushCommand(command);
+}
+
+void DroneController::move(const float x, const float y, const float z, const float yaw)
+{
+ unsigned ix = *(unsigned *)&x;
+ unsigned iy = *(unsigned *)&y;
+ unsigned iz = *(unsigned *)&z;
+ unsigned iyaw = *(unsigned *)&yaw;
+
+ const bool hoverMode = x == 0.0f && y == 0.0f
+ && z == 0.0f && yaw == 0.0f;
+
+ popCommand();
+ char command[ARDRONE_MAX_CMD_LENGTH];
+ sprintf(command, "%s%%u,%u,%u,%u,%u,%u\r", ARDRONE_AT_PCMD, hoverMode ? 0 : 1,
+ ix, iy, iz, iyaw);
+ pushCommand(command);
+}
+
+void DroneController::run()
+{
+ while(!m_stop) {
+ m_mutex.lock();
+ bool success = update();
+ m_mutex.unlock();
+ if(!success) break;
+ msleep(33); // ~30 FPS
+ }
+ m_stop = false;
+ m_seq.reset();
+}
+
+void DroneController::stop()
+{
+ m_mutex.lock();
+ m_stop = true;
+ m_mutex.unlock();
+}
+
+bool DroneController::isStopped() const
+{
+ return m_stop;
+}
+
+void DroneController::pushCommand(const char *const command)
+{
+ DroneCommand container;
+ strncpy(container.data, command, ARDRONE_MAX_CMD_LENGTH);
+
+ m_mutex.lock();
+ m_commandStack.push(container);
+ m_mutex.unlock();
+}
+
+void DroneController::popCommand()
+{
+ m_mutex.lock();
+ if(!m_commandStack.empty()) m_commandStack.pop();
+ m_mutex.unlock();
+}
+
+void DroneController::oneTimeCommand(const char *const command)
+{
+ pushCommand(command);
+ m_seq.wait(10);
+ popCommand();
+}
+
+bool DroneController::update()
+{
+ // Nothing to do.
+ if(m_commandStack.empty() || m_controlFd < 0) return true;
+
+ char realCommand[ARDRONE_MAX_CMD_LENGTH];
+ sprintf(realCommand, m_commandStack.top().data, m_seq.next());
+ printf("Sending %s\n", realCommand);
+ if(sendto(m_controlFd, realCommand, strlen(realCommand), 0,
+ (const sockaddr *)&m_addr, sizeof(m_addr)) < 0) {
+ perror("DroneController::run -> sendto");
+ return false;
+ }
+
+ return true;
+}
+
+ARDrone::~ARDrone()
+{
+ disconnect();
+}
+
+bool ARDrone::connect(const char *ip)
+{
+ m_controlFd = socket(AF_INET, SOCK_DGRAM, 0);
+ if(m_controlFd < 0) return false;
+
+ sockaddr_in si_other;
+ memset(&si_other, 0, sizeof(si_other));
+ si_other.sin_family = AF_INET;
+ si_other.sin_port = htons(ARDRONE_AT_PORT);
+ if(inet_aton(ip, &si_other.sin_addr) == 0) {
+ perror("inet_aton");
+ close(m_controlFd);
+ return false;
+ }
+
+ m_controller->setup(si_other, m_controlFd);
+
+ return true;
+}
+
+void ARDrone::disconnect()
+{
+ // Already disconnected?
+ if(m_controlFd < 0) return;
+
+ m_controller->land(true);
+ msleep(100);
+ // while(state() != ARDrone::Landed) msleep(100);
+
+ m_controller->invalidate();
+
+ close(m_controlFd);
+}
+
+void ARDrone::flatTrim()
+{
+ m_controller->flatTrim();
+}
+
+void ARDrone::takeoff()
+{
+ m_controller->takeoff();
+}
+
+void ARDrone::land()
+{
+ m_controller->land();
+}
+
+void ARDrone::hover()
+{
+ m_controller->move(0.0f, 0.0f, 0.0f, 0.0f);
+}
+
+void ARDrone::move(const float x, const float y, const float z, const float yaw)
+{
+ m_controller->move(x, y, z, yaw);
+}
+
+ARDrone::State ARDrone::state() const
+{
+ // TODO: Stub
+ return ARDrone::Disconnected;
+}
+
+ARDrone *ARDrone::instance()
+{
+ static ARDrone s_instance;
+ return &s_instance;
+}
+
+ARDrone::ARDrone()
+ : m_controller(new DroneController)
+ , m_controlFd(-1)
+{
+ m_controller->start();
+}
View
20 src/ardrone_constants_p.hpp
@@ -0,0 +1,20 @@
+#ifndef _ARDRONE_CONSTANTS_P_HPP_
+#define _ARDRONE_CONSTANTS_P_HPP_
+
+#define ARDRONE_AT_PREFIX "AT*"
+#define ARDRONE_AT_SUFFIX "="
+
+#define ARDRONE_AT_REF (ARDRONE_AT_PREFIX "REF" ARDRONE_AT_SUFFIX)
+#define ARDRONE_AT_PCMD (ARDRONE_AT_PREFIX "PCMD" ARDRONE_AT_SUFFIX)
+#define ARDRONE_AT_FTRIM (ARDRONE_AT_PREFIX "FTRIM" ARDRONE_AT_SUFFIX)
+#define ARDRONE_AT_CONFIG (ARDRONE_AT_PREFIX "CONFIG" ARDRONE_AT_SUFFIX)
+#define ARDRONE_AT_LED (ARDRONE_AT_PREFIX "LED" ARDRONE_AT_SUFFIX)
+#define ARDRONE_AT_ANIM (ARDRONE_AT_PREFIX "ANIM" ARDRONE_AT_SUFFIX)
+#define ARDRONE_AT_COMWDG (ARDRONE_AT_PREFIX "COMWDG" ARDRONE_AT_SUFFIX)
+
+#define ARDRONE_CMD_SEPARATOR ((char)10)
+#define ARDRONE_MAX_CMD_LENGTH (1024)
+
+#define ARDRONE_AT_PORT (5556)
+
+#endif
View
3 src/thread.cpp
@@ -55,9 +55,10 @@ Mutex::Mutex(const Mutex &)
{
}
-void *__runThread(void *data)
+static void *__runThread(void *data)
{
Thread *t = reinterpret_cast<Thread *>(data);
+ if(!t) return NULL;
t->run();
return NULL;
}
View
1 test/CMakeLists.txt
@@ -1,3 +1,4 @@
+add_subdirectory(ardrone)
add_subdirectory(battery)
add_subdirectory(draw)
add_subdirectory(servo)
View
2 test/ardrone/CMakeLists.txt
@@ -0,0 +1,2 @@
+ADD_EXECUTABLE(ardrone ardrone.cpp)
+TARGET_LINK_LIBRARIES(ardrone kovan)
View
29 test/ardrone/ardrone.cpp
@@ -0,0 +1,29 @@
+#include "kovan/kovan.hpp"
+#include <cstdlib>
+
+int main(int argc, char *argv[])
+{
+ ARDrone *drone = ARDrone::instance();
+ if(!drone->connect()) {
+ return EXIT_FAILURE;
+ }
+
+ drone->flatTrim();
+ msleep(100);
+
+ drone->takeoff();
+ msleep(5000);
+
+ drone->move(0.1f, 0.0f, 0.0f, 1.0f);
+ msleep(10000);
+
+ drone->hover();
+ msleep(10000);
+
+ drone->land();
+ msleep(2000);
+
+ drone->disconnect();
+
+ return EXIT_SUCCESS;
+}

0 comments on commit c93a8e6

Please sign in to comment.