/**
* @brief Handle SCALED_IMU MAVlink message.
* Message specification: https://mavlink.io/en/messages/common.html/#SCALED_IMU
* @param msg Received Mavlink msg
* @param imu_raw SCALED_IMU msg
*/
void handle_scaled_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SCALED_IMU &imu_raw)
{
if (has_hr_imu)
return;
ROS_INFO_COND_NAMED(!has_scaled_imu, "imu", "IMU: Scaled IMU message used.");
has_scaled_imu = true;
auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
auto header = m_uas->synchronized_header(frame_id, imu_raw.time_boot_ms);
auto gyro_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC);
auto accel_frd = Eigen::Vector3d(Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc) * MILLIG_TO_MS2);
auto accel_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(accel_frd);
publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd);
/** Magnetic field data:
* @snippet src/plugins/imu.cpp mag_field
*/
auto mag_field = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA);
publish_mag(header, mag_field);
}
/**
* @brief Handle ATTITUDE_QUATERNION MAVlink message.
* Message specification: https://mavlink.io/en/messages/common.html/#ATTITUDE_QUATERNION
* @param msg Received Mavlink msg
* @param att_q ATTITUDE_QUATERNION msg
*/
void handle_attitude_quaternion(const mavlink::mavlink_message_t *msg, mavlink::common::msg::ATTITUDE_QUATERNION &att_q)
{
ROS_INFO_COND_NAMED(!has_att_quat, "imu", "IMU: Attitude quaternion IMU detected!");
has_att_quat = true;
/** Orientation on the NED-aicraft frame:
* @snippet src/plugins/imu.cpp ned_aircraft_orient2
*/
// [ned_aircraft_orient2]
auto ned_aircraft_orientation = Eigen::Quaterniond(att_q.q1, att_q.q2, att_q.q3, att_q.q4);
// [ned_aircraft_orient2]
/** Angular velocity on the NED-aicraft frame:
* @snippet src/plugins/imu.cpp ned_ang_vel2
*/
// [frd_ang_vel2]
auto gyro_frd = Eigen::Vector3d(att_q.rollspeed, att_q.pitchspeed, att_q.yawspeed);
// [frd_ang_vel2]
/** MAVLink quaternion exactly matches Eigen convention.
* The RPY describes the rotation: aircraft->NED.
* It is required to change this to aircraft->base_link:
* @snippet src/plugins/imu.cpp ned->baselink->enu
*/
auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink(
ftf::transform_orientation_ned_enu(ned_aircraft_orientation));
/** The angular velocity expressed in the aircraft frame.
* It is required to apply the static rotation to get it into the base_link frame:
* @snippet src/plugins/imu.cpp rotate_gyro
*/
auto gyro_flu = ftf::transform_frame_aircraft_baselink(gyro_frd);
publish_imu_data(att_q.time_boot_ms, enu_baselink_orientation, ned_aircraft_orientation, gyro_flu, gyro_frd);
}
// almost the same as handle_attitude(), but for ATTITUDE_QUATERNION
void handle_attitude_quaternion(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_attitude_quaternion_t att_q;
mavlink_msg_attitude_quaternion_decode(msg, &att_q);
ROS_INFO_COND_NAMED(!has_att_quat, "imu", "Attitude quaternion IMU detected!");
has_att_quat = true;
sensor_msgs::ImuPtr imu_msg = boost::make_shared<sensor_msgs::Imu>();
// PX4 NED (w x y z) -> ROS ENU (x -y -z w) (body-fixed)
tf::Quaternion orientation(att_q.q2, -att_q.q3, -att_q.q4, att_q.q1);
fill_imu_msg_attitude(imu_msg, orientation,
att_q.rollspeed,
-att_q.pitchspeed,
-att_q.yawspeed);
uas_store_attitude(orientation,
imu_msg->angular_velocity,
imu_msg->linear_acceleration);
// publish data
imu_msg->header.frame_id = frame_id;
imu_msg->header.stamp = ros::Time::now();
imu_pub.publish(imu_msg);
}
void handle_rc_channels(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RC_CHANNELS &channels)
{
constexpr size_t MAX_CHANCNT = 18;
lock_guard lock(mutex);
ROS_INFO_COND_NAMED(!has_rc_channels_msg, "rc", "RC_CHANNELS message detected!");
has_rc_channels_msg = true;
if (channels.chancount > MAX_CHANCNT) {
ROS_WARN_THROTTLE_NAMED(60, "rc",
"FCU receives %u RC channels, but RC_CHANNELS can store %zu",
channels.chancount, MAX_CHANCNT);
channels.chancount = MAX_CHANCNT;
}
raw_rc_in.resize(channels.chancount);
// switch works as start point selector.
switch (channels.chancount) {
// [[[cog:
// for i in range(18, 0, -1):
// cog.outl("case %2d: raw_rc_in[%2d] = channels.chan%d_raw;" % (i, i - 1, i))
// ]]]
case 18: raw_rc_in[17] = channels.chan18_raw;
case 17: raw_rc_in[16] = channels.chan17_raw;
case 16: raw_rc_in[15] = channels.chan16_raw;
case 15: raw_rc_in[14] = channels.chan15_raw;
case 14: raw_rc_in[13] = channels.chan14_raw;
case 13: raw_rc_in[12] = channels.chan13_raw;
case 12: raw_rc_in[11] = channels.chan12_raw;
case 11: raw_rc_in[10] = channels.chan11_raw;
case 10: raw_rc_in[ 9] = channels.chan10_raw;
case 9: raw_rc_in[ 8] = channels.chan9_raw;
case 8: raw_rc_in[ 7] = channels.chan8_raw;
case 7: raw_rc_in[ 6] = channels.chan7_raw;
case 6: raw_rc_in[ 5] = channels.chan6_raw;
case 5: raw_rc_in[ 4] = channels.chan5_raw;
case 4: raw_rc_in[ 3] = channels.chan4_raw;
case 3: raw_rc_in[ 2] = channels.chan3_raw;
case 2: raw_rc_in[ 1] = channels.chan2_raw;
case 1: raw_rc_in[ 0] = channels.chan1_raw;
// [[[end]]] (checksum: 56e9ab5407bd2c864abde230a6cf3fed)
case 0: break;
}
auto rcin_msg = boost::make_shared<mavros_msgs::RCIn>();
rcin_msg->header.stamp = m_uas->synchronise_stamp(channels.time_boot_ms);
rcin_msg->rssi = channels.rssi;
rcin_msg->channels = raw_rc_in;
rc_in_pub.publish(rcin_msg);
}
void handle_highres_imu(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_highres_imu_t imu_hr;
mavlink_msg_highres_imu_decode(msg, &imu_hr);
ROS_INFO_COND_NAMED(!has_hr_imu, "imu", "High resolution IMU detected!");
has_hr_imu = true;
std_msgs::Header header;
header.stamp = ros::Time::now();
header.frame_id = frame_id;
/* imu/data_raw filled by HR IMU */
if (imu_hr.fields_updated & ((7<<3)|(7<<0))) {
sensor_msgs::ImuPtr imu_msg = boost::make_shared<sensor_msgs::Imu>();
fill_imu_msg_raw(imu_msg,
imu_hr.xgyro, -imu_hr.ygyro, -imu_hr.zgyro,
imu_hr.xacc, -imu_hr.yacc, -imu_hr.zacc);
imu_msg->header = header;
imu_raw_pub.publish(imu_msg);
}
if (imu_hr.fields_updated & (7<<6)) {
sensor_msgs::MagneticFieldPtr magn_msg = boost::make_shared<sensor_msgs::MagneticField>();
// Convert from local NED plane to ENU
magn_msg->magnetic_field.x = imu_hr.ymag * GAUSS_TO_TESLA;
magn_msg->magnetic_field.y = imu_hr.xmag * GAUSS_TO_TESLA;
magn_msg->magnetic_field.z = -imu_hr.zmag * GAUSS_TO_TESLA;
magn_msg->magnetic_field_covariance = magnetic_cov;
magn_msg->header = header;
magn_pub.publish(magn_msg);
}
if (imu_hr.fields_updated & (1<<9)) {
sensor_msgs::FluidPressurePtr atmp_msg = boost::make_shared<sensor_msgs::FluidPressure>();
atmp_msg->fluid_pressure = imu_hr.abs_pressure * MILLIBAR_TO_PASCAL;
atmp_msg->header = header;
press_pub.publish(atmp_msg);
}
if (imu_hr.fields_updated & (1<<12)) {
sensor_msgs::TemperaturePtr temp_msg = boost::make_shared<sensor_msgs::Temperature>();
temp_msg->temperature = imu_hr.temperature;
temp_msg->header = header;
temp_pub.publish(temp_msg);
}
}
/**
* @brief Handle RAW_IMU MAVlink message.
* Message specification: https://mavlink.io/en/messages/common.html/#RAW_IMU
* @param msg Received Mavlink msg
* @param imu_raw RAW_IMU msg
*/
void handle_raw_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::RAW_IMU &imu_raw)
{
ROS_INFO_COND_NAMED(!has_raw_imu, "imu", "IMU: Raw IMU message used.");
has_raw_imu = true;
if (has_hr_imu || has_scaled_imu)
return;
auto imu_msg = boost::make_shared<sensor_msgs::Imu>();
auto header = m_uas->synchronized_header(frame_id, imu_raw.time_usec);
/** @note APM send SCALED_IMU data as RAW_IMU
*/
auto gyro_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
Eigen::Vector3d(imu_raw.xgyro, imu_raw.ygyro, imu_raw.zgyro) * MILLIRS_TO_RADSEC);
auto accel_frd = Eigen::Vector3d(imu_raw.xacc, imu_raw.yacc, imu_raw.zacc);
auto accel_flu = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(accel_frd);
if (m_uas->is_ardupilotmega()) {
accel_frd *= MILLIG_TO_MS2;
accel_flu *= MILLIG_TO_MS2;
} else if (m_uas->is_px4()) {
accel_frd *= MILLIMS2_TO_MS2;
accel_flu *= MILLIMS2_TO_MS2;
}
publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd);
if (!m_uas->is_ardupilotmega()) {
ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: linear acceleration on RAW_IMU known on APM only.");
ROS_WARN_THROTTLE_NAMED(60, "imu", "IMU: ~imu/data_raw stores unscaled raw acceleration report.");
linear_accel_vec_flu.setZero();
linear_accel_vec_frd.setZero();
}
/** Magnetic field data:
* @snippet src/plugins/imu.cpp mag_field
*/
// [mag_field]
auto mag_field = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
Eigen::Vector3d(imu_raw.xmag, imu_raw.ymag, imu_raw.zmag) * MILLIT_TO_TESLA);
// [mag_field]
publish_mag(header, mag_field);
}
void handle_scaled_imu(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
if (has_hr_imu)
return;
ROS_INFO_COND_NAMED(!has_scaled_imu, "imu", "Scaled IMU message used.");
has_scaled_imu = true;
sensor_msgs::ImuPtr imu_msg = boost::make_shared<sensor_msgs::Imu>();
mavlink_scaled_imu_t imu_raw;
mavlink_msg_scaled_imu_decode(msg, &imu_raw);
std_msgs::Header header;
header.stamp = ros::Time::now();
header.frame_id = frame_id;
fill_imu_msg_raw(imu_msg,
imu_raw.xgyro * MILLIRS_TO_RADSEC,
-imu_raw.ygyro * MILLIRS_TO_RADSEC,
-imu_raw.zgyro * MILLIRS_TO_RADSEC,
imu_raw.xacc * MILLIG_TO_MS2,
-imu_raw.yacc * MILLIG_TO_MS2,
-imu_raw.zacc * MILLIG_TO_MS2);
imu_msg->header = header;
imu_raw_pub.publish(imu_msg);
/* -*- magnetic vector -*- */
sensor_msgs::MagneticFieldPtr magn_msg = boost::make_shared<sensor_msgs::MagneticField>();
// Convert from local NED plane to ENU
magn_msg->magnetic_field.x = imu_raw.ymag * MILLIT_TO_TESLA;
magn_msg->magnetic_field.y = imu_raw.xmag * MILLIT_TO_TESLA;
magn_msg->magnetic_field.z = -imu_raw.zmag * MILLIT_TO_TESLA;
magn_msg->magnetic_field_covariance = magnetic_cov;
magn_msg->header = header;
magn_pub.publish(magn_msg);
}
/**
* @brief Handle HIGHRES_IMU MAVlink message.
* Message specification: https://mavlink.io/en/messages/common.html/#HIGHRES_IMU
* @param msg Received Mavlink msg
* @param imu_hr HIGHRES_IMU msg
*/
void handle_highres_imu(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIGHRES_IMU &imu_hr)
{
ROS_INFO_COND_NAMED(!has_hr_imu, "imu", "IMU: High resolution IMU detected!");
has_hr_imu = true;
auto header = m_uas->synchronized_header(frame_id, imu_hr.time_usec);
/** @todo Make more paranoic check of HIGHRES_IMU.fields_updated
*/
/** Check if accelerometer + gyroscope data are available.
* Data is expressed in aircraft frame it is required to rotate to the base_link frame:
* @snippet src/plugins/imu.cpp accel_available
*/
// [accel_available]
if (imu_hr.fields_updated & ((7 << 3) | (7 << 0))) {
auto gyro_flu = ftf::transform_frame_aircraft_baselink(Eigen::Vector3d(imu_hr.xgyro, imu_hr.ygyro, imu_hr.zgyro));
auto accel_frd = Eigen::Vector3d(imu_hr.xacc, imu_hr.yacc, imu_hr.zacc);
auto accel_flu = ftf::transform_frame_aircraft_baselink(accel_frd);
publish_imu_data_raw(header, gyro_flu, accel_flu, accel_frd);
}
// [accel_available]
/** Check if magnetometer data is available:
* @snippet src/plugins/imu.cpp mag_available
*/
// [mag_available]
if (imu_hr.fields_updated & (7 << 6)) {
auto mag_field = ftf::transform_frame_aircraft_baselink<Eigen::Vector3d>(
Eigen::Vector3d(imu_hr.xmag, imu_hr.ymag, imu_hr.zmag) * GAUSS_TO_TESLA);
publish_mag(header, mag_field);
}
// [mag_available]
/** Check if static pressure sensor data is available:
* @snippet src/plugins/imu.cpp static_pressure_available
*/
// [static_pressure_available]
if (imu_hr.fields_updated & (1 << 9)) {
auto static_pressure_msg = boost::make_shared<sensor_msgs::FluidPressure>();
static_pressure_msg->header = header;
static_pressure_msg->fluid_pressure = imu_hr.abs_pressure;
static_press_pub.publish(static_pressure_msg);
}
// [static_pressure_available]
/** Check if differential pressure sensor data is available:
* @snippet src/plugins/imu.cpp differential_pressure_available
*/
// [differential_pressure_available]
if (imu_hr.fields_updated & (1 << 10)) {
auto differential_pressure_msg = boost::make_shared<sensor_msgs::FluidPressure>();
differential_pressure_msg->header = header;
differential_pressure_msg->fluid_pressure = imu_hr.diff_pressure;
diff_press_pub.publish(differential_pressure_msg);
}
// [differential_pressure_available]
/** Check if temperature data is available:
* @snippet src/plugins/imu.cpp temperature_available
*/
// [temperature_available]
if (imu_hr.fields_updated & (1 << 12)) {
auto temp_msg = boost::make_shared<sensor_msgs::Temperature>();
temp_msg->header = header;
temp_msg->temperature = imu_hr.temperature;
temp_imu_pub.publish(temp_msg);
}
// [temperature_available]
}
