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;
}

Basic WiFi communication setup part 2

This is a continuation from the previous post: http://asctecbasics.blogspot.com/2013/06/basic-wifi-communication-setup-part-1.html

Testing everything is OK

First, let's check the previous steps are OK. Let's assume you assigned 192.168.0.15 to your computer and 192.168.0.16 to your quad.

From your computer you should be able to ping your quad, run in a terminal:

 ping 192.168.0.16  

The same should work  from your quad to your computer changing the IP address correspondingly.

Setting up SSH

This part is based on the instructions found here: http://teamprincipia.wordpress.com/2008/05/29/beginning-ssh-on-ubuntu/ 

You need to install SSH client and server on both machines (client usually comes with Ubuntu  but checking is always good):

 sudo apt-get install openssh-client openssh-server  

Mapping IP addresses to hostnames

This part is based on information found here: http://nootrix.com/2012/06/ros-networking/

The idea is to associate the IP address to a name, so it isn't necessary to write the IP address everytime you wish to login through SSH.

To do this, first edit the /etc/hosts files on the computer being used as groundstation (assuming you are using the same IPs mentioned before):

 sudo gedit /etc/hosts  

Then add the following line at the end of the file:

 192.168.0.16 quad

Do the same on the quadrotor, adding this line instead:

 192.168.0.15 groundstation

Now you should be able to open a terminal on the computer and ping the quadrotor using the following command:

 ping quad

This should have a similar result as doing a ping from the quadrotor from the computer. Check both pings are possible to be sure everything is ok.

Running ROS

After setting up ssh and mapping the names, you should be able to run ROS on the quadrotor using commands from the groundstation, and start nodes on the groundstation to communicate with the quadrotor.

To check this we will run a listener node on the quadrotor and try to communicate to it from the computer.

You should choose if the groundstation or the quadrotor will run the roscore service. In this case the quadrotor will be running roscore.

To do this, from the groundstation computer log in to the quadrotor. Assuming the username on the quadrotor is "user", open a terminal on your computer and type:

 ssh user@quad 

Your password will be required for this. Type it and now you should have a terminal logged into the quadrotor. In this terminal type:

 roscore 

Now repeat this process, openning a new terminal and logging into the quadrotor and start a listener node on it typing:
 

 rosrun rospy_tutorials listener.py

Now you should set up the groundstation. Open a new terminal on your computer and type:
 

 export ROS_MASTER_URI=http://quad:11311

 rosrun rospy_tutorials talker.py 

Now you should see the answer on the listener terminal, showing the communication is working between both.

What's next?

Now everything is set up, you should try to run the asctec_mav_framework package on the quadrotor and communicate to it from the groundstation.

Basic WiFi communication setup part 1

This post adressess the steps needed to setup the communication between the High Level Processor (HLP) of the quadrotor and a groundstation (such as a laptop, computer) through a WiFi connection (between the groundstation and the Atomboard) and a serial connection (between the Atomboard and the HLP) . It is based on the official asctec_mav_framework ROS Tutorial but shows some additional problems addressed in the configuration.

It is separated in two parts: the first one deals with more hardware related issues, while the second one is more related to the software configuration.

I asume that you have the Asctec Quadrotor Pelican running with Ubuntu 10.04, have already installed ROS (this was done using ROS Fuerte, but should work with other versions), have installed the asctec_mav_framework package and have a WiFi connection with WPA2. On the other side, I asume you will be using a computer with Ubuntu and ROS as a groundstation (this was tested using Ubuntu 12.04 and ROS Fuerte), and have installed the asctec_mav_framework on this computer also.

The idea is to be able to initialize the quadrotor without using the LVDS screen (so it is ready to fly), for this the Secure Shell (SSH) protocol available in Ubuntu is used. SSH allows you to log into the quadrotor and start the asctec_mav_framework ROS package using the terminal from your groundstation. This way your quad will be able to receive commands and start/stop execution without the need of a mouse, keyboard or screen.

Atomboard-HLP Connection

The first step is to check the connection between the Atomboard and the HLP. This should be done using the cable with a "loop" on the HLP side.

This should connect one of the serial ports of the Atomboard (/dev/ttyUSB0 or /dev/ttyUSB1, I suggest the first one, but in case you need to use the /dev/ttyUSB1 remember to change the configuration parameters accordingly).

 WiFi Set up

It is needed that the quadrotor connects automatically to the WiFi at startup. For this, a new network manager is suggested: WPA Supplicant.

To install WPA supplicant in your quadrotor:

 sudo apt-get install wpasupplicant  

Then uninstall other networks managers (for example WICD or NetworkManager):

 sudo apt-get remove networkmanager   
 sudo apt-get remove wicd  

The next step is to configure it to connect to your WiFi network. Run this on a terminal (assuming your wireless network is called my_wifi_network, this is known as SSID, and the password is my_password):

 wpa_passphrase my_wifi_network my_password

This will show in the same terminal something like this:

 network={  
      ssid="my_wifi_network"  
      #psk="my_password"  
      psk=5dd3c4758a980d50d133d0cd43dc6a0460211f397925522d74e9e3b622420304  
 }  

Now, you should create a file to store this configuration information (for example in /etc/wpa_supplicant/wpa_supplicant.conf). One way to do this is in a new terminal run:

 sudo gedit /etc/wpa_supplicant/wpa_supplicant.conf   

This will open gedit, where you can paste the network information directly and save changes.

The next step is to open the /etc/network/interfaces file and edit the wlan0 section. Open it using any editor, for example:

 sudo gedit /etc/network/interfaces  

And modify the wlan0 section so it looks like this:

 auto wlan0  
 iface wlan0 inet dhcp  
     wpa-conf /etc/wpa_supplicant/wpa_supplicant.conf  
     post-up /sbin/iwconfig wlan0 power off  

The wpa-conf line loads the file created in the last step with the WiFi configuration. The post-up line disables power management permanently, this is done to avoid high latencies. You can check the power management status using the command:

 iwconfig wlan0 |grep "Power Management"  

Serial Ports Access and Grub

In case you are using Ubuntu 12.04, you need to run the following command in order to use the serial ports without root permissions (this adds the user to the dialout group):

 sudo adduser <username> dialout  

To speed up boot and remove some i8042 related messages add the following line in the file /etc/default/grub

 GRUB_CMDLINE_LINUX_DEFAULT="i8042.noaux=1"  

And update grub:

 sudo update-grub  

Configure Static IP

To allow the quadrotor and the ground station to easily locate each other while using SSH it is convenient to give both a static IP and give both machines names. To give static IPs there are at least two methods:

1. Using the router configuration:

You can configure your router to give an IP based on the MAC address of the device connected. Here is an example of how to do this: http://resource.dlink.com/connect/mastering-static-ip-addresses/

2. Setting up each machine: 

In some cases the router doesn't support static IPs by MAC, or you don't have access to them. In this section I will explain how I set up each machine to ask for a static IP address.

2.1 Setting up the ground station:

Assuming you are using Ubuntu 12.04. You can click on the wireless icon and choose edit connections. 

Then select the "wireless" tab, choose your connection and click Edit.
Then you should check the option "Connect automatically" and then go to the IPv4 Settings tab.
Choose "Manual" as Method. On the Addresses section, click Add. If you are on your private network, you can usually choose as address the ip 192.168.*.* (for example 192.168.1.52). The netmask usually is 255.255.255.0, and the gateway 192.168.*.1. Don't forget the DNS server address. After this reconnect to your wireless network.

  To check that everything worked fine, you can run the command ifconfig on a terminal, this should print the connection information in the terminal. Check the wlan0 section and the ip next to inet addr.

2.2 Setting up the quad connection 

Based on the instructions given here to set up a static IP, you should again modify the /etc/network/interfaces file so it looks like this:

 auto wlan0  
 iface wlan0 inet static  
   address 192.168.0.2  
   netmask 255.255.255.0  
   network 192.168.0.0  
   broadcast 192.168.0.255  
   gateway 192.168.0.1  
   dns-nameservers 192.168.0.1, 192.168.0.2    
   wpa-conf /etc/wpa_supplicant/wpa_supplicant.conf  
   post-up /sbin/iwconfig wlan0 power off  

 
Notice now the static value instead of dhcp and the other parameters added (address, netmask, etc.) .

Now you should reboot, and after logging you should run the command ifconfig and check the IP is correctly set.

The checkdisk error

We are configuring our quad so it is able to boot and connect to the internet automatically, but you may encounter the following problem.
If you turn off the Atomboard and disconnect any power source from it (either battery or alternative power sources), you may find that the next time you turn it on it you encounter something along the lines of:

 Checking root file system...fsck from util-linux-ng 2.16.1  
 humel-root: Superblock last mount time (Wed Oct 7 18:53:39 2009,  
   now = Sat Jan 1 00:00:14 2000) is in the future.  
 humel-root: UNEXPECTED INCONSISTENCY; RUN fsck MANUALLY.  
   (i.e., without -a or -p options)  

This (I believe) is caused before the last mount time is stored but the internal date is reset when there is no power source, so it registers the last mount time as in the future and forces to check the disk for errors.

To avoid this, I followed the instructions found here. You must edit the /etc/e2fsck.conf file and add the following:

   [problems]  
   # Superblock last mount time is in the future (PR_0_FUTURE_SB_LAST_MOUNT).  
   0x000031 = {  
     preen_ok = true  
     preen_nomessage = true  
   }  
   # Superblock last write time is in the future (PR_0_FUTURE_SB_LAST_WRITE).  
   0x000032 = {  
     preen_ok = true  
     preen_nomessage = true  
   }  

This tells e2fsck that the problem can be auto-fixed without mentioning it.

The next part shows how to set up SSH communication and configure ROS to work on both machines.