Ardrone.h

// Ardrone.h
//
// Remote Control module for AR.Drone
///
/// \mainpage Remote Control module for AR.Drone
///
/// This is the Ardrone library.
/// It provides a class for easy interfacing to Parrot AR.Drone quad-copter http://www.parrot.com via WiFi.
/// 
/// The version of the package that this documentation refers to can be downloaded 
/// from http://www.airspayce.com/mikem/arduino/Ardrone/Ardrone-1.3.zip
/// You can find the latest version at http://www.airspayce.com/mikem/arduino/Ardrone
///
/// \par Prerequisites
///
/// - WiShield or Yellowjacket (http://asynclabs.com) WiFi interface hardware
/// - WiShield library
///
/// You MUST alter the WiShield library header files to enable the UDP app support in Wishield that Ardrone needs. 
/// See below.
///
/// You MUST pre-configure the AR.Drone to accept WiShield or Yellowjacket connections.
/// See below.
///
/// \par WiShield Library Configuration
/// Requires the Asynclabs WiShield library. See http://asynclabs.com/wiki/index.php?title=WiShield_library
/// Install the  WiShield library in the libraries directory of your arduino IDE installation, 
/// then follow the configuration steps below:
///
/// Support of RSSI (receiver signal strength indicator) requires mods to WiShield library g2100.c as per 
/// http://asynclabs.com/forums/viewtopic.php?f=10&t=385&start=0. You dont have to add this but its a 
/// good feature.
///
/// Correct operation of the WiShield requires you to set the jumper on the WiShield to INT0 or DIG8 to select 
/// the arduino pin to use for WiShield interrupts, and also to make sure it agrees with the settings of 
/// USE_DIG0_INTR or  USE_DIG8_INTR in spi.h in the WiShield library (which defaults to 
/// using Arduino digital pin 2, and which means setting the WiShield jumper to INT0 setting). Yes, the naming
/// conventions are inconsistent :-(. In summary:
/// \code
///  WiShield jumper   spi.h          Arduino
///       INT0        USE_DIG0_INTR  Digital pin 2
///       D8          USE_DIG8_INTR  Digital pin 8
/// \endcode
///
/// For YellowJacket (which has no jumper), leave it as USE_DIG0_INTR.
///
/// In order for WiShield library to support UDP (as needed by this module), 
/// you MUST set UIP_CONF_UDP to 1 in uip-conf.h. This is an unfortunate but necessary requirement, 
/// otherwise UDP support will not be compiled into the WiShield library. 
/// Further, you must edit apps-conf.h and make sure the only APP_* defined is APP_UDPAPP. 
/// Failure to do this will cause compile errors. A modified version of the WiShield library already 
/// edited for use with Ardrone and Arduino 1.0 is available at 
/// http://www.airspayce.com/mikem/arduino/WiShield-v1.3.0-0-mikem-RCKit.zip
///
/// WiShield will work with Arduino Mega, but with difficulty. The problem is that with the Mega, the SPI 
/// pins that are required for interface with WiShield come out on different pins to the smaller form 
/// factor arduinos like Diecimila and Duemilanove. So, to make the Mega work with the WiShield, you 
/// have to reroute the SPI pin to different Arduino pins, as per 
/// http://asynclabs.com/forums/viewtopic.php?f=13&t=19&hilit=mega&start=10
///
/// \par AR.Drone pre-configuration
///
/// As delivered, the AR.Drone does not support the 1Mbit/sec data rate which is the only data rate WiShield supports.
/// Without this Ardrone module will associate correctly with the AR.Drone. Experienced Linux users can follow these steps.
/// If you are inexperienced unsure of some steps, more detail is at:
/// http://www.rcgroups.com/forums/showthread.php?t=1335257&pp=50#post16498030
/// Caution: if you do not follow these steps precisely, you may make your AR.Drone uncontactable by WiFi.
///
/// - Establish an ad-hoc WiFi connection from your computer to your drone
/// - Telnet to the drone at 192.168.1.1. Should get a BusyBox prompt
/// - Edit Wifi initialisation script, with vi /bin/wifi_setup.sh
/// - Alter the line which reads:
/// \code
///   wmiconfig -i ath0 --setfixrates 3 8 11
/// \endcode
/// to read
/// \code
///   wmiconfig -i ath0 --setfixrates 1 3 8 11
/// \endcode
/// - Stop editing and save the file with:
/// \code
/// :wq
/// \endcode
/// Save the changes to flash with
/// \code
/// sync
/// \endcode
/// and wait a minute or so
/// - Make sure the script is still working with:
/// \code
/// # /bin/wifi_setup.sh
/// bmiloader: ath0: Operation not permitted
/// bmiloader: ath0: Operation not permitted
/// bmiloader: ath0: Operation not permitted
/// bmiloader: ath0: Operation not permitted
/// sh: 0x0: unknown operand
/// ......
/// \endcode
/// - Unplug the drone battery and restart it.
///
/// \par Example programs
///
/// Example Arduino programs are included to show the main modes of use.
///
/// The following example programs are provided:
/// - ardrone_test: Simple demonstration that shows how to create and initialise an Ardrone instance
/// - usbjoystick_test: Shows how to use both Ardrone and USBJoystick libraries to control an AR.Drone
///  using a USB Joystick or game pad. Requires USB Host Shield hardware, USB_Host_Shield library USB Joystick, 
/// along with the USBJoystick library from http://www.airspayce.com/mikem/arduino/USBJoystick
///
/// \par Installation
///
/// Install in the usual way: unzip the distribution zip file to the libraries
/// sub-folder of your sketchbook. 
///
/// \author  Mike McCauley (mikem@airspayce.com)
///
/// This software is Copyright (C) 2011 Mike McCauley. Use is subject to license
/// conditions. The main licensing options available are GPL V2 or Commercial:
/// 
/// \par Open Source Licensing GPL V2
/// This is the appropriate option if you want to share the source code of your
/// application with everyone you distribute it to, and you also want to give them
/// the right to share who uses it. If you wish to use this software under Open
/// Source Licensing, you must contribute all your source code to the open source
/// community in accordance with the GPL Version 2 when your application is
/// distributed. See http://www.gnu.org/copyleft/gpl.html
/// 
/// \par Commercial Licensing
/// This is the appropriate option if you are creating proprietary applications
/// and you are not prepared to distribute and share the source code of your
/// application. Contact info@airspayce.com for details.
///
/// \par Revision History
/// - Version 1.0: Initial release
/// - Version 1.1: Fixed compile error in ardrone_test example.
/// - Version 1.2: Compiles on Arduino 1.0. Requires latest version of 
///                http://www.airspayce.com/mikem/arduino/WiShield-v1.3.0-0-mikem-RCKit.zip
///                which now compiles on Arduino 1.0
/// - Version 1.3: Updated author and distribution location details to airspayce.com
///


// Copyright (C) 2010 Mike McCauley
// $Id: Ardrone.h,v 1.1 2011/03/16 23:46:12 mikem Exp mikem $

#ifndef Ardrone_h
#define Ardrone_h

#include <WiShield.h>
#include <inttypes.h>

/////////////////////////////////////////////////////////////////////
/// \class Ardrone Ardrone.h <Ardrone.h>
/// \brief Remote Control module for AR.Drone
///
/// This class provides a simple interface to the AR.Drone over a WiFi connection.
///
/// \par Overview
///
/// This module presents a simple interface through wchich you can control and fly a AR.Drone.
/// It requires a WiFi interface hardware and the WiShield library.
/// It initialises the WiFi library, hardware and drone, then sends a stream of flying commands 
/// to the drone's AT UDP port. Member functions allow you to control the pitch, roll, yaw and altitude of the 
/// drone, and to make it land and take off, based on some input device (or perhaps a canned sequence of commands).
///
/// \par Initialisation
///
/// This module intialises the drone in the following way:
/// - initialises the WiShield WiFi hardware to connect to the ad-hoc network whose name is given by ssid
/// - Makes a TCP connection to ARDRONE_CONFIGDATA_PORT. This forces ARP to be done, and 
///   waits for the drone to be booted and available. The following AT cpommands are then sent to the 
///   ARDRONE_AT_PORT UDP port on the drone.
/// - Sets the control:outdoor, control:control_yaw, control:control_vz_max, control:euler_angle_max,
///   control:altitude_max configuration parameters.
/// - sets control_level to 1
/// - Sets a FTRIM command which indicates the drone is level
/// - flashes the LEDs on teh drone.
///
/// After initialisation, the update() function sends PCMD, REF and COMWDG commands every 
/// ARDRONE_AT_CMD_INTERVAL milliseconds.
/// 
class Ardrone
{
#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

public:

    /// \brief Defines types of LED animation for use with addAtLedAnimCmd()
    ///
    /// Each animation produces a different type of flashing pattern
    typedef enum {
        ARDRONE_LED_ANIMATION_BLINK_GREEN_RED = 0,
        ARDRONE_LED_ANIMATION_BLINK_GREEN,
        ARDRONE_LED_ANIMATION_BLINK_RED,
        ARDRONE_LED_ANIMATION_BLINK_ORANGE,
        ARDRONE_LED_ANIMATION_SNAKE_GREEN_RED,
        ARDRONE_LED_ANIMATION_FIRE,
        ARDRONE_LED_ANIMATION_STANDARD,
        ARDRONE_LED_ANIMATION_RED,
        ARDRONE_LED_ANIMATION_GREEN,
        ARDRONE_LED_ANIMATION_RED_SNAKE,
        ARDRONE_LED_ANIMATION_BLANK,
        ARDRONE_LED_ANIMATION_RIGHT_MISSILE,
        ARDRONE_LED_ANIMATION_LEFT_MISSILE,
        ARDRONE_LED_ANIMATION_DOUBLE_MISSILE,
        ARDRONE_LED_ANIMATION_FRONT_LEFT_GREEN_OTHERS_RED,
        ARDRONE_LED_ANIMATION_FRONT_RIGHT_GREEN_OTHERS_RED,
        ARDRONE_LED_ANIMATION_REAR_RIGHT_GREEN_OTHERS_RED,
        ARDRONE_LED_ANIMATION_REAR_LEFT_GREEN_OTHERS_RED,
        ARDRONE_LED_ANIMATION_LEFT_GREEN_RIGHT_RED,
        ARDRONE_LED_ANIMATION_LEFT_RED_RIGHT_GREEN,
        ARDRONE_LED_ANIMATION_BLINK_STANDARD,
    } LEDAnimation;

    /// \brief Defines types of animation for use with addAtAnimCmd()
    ///
    /// Each animation produces a different type of preprogrammed pattern of movement, superposed
    /// on the flying commands
    typedef enum {
        ARDRONE_ANIMATION_PHI_M30_DEG = 0,
        ARDRONE_ANIMATION_PHI_30_DEG,
        ARDRONE_ANIMATION_THETA_M30_DEG,
        ARDRONE_ANIMATION_THETA_30_DEG,
        ARDRONE_ANIMATION_THETA_20DEG_YAW_200DEG,
        ARDRONE_ANIMATION_THETA_20DEG_YAW_M200DEG,
        ARDRONE_ANIMATION_TURNAROUND,
        ARDRONE_ANIMATION_TURNAROUND_GODOWN,
        ARDRONE_ANIMATION_YAW_SHAKE,
        ARDRONE_ANIMATION_YAW_DANCE,
        ARDRONE_ANIMATION_PHI_DANCE,
        ARDRONE_ANIMATION_THETA_DANCE,
        ARDRONE_ANIMATION_VZ_DANCE,
        ARDRONE_ANIMATION_WAVE,
        ARDRONE_ANIMATION_PHI_THETA_MIXED,
        ARDRONE_ANIMATION_DOUBLE_PHI_THETA_MIXED,
    } Animation;
    
    /// Constructor. 
    /// After contruction and initialisation, call the init() and run() functions.
    /// You must also define the SSID of the AR.Drone's ad-hoc wireless network, like this:
    /// \code
    /// Ardrone ardrone;
    /// const prog_char ssid[] PROGMEM = "ardrone_165416";
    /// \endcode
    /// where ardrone_165416 is replaced with the actual network name for your particular AR.Drone.
    Ardrone();

    /// Initialises the wireless WiFi receiver
    /// Call once at startup time after addresses, flight parameters etc have been configured.
    void init();

    /// Call this to process pending Wireless events. Call this as often as possible in your
    /// main loop. Runs the wireless driver stack, which calls periodicTask(), among other things
    void run();

    /// Sets the intialisation value for control:control_yaw
    /// which limits the maximum rate of change of yaw
    /// Defaults to 1.75 radians/sec
    /// \param[in] control_yaw maximum rate of change of yaw, radians/second
    void setControlYaw(float control_yaw);

    /// Sets the intialisation value for control:control_vz_max
    /// which limits the maximum rate of change of altitude
    /// Defaults to 700.0 mm/sec
    /// \param[in] control_vz_max maximum rate of change of altitude, mm/second
    void setControlVZMax(float control_vz_max);

    /// Sets the intialisation value for control:euler_angle_max
    /// which limits the maximum roll and pitch tilt
    /// Defaults to 0.1 radians
    /// \param[in] euler_angle_max maximum tilt for roll and pitch in radians
    void setEulerAngleMax(float euler_angle_max);

    /// Sets the intialisation value for control:altitude_max
    /// which limits the maximum height
    /// Defaults to 3000.0 mm
    /// \param[in] altitude_max maximum altitude in mm
    void setAlitudeMax(float altitude_max);

    /// Sets the intialisation value for control:outdoor
    /// which specifies whether the default drone limits for indoor or outdoor flying should be used
    /// Defaults to false.
    /// \param[in] outdoor true if flying outdoors
    void setOutdoor(float outdoor);

    /// Sets the flying parameter
    /// Causes the drone to take off or land. Defaults at initialisation to false.
    /// \param[in] flying If true, drone will take off to 1m hover. If false, drone will land.
    void    setFlying(boolean flying);

    /// Returns the most recently set value of the flying parameter.
    /// \return the current value of the flying parameter
    boolean flying();

    /// Sets the roll parameter. Range from -1.0 (roll left to euler_angle_max radians) 
    /// to +1.0 (roll right to euler_angle_max radians). 0.0 means level.  Defaults at initialisation to 0.0.
    /// The new roll parameter will be sent during the next update() if the drone is flying.
    /// \param[in] roll The new roll value
    void    setRoll(float roll);

    /// Returns the most recently set value of the roll parameter.
    /// \return The current roll value;
    float   roll();

    /// Sets the pitch parameter. Range from -1.0 (pitch forward to euler_angle_max radians) 
    /// to +1.0 (pitch back to euler_angle_max radians). 0.0 means level.  Defaults at initialisation to 0.0.
    /// The new pitch parameter will be sent during the next update() if the drone is flying.
    /// \param[in] pitch The new pitch value
    void    setPitch(float pitch);

    /// Returns the most recently set value of the pitch parameter.
    /// \return The current pitch value;
    float   pitch();

    /// Sets the gaz (rate of change of altitude). Range from -1.0 (descend at control_vz_max mm/sec) 
    /// to +1.0 (ascend at control_vz_max mm/sec). 0.0 means level.  Defaults at initialisation to 0.0.
    /// The new gaz parameter will be sent during the next update() if the drone is flying.
    /// \param[in] gaz The new gaz value
    void    setGaz(float gaz);

    /// Returns the most recently set value of the gaz parameter.
    /// \return The current gaz value;
    float   gaz();

    /// Sets the yaw (rate of change of yaw). Range from -1.0 (rotate left at control_yaw radians/sec) 
    /// to +1.0 (rotate right at control_yaw radians/sec). 0.0 means no rotation.  Defaults at initialisation to 0.0.
    /// The new yaw parameter will be sent during the next update() if the drone is flying.
    /// \param[in] yaw The new yaw value
    void    setYaw(float yaw);

    /// Returns the most recently set value of the yaw parameter.
    /// \return The current yaw value;
    float   yaw();

    /// For internal use only.
    /// Indicates a successful connection to the drone has been made, and initialisation can commence
    void    connected();

    /// For internal use only.
    /// Runs various periodic tasks for the drone
    void    periodicTask();

    /// Adds a drone PCMD comamnd to the command buffer, which will be sent during the next update();
    /// \param[in] flags Bitmask of ARDRONE_PCMD_FLAG_* flags
    /// \param[in] roll The new roll command value.
    /// \param[in] pitch The new pitch command value.
    /// \param[in] gaz The new gaz command value.
    /// \param[in] yaw The new yaw command value.
    void addAtPCmd(unsigned int flags, float roll, float pitch, float gaz, float yaw);

    /// Adds a drone CONFIG comamnd to the command buffer, which will be sent during the next update();
    /// \param[in] name String name of the config parameter to change
    /// \param[in] value String value to change it to
    void addAtConfigCmd(const char* name, const char* value);

    /// Adds a drone LED comamnd to the command buffer, which will be sent during the next update();
    /// \param[in] animation One of the LEDAnimation values, which specifies the type of flashing pattern to use
    /// \param[in] frequency Flashing frequency in Hz.
    /// \param[in] duration Period of time to run the animation for in seconds
    void addAtLedCmd(Ardrone::LEDAnimation animation = ARDRONE_LED_ANIMATION_BLINK_GREEN_RED, float frequency = 5.0, uint16_t duration = 5);

    /// Adds a drone ANIM comamnd to the command buffer, which will be sent during the next update();
    /// \param[in] animation One of the Animation values, which specifies the type of flying animation pattern to use
    /// \param[in] duration Period of time to run the animation for in seconds
    void addAtAnimCmd(Ardrone::Animation animation = ARDRONE_ANIMATION_WAVE, uint16_t duration = 5);

    /// Adds a drone FTRIM comamnd to the command buffer, which will be sent during the next update();
    void addAtFtrimCmd();

    /// Adds a drone REF comamnd to the command buffer, which will be sent during the next update();
    /// \param[in] flags Bitmask of ARDRONE_REF_FLAG_*
    void addAtRefCmd(uint32_t flags);

    /// Adds a drone COMWDG comamnd to the command buffer, which will be sent during the next update();
    void addAtComWdgCmd();

    /// Adds a drone PMODE comamnd to the command buffer, which will be sent during the next update();
    /// \param[in] mode  Not documented
    void addAtPmodeCmd(uint8_t mode);

    /// Adds a drone MISC comamnd to the command buffer, which will be sent during the next update();
    /// \param[in] a Not documented
    /// \param[in] b Not documented
    /// \param[in] c Not documented
    /// \param[in] d Not documented
    void addAtMiscCmd(uint16_t a, uint16_t b, uint16_t c, uint16_t d);

protected:

    /// For internal use only.
    /// Called periodically during device initialisation. Sends a series of commands to initialise
    /// the drone.
    void device_init();

    /// For internal use only.
    /// Called periodically after device initialisation. Sends any commands accumulated by earlier calls to 
    /// addAt*Cmd() functions.
    /// If the drone is flying sends a PCMD with the curent flying parameters
    /// Also sends a REF command with appropriate flags for flying or grounded
    /// Also sends a COMWDG command to refresh teh comms watchdog.
    void update();

    /// Sends a command to the drone AT port
    /// \param[in] msg The message to be sent
    /// \param[in] len The length of the message in bytes
    void sendAtCmd(uint8_t* msg, uint16_t len);

    /// Sends a string to the drone AT port
    /// \param[in] s The ASCII string to send
    void sendAtCmd(char* s);

    /// Sends the entire currently accumulated command buffer to the drone AT port
    void sendCurrentAtCmd();
    
private:
    uint8_t       _state;
    uint8_t       _init_step;
    unsigned long _sequence;

    // Initialisation values
    // These are used to set the CONFIG control: variables of the same name
    float         _control_yaw;
    float         _control_vz_max;
    float         _euler_angle_max;
    float         _altitude_max;
    boolean       _outdoor;

    // Flying command values
    boolean       _flying;
    float         _roll;
    float         _pitch;
    float         _gaz;
    float         _yaw;

};

#endif