imu.cpp
Go to the documentation of this file.
1 
9 /*
10  * Copyright 2013-2017 Vladimir Ermakov.
11  *
12  * This file is part of the mavros package and subject to the license terms
13  * in the top-level LICENSE file of the mavros repository.
14  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
15  */
16 
17 #include <cmath>
18 #include <mavros/mavros_plugin.h>
20 
21 #include <sensor_msgs/Imu.h>
22 #include <sensor_msgs/MagneticField.h>
23 #include <sensor_msgs/Temperature.h>
24 #include <sensor_msgs/FluidPressure.h>
25 #include <geometry_msgs/Vector3.h>
26 
27 namespace mavros {
28 namespace std_plugins {
30 static constexpr double GAUSS_TO_TESLA = 1.0e-4;
32 static constexpr double MILLIT_TO_TESLA = 1000.0;
34 static constexpr double MILLIRS_TO_RADSEC = 1.0e-3;
36 static constexpr double MILLIG_TO_MS2 = 9.80665 / 1000.0;
38 static constexpr double MILLIMS2_TO_MS2 = 1.0e-3;
40 static constexpr double MILLIBAR_TO_PASCAL = 1.0e2;
42 static constexpr double RAD_TO_DEG = 180.0 / M_PI;
43 
44 
46 class IMUPlugin : public plugin::PluginBase {
47 public:
48  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 
51  imu_nh("~imu"),
52  has_hr_imu(false),
53  has_raw_imu(false),
54  has_scaled_imu(false),
55  has_att_quat(false),
56  received_linear_accel(false),
57  linear_accel_vec_flu(Eigen::Vector3d::Zero()),
58  linear_accel_vec_frd(Eigen::Vector3d::Zero())
59  { }
60 
61  void initialize(UAS &uas_) override
62  {
63  PluginBase::initialize(uas_);
64 
65  double linear_stdev, angular_stdev, orientation_stdev, mag_stdev;
66 
73  imu_nh.param<std::string>("frame_id", frame_id, "base_link");
74  imu_nh.param("linear_acceleration_stdev", linear_stdev, 0.0003); // check default by MPU6000 spec
75  imu_nh.param("angular_velocity_stdev", angular_stdev, 0.02 * (M_PI / 180.0)); // check default by MPU6000 spec
76  imu_nh.param("orientation_stdev", orientation_stdev, 1.0);
77  imu_nh.param("magnetic_stdev", mag_stdev, 0.0);
78 
80  setup_covariance(angular_velocity_cov, angular_stdev);
81  setup_covariance(orientation_cov, orientation_stdev);
82  setup_covariance(magnetic_cov, mag_stdev);
84 
85  imu_pub = imu_nh.advertise<sensor_msgs::Imu>("data", 10);
86  magn_pub = imu_nh.advertise<sensor_msgs::MagneticField>("mag", 10);
87  temp_imu_pub = imu_nh.advertise<sensor_msgs::Temperature>("temperature_imu", 10);
88  temp_baro_pub = imu_nh.advertise<sensor_msgs::Temperature>("temperature_baro", 10);
89  static_press_pub = imu_nh.advertise<sensor_msgs::FluidPressure>("static_pressure", 10);
90  diff_press_pub = imu_nh.advertise<sensor_msgs::FluidPressure>("diff_pressure", 10);
91  imu_raw_pub = imu_nh.advertise<sensor_msgs::Imu>("data_raw", 10);
92 
93  // Reset has_* flags on connection change
95  }
96 
98  return {
105  };
106  }
107 
108 private:
110  std::string frame_id;
111 
119 
125  Eigen::Vector3d linear_accel_vec_flu;
126  Eigen::Vector3d linear_accel_vec_frd;
132 
133  /* -*- helpers -*- */
134 
141  void setup_covariance(ftf::Covariance3d &cov, double stdev)
142  {
143  ftf::EigenMapCovariance3d c(cov.data());
144 
145  c.setZero();
146  if (stdev) {
147  double sr = stdev * stdev;
148  c.diagonal() << sr, sr, sr;
149  }
150  else {
151  c(0,0) = -1.0;
152  }
153  }
154 
163  void publish_imu_data(uint32_t time_boot_ms, Eigen::Quaterniond &orientation_enu,
164  Eigen::Quaterniond &orientation_ned, Eigen::Vector3d &gyro_flu, Eigen::Vector3d &gyro_frd)
165  {
166  auto imu_ned_msg = boost::make_shared<sensor_msgs::Imu>();
167  auto imu_enu_msg = boost::make_shared<sensor_msgs::Imu>();
168 
169  // Fill message header
170  imu_enu_msg->header = m_uas->synchronized_header(frame_id, time_boot_ms);
171  imu_ned_msg->header = m_uas->synchronized_header("aircraft", time_boot_ms);
172 
173  // Convert from Eigen::Quaternond to geometry_msgs::Quaternion
174  tf::quaternionEigenToMsg(orientation_enu, imu_enu_msg->orientation);
175  tf::quaternionEigenToMsg(orientation_ned, imu_ned_msg->orientation);
176 
177  // Convert from Eigen::Vector3d to geometry_msgs::Vector3
178  tf::vectorEigenToMsg(gyro_flu, imu_enu_msg->angular_velocity);
179  tf::vectorEigenToMsg(gyro_frd, imu_ned_msg->angular_velocity);
180 
181  // Eigen::Vector3d from HIGHRES_IMU or RAW_IMU, to geometry_msgs::Vector3
182  tf::vectorEigenToMsg(linear_accel_vec_flu, imu_enu_msg->linear_acceleration);
183  tf::vectorEigenToMsg(linear_accel_vec_frd, imu_ned_msg->linear_acceleration);
184 
185  // Pass ENU msg covariances
186  imu_enu_msg->orientation_covariance = orientation_cov;
187  imu_enu_msg->angular_velocity_covariance = angular_velocity_cov;
188  imu_enu_msg->linear_acceleration_covariance = linear_acceleration_cov;
189 
190  // Pass NED msg covariances
191  imu_ned_msg->orientation_covariance = orientation_cov;
192  imu_ned_msg->angular_velocity_covariance = angular_velocity_cov;
193  imu_ned_msg->linear_acceleration_covariance = linear_acceleration_cov;
194 
195  if (!received_linear_accel) {
196  // Set element 0 of covariance matrix to -1 if no data received as per sensor_msgs/Imu defintion
197  imu_enu_msg->linear_acceleration_covariance[0] = -1;
198  imu_ned_msg->linear_acceleration_covariance[0] = -1;
199  }
200 
204  // [store_enu]
205  m_uas->update_attitude_imu_enu(imu_enu_msg);
206  // [store_enu]
207 
211  // [store_enu]
212  m_uas->update_attitude_imu_ned(imu_ned_msg);
213  // [store_ned]
214 
218  // [pub_enu]
219  imu_pub.publish(imu_enu_msg);
220  // [pub_enu]
221  }
222 
230  void publish_imu_data_raw(std_msgs::Header &header, Eigen::Vector3d &gyro_flu,
231  Eigen::Vector3d &accel_flu, Eigen::Vector3d &accel_frd)
232  {
233  auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
234 
235  // Fill message header
236  imu_msg->header = header;
237 
238  tf::vectorEigenToMsg(gyro_flu, imu_msg->angular_velocity);
239  tf::vectorEigenToMsg(accel_flu, imu_msg->linear_acceleration);
240 
241  // Save readings
242  linear_accel_vec_flu = accel_flu;
243  linear_accel_vec_frd = accel_frd;
244  received_linear_accel = true;
245 
246  imu_msg->orientation_covariance = unk_orientation_cov;
247  imu_msg->angular_velocity_covariance = angular_velocity_cov;
248  imu_msg->linear_acceleration_covariance = linear_acceleration_cov;
249 
250  // Publish message [ENU frame]
251  imu_raw_pub.publish(imu_msg);
252  }
253 
259  void publish_mag(std_msgs::Header &header, Eigen::Vector3d &mag_field)
260  {
261  auto magn_msg = boost::make_shared<sensor_msgs::MagneticField>();
262 
263  // Fill message header
264  magn_msg->header = header;
265 
266  tf::vectorEigenToMsg(mag_field, magn_msg->magnetic_field);
267  magn_msg->magnetic_field_covariance = magnetic_cov;
268 
269  // Publish message [ENU frame]
270  magn_pub.publish(magn_msg);
271  }
272 
273  /* -*- message handlers -*- */
274 
281  void handle_attitude(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE &att)
282  {
283  if (has_att_quat)
284  return;
285 
289  // [ned_aircraft_orient1]
290  auto ned_aircraft_orientation = ftf::quaternion_from_rpy(att.roll, att.pitch, att.yaw);
291  // [ned_aircraft_orient1]
292 
296  // [frd_ang_vel1]
297  auto gyro_frd = Eigen::Vector3d(att.rollspeed, att.pitchspeed, att.yawspeed);
298  // [frd_ang_vel1]
299 
304  // [ned->baselink->enu]
305  auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink(
306  ftf::transform_orientation_ned_enu(ned_aircraft_orientation));
307  // [ned->baselink->enu]
308 
313  // [rotate_gyro]
314  auto gyro_flu = ftf::transform_frame_aircraft_baselink(gyro_frd);
315  // [rotate_gyro]
316 
317  publish_imu_data(att.time_boot_ms, enu_baselink_orientation, ned_aircraft_orientation, gyro_flu, gyro_frd);
318  }
319 
326  void handle_attitude_quaternion(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE_QUATERNION &att_q)
327  {
328  ROS_INFO_COND_NAMED(!has_att_quat, "imu", "IMU: Attitude quaternion IMU detected!");
329  has_att_quat = true;
330 
334  // [ned_aircraft_orient2]
335  auto ned_aircraft_orientation = Eigen::Quaterniond(att_q.q1, att_q.q2, att_q.q3, att_q.q4);
336  // [ned_aircraft_orient2]
337 
341  // [frd_ang_vel2]
342  auto gyro_frd = Eigen::Vector3d(att_q.rollspeed, att_q.pitchspeed, att_q.yawspeed);
343  // [frd_ang_vel2]
344 
350  auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink(
351  ftf::transform_orientation_ned_enu(ned_aircraft_orientation));
352 
357  auto gyro_flu = ftf::transform_frame_aircraft_baselink(gyro_frd);
358 
359  publish_imu_data(att_q.time_boot_ms, enu_baselink_orientation, ned_aircraft_orientation, gyro_flu, gyro_frd);
360  }
361 
368  void handle_highres_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIGHRES_IMU &imu_hr)
369  {
370  ROS_INFO_COND_NAMED(!has_hr_imu, "imu", "IMU: High resolution IMU detected!");
371  has_hr_imu = true;
372 
373  auto header = m_uas->synchronized_header(frame_id, imu_hr.time_usec);
381  // [accel_available]
382  if (imu_hr.fields_updated & ((7 << 3) | (7 << 0))) {
383  auto gyro_flu = ftf::transform_frame_aircraft_baselink(Eigen::Vector3d(imu_hr.xgyro, imu_hr.ygyro, imu_hr.zgyro));
384 
385  auto accel_frd = Eigen::Vector3d(imu_hr.xacc, imu_hr.yacc, imu_hr.zacc);
386  auto accel_flu = ftf::transform_frame_aircraft_baselink(accel_frd);
387 
388  publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd);
389  }
390  // [accel_available]
391 
395  // [mag_available]
396  if (imu_hr.fields_updated & (7 << 6)) {
397  auto mag_field = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
398  Eigen::Vector3d(imu_hr.xmag, imu_hr.ymag, imu_hr.zmag) * GAUSS_TO_TESLA);
399 
400  publish_mag(header, mag_field);
401  }
402  // [mag_available]
403 
407  // [static_pressure_available]
408  if (imu_hr.fields_updated & (1 << 9)) {
409  auto static_pressure_msg = boost::make_shared<sensor_msgs::FluidPressure>();
410 
411  static_pressure_msg->header = header;
412  static_pressure_msg->fluid_pressure = imu_hr.abs_pressure;
413 
414  static_press_pub.publish(static_pressure_msg);
415  }
416  // [static_pressure_available]
417 
421  // [differential_pressure_available]
422  if (imu_hr.fields_updated & (1 << 10)) {
423  auto differential_pressure_msg = boost::make_shared<sensor_msgs::FluidPressure>();
424 
425  differential_pressure_msg->header = header;
426  differential_pressure_msg->fluid_pressure = imu_hr.diff_pressure;
427 
428  diff_press_pub.publish(differential_pressure_msg);
429  }
430  // [differential_pressure_available]
431 
435  // [temperature_available]
436  if (imu_hr.fields_updated & (1 << 12)) {
437  auto temp_msg = boost::make_shared<sensor_msgs::Temperature>();
438 
439  temp_msg->header = header;
440  temp_msg->temperature = imu_hr.temperature;
441 
442  temp_imu_pub.publish(temp_msg);
443  }
444  // [temperature_available]
445  }
446 
453  void handle_raw_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RAW_IMU &imu_raw)
454  {
455  ROS_INFO_COND_NAMED(!has_raw_imu, "imu", "IMU: Raw IMU message used.");
456  has_raw_imu = true;
457 
458  if (has_hr_imu || has_scaled_imu)
459  return;
460 
461  auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
462  auto header = m_uas->synchronized_header(frame_id, imu_raw.time_usec);
463 
466  auto gyro_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
467  Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC);
468  auto accel_frd = Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
469  auto accel_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(accel_frd);
470 
471  if (m_uas->is_ardupilotmega()) {
472  accel_frd *= MILLIG_TO_MS2;
473  accel_flu *= MILLIG_TO_MS2;
474  } else if (m_uas->is_px4()) {
475  accel_frd *= MILLIMS2_TO_MS2;
476  accel_flu *= MILLIMS2_TO_MS2;
477  }
478 
479  publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd);
480 
481  if (!m_uas->is_ardupilotmega()) {
482  ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: linear acceleration on RAW_IMU known on APM only.");
483  ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: ~imu/data_raw stores unscaled raw acceleration report.");
484  linear_accel_vec_flu.setZero();
485  linear_accel_vec_frd.setZero();
486  }
487 
491  // [mag_field]
492  auto mag_field = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
493  Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA);
494  // [mag_field]
495 
496  publish_mag(header, mag_field);
497  }
498 
505  void handle_scaled_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SCALED_IMU &imu_raw)
506  {
507  if (has_hr_imu)
508  return;
509 
510  ROS_INFO_COND_NAMED(!has_scaled_imu, "imu", "IMU: Scaled IMU message used.");
511  has_scaled_imu = true;
512 
513  auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
514  auto header = m_uas->synchronized_header(frame_id, imu_raw.time_boot_ms);
515 
516  auto gyro_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
517  Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC);
518  auto accel_frd = Eigen::Vector3d(Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc) * MILLIG_TO_MS2);
519  auto accel_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(accel_frd);
520 
521  publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd);
522 
526  auto mag_field = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
527  Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA);
528 
529  publish_mag(header, mag_field);
530  }
531 
538  void handle_scaled_pressure(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SCALED_PRESSURE &press)
539  {
540  if (has_hr_imu)
541  return;
542 
543  auto header = m_uas->synchronized_header(frame_id, press.time_boot_ms);
544 
545  auto temp_msg = boost::make_shared<sensor_msgs::Temperature>();
546  temp_msg->header = header;
547  temp_msg->temperature = press.temperature / 100.0;
548  temp_baro_pub.publish(temp_msg);
549 
550  auto static_pressure_msg = boost::make_shared<sensor_msgs::FluidPressure>();
551  static_pressure_msg->header = header;
552  static_pressure_msg->fluid_pressure = press.press_abs * 100.0;
553  static_press_pub.publish(static_pressure_msg);
554 
555  auto differential_pressure_msg = boost::make_shared<sensor_msgs::FluidPressure>();
556  differential_pressure_msg->header = header;
557  differential_pressure_msg->fluid_pressure = press.press_diff * 100.0;
558  diff_press_pub.publish(differential_pressure_msg);
559  }
560 
561  // Checks for connection and overrides variable values
562  void connection_cb(bool connected) override
563  {
564  has_hr_imu = false;
565  has_scaled_imu = false;
566  has_att_quat = false;
567  }
568 };
569 } // namespace std_plugins
570 } // namespace mavros
571 
ros::Publisher diff_press_pub
Definition: imu.cpp:118
MAVROS Plugin base class.
Definition: mavros_plugin.h:36
std_msgs::Header synchronized_header(const std::string &frame_id, const T time_stamp)
Create message header from time_boot_ms or time_usec stamps and frame_id.
Definition: mavros_uas.h:375
#define ROS_WARN_THROTTLE_NAMED(rate, name,...)
#define ROS_INFO_COND_NAMED(cond, name,...)
void publish(const boost::shared_ptr< M > &message) const
static constexpr double GAUSS_TO_TESLA
Gauss to Tesla coeff.
Definition: imu.cpp:30
void quaternionEigenToMsg(const Eigen::Quaterniond &e, geometry_msgs::Quaternion &m)
MAVROS Plugin base.
HandlerInfo make_handler(const mavlink::msgid_t id, void(_C::*fn)(const mavlink::mavlink_message_t *msg, const mavconn::Framing framing))
Definition: mavros_plugin.h:87
IMU and attitude data publication plugin.
Definition: imu.cpp:46
Eigen::Vector3d linear_accel_vec_flu
Definition: imu.cpp:125
ftf::Covariance3d magnetic_cov
Definition: imu.cpp:131
void handle_raw_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RAW_IMU &imu_raw)
Handle RAW_IMU MAVlink message. Message specification: https://mavlink.io/en/messages/common.html/#RAW_IMU.
Definition: imu.cpp:453
void publish_imu_data(uint32_t time_boot_ms, Eigen::Quaterniond &orientation_enu, Eigen::Quaterniond &orientation_ned, Eigen::Vector3d &gyro_flu, Eigen::Vector3d &gyro_frd)
Fill and publish IMU data message.
Definition: imu.cpp:163
ros::Publisher imu_raw_pub
Definition: imu.cpp:113
void connection_cb(bool connected) override
Definition: imu.cpp:562
void handle_attitude_quaternion(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE_QUATERNION &att_q)
Handle ATTITUDE_QUATERNION MAVlink message. Message specification: https://mavlink.io/en/messages/common.html/#ATTITUDE_QUATERNION.
Definition: imu.cpp:326
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:74
void update_attitude_imu_enu(sensor_msgs::Imu::Ptr &imu)
Store IMU data [ENU].
Definition: uas_data.cpp:141
ftf::Covariance3d angular_velocity_cov
Definition: imu.cpp:128
ftf::Covariance3d linear_acceleration_cov
Definition: imu.cpp:127
T transform_frame_aircraft_baselink(const T &in)
Transform data expressed in Aircraft frame to Baselink frame.
Definition: frame_tf.h:233
ros::Publisher imu_pub
Definition: imu.cpp:112
UAS for plugins.
Definition: mavros_uas.h:67
ftf::Covariance3d unk_orientation_cov
Definition: imu.cpp:130
EIGEN_MAKE_ALIGNED_OPERATOR_NEW IMUPlugin()
Definition: imu.cpp:50
Subscriptions get_subscriptions() override
Return vector of MAVLink message subscriptions (handlers)
Definition: imu.cpp:97
void initialize(UAS &uas_) override
Plugin initializer.
Definition: imu.cpp:61
void vectorEigenToMsg(const Eigen::Vector3d &e, geometry_msgs::Vector3 &m)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
bool is_ardupilotmega()
Check that FCU is APM.
Definition: mavros_uas.h:410
ros::Publisher temp_baro_pub
Definition: imu.cpp:116
static constexpr double MILLIG_TO_MS2
millG to m/s**2 coeff
Definition: imu.cpp:36
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static constexpr double MILLIMS2_TO_MS2
millm/s**2 to m/s**2 coeff
Definition: imu.cpp:38
Eigen::Vector3d linear_accel_vec_frd
Definition: imu.cpp:126
Eigen::Map< Eigen::Matrix< double, 3, 3, Eigen::RowMajor > > EigenMapCovariance3d
Eigen::Map for Covariance3d.
Definition: frame_tf.h:46
ros::NodeHandle imu_nh
Definition: imu.cpp:109
void publish_mag(std_msgs::Header &header, Eigen::Vector3d &mag_field)
Publish magnetic field data.
Definition: imu.cpp:259
sensor_msgs::Imu::_angular_velocity_covariance_type Covariance3d
Type matching rosmsg for 3x3 covariance matrix.
Definition: frame_tf.h:37
void handle_scaled_pressure(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SCALED_PRESSURE &press)
Handle SCALED_PRESSURE MAVlink message. Message specification: https://mavlink.io/en/messages/common...
Definition: imu.cpp:538
void publish_imu_data_raw(std_msgs::Header &header, Eigen::Vector3d &gyro_flu, Eigen::Vector3d &accel_flu, Eigen::Vector3d &accel_frd)
Fill and publish IMU data_raw message; store linear acceleration for IMU data.
Definition: imu.cpp:230
Eigen::Quaterniond quaternion_from_rpy(const Eigen::Vector3d &rpy)
Convert euler angles to quaternion.
void handle_attitude(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE &att)
Handle ATTITUDE MAVlink message. Message specification: https://mavlink.io/en/messages/common.html#ATTITUDE.
Definition: imu.cpp:281
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:47
ros::Publisher static_press_pub
Definition: imu.cpp:117
ftf::Covariance3d orientation_cov
Definition: imu.cpp:129
T transform_orientation_ned_enu(const T &in)
Transform from attitude represented WRT NED frame to attitude represented WRT ENU frame...
Definition: frame_tf.h:160
void handle_scaled_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SCALED_IMU &imu_raw)
Handle SCALED_IMU MAVlink message. Message specification: https://mavlink.io/en/messages/common.html/#SCALED_IMU.
Definition: imu.cpp:505
void setup_covariance(ftf::Covariance3d &cov, double stdev)
Setup 3x3 covariance matrix.
Definition: imu.cpp:141
void update_attitude_imu_ned(sensor_msgs::Imu::Ptr &imu)
Store IMU data [NED].
Definition: uas_data.cpp:147
static constexpr double MILLIT_TO_TESLA
millTesla to Tesla coeff
Definition: imu.cpp:32
T transform_orientation_aircraft_baselink(const T &in)
Transform from attitude represented WRT aircraft frame to attitude represented WRT base_link frame...
Definition: frame_tf.h:178
ros::Publisher magn_pub
Definition: imu.cpp:114
ros::Publisher temp_imu_pub
Definition: imu.cpp:115
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
bool is_px4()
Check that FCU is PX4.
Definition: mavros_uas.h:417
static constexpr double MILLIRS_TO_RADSEC
millRad/Sec to Rad/Sec coeff
Definition: imu.cpp:34
static constexpr double RAD_TO_DEG
Radians to degrees.
Definition: imu.cpp:42
static constexpr double MILLIBAR_TO_PASCAL
millBar to Pascal coeff
Definition: imu.cpp:40
void handle_highres_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIGHRES_IMU &imu_hr)
Handle HIGHRES_IMU MAVlink message. Message specification: https://mavlink.io/en/messages/common.html/#HIGHRES_IMU.
Definition: imu.cpp:368


mavros
Author(s): Vladimir Ermakov
autogenerated on Tue Jun 1 2021 02:36:26