[Robotflow-cvs] RobotFlow/Devices/src IMU400CC_200.cc,NONE,1.1 Makefile.am,1.12,1.13

[Dominic L. <ma...@us...>]

Update of /cvsroot/robotflow/RobotFlow/Devices/src
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv6380

Modified Files:
	Makefile.am 
Added Files:
	IMU400CC_200.cc 
Log Message:
first working driver

Index: Makefile.am
===================================================================
RCS file: /cvsroot/robotflow/RobotFlow/Devices/src/Makefile.am,v
retrieving revision 1.12
retrieving revision 1.13
diff -C2 -d -r1.12 -r1.13
*** Makefile.am	30 Mar 2005 19:10:59 -0000	1.12
--- Makefile.am	25 Apr 2005 19:51:13 -0000	1.13
***************
*** 14,18 ****
  	CANON_VCC4.cc \
  	SNCRZ30.cc \
! 	SNCRZ30RS232.cc
  
  libDevices_la_LDFLAGS = -release $(LT_RELEASE)
--- 14,19 ----
  	CANON_VCC4.cc \
  	SNCRZ30.cc \
! 	SNCRZ30RS232.cc \
! 	IMU400CC_200.cc
  
  libDevices_la_LDFLAGS = -release $(LT_RELEASE)

--- NEW FILE: IMU400CC_200.cc ---
/* Copyright (C) 2005 Dominic Letourneau

   This library is free software; you can redistribute it and/or
   modify it under the terms of the GNU Lesser General Public
   License as published by the Free Software Foundation; either
   version 2.1 of the License, or (at your option) any later version.
   
   This library is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
   Lesser General Public License for more details.
   
   You should have received a copy of the GNU Lesser General Public
   License along with this library; if not, write to the Free Software
   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#ifndef _IMU400CC_200_CC_
#define _IMU400CC_200_CC_

#include "BufferedNode.h"
#include "SerialDriver.h"
#include "BaseException.h"
#include "misc.h"
#include <fcntl.h>
#include <stdio.h>
#include <string.h>
#include <sys/types.h>
#include <unistd.h>
#include <signal.h>
#include <termios.h>
#include <stdlib.h>
#include <list>
#include <pthread.h>
#include <string>
#include <sched.h>
#include "pseudosem.h"

using namespace std;
using namespace FD;

namespace RobotFlow {

class IMU400CC_200;

DECLARE_NODE(IMU400CC_200)

/*Node
 * @name IMU400CC_200
 * @category RobotFlow:Devices
  * @description Crossbow IMU400CC-200 driver.
 *
 * @parameter_name SERIAL_PORT
 * @parameter_type string
 * @parameter_value /dev/ttyUSB0
 *
 * @output_name X_ACCEL
 * @output_type float
 * @output_description X ACCELERATION (G force)
 *
 * @output_name Y_ACCEL
 * @output_type float
 * @output_description Y ACCELERATION (G force)
 *
 * @output_name Z_ACCEL
 * @output_type float
 * @output_description Z ACCELERATION (G force)
 *
 * @output_name YAW_RATE
 * @output_type float
 * @output_description YAW RATE (deg/sec)
 *
 * @output_name PITCH_RATE
 * @output_type float
 * @output_description PITCH RATE (deg/sec)
 *
 * @output_name ROLL_RATE
 * @output_type float
 * @output_description ROLL_RATE (deg/sec)
 *    
 * @output_name X
 * @output_type float
 * @output_description X position (meters)
 *
 * @output_name Y
 * @output_type float
 * @output_description Y position (meters)
 *
 * @output_name Z
 * @output_type float
 * @output_description Z position (meters)
 *
 * @output_name YAW
 * @output_type float
 * @output_description YAW (deg)
 *
 * @output_name PITCH
 * @output_type float
 * @output_description PITCH (deg)
 *
 * @output_name ROLL
 * @output_type float
 * @output_description ROLL (deg)
 *         
 END*/


#define IMU_PACKET_SIZE 18
#define IMU_GR_PRECISION 10.0         
#define IMU_RR_PRECISION 200.0
#define IMU_TIME_PRECISION 0.79E-6
#define IMU_GRAVITY_ACCEL_VALUE 9.81         

class IMU400_DATA {
  public:

  IMU400_DATA() {
      m_pitch_rate = 0;
      m_roll_rate = 0;
      m_yaw_rate = 0;
      m_x_accel = 0;
      m_y_accel = 0;
      m_z_accel = 0;
      m_time = 0;
      m_temperature = 0;
  }

  IMU400_DATA(const IMU400_DATA &cpy) {
      m_pitch_rate = cpy.m_pitch_rate;
      m_roll_rate = cpy.m_roll_rate;
      m_yaw_rate = cpy.m_yaw_rate;
      m_x_accel = cpy.m_x_accel;
      m_y_accel = cpy.m_y_accel;
      m_z_accel = cpy.m_z_accel;
      m_time = cpy.m_time;
      m_temperature = cpy.m_temperature;
  }
  
  IMU400_DATA& operator=(const IMU400_DATA &cpy) {
      m_pitch_rate = cpy.m_pitch_rate;
      m_roll_rate = cpy.m_roll_rate;
      m_yaw_rate = cpy.m_yaw_rate;
      m_x_accel = cpy.m_x_accel;
      m_y_accel = cpy.m_y_accel;
      m_z_accel = cpy.m_z_accel;
      m_time = cpy.m_time;
      m_temperature = cpy.m_temperature;
  }

  void print(ostream &out) {
    out <<"RATE "<<m_roll_rate <<","<<m_pitch_rate<<","<<m_yaw_rate<<endl;
    out <<"ACCEL "<<m_x_accel<<","<<m_y_accel<<","<<m_z_accel<<endl;
    out <<"TEMP "<<m_temperature<<" TIME "<<m_time<<endl;
  }

  //VARIABLES
  float m_pitch_rate;
  float m_roll_rate;
  float m_yaw_rate;
  float m_x_accel;
  float m_y_accel;
  float m_z_accel;
  float m_time;
  float m_temperature;
};


class IMU400CC_200 : public BufferedNode
{
    
     friend  void* imu400_recv_thread (void *dev);
     friend  void* imu400_send_thread (void *dev);
  
private:
  
  //outputs
  int m_xAccelID;
  int m_yAccelID;
  int m_zAccelID;
  int m_yawRateID;
  int m_pitchRateID;
  int m_rollRateID;
  int m_xPosID;
  int m_yPosID;
  int m_zPosID;
  int m_yawID;
  int m_pitchID;
  int m_rollID;
     
  //parameters
  string m_serial_port;

  //Internal variables
  int m_fd;
  int m_killed;
  pthread_t m_sendThread;
  pthread_t m_recvThread;
  pthread_mutex_t m_send_list_lock;  // Used to block the access to the sending list
  pthread_mutex_t m_recv_list_lock;  // Used to block the access to the reception list
  pseudosem_t m_semaphore;          // Used to tell the sending thread that it can try to send something
  list<string> m_send_list;
  list<IMU400_DATA> m_recv_list;
  IMU400_DATA m_actual_IMU_data;
  float m_x_vel;
  float m_y_vel;
  float m_z_vel;
  float m_x_pos;
  float m_y_pos;
  float m_z_pos;
  float m_yaw;
  float m_pitch;
  float m_roll;
  float m_yaw_bias;
  float m_pitch_bias;
  float m_roll_bias;


  void SendPacket()
  {

       string to_send;

       //lock
       pthread_mutex_lock(&m_send_list_lock);

       if (!m_send_list.empty())
       {

            //Copy and Delete first element
            to_send = m_send_list.front();

            m_send_list.pop_front();

            //unlock here to avoid blocking on write (long)
            pthread_mutex_unlock(&m_send_list_lock);

         
            if(write(m_fd, to_send.c_str(),to_send.size()) < 0)
            {
                 perror("IMU400CC_200::Send():write():");
                 throw new GeneralException("IMU400CC_200::Send():write() error",__FILE__,__LINE__);
            }
       }//if !empty
       else
       {
            //unlock
            pthread_mutex_unlock(&m_send_list_lock);
       }
  }

  void processPacket(unsigned char packet[IMU_PACKET_SIZE], ostream &out)
  {
       short roll_rate = 0;
       short pitch_rate = 0;
       short yaw_rate = 0;
       short x_accel = 0;
       short y_accel = 0;
       short z_accel = 0;
       unsigned short temperature = 0;
       unsigned short time = 0;
       static unsigned short old_time=65535;//free running counter starts at 65535
       unsigned short temp1,temp2 = 0;

       //float f_roll_rate = 0;
       //float f_pitch_rate = 0;
       //float f_yaw_rate = 0;
       //float f_x_accel = 0;
       //float f_y_accel = 0;
       //float f_z_accel = 0;
       //float f_temperature = 0;
       IMU400_DATA data;
       
       //ROLL RATE
       temp1 = packet[1];
       temp2 = packet[2];
       roll_rate = (temp1<<8 | temp2);
       data.m_roll_rate = (float)roll_rate * IMU_RR_PRECISION * 1.5 / 32768.0;
       data.m_roll_rate -= m_roll_bias;


       //PITCH RATE
       temp1 = packet[3];
       temp2 = packet[4];
       pitch_rate = (temp1<<8 | temp2);
       data.m_pitch_rate = (float)pitch_rate * IMU_RR_PRECISION * 1.5 / 32768.0;
       data.m_pitch_rate -= m_pitch_bias;

       //YAW RATE
       temp1 = packet[5];
       temp2 = packet[6];
       yaw_rate = (temp1<<8 | temp2);
       data.m_yaw_rate = (float)yaw_rate * IMU_RR_PRECISION * 1.5 / 32768.0;
       data.m_yaw_rate -= m_yaw_bias;

       //X ACCEL
       temp1 = packet[7];
       temp2 = packet[8];
       x_accel = (temp1<<8 | temp2);
       data.m_x_accel = (float)x_accel * IMU_GR_PRECISION * 1.5 * IMU_GRAVITY_ACCEL_VALUE / 32768.0;
      

       //Y ACCEL
       temp1 = packet[9];
       temp2 = packet[10];
       y_accel = (temp1<<8 | temp2);
       data.m_y_accel = (float)y_accel * IMU_GR_PRECISION * 1.5 * IMU_GRAVITY_ACCEL_VALUE / 32768.0;
      

       //Z ACCEL
       temp1 = packet[11];
       temp2 = packet[12];
       z_accel = (temp1<<8 | temp2);
       data.m_z_accel = (float)z_accel * IMU_GR_PRECISION * 1.5 * IMU_GRAVITY_ACCEL_VALUE / 32768.0;
      

       //TEMPERATURE
       temp1 = packet[13];
       temp2 = packet[14];
       temperature = (temp1<<8 | temp2);
       data.m_temperature = (((float) temperature * 5.0 / 4096.0) - 1.375) * 44.44;
       
       //TIME
       temp1 = packet[15];
       temp2 = packet[16];
       time  = (temp1<<8 | temp2);

       if (time < old_time) {
	 //delta time
	 float new_time = old_time - time;
	 data.m_time = new_time * IMU_TIME_PRECISION;
       }
       else {
	 float new_time = 65535 - time + old_time;
	 data.m_time = new_time * IMU_TIME_PRECISION;
       }
       //data.print(cout);
       
       pthread_mutex_lock(&m_recv_list_lock);
       if (m_recv_list.size() < 1000) {
	 m_recv_list.push_back(data);
       }
       else {
	 cerr<<"IMU400CC_200 :  ERROR receive list full, discarding first element"<<endl;
	 m_recv_list.pop_front();
	 m_recv_list.push_back(data);
       }

       //store actual data
       m_actual_IMU_data = data;


       pthread_mutex_unlock(&m_recv_list_lock);
       //store old time
       old_time = time;
  }

  
  bool Checksum(unsigned char packet[IMU_PACKET_SIZE])
  {
       unsigned int checksum = 0;
       
       for (int i = 1; i < IMU_PACKET_SIZE - 1; i++) {
            checksum += packet[i];
       }

       if ((checksum & 0xFF) == packet[IMU_PACKET_SIZE -1 ]) {
            //cerr<<"CHECKSUM OK!"<<endl;
            return true;
       }
       else {
            cerr<<"CHECKSUM ERROR"<<endl;
            cerr<<"CALC: "<<checksum<<" packet : "<<(unsigned int) packet[IMU_PACKET_SIZE -1 ]<<endl;
          return false;
       }
  }

  void ReceivePacket()
  {
       unsigned char recv_message[IMU_PACKET_SIZE];

       int cnt = 0;               

       do {

          //waiting for packet header 0xFF
          while(!m_killed) {
               
               recv_message[0] = 0;
               cnt = 0;
                              
               if ( (cnt+=read(m_fd, &recv_message[0], 1 )) < 0 ) {
                    perror( "IMU400CC_200:ReceivePacket():" );
                    throw new GeneralException( "Cannot read packet",__FILE__,__LINE__ );
               }

               if (recv_message[0] == 0xFF) {
                    //cerr<<"got packet header 0xFF"<<endl;
                    break;
               }
          }

          //cerr<<"reading the rest of the message"<<endl;

          while(cnt != IMU_PACKET_SIZE && !m_killed) {

               if ( (cnt+=read(m_fd, &recv_message[cnt],  IMU_PACKET_SIZE - cnt )) < 0 ) {
                    perror( "IMU400CC_200:ReceivePacket():" );
                    throw new GeneralException("Cannot read packet",__FILE__,__LINE__);
               }               
          }
         
       } while(!m_killed && !Checksum(recv_message));


    processPacket(recv_message,cerr);

       
    //PUT MESSAGE IN RECV LIST
    //m_recv_list.push_back(string((char*)recv_message,IMU_PACKET_SIZE));
  }

public:
  
  IMU400CC_200(string nodeName, ParameterSet params)
    : BufferedNode(nodeName, params) , m_killed(0), 
      m_x_pos(0), m_y_pos(0), m_z_pos(0),
      m_yaw(0), m_pitch(0), m_roll(0),
      m_x_vel(0), m_y_vel(0), m_z_vel(0),
      m_yaw_bias(0), m_pitch_bias(0), m_roll_bias(0)
  {
    
     //inputs

    
     //outputs
    m_xAccelID = addOutput("X_ACCEL");
    m_yAccelID = addOutput("Y_ACCEL");
    m_zAccelID = addOutput("Z_ACCEL");
    m_yawRateID = addOutput("YAW_RATE");
    m_pitchRateID = addOutput("PITCH_RATE");
    m_rollRateID = addOutput("ROLL_RATE");
    m_xPosID = addOutput("X");
    m_yPosID = addOutput("Y");
    m_zPosID = addOutput("Z");
    m_yawID = addOutput("YAW");
    m_pitchID = addOutput("PITCH");
    m_rollID = addOutput("ROLL");
    
    //parameters
    m_serial_port = object_cast<String>(parameters.get("SERIAL_PORT"));
    
    //open serial port
    m_fd = Port_initialisation(m_serial_port.c_str(),BAUDRATE_38400);
    cerr<<"Open serial port : "<<m_serial_port<<" fd : "<<m_fd<<endl;
    
  }

  void initialize() {

       BufferedNode::initialize();
       
       unsigned char message[IMU_PACKET_SIZE];
       int timeout = 0;
       
       //verify if serial port opened
       if (m_fd >= 0) {

          
          cerr<<"STARTING POLLED MODE"<<endl;
	  message[0] = 'P'; 
	  write(m_fd, message,1);

	  timeout = 0;
	  do {
	    //PING MESSAGE
	    message[0] = 'R';
	    write(m_fd, message,1);
	    //READ DEVICE ANSWER
	    read(m_fd,message,1);
	    usleep(50000);
	    cerr<<"GOT PING ANSWER  : "<<message[0]<<endl;
	  } while(message[0] != 'H' && timeout++ < 100);

	  if (message[0] != 'H') {
	    throw new GeneralException("No device found",__FILE__,__LINE__);
	  }

          
          message[0] = 'z'; //ZERO
          message[1] = 0xC8;       	                   
          write(m_fd, message,2);
	  cerr<<"WAITING FOR ZERO"<<endl;	  
          read(m_fd,message,1);	  
          cerr<<"GOT ZERO MESSAGE  : "<<message[0]<<endl;
          
	  sleep(10);//wait 10 seconds
	  do {
	    message[0] = 'c'; //SCALED MODE MESSAGE
	    write(m_fd, message,1);
	    read(m_fd,message,1);
	    cerr<<"GOT SCALED MODE ANSWER  : "<<message[0]<<endl;
          } while (message[0] != 'C');

          cerr<<"STARTING CONTINUOUS MODE"<<endl;
          message[0] = 'C'; //CONTINUOUS MODE START
          write(m_fd, message,1);          
          
          //message send list list mutex
          pthread_mutex_init(&m_send_list_lock,NULL);
     
          //message recv list list mutex
          pthread_mutex_init(&m_recv_list_lock ,NULL);
     
          //semaphore to wake up send thread when something is ready.
          pseudosem_init(&m_semaphore,0,0);
          
          //creating recv & send threads
          pthread_create(&m_sendThread, NULL, imu400_send_thread, this);
          pthread_create(&m_recvThread, NULL, imu400_recv_thread, this);


	  //bias calib values over 1000 samples 
	  while(1) {	    
	       pthread_mutex_lock(&m_recv_list_lock);
	       if (m_recv_list.size() == 1000) break;
	       pthread_mutex_unlock(&m_recv_list_lock);
	  }


	  float count = 0;

	  //calculate bias
	  for (list<IMU400_DATA>::iterator iter = m_recv_list.begin();
	       iter != m_recv_list.end(); iter++) {
	    
	    m_yaw_bias += iter->m_yaw_rate;
	    m_pitch_bias += iter->m_pitch_rate;
	    m_roll_bias += iter->m_roll_rate;

	    count += 1.0;
	  }

	  if (count > 0) {	   
	    m_yaw_bias /= count;
	    m_pitch_bias /= count;
	    m_roll_bias /= count;
	  }
	  cerr<<"BIASES : "<<m_yaw_bias<<" "<<m_pitch_bias<<" "<<m_roll_bias<<endl;

	  //clear list
	  m_recv_list.resize(0);
	  
	  //very important to 
	  pthread_mutex_unlock(&m_recv_list_lock);
       }
  }

  void Add_message_to_send(const string &msg_to_send)
  {    
     //wait for the mutex
     pthread_mutex_lock(&m_send_list_lock);
     
     // add the new item into the list
     m_send_list.push_back(msg_to_send);
     
     // Unlock the mutex
     pthread_mutex_unlock(&m_send_list_lock);
     
     // Something new is in the list
     pseudosem_post(&m_semaphore);
  }

  void calculate(int output_id, int count, Buffer &out)
  {
    
    IMU400_DATA data;


    
    
    pthread_mutex_lock(&m_recv_list_lock);
    
    for (list<IMU400_DATA>::iterator iter = m_recv_list.begin();
	 iter != m_recv_list.end(); iter++) {
      //X 
      m_x_vel += iter->m_x_accel * iter->m_time;
      m_x_pos += m_x_vel * iter->m_time;
      //Y
      m_y_vel += iter->m_y_accel * iter->m_time;
      m_y_pos += m_y_vel * iter->m_time;
      //Z
      m_z_vel += iter->m_z_accel * iter->m_time;
      m_z_pos += m_z_vel * iter->m_time;
      //YAW
      m_yaw += iter->m_yaw_rate * iter->m_time;
      //PITCH
      m_pitch += iter->m_pitch_rate * iter->m_time;
      //ROLL
      m_roll += iter->m_roll_rate * iter->m_time;
    }
    
    //clean up list
    m_recv_list.resize(0);
    
    //get actual data
    data = m_actual_IMU_data;
      
    pthread_mutex_unlock(&m_recv_list_lock);

    

     //output result
    (*outputs[m_xAccelID].buffer)[count] = ObjectRef(Float::alloc(data.m_x_accel));
    (*outputs[m_yAccelID].buffer)[count] = ObjectRef(Float::alloc(data.m_y_accel));
    (*outputs[m_zAccelID].buffer)[count] = ObjectRef(Float::alloc(data.m_z_accel));
    (*outputs[m_yawRateID].buffer)[count] = ObjectRef(Float::alloc(data.m_yaw_rate));
    (*outputs[m_pitchRateID].buffer)[count] = ObjectRef(Float::alloc(data.m_pitch_rate));
    (*outputs[m_rollRateID].buffer)[count] = ObjectRef(Float::alloc(data.m_roll_rate));    
    (*outputs[m_xPosID].buffer)[count] = ObjectRef(Float::alloc(m_x_pos));
    (*outputs[m_yPosID].buffer)[count] = ObjectRef(Float::alloc(m_y_pos));
    (*outputs[m_zPosID].buffer)[count] = ObjectRef(Float::alloc(m_z_pos));
    (*outputs[m_yawID].buffer)[count] = ObjectRef(Float::alloc(m_yaw));
    (*outputs[m_pitchID].buffer)[count] = ObjectRef(Float::alloc(m_pitch));
    (*outputs[m_rollID].buffer)[count] = ObjectRef(Float::alloc(m_roll));




    } // end of calculate
    
  ~IMU400CC_200()
  {
     unsigned char message[IMU_PACKET_SIZE];
     cerr << "IMU400CC_200 : destructor" << endl;
     cerr << "IMU400CC_200 : waiting for send & receive threads" << endl;
     pthread_mutex_unlock(&m_send_list_lock); // unlock all
     pthread_mutex_unlock(&m_recv_list_lock);

     m_killed = 1;
     pseudosem_post(&m_semaphore);
     
     pthread_join(m_sendThread,NULL);
     cerr << "IMU400CC_200 : Sending_thread killed (joined)" << endl;
     
     pthread_join(m_recvThread,NULL);
     cerr << "IMU400CC_200 : Receiving_thread killed (joined)" << endl;

     pseudosem_destroy(&m_semaphore);
     pthread_mutex_destroy(&m_send_list_lock);
     pthread_mutex_destroy(&m_recv_list_lock);


     cerr<<"STARTING POLLED MODE"<<endl;
     message[0] = 'P'; 
     write(m_fd, message,1);
     sleep(1);

     End_session(m_fd);
     
     cerr << "IMU400CC_200 (done!)" << endl;
     
  }

}; // end of IMU400CC_200 class


void *imu400_send_thread (void *dev)
{

   IMU400CC_200 *device = reinterpret_cast<IMU400CC_200*>(dev);
     
   cerr<<"IMU400CC_200 : Send thread starting."<<endl;

   if (device) {  
   
     try
     {
          while(!device->m_killed)
          {
               
               pseudosem_wait(&device->m_semaphore);
               device->SendPacket();
          }
     }
     catch (BaseException *e)
     {
          e->print(cerr);
          delete e;
     }

   }
   cerr<<"IMU400CC_200 : Send thread done."<<endl;

   return NULL;
}


void *imu400_recv_thread (void *dev)
{

   IMU400CC_200 *device = reinterpret_cast<IMU400CC_200*>(dev);
       
   cerr<<"IMU400CC_200 : Receive thread starting."<<endl;

   if (device) {
   
     try
     {
          while (!device->m_killed)
          {
          device->ReceivePacket();
          }
     }
     catch (BaseException *e)
     {
          e->print(cerr);
          delete e;
     }
   }
   cerr<<"IMU400CC_200 : Receive thread done."<<endl;

   return NULL;
}
    

}//namespace RobotFlow

#endif