C++ (Cpp) imu_get_magnetic_field Examples

Example #1

File: tinkerforge_sensors_core.cpp Project: gus484/ros-tinkerforge_sensors

void TinkerforgeSensors::publishMagneticFieldMessage(SensorDevice *sensor)
{

  if (sensor != NULL)
  {
    if (sensor->getType() != IMU_V2_DEVICE_IDENTIFIER)
      return;

    int16_t x = 0, y = 0, z = 0;

    // for the conversions look at rep 103 http://www.ros.org/reps/rep-0103.html
    // for IMU v1 http://www.tinkerforge.com/de/doc/Software/Bricks/IMU_Brick_C.html#imu-brick-c-api
    // for IMU v2 http://www.tinkerforge.com/de/doc/Software/Bricks/IMUV2_Brick_C.html#imu-v2-brick-c-api
    if (sensor->getType() == IMU_DEVICE_IDENTIFIER)
    {
      imu_get_magnetic_field((IMU*)sensor->getDev(), &x, &y, &z); // nT -> T
      x = x / 10000000.0;
      y = y / 10000000.0;
      z = z / 10000000.0;
    }
    else
    {
      imu_v2_get_magnetic_field((IMUV2*)sensor->getDev(), &x, &y, &z); // µT -> T
      x = x / 1000000.0;
      y = y / 1000000.0;
      z = z / 1000000.0;
    }

    sensor_msgs::MagneticField mf_msg;

    // message header
    mf_msg.header.seq =  sensor->getSeq();
    mf_msg.header.stamp = ros::Time::now();
    mf_msg.header.frame_id = sensor->getFrame();

    // magnetic field from mG to T
    mf_msg.magnetic_field.x = x;
    mf_msg.magnetic_field.y = y;
    mf_msg.magnetic_field.z = z;

    boost::array<const double, 9> mfc =
      { 0.01, 0.01, 0.01,
        0.01, 0.01, 0.01,
        0.01, 0.01, 0.01};

    mf_msg.magnetic_field_covariance = mfc;

    sensor->getPub().publish(mf_msg);
  }
  return;
}

Example #2

File: laser_transform_core.cpp Project: asimay/ros

void LaserTransform::publishMagneticFieldMessage(ros::Publisher *pub_message)
{
  static uint32_t seq = 0;
  if (is_imu_connected || is_imu_v2_connected)
  {
    int16_t x = 0, y = 0, z = 0;

    // for the conversions look at rep 103 http://www.ros.org/reps/rep-0103.html
    // for IMU v1 http://www.tinkerforge.com/de/doc/Software/Bricks/IMU_Brick_C.html#imu-brick-c-api
    // for IMU v2 http://www.tinkerforge.com/de/doc/Software/Bricks/IMUV2_Brick_C.html#imu-v2-brick-c-api
    if (is_imu_connected)
    {
      imu_get_magnetic_field(&imu, &x, &y, &z); // nT -> T
      x = x / 10000000.0;
      y = y / 10000000.0;
      z = z / 10000000.0;
    }
    else
    {
      imu_v2_get_magnetic_field(&imu_v2, &x, &y, &z); // µT -> T
      x = x / 1000000.0;
      y = y / 1000000.0;
      z = z / 1000000.0;
    }

    sensor_msgs::MagneticField mf_msg;

    // message header
    mf_msg.header.seq =  seq;
    mf_msg.header.stamp = ros::Time::now();
    mf_msg.header.frame_id = "base_link";

    // magnetic field from mG to T
    mf_msg.magnetic_field.x = x;
    mf_msg.magnetic_field.y = y;
    mf_msg.magnetic_field.z = z;

    boost::array<const double, 9> mfc =
      { 0.01, 0.01, 0.01,
        0.01, 0.01, 0.01,
        0.01, 0.01, 0.01};

    mf_msg.magnetic_field_covariance = mfc;

    pub_message->publish(mf_msg);

    seq++;
  }
  return;
}