Quadrotor

 /*!

  *  \file    interface.h

  *  \brief   Defines the MikrokopterInterface class

  *  \author  Andrew Chambers <achamber@andrew.cmu.edu>

  *  \author  Justin Haines   <jhaines@andrew.cmu.edu>

  *  \author  Sebastian Scherer <basti@andrew.cmu.edu>

  *  \date    2011 October 25

  *  \details This class allows communication with and control of a Mikrokopter rotorcraft via ROS

  */

#ifndef _MIKROKOPTER_INTERFACE_H_

#define _MIKROKOPTER_INTERFACE_H_

#include <SerialStream.h>

using namespace LibSerial;

#include <string>

#include <ros/ros.h>

//Messages types:

//Command Output

#include <std_msgs/Empty.h>

#include <mikrokopter/PPM.h>

#include <mikrokopter/NcDebug.h>

#include <mikrokopter/FcDebug.h>

#include <mikrokopter/Version.h>

#include <mikrokopter/AnalogLabel.h>

#include <mikrokopter/Control.h>

#include <mikrokopter/Navi.h>

#include <mikrokopter/AngleOut.h>

namespace CA

{

  class MikrokopterInterface

  {

  public:

    MikrokopterInterface(ros::NodeHandle nh);

    virtual ~MikrokopterInterface();

    enum Address

    {

      BlankAddress  = 'a',

      FcAddress     = 'b',

      NcAddress     = 'c',

      Mk3MagAddress = 'd',

      BlCtrlAddress = 'f'

    };

  protected:

  private:

    // Member variables  

    SerialStream fd;

    std::vector<unsigned char> buffer;

    std::map<unsigned char, unsigned int> msg_len_table;

    ros::NodeHandle n;

    // Publishers

    ros::Publisher ppm_data_pub;

    ros::Publisher nc_debug_data_pub;

    ros::Publisher fc_debug_data_pub;

    ros::Publisher version_data_pub;

    ros::Publisher label_data_pub;

    ros::Publisher set_control_ack_pub;  

    ros::Publisher control_data_pub;

    ros::Publisher navi_data_pub;

    ros::Publisher angleout_data_pub;

    // Subscribers

    ros::Subscriber get_control_sub;

    ros::Subscriber set_control_sub;

    ros::Subscriber req_reset_sub;

    // Timers

    ros::Timer readSerialTimer;

    ros::Timer req_version_timer;

    ros::Timer req_analog_label_timer;

    ros::Timer req_nc_debug_timer;

    ros::Timer req_fc_debug_timer;

    ros::Timer req_navi_timer;

    ros::Timer req_external_control_timer;

    // Parameters

    double req_version_rate;

    int    req_version_addr;

    double req_analog_label_rate;

    int    req_analog_label_idx;

    double req_fc_debug_rate;

    double req_nc_debug_rate;

    double req_navi_rate;

    double req_reset_rate;

    double req_external_control_rate;

    //

    // Callback functions

    //

    void readSerialCallback(const ros::TimerEvent&);

    void reqVersionCallback(const ros::TimerEvent&);

    void reqAnalogLabelCallback(const ros::TimerEvent&);

    void reqNcDebugCallback(const ros::TimerEvent&);

    void reqFcDebugCallback(const ros::TimerEvent&);

    void reqNaviCallback(const ros::TimerEvent&);

    void reqResetCallback(const std_msgs::Empty& msg);

    void setControlCallback(const mikrokopter::Control::ConstPtr& msg);

    void getControlCallback(const ros::TimerEvent&);

    //

    // Message field extraction functions

    //

    void extract_nc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::NcDebug &debug_data);

    void extract_fc_debug_message(std::vector<unsigned char>& buffer, mikrokopter::FcDebug &debug_data);

    void extract_navi_message(std::vector<unsigned char>& buffer, mikrokopter::Navi &msg);

    void extract_ppm_message(std::vector<unsigned char>& buffer, mikrokopter::PPM &message);

    void extract_label_message(std::vector<unsigned char>& buffer, mikrokopter::AnalogLabel &message);

    void extract_control_message(std::vector<unsigned char>& buffer, mikrokopter::Control &message);

    void extract_version_message(std::vector<unsigned char>& buffer, mikrokopter::Version &message);

    void extract_angleout_message(std::vector<unsigned char>& buffer, mikrokopter::AngleOut &msg);

    //

    // Message handling functions

    //

    void process_nc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_fc_debug_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_navi_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_ppm_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_label_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_control_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_version_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_angleout_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    void process_external_control_ack_message(std::vector<unsigned char>& buffer, ros::Time stamp);

    //

    // Utility functions

    //

    void redirectSerialTo(MikrokopterInterface::Address addr);

    void decode64(std::vector<unsigned char>& in_arr, int offset, unsigned char* out_arr);

    int calculateChecksum(std::vector<unsigned char>& buffer);

    int calculateChecksum(unsigned char* buffer, int len);

    bool isValidChecksum(std::vector<unsigned char>& buffer);

    void updateWithChecksum(unsigned char* buffer, int len);

    void createCommand(unsigned char *commandbuffer,int &msglen,const unsigned char cmd, const unsigned char address,const unsigned char *data,int len);

    mikrokopter::GpsPos processGpsPos(unsigned char* buffer);

    mikrokopter::GpsPosDev processGpsPosDev(unsigned char* buffer);

    uint8_t connectedToAddress;

  };

}

#endif