Communicating with the LLP via XBee

Introduction

There are several ways to interact with the Asctec Pelican. The most direct way to do so is to communicate with the LLP using the Xbees provided.
One of the Xbees is under the arm of the Pelican as you can see below:

The other one is provided with a cable so you can plug it to any USB port:

Communication Protocol

Briefly, Xbees allow you to communicate wireslessly using serial communication standards.
The communication works as follows: you can poll data sending the right instruction, in which case the data will be sent back to your computer, and you can send (pitch, roll, yaw, thrust commands) specifying the fly mode with a minimum frequency. This communication protocol is explained in more detail in the Official Manual.

Hardware Setup

In order for your computer to recognize the USB Xbee module you must download the FTDI drivers. They are available in this website (download D2XX drivers, not VCP drivers).

Also, the Xbee connected to your computer should have the name Quad_USB1 (this is recommended in the Simulink Toolkit Manual). To do this download MPROG (http://www.ftdichip.com/Support/Utilities/MProg3.5.zip), read the data from the Xbee, change the name and then write the data.

The Xbee in the Pelican comes already set up, but in case check it is connected to the HL Serial 0.

Remember that in order to poll data it is not necessary that the Futaba control is on, but to send commands the Futaba control must be on, with the serial interface switch enabled:

Software (the code!)

This code uses the LibSerial library.

The code is split into 4 files:
   - definitions.h: Defines the data structures needed according to the official manual.
   - Serial.h and Serial.cpp: Implements basic functions to communicate with the Pelican via serial.
   - main.cpp: Implements two basic examples. The first one requests IMU data and displays it. The second one starts the motors and stops them every 5 seconds.

With this, you can have an idea of how the communication works and start coding on top. The most important thing you must remember is that to accelerate the motors you must first start them. The signal required to start/stop the motors is equivalent to a full left yaw with zero thrust.

definitions.h

 #ifndef DEFINITIONS_H_  
 #define DEFINITIONS_H_  
 struct CTRL_INPUT  
 {  
      short pitch;  
      short roll;  
      short yaw;  
      short thrust;  
      short ctrl;  
      short chksum;  
 };  
 struct IMU_RAWDATA  
     {  
       //pressure sensor 24-bit value, not scaled but bias free  
       int pressure;  
       //16-bit gyro readings; 32768 = 2.5V  
       short gyro_x;  
       short gyro_y;  
       short gyro_z;  
       //10-bit magnetic field sensor readings  
       short mag_x;  
       short mag_y;  
       short mag_z;  
       //16-bit accelerometer readings  
       short acc_x;  
       short acc_y;  
       short acc_z;  
       //16-bit temperature measurement using yaw-gyro internal sensor  
       unsigned short temp_gyro;  
       //16-bit temperature measurement using ADC internal sensor  
       unsigned int temp_ADC;  
     };  
 struct IMU_CALCDATA  
 {  
      //angles derived by integration of gyro_outputs, drift compensated by data  
      //fusion; -90000..+90000 pitch(nick) and roll, 0..360000 yaw; 1000 = 1  
      //degree  
      int angle_nick;  
      int angle_roll;  
      int angle_yaw;  
      //angular velocities, raw values [16 bit] but bias free  
      int angvel_nick;  
      int angvel_roll;  
      int angvel_yaw;  
      //acc-sensor outputs, calibrated: -10000..+10000 = -1g..+1g  
      short acc_x_calib;  
      short acc_y_calib;  
      short acc_z_calib;  
      //horizontal / vertical accelerations: -10000..+10000 = -1g..+1g  
      short acc_x;  
      short acc_y;  
      short acc_z;  
      //reference angles derived by accelerations only: -90000..+90000; 1000 = 1  
      //degree  
      int acc_angle_nick;  
      int acc_angle_roll;  
      //total acceleration measured (10000 = 1g)  
      int acc_absolute_value;  
      //magnetic field sensors output, offset free and scaled; units not  
      //determined, as only the direction of the field vector is taken into  
      //account  
      int Hx;  
      int Hy;  
      int Hz;  
      //compass reading: angle reference for angle_yaw: 0..360000; 1000 = 1 degree  
      int mag_heading;  
      //pseudo speed measurements: integrated accelerations, pulled towards zero;  
      //units unknown; used for short-term position stabilization  
      int speed_x;  
      int speed_y;  
      int speed_z;  
      //height in mm (after data fusion)  
      int height;  
      //diff. height in mm/s (after data fusion)  
      int dheight;  
      //diff. height measured by the pressure sensor [mm/s]  
      int dheight_reference;  
      //height measured by the pressure sensor [mm]  
      int height_reference;  
 };  
 #endif /* DEFINITIONS_H_ */  

Serial.h

 #ifndef SERIAL_H_
#define SERIAL_H_

#include <SerialStream.h>
#include "definitions.h"
#include <cstdlib>

class Serial {
 LibSerial::SerialStream sstream;
public:
 Serial();
 virtual ~Serial();
 int sendmsg(char* msg, int buffersize);
 void request_IMUCalc();
 void read(char* data);
 void requestandread();
 IMU_CALCDATA processIMU(char* data, int size);
 void displayData(IMU_CALCDATA imudata);
 void motors_on();
 void closeports();
};

#endif /* SERIAL_H_ */

Serial.cpp

 #include "Serial.h"
#include <cstdlib>
#include <iostream>
#include <string.h>
using namespace LibSerial;


Serial::Serial() {
 // TODO Auto-generated constructor stub
 const char* const SERIAL_PORT_DEVICE = "/dev/ttyUSB0" ;
     sstream.Open( SERIAL_PORT_DEVICE ) ;
     if ( ! sstream.good() )
     {
         std::cout << "Error: Could not open serial port "
                   << SERIAL_PORT_DEVICE
                   << std::endl ;
         exit(1) ;
     }
     //
     // Set the baud rate of the serial port.
     //
     sstream.SetBaudRate( SerialStreamBuf::BAUD_57600 ) ;
     if ( ! sstream.good() )
     {
         std::cout << "Error: Could not set the baud rate." << std::endl ;
         exit(1) ;
     }
     //
     // Set the number of data bits.
     //
     sstream.SetCharSize( SerialStreamBuf::CHAR_SIZE_8 ) ;
     if ( ! sstream.good() )
     {
         std::cout << "Error: Could not set the character size." << std::endl ;
         exit(1) ;
     }
     //
     // Disable parity.
     //
     sstream.SetParity( SerialStreamBuf::PARITY_NONE ) ;
     if ( ! sstream.good() )
     {
         std::cout << "Error: Could not disable the parity." << std::endl ;
         exit(1) ;
     }
     //
     // Set the number of stop bits.
     //
     sstream.SetNumOfStopBits( 1 ) ;
     if ( ! sstream.good() )
     {
         std::cout << "Error: Could not set the number of stop bits."
                   << std::endl ;
         exit(1) ;
     }
     //
     // Turn on hardware flow control.
     //
     sstream.SetFlowControl( SerialStreamBuf::FLOW_CONTROL_HARD ) ;
     if ( ! sstream.good() )
     {
         std::cout << "Error: Could not use hardware flow control."
                   << std::endl ;
         exit(1) ;
     }
}

Serial::~Serial() {
 // TODO Auto-generated destructor stub
}

void Serial::closeports(){
 sstream.Close();
}

int Serial::sendmsg(char* msg, int buffersize){
 puts("Sending message...");
  this->sstream.write(msg , buffersize);
  puts("Message sent");
  return 1;
}

void Serial::request_IMUCalc(){
 sstream.write(">*>p4",6); //not sure of buffer size needed
 puts("IMU requested");
}

void Serial::read(char* data){
 std::cout << "Reading..." << std::endl ;
 int i = 0;
 usleep(1000*80); //wait for the message to arrive
     while( sstream.rdbuf()->in_avail() > 0  )
         {

             char next_byte;
             sstream.get(next_byte);
             std::cout << (int)next_byte << " ";
             data[i] = next_byte;
             ++i;
         }

     std::cout << std::endl ;
     std::cout << "Done reading" << std::endl ;
}

void Serial::requestandread(){
 std::cout << "Dumping file to serial port." << std::endl ;
     sstream.write(">*>p4",6);
     sleep(1);
     std::cout << "Reading..." << std::endl ;
     while( sstream.rdbuf()->in_avail() > 0 )
         {

             char next_byte;
             sstream.get(next_byte);
             std::cout << (int)next_byte << " ";
         }

     std::cout << std::endl ;
     std::cout << "Done." << std::endl ;
}

IMU_CALCDATA Serial::processIMU(char* data, int size){

 int indice_inicio;
 for(int i=0;i<size-3;i++){
  if((int)((data)[i])==62 && (int)((data)[i+1])=='*' && (int)((data)[i+2])=='>'){
   indice_inicio = i;
   break;
  }
 }
 std::cout << "Start index: " << (int)indice_inicio << std::endl ;
 std::cout << data[indice_inicio] << data[indice_inicio+1] << data[indice_inicio+2] << std::endl ;

 unsigned short length = (data[indice_inicio+4] << 8) | data[indice_inicio+3];
 std::cout << "Length: " << length << std::endl ;

 char imudata[length];
 for(int i=0;i<length;i++){
  imudata[i] = data[indice_inicio+6+i];
  std::cout << (int)imudata[i] << " ";
 }
 std::cout << std::endl;
 //Packet Descriptor
 unsigned char descriptor = data[indice_inicio+5] ;
 std::cout << "Packet descriptor: " << (int)descriptor << std::endl;

 IMU_CALCDATA* imustructp = (IMU_CALCDATA*) imudata;

 //CRC16
 unsigned short crc16 = (data[indice_inicio+5+length+2] << 8) | data[indice_inicio+5+length+1];

 //Endstring
 std::cout << "Endstring: " << data[indice_inicio+5+length+3] << data[indice_inicio+5+length+4]
           << data[indice_inicio+5+length+5] << std::endl;

 //CRC16 comprobation not implemented

 return *imustructp;

}

void Serial::displayData(IMU_CALCDATA data){
 //clear console
 std::cout << std::string( 10, '\n' );
 //print data
 std::cout << "Acc X: " << data.acc_x_calib << std::endl;
 std::cout << "Acc Y: " << data.acc_y_calib << std::endl;
 std::cout << "Acc Z: " << data.acc_z_calib << std::endl;
 std::cout << "Angle nick: " << data.angle_nick << std::endl;
 std::cout << "Angle roll: " << data.angle_roll << std::endl;
 std::cout << "Angle yaw: " << data.angle_yaw << std::endl;
 std::cout << "Speed X: " << data.speed_x << std::endl;
 std::cout << "Speed Y: " << data.speed_y << std::endl;
 std::cout << "Speed Z: " << data.speed_z << std::endl;
 std::cout << "Height: " << data.height << std::endl;
}

void Serial::motors_on(){
 CTRL_INPUT on_input;
  on_input.thrust = 0;
  on_input.pitch = 0;
  on_input.roll = 0;
  on_input.yaw = -2048;
  on_input.ctrl = 0xF;
  on_input.chksum = on_input.thrust + on_input.pitch + on_input.roll
    + on_input.yaw + on_input.ctrl + 0xAAAA;

  //cast to char array
  char* on_array = (char*)&on_input;

  //send msg
  sstream.write(">*>di",5);
  sstream.write(on_array,sizeof(on_input));
}

main.cpp

 #include "definitions.h"
#include <SerialStream.h>
#include <string.h>
#include <stdio.h>
#include <iostream>
#include "Serial.h"

using namespace LibSerial;
using namespace std;

SerialStream* openport(){

 SerialStream sstream;
 sstream.SetBaudRate( SerialStreamBuf::BAUD_57600 );
 sstream.SetFlowControl( SerialStreamBuf::FLOW_CONTROL_NONE );
 sstream.SetCharSize( SerialStreamBuf::CHAR_SIZE_8 );
 sstream.SetNumOfStopBits(1);
 sstream.SetParity( SerialStreamBuf::PARITY_NONE );
 sstream.Open( "/dev/ttyUSB0" );

 if ( ! sstream.good() )
     {
         std::cerr << "[" << __FILE__ << ":" << __LINE__ << "] "
                   << "Error: Could not open serial port."
                   << std::endl ;
     }

 SerialStream* pointer = &sstream;

 return pointer;
}

//receives command struct and appends the necessary chars
char* input_array(char* structure, int size){

 char inicio[5] = {'>','*','>','d','i'};
 char input[size+5];
 strcpy(input, inicio);     // copy to destination
 strncat(input, structure+5,size); // append part of the second string

 puts(input);
 return input;
}

//sends the structure
int send_msg(SerialStream* ssp, char* msg, int buffersize)
{
 puts("enviando msje");
 (*ssp).write(msg , buffersize);
 puts("mensaje enviado");
 return 1;
}

int close_ports(SerialStream* ssp)
{
 ssp->Close();
 return 1;
}

int main()
{
 int sel = 1; // sel = 0 requests data, sel = 1 toggle motors

 if(sel == 0){
  //simple code to request data
  Serial* serialport = new Serial();
  bool cond = true;
  while(cond){
   serialport->request_IMUCalc();
   int size = 250; //size of data buffer
   char databuff[size];
   serialport->read(databuff);
   IMU_CALCDATA imudata = serialport->processIMU(databuff, size);
   serialport->displayData(imudata);
  }
  serialport->closeports();
 }

 else if (sel == 1){
  //simple code to start motors and stop them after 5 seconds
  Serial* serialport = new Serial();
  serialport->motors_on();
  std::cout << "motors on" << std::endl;
  sleep(5);
  serialport->motors_on(); //the same signal is needed to turn them off
  std::cout << "motors off" << std::endl;
  serialport->closeports();
 }

 return 0;
}