rt_vectornav.h File Reference

#include <lcm/lcm-cpp.hpp>
#include "SimUtilities/IMUTypes.h"
#include "vn/sensors.h"

Include dependency graph for rt_vectornav.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Functions
bool	init_vectornav (VectorNavData *vd_data)

Function Documentation

bool init_vectornav

(

VectorNavData *

vd_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: