epson_imu_driver_node.cpp

Go to the documentation of this file.

extern "C" {
#include "hcl.h"
#include "hcl_gpio.h"
#include "hcl_uart.h"
#include "sensor_epsonCommon.h"
}
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <termios.h>
#include <string>
#include <iostream>

using namespace std;

int comPort;
string serial_port;
int baudrate;

//=========================================================================
//------------------------ IMU Initializaion ------------------------------
//=========================================================================

bool init(struct EpsonOptions options){
  ROS_INFO("Initializing HCL layer...");
  if (!seInit()){
    ROS_ERROR("Error: could not initialize the Seiko Epson HCL layer. Exiting...");
    return false;
  }
  
  ROS_INFO("Initializing GPIO interface...");
  if (!gpioInit()){
    ROS_ERROR("Error: could not initialize the GPIO layer. Exiting...");
    return false;
  }
  
  ROS_INFO("Initializing UART interface...");
  comPort = uartInit(serial_port.c_str(), baudrate);
  if(comPort == -1){   
    ROS_ERROR("Error: could not initialize UART interface. Exiting...");
    return false;
  }
  
  ROS_INFO("Checking sensor NOT_READY status...");
  if(!sensorPowerOn()){
    ROS_ERROR("Error: failed to power on Sensor. Exiting...");
    uartRelease(comPort);
    return -1;
  }
  printf("...done.");
  
  ROS_INFO("Initializing Sensor...");
  if(!sensorInitOptions(options)){
    ROS_ERROR("Error: could not initialize Epson Sensor. Exiting...");
    uartRelease(comPort);
    return -1;
  }
  
  ROS_INFO("...Epson IMU initialized.");
}

//=========================================================================
//----------------------- Timestamp Correction ----------------------------
//=========================================================================

class TimeCorrection {
private:
  int MAX_COUNT;
  int ALMOST_ROLLOVER;
  int ONE_SEC_NSEC;
  int HALF_SEC_NSEC;

  int32_t count_corrected;
  int32_t count_old;
  int32_t count_diff;
  int32_t time_current;
  int32_t time_old;
  int32_t time_nsec_current;
  int32_t count_corrected_old;
  bool rollover;
  bool flag_imu_lead;
public:
  TimeCorrection();
  ros::Time get_stamp(int);
};

TimeCorrection::TimeCorrection(){
  MAX_COUNT = 1397861550;
  ALMOST_ROLLOVER = 1300000000;
  ONE_SEC_NSEC = 1000000000;
  HALF_SEC_NSEC = 500000000;
  
  count_corrected = 0;
  count_old = 0;
  count_diff = 0;
  time_current = 0;
  time_old = 0;
  time_nsec_current = 0;
  count_corrected_old = 0;
  rollover = false;
  flag_imu_lead = false;
}

ros::Time TimeCorrection::get_stamp(int count){
  //converting count into time
  time_current = ros::Time::now().toSec();
  time_nsec_current = ros::Time::now().nsec;

  count_diff = count - count_old;
  if(count > ALMOST_ROLLOVER){
    rollover = 1;
  }
  else if(count_diff<0){
    if(rollover){
      count_diff = count + (MAX_COUNT - count_old);
    }
    else {
      count_diff = count;
      count_corrected = 0;
      std::cout.precision(20);
      std::cout << "reset at: " << ros::Time::now().toSec() << std::endl;
    }
    rollover = 0;
  }
  count_corrected = (count_corrected + count_diff) % ONE_SEC_NSEC;
  if(time_current != time_old && count_corrected > HALF_SEC_NSEC){
    time_current = time_current - 1;
  }
  else if((count_corrected-count_corrected_old) < 0 && time_nsec_current > HALF_SEC_NSEC){
    time_current = time_current + 1;
    flag_imu_lead = 1;
  }
  else if(flag_imu_lead && time_nsec_current > HALF_SEC_NSEC){
    time_current = time_current + 1;
  }
  else{
    flag_imu_lead = 0;
  }

  ros::Time time;
  time.nsec = count_corrected;
  time.sec = time_current;
  time_old=time_current;
  count_old = count;
  count_corrected_old = count_corrected;
  return time;
}

//=========================================================================
//------------------------------ Main -------------------------------------
//=========================================================================

int main(int argc, char** argv){
  ros::init(argc, argv, "epson_imu_driver_node");
  ros::NodeHandle nh;
  ros::NodeHandle np("~");
  
  np.param<string>("port", serial_port, "/dev/ttyUSB0");

  struct EpsonOptions options;
  int time_correction = 0;
  
  np.param("enable_xgyro", options.enable_xgyro, 0);
  np.param("enable_ygyro", options.enable_ygyro, 0);
  np.param("enable_zgyro", options.enable_zgyro, 0);
  np.param("enable_xaccel", options.enable_xaccel, 1);
  np.param("enable_yaccel", options.enable_yaccel, 1);
  np.param("enable_zaccel", options.enable_zaccel, 1);
  np.param("enable_temp", options.enable_temp, 1);
  np.param("enable_xgyro_delta", options.enable_xgyro_delta, 0);
  np.param("enable_ygyro_delta", options.enable_ygyro_delta, 0);
  np.param("enable_zgyro_delta", options.enable_zgyro_delta, 0);
  np.param("enable_xaccel_delta", options.enable_xaccel_delta, 0);
  np.param("enable_yaccel_delta", options.enable_yaccel_delta, 0);
  np.param("enable_zaccel_delta", options.enable_zaccel_delta, 0);
  
  np.param("ext_sel", options.ext_sel, 1);
  np.param("ext_pol", options.ext_pol, 0);
  np.param("drdy_on", options.drdy_on, 0);
  np.param("drdy_pol", options.drdy_pol, 0);
  np.param("flash_test", options.flash_test, 0);
  np.param("self_test", options.self_test, 0);
  
  np.param("dout_rate", options.dout_rate, 3);
  
  np.param("filter_sel", options.filter_sel, 5);
  
  np.param("baud_rate", options.baud_rate, 0);
  np.param("auto_start", options.auto_start, 0);
  np.param("uart_auto", options.uart_auto, 1);
  
  np.param("flag_out", options.flag_out, 1);
  np.param("temp_out", options.temp_out, 1);
  np.param("gyro_out", options.gyro_out, 1);
  np.param("accel_out", options.accel_out, 1);
  np.param("gyro_delta_out", options.gyro_delta_out, 0);
  np.param("accel_delta_out", options.accel_delta_out, 0);
  np.param("gpio_out", options.gpio_out, 0);
  np.param("count_out", options.count_out, 1);
  np.param("checksum_out", options.checksum_out, 1);
  
  np.param("temp_bit", options.temp_bit, 1);
  np.param("gyro_bit", options.gyro_bit, 1);
  np.param("accel_bit", options.accel_bit, 1);
  np.param("gyro_delta_bit", options.gyro_delta_bit, 1);
  np.param("accel_delta_bit", options.accel_delta_bit, 1);
  
  np.param("invert_xgyro", options.invert_xgyro, 0);
  np.param("invert_ygyro", options.invert_ygyro, 0);
  np.param("invert_zgyro", options.invert_zgyro, 0);
  np.param("invert_xaccel", options.invert_xaccel, 0);
  np.param("invert_yaccel", options.invert_yaccel, 0);
  np.param("invert_zaccel", options.invert_zaccel, 0);

  np.param("dlt_ovf_en", options.dlt_ovf_en, 0);
  np.param("dlt_range_ctrl", options.dlt_range_ctrl, 8);
  
  np.param("time_correction", time_correction, 1);
  
  if(options.baud_rate == 0)
    baudrate = B460800;
  else
    baudrate = B230400;
  
  init(options);
  sensorStart();
  
  struct EpsonData epson_data;
  TimeCorrection tc;
  
  sensor_msgs::Imu imu_msg;
  for(int i = 0; i < 9; i++){
    imu_msg.orientation_covariance[i] = 0;
    imu_msg.angular_velocity_covariance[i] = 0;
    imu_msg.linear_acceleration_covariance[i] = 0;
  }
  imu_msg.orientation_covariance[0] = -1;
  
  ros::Publisher imu_pub = nh.advertise<sensor_msgs::Imu>("epson_imu", 1);
  
  while(ros::ok()){
    if(sensorDataReadBurstNOptions(options, &epson_data)){
      imu_msg.header.frame_id = "epson";
      if(!time_correction)
        imu_msg.header.stamp = ros::Time::now();
      else
        imu_msg.header.stamp = tc.get_stamp(epson_data.count);
      imu_msg.angular_velocity.x = epson_data.gyro_x;
      imu_msg.angular_velocity.y = epson_data.gyro_y;
      imu_msg.angular_velocity.z = epson_data.gyro_z;
      imu_msg.linear_acceleration.x = epson_data.accel_x;
      imu_msg.linear_acceleration.y = epson_data.accel_y;
      imu_msg.linear_acceleration.z = epson_data.accel_z;
      imu_pub.publish(imu_msg);
    }
  }

  sensorStop();
  uartRelease(comPort);
  gpioRelease();
  seRelease();
  
  return 0;
}