rt_vectornav.cpp File Reference

#include <inttypes.h>
#include <pthread.h>
#include <stdio.h>
#include <string>
#include <SimUtilities/IMUTypes.h>
#include <lcm/lcm-cpp.hpp>
#include <stdexcept>
#include "Utilities/utilities.h"
#include "rt/rt_vectornav.h"
#include "vectornav_lcmt.hpp"

Include dependency graph for rt_vectornav.cpp:

Go to the source code of this file.

Classes
struct	vn_sensor

Macros
#define	K_MINI_CHEETAH_VECTOR_NAV_SERIAL   "/dev/ttyS0"

Functions
int	processErrorReceived (const std::string &errorMessage, VnError errorCode)
void	vectornav_handler (void *userData, VnUartPacket *packet, size_t running_index)
bool	init_vectornav (VectorNavData *vn_data)

Variables
vn_sensor	vn
static lcm::LCM *	vectornav_lcm
vectornav_lcmt	vectornav_lcm_data
static VectorNavData *	g_vn_data = nullptr
int	got_first_vectornav_message = 0

Macro Definition Documentation

#define K_MINI_CHEETAH_VECTOR_NAV_SERIAL   "/dev/ttyS0"

Definition at line 13 of file rt_vectornav.cpp.

Function Documentation

bool init_vectornav

(

VectorNavData *

vn_data

)

Definition at line 32 of file rt_vectornav.cpp.

References vn_sensor::bor, getLcmUrl(), K_MINI_CHEETAH_VECTOR_NAV_SERIAL, processErrorReceived(), vectornav_handler(), vectornav_lcm, and vn_sensor::vs.

                                            {
  g_vn_data = vn_data;
  printf("[Simulation] Setup LCM...\n");
  vectornav_lcm = new lcm::LCM(getLcmUrl(255));
  if (!vectornav_lcm->good()) {
    printf("[ERROR] Failed to set up LCM\n");
    throw std::runtime_error("lcm bad");
  }

  VnError error;
  VpeBasicControlRegister vpeReg;
  ImuFilteringConfigurationRegister filtReg;
  const char SENSOR_PORT[] = K_MINI_CHEETAH_VECTOR_NAV_SERIAL;
  const uint32_t SENSOR_BAUDRATE = 115200;
  char modelNumber[30];
  char strConversions[50];
  uint32_t newHz, oldHz;
  // uint32_t hz_desired = 200;

  printf("[rt_vectornav] init_vectornav()\n");

  // initialize vectornav library
  VnSensor_initialize(&(vn.vs));

  // connect to sensor
  if ((error = VnSensor_connect(&(vn.vs), SENSOR_PORT, SENSOR_BAUDRATE)) !=
      E_NONE) {
    printf("[rt_vectornav] VnSensor_connect failed.\n");
    processErrorReceived("Error connecting to sensor.", error);
    return false;
  }

  // read the sensor's model number
  if ((error = VnSensor_readModelNumber(&(vn.vs), modelNumber,
                                        sizeof(modelNumber))) != E_NONE) {
    printf("[rt_vectornav] VnSensor_readModelNumber failed.\n");
    processErrorReceived("Error reading model number.", error);
    return false;
  }
  printf("Model Number: %s\n", modelNumber);

  // switch the sensor to 1 kHz mode
  if ((error = VnSensor_readAsyncDataOutputFrequency(&(vn.vs), &oldHz)) !=
      E_NONE) {
    printf("[rt_vectornav] VnSensor_readAsyncDataOutputFrequency failed.\n");
    processErrorReceived("Error reading async data output frequency.", error);
    return false;
  }

  // non-zero frequency causes the IMU to output ascii packets at the set
  // frequency, as well as binary
  if ((error = VnSensor_writeAsyncDataOutputFrequency(&(vn.vs), 0, true)) !=
      E_NONE) {
    printf("[rt_vectornav] VnSensor_wrtieAsyncDataOutputFrequency failed.\n");
    processErrorReceived("Error writing async data output frequency.", error);
    return false;
  }
  if ((error = VnSensor_readAsyncDataOutputFrequency(&(vn.vs), &newHz)) !=
      E_NONE) {
    printf("[rt_vectornav] VnSensor_readAsyncDataOutputFrequency failed.\n");
    processErrorReceived("Error reading async data output frequency.", error);
    return false;
  }
  printf("[rt_vectornav] Changed frequency from %d to %d Hz.\n", oldHz, newHz);

  // change to relative heading mode to avoid compass weirdness
  if ((error = VnSensor_readVpeBasicControl(&(vn.vs), &vpeReg)) != E_NONE) {
    printf("[rt_vectornav] VnSensor_ReadVpeBasicControl failed.\n");
    processErrorReceived("Error reading VPE basic control.", error);
    return false;
  }
  strFromHeadingMode(strConversions, (VnHeadingMode)vpeReg.headingMode);
  printf("[rt_vectornav] Sensor was in mode: %s\n", strConversions);
  vpeReg.headingMode = VNHEADINGMODE_RELATIVE;
  if ((error = VnSensor_writeVpeBasicControl(&(vn.vs), vpeReg, true)) !=
      E_NONE) {
    printf("[rt_vectornav] VnSensor_writeVpeBasicControl failed.\n");
    processErrorReceived("Error writing VPE basic control.", error);
    return false;
  }
  if ((error = VnSensor_readVpeBasicControl(&(vn.vs), &vpeReg)) != E_NONE) {
    processErrorReceived("Error reading VPE basic control.", error);
    printf("[rt_vectornav] VnSensor_ReadVpeBasicControl failed.\n");
    return false;
  }
  strFromHeadingMode(strConversions, (VnHeadingMode)vpeReg.headingMode);
  printf("[rt_vectornav] Sensor now id mode: %s\n", strConversions);

  if ((error = VnSensor_readImuFilteringConfiguration(&(vn.vs), &filtReg)) !=
      E_NONE) {
    printf("[rt_vectornav] VnSensor_readGyroCompensation failed.\n");
  }
  printf("[rt_vectornav] AccelWindow: %d\n", filtReg.accelWindowSize);
  //        filtReg.accelWindowSize = 4;    // We're sampling at 200 hz, but the
  //        imu samples at 800 hz. filtReg.accelFilterMode = 3; if((error =
  //        VnSensor_writeImuFilteringConfiguration(&(vn.vs), filtReg, true)) !=
  //        E_NONE)
  //        {
  //            printf("[rt_vectornav] VnSensor_writeGyroCompensation
  //            failed.\n");
  //        }

  // setup binary output message type
  BinaryOutputRegister_initialize(
      &(vn.bor), ASYNCMODE_PORT2,
      4,  // divisor:  output frequency = 800/divisor
      (CommonGroup)(COMMONGROUP_QUATERNION | COMMONGROUP_ANGULARRATE |
                    COMMONGROUP_ACCEL),
      TIMEGROUP_NONE, IMUGROUP_NONE, GPSGROUP_NONE, ATTITUDEGROUP_NONE,
      INSGROUP_NONE, GPSGROUP_NONE);

  if ((error = VnSensor_writeBinaryOutput1(&(vn.vs), &(vn.bor), true)) !=
      E_NONE) {
    printf("[rt_vectornav] VnSensor_writeBinaryOutput1 failed.\n");
    processErrorReceived("Error writing binary output 1.", error);
    return false;
  }

  // setup handler
  VnSensor_registerAsyncPacketReceivedHandler(&(vn.vs), vectornav_handler,
                                              NULL);
  printf("[rt_vectornav] IMU is set up!\n");
  return true;
}

Here is the call graph for this function:

Here is the caller graph for this function:

int processErrorReceived	(	const std::string &	errorMessage,
		VnError	errorCode
	)

Definition at line 212 of file rt_vectornav.cpp.

                                                                           {
  char errorCodeStr[100];
  strFromVnError(errorCodeStr, errorCode);
  printf("%s\nVECTORNAV ERROR: %s\n", errorMessage.c_str(), errorCodeStr);
  return -1;
}

Here is the caller graph for this function:

void vectornav_handler	(	void *	userData,
		VnUartPacket *	packet,
		size_t	running_index
	)

Definition at line 158 of file rt_vectornav.cpp.

References VectorNavData::accelerometer, VectorNavData::gyro, VectorNavData::quat, vectornav_lcm, and vectornav_lcm_data.

                                             {
  (void)userData;
  (void)running_index;
  vec4f quat;
  vec3f omega;
  vec3f a;

  if (VnUartPacket_type(packet) != PACKETTYPE_BINARY) {
    printf("[vectornav_handler] got a packet that wasn't binary.\n");
    return;
  }

  if (!VnUartPacket_isCompatible(
          packet,
          (CommonGroup)(COMMONGROUP_QUATERNION | COMMONGROUP_ANGULARRATE |
                        COMMONGROUP_ACCEL),
          TIMEGROUP_NONE, IMUGROUP_NONE, GPSGROUP_NONE, ATTITUDEGROUP_NONE,
          INSGROUP_NONE, GPSGROUP_NONE)) {
    printf("[vectornav_handler] got a packet with the wrong type of data.\n");
    return;
  }

  quat = VnUartPacket_extractVec4f(packet);
  omega = VnUartPacket_extractVec3f(packet);
  a = VnUartPacket_extractVec3f(packet);

  for (int i = 0; i < 4; i++) {
    vectornav_lcm_data.q[i] = quat.c[i];
    g_vn_data->quat[i] = quat.c[i];
  }

  for (int i = 0; i < 3; i++) {
    vectornav_lcm_data.w[i] = omega.c[i];
    vectornav_lcm_data.a[i] = a.c[i];
    g_vn_data->gyro[i] = omega.c[i];
    g_vn_data->accelerometer[i] = a.c[i];
  }

  vectornav_lcm->publish("hw_vectornav", &vectornav_lcm_data);

#ifdef PRINT_VECTORNAV_DEBUG
  char strConversions[50];
  str_vec4f(strConversions, quat);
  printf("[QUAT] %s\n", strConversions);

  str_vec3f(strConversions, omega);
  printf("[OMEGA] %s\n", strConversions);

  str_vec3f(strConversions, a);
  printf("[ACC] %s\n", strConversions);
#endif
}

Here is the caller graph for this function:

Variable Documentation

VectorNavData* g_vn_data = nullptr

static

Definition at line 31 of file rt_vectornav.cpp.

int got_first_vectornav_message = 0

Definition at line 157 of file rt_vectornav.cpp.

lcm::LCM* vectornav_lcm

static

Definition at line 28 of file rt_vectornav.cpp.

vectornav_lcmt vectornav_lcm_data

Definition at line 29 of file rt_vectornav.cpp.

vn_sensor vn

Definition at line 26 of file rt_vectornav.cpp.