Quadrotor

 /*!

  *  \file    interface.cpp

  *  \brief   Implements 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

  */

#include "mikrokopter/interface.h"

using namespace CA;

MikrokopterInterface::MikrokopterInterface(ros::NodeHandle nh):

    n(nh)

{

  //

  // Advertise all of the messages that this node might publish

  //

  ppm_data_pub = n.advertise<mikrokopter::PPM>("/mikrokopter/ppm", 100);

  fc_debug_data_pub = n.advertise<mikrokopter::FcDebug>("/mikrokopter/fc_debug", 100);

  nc_debug_data_pub = n.advertise<mikrokopter::NcDebug>("/mikrokopter/nc_debug", 100);

  navi_data_pub = n.advertise<mikrokopter::Navi>("/mikrokopter/navi", 100);

  version_data_pub = n.advertise<mikrokopter::Version>("/mikrokopter/version", 100);

  label_data_pub = n.advertise<mikrokopter::AnalogLabel>("/mikrokopter/analog_label", 100);

  set_control_ack_pub = n.advertise<mikrokopter::Control>("/mikrokopter/set_control_ack", 100);  

  control_data_pub = n.advertise<mikrokopter::Control>("/mikrokopter/extern_control", 100);

  angleout_data_pub = n.advertise<mikrokopter::AngleOut>("/mikrokopter/angle_out", 100);

  //

  // Subscribe to the external control and reset request topics

  //

  set_control_sub = n.subscribe("/mikrokopter/req_set_control", 100, &MikrokopterInterface::setControlCallback, this);

  req_reset_sub = n.subscribe("/mikrokopter/req_reset", 100, &MikrokopterInterface::reqResetCallback, this);

  //

  // Check to see if we should be sending periodic requests to the MK

  //

  if(n.getParam("/mikrokopter/req_version_rate",req_version_rate) && req_version_rate > 0.0)

  {

    if(!n.getParam("mikrokopter/req_version_addr",req_version_addr))

    {

      ROS_ERROR_STREAM("Must define req_version_addr");

      ros::shutdown();

    }

    req_version_timer = n.createTimer(ros::Duration(1/req_version_rate), &MikrokopterInterface::reqVersionCallback, this);

  }

  if(n.getParam("/mikrokopter/req_analog_label_rate",req_analog_label_rate) && req_analog_label_rate > 0.0)

  {

    req_analog_label_idx = 0;

    req_analog_label_timer = n.createTimer(ros::Duration(1/req_analog_label_rate), &MikrokopterInterface::reqAnalogLabelCallback, this);

  }

  if(n.getParam("/mikrokopter/req_nc_debug_rate",req_nc_debug_rate) && req_nc_debug_rate > 0.0)

  {

    req_nc_debug_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqNcDebugCallback, this);

  }

  if(n.getParam("/mikrokopter/req_fc_debug_rate",req_fc_debug_rate) && req_fc_debug_rate > 0.0)

  {

    req_fc_debug_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqFcDebugCallback, this);

  }

  if(n.getParam("/mikrokopter/req_navi_rate",req_navi_rate) && req_navi_rate > 0.0)

  {

    req_navi_timer = n.createTimer(ros::Duration(1.0), &MikrokopterInterface::reqNaviCallback, this);

  }

  if(n.getParam("/mikrokopter/req_external_control_rate",req_external_control_rate) && req_external_control_rate > 0.0)

  {

    req_external_control_timer = n.createTimer(ros::Duration(1/req_external_control_rate), &MikrokopterInterface::getControlCallback, this);

  }

  //

  // Set up the serial port and callback

  //

  readSerialTimer = n.createTimer(ros::Duration(0.02), &MikrokopterInterface::readSerialCallback, this);

  // Open serial port

  std::string portname;

  if(!n.getParam("/mikrokopter/port",portname))

  {

    ROS_ERROR_STREAM("Port not defined");

    ros::shutdown();

  }

  fd.Open(portname,std::ios::in|std::ios::out);

  if (!fd.good())

  {

    ROS_ERROR_STREAM( "Error : unable to open " << portname);

    ros::shutdown();

  }

  else

  {

    ROS_INFO_STREAM( "Opening port: " << portname );

  }

  fd.SetBaudRate(SerialStreamBuf::BAUD_57600);

  fd.SetCharSize(SerialStreamBuf::CHAR_SIZE_8);

  fd.SetNumOfStopBits(1);

  fd.SetParity(SerialStreamBuf::PARITY_NONE);

  fd.SetFlowControl(SerialStreamBuf::FLOW_CONTROL_NONE);//NONE

  //

  // Create a lookup table mapping from message ID to message length

  //

  msg_len_table['A'] = 30;

  msg_len_table['B'] = 10;

  msg_len_table['V'] = 22;

  msg_len_table['D'] = 94;

  msg_len_table['G'] = 22;

  msg_len_table['k'] = 18;

  msg_len_table['P'] = 78;

  msg_len_table['O'] = 118;

  redirectSerialTo(NcAddress);

}

MikrokopterInterface::~MikrokopterInterface()

{

  fd.Close();

}

void MikrokopterInterface::redirectSerialTo(MikrokopterInterface::Address addr)

{

    //

    // Nothing to do if we are already connected to the right board

    //

    if(connectedToAddress == addr)

    {

      return;

    }

    if (addr == FcAddress)

    {

      unsigned char commandbuffer[512];

      int cmdlen = 0;

      unsigned char board[]={0};  // uart selector for FC

      createCommand(commandbuffer,cmdlen,'u',NcAddress,board,1);

      fd.write((const char*)commandbuffer,cmdlen);  

    } else if (addr == Mk3MagAddress)

    {

      unsigned char commandbuffer[512];

      int cmdlen = 0;

      unsigned char board[]={1};  // uart selector for MK3MAG

      createCommand(commandbuffer,cmdlen,'u',NcAddress,board,1);

      fd.write((const char*)commandbuffer,cmdlen); 

    }

    /*else if (addr == MkGpsAddress)

    {

      unsigned char commandbuffer[512];

      int cmdlen = 0;

      unsigned char board[]={2};  // uart selector for MKGPS

      createCommand(commandbuffer,cmdlen,'u',NC_ADDRESS,board,1);

      fd.write((const char*)commandbuffer,cmdlen); 

    }*/

    else // By default connect to NaviCtrl Bd

    {

      unsigned char commandbuffer[] = {0x1B,0x1B,0x55,0xAA,0x00};  // Magic packet defined by Mikrokopter serial protocol

      int cmdlen = 5;

      fd.write((const char*)commandbuffer,cmdlen);

    }

  connectedToAddress = addr;

  return;

}

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

{

  int cmdlen;

  unsigned char commandbuffer[512];  

  createCommand(commandbuffer,cmdlen,'R',BlankAddress,NULL,0);

  fd.write((const char*)commandbuffer,cmdlen);

  ROS_DEBUG_STREAM("Sent Reset Request to MK");

}

void MikrokopterInterface::reqVersionCallback(const ros::TimerEvent&)

{

  int cmdlen;

  unsigned char commandbuffer[512];  

  createCommand(commandbuffer,cmdlen,'v',req_version_addr,NULL,0);

  fd.write((const char*)commandbuffer,cmdlen);

  ROS_DEBUG_STREAM("Sent Version Request to MK");

}

void MikrokopterInterface::reqAnalogLabelCallback(const ros::TimerEvent&)

{

  unsigned char index[1];

  index[0] = req_analog_label_idx;

  int cmdlen;

  unsigned char commandbuffer[512];

  createCommand(commandbuffer,cmdlen,'a',BlankAddress,index,1);

  fd.write((const char*)commandbuffer,cmdlen);

  req_analog_label_idx++;

  req_analog_label_idx %= 32;

  ROS_DEBUG_STREAM("Sent Analog Label Request to MK");

}

void MikrokopterInterface::reqNcDebugCallback(const ros::TimerEvent&)

{

  redirectSerialTo(NcAddress);

  // Debug Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds. 

  // Subsciption needs to be renewed every 4s.

  unsigned char rate[1];

  rate[0] = (unsigned char) 100.0/(req_nc_debug_rate);

  int cmdlen;

  unsigned char commandbuffer[512];

  createCommand(commandbuffer,cmdlen,'d',NcAddress,rate,1);

  fd.write((const char*)commandbuffer,cmdlen);

  ROS_DEBUG_STREAM("Sent NC Debug Request to MK");

}

void MikrokopterInterface::reqFcDebugCallback(const ros::TimerEvent&)

{

  redirectSerialTo(FcAddress);

  // Debug Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds. 

  // Subsciption needs to be renewed every 4s.

  unsigned char rate[1];

  rate[0] = (unsigned char) 100.0/(req_fc_debug_rate);

  int cmdlen;

  unsigned char commandbuffer[512];

  createCommand(commandbuffer,cmdlen,'d',FcAddress,rate,1);

  fd.write((const char*)commandbuffer,cmdlen);

  ROS_DEBUG_STREAM("Sent FC Debug Request to MK");

}

void MikrokopterInterface::reqNaviCallback(const ros::TimerEvent&)

{

  redirectSerialTo(NcAddress);

  // Navi Request: u8 AutoSendInterval. Value is multiplied by 10 in receiver and then used as milliseconds.

  unsigned char rate[1];

  rate[0] = (unsigned char) 100.0/(req_navi_rate);

  int cmdlen;

  unsigned char commandbuffer[512];

  createCommand(commandbuffer,cmdlen,'o',NcAddress,rate,1);

  fd.write((const char*)commandbuffer,cmdlen);

  ROS_DEBUG_STREAM("Sent Navi Request to MK");

}

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

{

  redirectSerialTo(FcAddress);

  unsigned char dataBytes[11];

  dataBytes[0] = msg->digital[0];

  dataBytes[1] = msg->digital[1];

  dataBytes[2] = msg->remoteKey;

  dataBytes[3] = msg->pitch;

  dataBytes[4] = msg->roll;

  dataBytes[5] = msg->yaw;

  dataBytes[6] = msg->thrust;

  dataBytes[7] = msg->height;

  dataBytes[8] = msg->free;

  dataBytes[9] = msg->frame;

  dataBytes[10] = msg->config;

  int cmdlen;

  unsigned char commandbuffer[512];

  createCommand(commandbuffer,cmdlen,'b',FcAddress,dataBytes,11);

  fd.write((const char*)commandbuffer,cmdlen);

  ROS_DEBUG_STREAM("Sent Set Control Request to MK");

}

void MikrokopterInterface::getControlCallback(const ros::TimerEvent&)

{

  int cmdlen;

  unsigned char commandbuffer[512];

  createCommand(commandbuffer,cmdlen,'g',FcAddress,NULL,0);

  fd.write((const char*)commandbuffer,cmdlen);

  ROS_DEBUG_STREAM("Sent Get Control Request to MK");

}

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

{

  unsigned char outbuffer[512];

  decode64(buffer,0,outbuffer);

  message.swMajor     = outbuffer[0];

  message.swMinor     = outbuffer[1];

  message.protoMajor  = outbuffer[2];

  message.protoMinor  = outbuffer[3];

  message.swPatch     = outbuffer[4];

  for (int i = 0; i < 5; i++)

  {

    message.hardwareError[i] = outbuffer[i+5];

  }

}

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

{

  unsigned char outbuffer[512];

  decode64(buffer,0,outbuffer);

  message.digital[0]    = outbuffer[0];

  message.digital[1]    = outbuffer[1];

  message.remoteKey     = outbuffer[2];

  message.pitch         = (signed int)outbuffer[3];

  message.roll          = (signed int)outbuffer[4];

  message.yaw           = (signed int)outbuffer[5];  

  message.thrust        = outbuffer[6];

  message.height        = (signed int)outbuffer[7];

  message.free          = outbuffer[8];

  message.frame         = outbuffer[9];

  message.config        = outbuffer[10];

}

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

{

  unsigned char outbuffer[512];

  decode64(buffer,0,outbuffer);

  msg.angle[0] = (int16_t) (outbuffer[1] << 8 | outbuffer[0]);

  msg.angle[1] = (int16_t) (outbuffer[3] << 8 | outbuffer[2]);

  msg.userParameter[0] = (uint8_t) outbuffer[4];

  msg.userParameter[1] = (uint8_t) outbuffer[5];

  msg.calcState = (uint8_t) outbuffer[6];

  msg.orientation = (uint8_t) outbuffer[7];  

}

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

{

  unsigned char outbuffer[512];

  decode64(buffer,0,outbuffer);

  message.index     = outbuffer[0];

  for (int i = 0; i < 16; i++)

    {

      message.label += outbuffer[i+1];

    }

}

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

{

  unsigned char outbuffer[512];

  decode64(buffer,0,outbuffer);

  for (int i=0; i<27; i++)

  {

    message.channels[i] = (signed int) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]);

  }

}

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

{

  unsigned char outbuffer[512];

  decode64(buffer,0,outbuffer);

  debug_data.status[0] = outbuffer[0];

  debug_data.status[1] = outbuffer[1];

  int val[32];

  for (int i=0; i<32; i++)

  {

    val[i] = (int16_t) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]);

  }

  debug_data.pitchAngle                       = val[0];

  debug_data.rollAngle                        = val[1];

  debug_data.accPitch                         = val[2];

  debug_data.accRoll                          = val[3];

  debug_data.gyroYaw                          = val[4];

  debug_data.heightValue                      = val[5];

  debug_data.accZ                             = val[6];

  debug_data.gas                              = val[7];

  debug_data.compassValue                     = val[8];

  debug_data.batteryVoltage                   = val[9];

  debug_data.receiverLevel                    = val[10];

  debug_data.gyroCompass                      = val[11];

  debug_data.engineFront                      = val[12];

  debug_data.engineBack                       = val[13];

  debug_data.engineLeft                       = val[14];

  debug_data.engineRight                      = val[15];

  debug_data.onesix                           = val[16];

  debug_data.oneseven                                            = val[17];

  debug_data.oneeight                         = val[18];

  debug_data.onenine                          = val[19];

  debug_data.servo                            = val[20];

  debug_data.hovergas                         = val[21];

  debug_data.current                          = val[22];

  debug_data.capacity_mAh                     = val[23];

  debug_data.heightSetpoint                   = val[24];

  debug_data.twofive                          = val[25];

  debug_data.twosix                           = val[26];

  debug_data.compassSetpoint                  = val[27];

  debug_data.i2c_error                        = val[28];

  debug_data.bl_limit                                = val[29];

  debug_data.GPS_Pitch                        = val[30];

  debug_data.GPS_Roll                         = val[31];

}

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

{

  unsigned char outbuffer[512];

  decode64(buffer,0,outbuffer);

  debug_data.status[0] = outbuffer[0];

  debug_data.status[1] = outbuffer[1];

  int val[32];

  for (int i=0; i<32; i++){

    val[i] = (int16_t) ((outbuffer[2*i+3] << 8 )| outbuffer[2*i+2]);

  }

  debug_data.angleNick = val[0];

  debug_data.angleRoll = val[1];

  debug_data.accNick = val[2];

  debug_data.accRoll = val[3];

  debug_data.operatingRadius = val[4];

  debug_data.fcFlags = val[5];

  debug_data.ncFlags = val[6];

  debug_data.nickServo = val[7];

  debug_data.rollServo = val[8];

  debug_data.gpsData = val[9];

  debug_data.compassHeading = val[10];

  debug_data.gyroHeading = val[11];

  debug_data.spiError = val[12];

  debug_data.spiOkay = val[13];

  debug_data.i2cError = val[14];

  debug_data.i2cOkay = val[15];

  debug_data.poiIndex = val[16];

  debug_data.accSpeedN = val[17];

  debug_data.accSpeedE = val[18];

  debug_data.speedZ = val[19];

  debug_data.empty20 = val[20];

  debug_data.nSpeed = val[21];

  debug_data.eSpeed = val[22];

  debug_data.empty23 = val[23];

  debug_data.magnetX = val[24];

  debug_data.magnetY = val[25];

  debug_data.magnetZ = val[26];

  debug_data.distanceN = val[27];

  debug_data.distanceE = val[28];

  debug_data.gpsNick = val[29];

  debug_data.gpsRoll = val[30];

  debug_data.usedSats = val[31];

}

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

{

  unsigned char outbuffer[512];

  decode64(buffer,0,outbuffer);

  msg.version                 = (uint8_t)outbuffer[0];

  msg.currentPosition         = processGpsPos(outbuffer+1);

  msg.targetPosition          = processGpsPos(outbuffer+14);

  msg.targetPositionDeviation = processGpsPosDev(outbuffer+27);

  msg.homePosition            = processGpsPos(outbuffer+31);

  msg.homePositionDeviation   = processGpsPosDev(outbuffer+44);

  msg.waypointIndex           = (uint8_t) outbuffer[48];

  msg.waypointNumber          = (uint8_t) outbuffer[49];

  msg.satsInUse               = (uint8_t) outbuffer[50];

  msg.altimeter               = (int16_t) (outbuffer[52] << 8 | outbuffer[51]);

  msg.variometer              = (int16_t) (outbuffer[54] << 8 | outbuffer[53]);

  msg.flyingTime              = (uint16_t) (outbuffer[56] << 8 | outbuffer[55]);

  msg.uBat                    = (uint8_t) outbuffer[57];

  msg.groundSpeed             = (uint16_t) (outbuffer[59] << 8 | outbuffer[58]);

  msg.heading                 = (int16_t) (outbuffer[61] << 8 | outbuffer[60]);

  msg.compassHeading          = (int16_t) (outbuffer[63] << 8 | outbuffer[62]);

  msg.angleNick               = (int8_t) outbuffer[64];

  msg.angleRoll               = (int8_t) outbuffer[65];

  msg.rcQuality               = (uint8_t) outbuffer[66];

  msg.fcStatusFlags           = (uint8_t) outbuffer[67];

  msg.ncFlags                 = (uint8_t) outbuffer[68];

  msg.errorCode               = (uint8_t) outbuffer[69];

  msg.operatingRadius         = (uint8_t) outbuffer[70];

  msg.topSpeed                = (int16_t) (outbuffer[72] << 8 | outbuffer[71]);

  msg.targetHoldTime          = (uint8_t) outbuffer[73];

  msg.fcStatusFlags2          = (uint8_t) outbuffer[74];

  msg.setpointAltitude        = (int16_t) (outbuffer[76] << 8 | outbuffer[75]);

  msg.gas                     = (uint8_t) outbuffer[77];

  msg.current                 = (uint16_t) (outbuffer[79] << 8 | outbuffer[78]);

  msg.usedCapacity            = (uint16_t) (outbuffer[81] << 8 | outbuffer[80]);

}

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

{

  if ( isValidChecksum(buffer) )

  {

    uint8_t slaveAddr =  buffer[1] - 'a';

    mikrokopter::NcDebug debug_data;

    debug_data.header.frame_id="mikrokopter";

    debug_data.header.stamp=stamp;

    debug_data.slaveAddr=slaveAddr - 'a';

    buffer.erase(buffer.begin(),buffer.begin()+3);

    extract_nc_debug_message(buffer, debug_data);

    nc_debug_data_pub.publish(debug_data);

    ROS_DEBUG_STREAM( "Published NC debug package");

  } else {

    ROS_WARN_STREAM( " Checksum failed for nc debug msg");

  }

}

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

{

  if ( isValidChecksum(buffer) )

  {

    uint8_t slaveAddr =  buffer[1] - 'a';

    mikrokopter::FcDebug debug_data;

    debug_data.header.frame_id="mikrokopter";

    debug_data.header.stamp=stamp;

    debug_data.slaveAddr=slaveAddr - 'a';

    buffer.erase(buffer.begin(),buffer.begin()+3);

    extract_fc_debug_message(buffer, debug_data);

    fc_debug_data_pub.publish(debug_data);

    ROS_DEBUG_STREAM( "Published FC debug package");

  } else {

    ROS_WARN_STREAM( " Checksum failed for fc debug msg");

  }

}

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

{

  if ( buffer[1] == NcAddress && isValidChecksum(buffer) ) {

    mikrokopter::Navi naviMsg;

    naviMsg.header.frame_id="mikrokopter";

    naviMsg.header.stamp = stamp;

    buffer.erase(buffer.begin(),buffer.begin()+3);

    extract_navi_message(buffer, naviMsg);

    navi_data_pub.publish(naviMsg);

    ROS_DEBUG_STREAM( "Published navi data package");

  } else {

    ROS_WARN_STREAM( " Checksum failed for navi data msg");

  }

}

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

{

  if ( buffer[1] == FcAddress && isValidChecksum(buffer) ) {

    mikrokopter::PPM ppm_data;

    ppm_data.header.frame_id="mikrokopter";

    ppm_data.header.stamp = stamp;

    buffer.erase(buffer.begin(),buffer.begin()+3);

    extract_ppm_message(buffer, ppm_data);

    ppm_data_pub.publish(ppm_data);

    ROS_DEBUG_STREAM( "Published ppm package");

  } else {

    ROS_WARN_STREAM( " Checksum failed for ppm msg");

  }

}

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

{

  if ( isValidChecksum(buffer) )

  {

    mikrokopter::AnalogLabel label_data;

    label_data.header.frame_id="mikrokopter";

    label_data.header.stamp=stamp;

    label_data.slaveAddr   = buffer[1];

    buffer.erase(buffer.begin(),buffer.begin()+3);

    extract_label_message(buffer, label_data);

    label_data_pub.publish(label_data);

    ROS_DEBUG_STREAM( "Published Analog Label message");

  } else {

    ROS_WARN_STREAM( " Checksum failed for analog label msg");

  }

}

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

{

  if ( isValidChecksum(buffer) )

  {

    mikrokopter::Control msg;

    msg.header.frame_id="mikrokopter";

    msg.header.stamp=stamp;

    buffer.erase(buffer.begin(),buffer.begin()+3);

    extract_control_message(buffer, msg);

    control_data_pub.publish(msg);

    ROS_DEBUG_STREAM( "Published ExternControl message");

  } else {

    ROS_WARN_STREAM( " Checksum failed for ExternControl msg");

  }

}

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

{

  if ( isValidChecksum(buffer) )

  {

    mikrokopter::Version version_data;

    version_data.header.frame_id="mikrokopter";

    version_data.header.stamp=stamp;

    version_data.slaveAddr   = buffer[1] - 'a';

    buffer.erase(buffer.begin(),buffer.begin()+3);

    extract_version_message(buffer, version_data);

    version_data_pub.publish(version_data);

    ROS_DEBUG_STREAM( "Published Version Info message");

  } else {

    ROS_WARN_STREAM( " Checksum failed for version info msg");

  }

}

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

{

  if ( buffer[1] == Mk3MagAddress && isValidChecksum(buffer) )

  {

     mikrokopter::AngleOut msg;

     msg.header.frame_id = "mikrokopter";

     msg.header.stamp = stamp;

     msg.slaveAddr = buffer[1] - 'a';

     buffer.erase(buffer.begin(),buffer.begin()+3);

     extract_angleout_message(buffer, msg);

     angleout_data_pub.publish(msg);

     ROS_DEBUG_STREAM("Published Angle Out");

   } else {

     ROS_WARN_STREAM("Checksum failed for heading msg");

   }

}

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

{

  if ( isValidChecksum(buffer) )

  {

    mikrokopter::Control msg;

    msg.header.frame_id="mikrokopter";

    msg.header.stamp=stamp;

    unsigned char outbuffer[512];

    buffer.erase(buffer.begin(),buffer.begin()+3);

    decode64(buffer,0,outbuffer);

    msg.frame     = outbuffer[0];

    set_control_ack_pub.publish(msg);

    ROS_DEBUG_STREAM( "Published ExternControl ACK");

  } else {

    ROS_WARN_STREAM( " Checksum failed for ExternControl ACK");

  }

}

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

{

  mikrokopter::GpsPos gps;

  gps.longitude = (int32_t)(buffer[3] << 24 | buffer[2] << 16 | buffer[1] << 8 | buffer[0]);

  gps.latitude = (int32_t)(buffer[7] << 24 | buffer[6] << 16 | buffer[5] << 8 | buffer[4]);

  gps.altitude = (int32_t)(buffer[11] << 24 | buffer[10] << 16 | buffer[9] << 8 | buffer[8]);

  gps.status = buffer[12];

  return gps;

}

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

{

  mikrokopter::GpsPosDev dev;

  dev.distance = (uint32_t)(buffer[1] << 8 | buffer[0]);

  dev.bearing = (int32_t)(buffer[3] << 8 | buffer[2]);

  return dev;

}

void MikrokopterInterface::readSerialCallback(const ros::TimerEvent&)

{

  char c;

  static ros::Time nextStamp, stamp;

  while (fd.rdbuf()->in_avail()>0 && ros::ok()) // While there is data in the serial receive buffer

  {

    fd.get(c);

    buffer.push_back(c);

    if (c == '#')

    {

      stamp = nextStamp;

      nextStamp = ros::Time::now();

      buffer.erase(buffer.begin(),buffer.end()-1);

    }

    if ( buffer[0] == '#' && buffer.size() >= 2)

    {

      if( buffer.size() == msg_len_table[buffer[2]])

      {

        switch (buffer[2])

        {

          case 'A':

            process_label_message(buffer,stamp);

            break;

          case 'B':

            process_external_control_ack_message(buffer,stamp);

            break;

          case 'H':

            ROS_WARN_STREAM("Unimplemented message \"H\"");

            break;

          case 'L':

            ROS_WARN_STREAM("Unimplemented message \"L\"");

            break;

          case 'V':

            process_version_message(buffer,stamp);

            break;

          case 'D':

            {

              uint8_t slaveAddr =  buffer[1];

              if(slaveAddr == FcAddress)

              {

                process_fc_debug_message(buffer,stamp);

              }

              else if(slaveAddr == NcAddress)

              {

                process_nc_debug_message(buffer,stamp);

              }

            }

            break;

          case 'G':

            process_control_message(buffer,stamp);

            break;

          case 'k':

            process_angleout_message(buffer,stamp);

            break;

          case 'P':

            process_ppm_message(buffer,stamp);

            break;

          case 'O':

            process_navi_message(buffer,stamp);

            break;

          default:

            ROS_WARN_STREAM("Unknown message type: "<<buffer[2]);

            break;

        } // end switch (buffer[2])

      } // end if( buffer.size() == msg_len_map[buffer[2]])

    } // end if ( buffer[0] == '#')

  } // end while (fd.rdbuf()->in_avail()>0 && ros::ok())

}

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

  int ptrIn=offset; 

  int a,b,c,d,x,y,z;

  int ptr=0;

  int len = in_arr.size();

  while(len!=0)

    {

      a = in_arr[ptrIn++] - '=';

      b = in_arr[ptrIn++] - '=';

      c = in_arr[ptrIn++] - '=';

      d = in_arr[ptrIn++] - '=';

      x = (a << 2) | (b >> 4);

      y = ((b & 0x0f) << 4) | (c >> 2);

      z = ((c & 0x03) << 6) | d;

      if((len--)!=0) out_arr[ptr++] = x; else break;

      if((len--)!=0) out_arr[ptr++] = y; else break;

      if((len--)!=0) out_arr[ptr++] = z; else break;

    }

}

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

{

  int chk = 0;

  unsigned int l=0;

  for (l=0; l<buffer.size()-3; l++){

    chk += buffer[l];

  }

  chk %= 4096;

  return chk;

}

int MikrokopterInterface::calculateChecksum(unsigned char* buffer, int len)

{

  int chk = 0;

  int l=0;

  for (l=0; l<len-3; l++){

    chk += buffer[l];

  }

  chk %= 4096;

  return chk;

}

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

{

  int chk = calculateChecksum(buffer);

  // Verify the Checksum

  int l=buffer.size()-3;

  return (buffer[l++] == ('=' + chk / 64) && 

          buffer[l++] == ('=' + chk % 64) && 

          buffer[l++] == '\r');

}

void MikrokopterInterface::updateWithChecksum(unsigned char* buffer, int len)

{

  int chks = calculateChecksum(buffer, len);

  buffer[len-3] = ('=' + chks / 64);

  buffer[len-2] = ('=' + chks % 64);

}

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

{

  msglen = 0;

  commandbuffer[msglen++] = '#';

  commandbuffer[msglen++] = address;

  commandbuffer[msglen++] = cmd;

  unsigned char a,b,c;

  a=0;b=0;c=0;

  int ptr=0;

  while(len)

    {

      if(len>0)

        {

          a = data[ptr++];

          len--;

        }

      else a = 0;

      if(len>0)

        {

          b = data[ptr++];

          len--;

        }

      else b = 0;

      if(len>0)

        {

          c = data[ptr++];

          len--;

        }

      else c = 0;

      commandbuffer[msglen++] = '=' + (a >> 2);

      commandbuffer[msglen++] = '=' + (((a & 0x03) << 4) | ((b & 0xf0) >> 4));

      commandbuffer[msglen++] = '=' + (((b & 0x0f) << 2) | ((c & 0xc0) >> 6));

      commandbuffer[msglen++] = '=' + ( c & 0x3f);

    }

  commandbuffer[msglen++] ='x';

  commandbuffer[msglen++] ='x';

  commandbuffer[msglen++] ='\r';

  updateWithChecksum(commandbuffer,msglen);

}