hil.cpp

Go to the documentation of this file.

/*
 * Copyright 2016,2017 Mohamed Abdelkader, Nuno Marques, Pavel Vechersky, Beat Küng.
 *
 * This file is part of the mavros package and subject to the license terms
 * in the top-level LICENSE file of the mavros repository.
 * https://github.com/mavlink/mavros/tree/master/LICENSE.md
 */
#include <mavros/mavros_plugin.h>
#include <eigen_conversions/eigen_msg.h>

#include <mavros_msgs/HilControls.h>
#include <mavros_msgs/HilActuatorControls.h>
#include <mavros_msgs/HilStateQuaternion.h>
#include <mavros_msgs/HilGPS.h>
#include <mavros_msgs/HilSensor.h>
#include <mavros_msgs/OpticalFlowRad.h>
#include <mavros_msgs/RCIn.h>

namespace mavros {
namespace std_plugins {
static constexpr double TESLA_TO_GAUSS = 1.0e4;
static constexpr double PASCAL_TO_MILLIBAR = 1.0e-2;

class HilPlugin : public plugin::PluginBase {
public:
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW

        HilPlugin() : PluginBase(),
                hil_nh("~hil")
        { }

        void initialize(UAS &uas_)
        {
                PluginBase::initialize(uas_);

                hil_state_quaternion_sub = hil_nh.subscribe("state", 10, &HilPlugin::state_quat_cb, this);
                hil_gps_sub = hil_nh.subscribe("gps", 10, &HilPlugin::gps_cb, this);
                hil_sensor_sub = hil_nh.subscribe("imu_ned", 10, &HilPlugin::sensor_cb, this);
                hil_flow_sub = hil_nh.subscribe("optical_flow", 10, &HilPlugin::optical_flow_cb, this);
                hil_rcin_sub = hil_nh.subscribe("rc_inputs", 10, &HilPlugin::rcin_raw_cb, this);

                hil_controls_pub = hil_nh.advertise<mavros_msgs::HilControls>("controls", 10);
                hil_actuator_controls_pub = hil_nh.advertise<mavros_msgs::HilActuatorControls>("actuator_controls", 10);
        }

        Subscriptions get_subscriptions()
        {
                return {
                               make_handler(&HilPlugin::handle_hil_controls),
                               make_handler(&HilPlugin::handle_hil_actuator_controls),
                };
        }

private:
        ros::NodeHandle hil_nh;

        ros::Publisher hil_controls_pub;
        ros::Publisher hil_actuator_controls_pub;

        ros::Subscriber hil_state_quaternion_sub;
        ros::Subscriber hil_gps_sub;
        ros::Subscriber hil_sensor_sub;
        ros::Subscriber hil_flow_sub;
        ros::Subscriber hil_rcin_sub;

        Eigen::Quaterniond enu_orientation;

        /* -*- rx handlers -*- */

        void handle_hil_controls(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIL_CONTROLS &hil_controls) {
                auto hil_controls_msg = boost::make_shared<mavros_msgs::HilControls>();

                hil_controls_msg->header.stamp = m_uas->synchronise_stamp(hil_controls.time_usec);
                // [[[cog:
                // for f in (
                //     'roll_ailerons', 'pitch_elevator', 'yaw_rudder', 'throttle',
                //     'aux1', 'aux2', 'aux3', 'aux4', 'mode', 'nav_mode'):
                //     cog.outl("hil_controls_msg->%s = hil_controls.%s;" % (f, f))
                // ]]]
                hil_controls_msg->roll_ailerons = hil_controls.roll_ailerons;
                hil_controls_msg->pitch_elevator = hil_controls.pitch_elevator;
                hil_controls_msg->yaw_rudder = hil_controls.yaw_rudder;
                hil_controls_msg->throttle = hil_controls.throttle;
                hil_controls_msg->aux1 = hil_controls.aux1;
                hil_controls_msg->aux2 = hil_controls.aux2;
                hil_controls_msg->aux3 = hil_controls.aux3;
                hil_controls_msg->aux4 = hil_controls.aux4;
                hil_controls_msg->mode = hil_controls.mode;
                hil_controls_msg->nav_mode = hil_controls.nav_mode;
                // [[[end]]] (checksum: a2c87ee8f36e7a32b08be5e0fe665b5a)

                hil_controls_pub.publish(hil_controls_msg);
        }

        void handle_hil_actuator_controls(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIL_ACTUATOR_CONTROLS &hil_actuator_controls) {
                auto hil_actuator_controls_msg = boost::make_shared<mavros_msgs::HilActuatorControls>();

                hil_actuator_controls_msg->header.stamp = m_uas->synchronise_stamp(hil_actuator_controls.time_usec);
                const auto &arr = hil_actuator_controls.controls;
                std::copy(arr.cbegin(), arr.cend(), hil_actuator_controls_msg->controls.begin());
                hil_actuator_controls_msg->mode = hil_actuator_controls.mode;
                hil_actuator_controls_msg->flags = hil_actuator_controls.flags;

                hil_actuator_controls_pub.publish(hil_actuator_controls_msg);
        }

        /* -*- callbacks / low level send -*- */

        void state_quat_cb(const mavros_msgs::HilStateQuaternion::ConstPtr &req) {
                mavlink::common::msg::HIL_STATE_QUATERNION state_quat;

                state_quat.time_usec = req->header.stamp.toNSec() / 1000;
                auto q = ftf::transform_orientation_baselink_aircraft(
                                        ftf::transform_orientation_enu_ned(
                                                ftf::to_eigen(req->orientation)));
                ftf::quaternion_to_mavlink(q, state_quat.attitude_quaternion);
                state_quat.lat = req->geo.latitude * 1E7;
                state_quat.lon = req->geo.longitude * 1E7;
                // @warning geographic_msgs/GeoPoint.msg uses WGS 84 reference ellipsoid
                // @TODO: Convert altitude to AMSL to be received by the FCU
                // related to issue #529
                state_quat.alt = req->geo.altitude * 1E3;
                state_quat.ind_airspeed = req->ind_airspeed * 1E2;
                state_quat.true_airspeed = req->true_airspeed * 1E2;
                // WRT world frame
                auto ang_vel = ftf::transform_frame_enu_ned(
                                ftf::transform_frame_baselink_aircraft(
                                        ftf::to_eigen(req->angular_velocity)));
                auto lin_vel = ftf::transform_frame_enu_ned<Eigen::Vector3d>(
                                ftf::to_eigen(req->linear_velocity)) * 1E2;
                // linear acceleration - WRT world frame
                auto lin_acc = ftf::transform_frame_baselink_aircraft(
                                ftf::to_eigen(req->linear_acceleration));

                // [[[cog:
                // for a, b in zip(('rollspeed', 'pitchspeed', 'yawspeed'), "xyz"):
                //     cog.outl("state_quat.%s = ang_vel.%s();" % (a, b))
                // for f in "xyz":
                //     cog.outl("state_quat.v%s = lin_vel.%s();" % (f, f))
                // for f in "xyz":
                //     cog.outl("state_quat.%sacc = lin_acc.%s();" % (f, f))
                // ]]]
                state_quat.rollspeed = ang_vel.x();
                state_quat.pitchspeed = ang_vel.y();
                state_quat.yawspeed = ang_vel.z();
                state_quat.vx = lin_vel.x();
                state_quat.vy = lin_vel.y();
                state_quat.vz = lin_vel.z();
                state_quat.xacc = lin_acc.x();
                state_quat.yacc = lin_acc.y();
                state_quat.zacc = lin_acc.z();
                // [[[end]]] (checksum: a29598b834ac1ec32ede01595aa5b3ac)

                UAS_FCU(m_uas)->send_message_ignore_drop(state_quat);
        }

        void gps_cb(const mavros_msgs::HilGPS::ConstPtr &req) {
                mavlink::common::msg::HIL_GPS gps;

                gps.time_usec = req->header.stamp.toNSec() / 1000;
                gps.fix_type = req->fix_type;
                gps.lat = req->geo.latitude * 1E7;
                gps.lon = req->geo.longitude * 1E7;
                // @warning geographic_msgs/GeoPoint.msg uses WGS 84 reference ellipsoid
                // @TODO: Convert altitude to AMSL to be received by the FCU
                // related to issue #529
                gps.alt = req->geo.altitude * 1E3;
                // [[[cog:
                // for f in (
                //     'eph', 'epv', 'vel', 'vn', 've', 'vd', 'cog'):
                //     cog.outl("gps.%s = req->%s * 1E2;" % (f, f))
                // ]]]
                gps.eph = req->eph * 1E2;
                gps.epv = req->epv * 1E2;
                gps.vel = req->vel * 1E2;
                gps.vn = req->vn * 1E2;
                gps.ve = req->ve * 1E2;
                gps.vd = req->vd * 1E2;
                gps.cog = req->cog * 1E2;
                // [[[end]]] (checksum: a283bcc78f496cead2e9f893200d825d)
                gps.satellites_visible = req->satellites_visible;

                UAS_FCU(m_uas)->send_message_ignore_drop(gps);
        }

        void sensor_cb(const mavros_msgs::HilSensor::ConstPtr &req) {
                mavlink::common::msg::HIL_SENSOR sensor;

                sensor.time_usec = req->header.stamp.toNSec() / 1000;
                // WRT world frame
                auto acc = ftf::transform_frame_baselink_aircraft(
                                ftf::to_eigen(req->acc));
                auto gyro = ftf::transform_frame_baselink_aircraft(
                                ftf::to_eigen(req->gyro));
                auto mag = ftf::transform_frame_baselink_aircraft<Eigen::Vector3d>(
                                ftf::to_eigen(req->mag) * TESLA_TO_GAUSS);

                // [[[cog:
                // for a in ('acc', 'gyro', 'mag'):
                //     for b in "xyz":
                //         cog.outl("sensor.{b}{a} = {a}.{b}();".format(**locals()))
                // for f in (('abs_pressure', 'PASCAL_TO_MILLIBAR'),
                //           ('diff_pressure', 'PASCAL_TO_MILLIBAR'),
                //           'pressure_alt', 'temperature', 'fields_updated'):
                //           f1 = f if isinstance(f, str) else f[0]
                //           f2 = f if isinstance(f, str) else '{f[0]} * {f[1]}'.format(f=f)
                //           cog.outl("sensor.{f1} = req->{f2};".format(**locals()))
                // ]]]
                sensor.xacc = acc.x();
                sensor.yacc = acc.y();
                sensor.zacc = acc.z();
                sensor.xgyro = gyro.x();
                sensor.ygyro = gyro.y();
                sensor.zgyro = gyro.z();
                sensor.xmag = mag.x();
                sensor.ymag = mag.y();
                sensor.zmag = mag.z();
                sensor.abs_pressure = req->abs_pressure * PASCAL_TO_MILLIBAR;
                sensor.diff_pressure = req->diff_pressure * PASCAL_TO_MILLIBAR;
                sensor.pressure_alt = req->pressure_alt;
                sensor.temperature = req->temperature;
                sensor.fields_updated = req->fields_updated;
                // [[[end]]] (checksum: 316bef821ad6fc33d9726a1c8e8c5404)

                UAS_FCU(m_uas)->send_message_ignore_drop(sensor);
        }

        void optical_flow_cb(const mavros_msgs::OpticalFlowRad::ConstPtr &req) {
                mavlink::common::msg::HIL_OPTICAL_FLOW of;

                auto int_xy = ftf::transform_frame_aircraft_baselink(
                                        Eigen::Vector3d(
                                                req->integrated_x,
                                                req->integrated_y,
                                                0.0));
                auto int_gyro = ftf::transform_frame_aircraft_baselink(
                                        Eigen::Vector3d(
                                                req->integrated_xgyro,
                                                req->integrated_ygyro,
                                                req->integrated_zgyro));

                of.time_usec = req->header.stamp.toNSec() / 1000;
                of.sensor_id = INT8_MAX;//while we don't find a better way of handling it
                of.integration_time_us = req->integration_time_us;
                // [[[cog:
                // for f in "xy":
                //     cog.outl("of.integrated_%s = int_xy.%s();" % (f, f))
                // for f in "xyz":
                //     cog.outl("of.integrated_%sgyro = int_gyro.%s();" % (f, f))
                // for f in ('time_delta_distance_us', 'distance', 'quality'):
                //     cog.outl("of.%s = req->%s;" % (f, f))
                // ]]]
                of.integrated_x = int_xy.x();
                of.integrated_y = int_xy.y();
                of.integrated_xgyro = int_gyro.x();
                of.integrated_ygyro = int_gyro.y();
                of.integrated_zgyro = int_gyro.z();
                of.time_delta_distance_us = req->time_delta_distance_us;
                of.distance = req->distance;
                of.quality = req->quality;
                // [[[end]]] (checksum: acbfae28f4f3bb8ca135423efaaa479e)
                of.temperature = req->temperature * 100.0f;     // in centi-degrees celsius

                UAS_FCU(m_uas)->send_message_ignore_drop(of);
        }

        void rcin_raw_cb(const mavros_msgs::RCIn::ConstPtr &req) {
                mavlink::common::msg::HIL_RC_INPUTS_RAW rcin {};

                constexpr size_t MAX_CHANCNT = 12;

                std::array<uint16_t, MAX_CHANCNT> channels;
                auto n = std::min(req->channels.size(), channels.size());
                std::copy(req->channels.begin(), req->channels.begin() + n, channels.begin());
                std::fill(channels.begin() + n, channels.end(), UINT16_MAX);

                rcin.time_usec = req->header.stamp.toNSec() / 100000;
                // [[[cog:
                // for i in range(1,13):
                //     cog.outl("rcin.chan%d_raw\t= channels[%2d];" % (i, i-1))
                // ]]]
                rcin.chan1_raw  = channels[ 0];
                rcin.chan2_raw  = channels[ 1];
                rcin.chan3_raw  = channels[ 2];
                rcin.chan4_raw  = channels[ 3];
                rcin.chan5_raw  = channels[ 4];
                rcin.chan6_raw  = channels[ 5];
                rcin.chan7_raw  = channels[ 6];
                rcin.chan8_raw  = channels[ 7];
                rcin.chan9_raw  = channels[ 8];
                rcin.chan10_raw = channels[ 9];
                rcin.chan11_raw = channels[10];
                rcin.chan12_raw = channels[11];
                // [[[end]]] (checksum: 8d6860789d596dc39e81b351c3a50fcd)

                UAS_FCU(m_uas)->send_message_ignore_drop(rcin);
        }
};
}       // namespace std_plugins
}       // namespace mavros

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::HilPlugin, mavros::plugin::PluginBase)