hil.cpp
Go to the documentation of this file.
1 
12 /*
13  * Copyright 2016,2017 Mohamed Abdelkader, Nuno Marques, Pavel Vechersky, Beat Küng.
14  *
15  * This file is part of the mavros package and subject to the license terms
16  * in the top-level LICENSE file of the mavros repository.
17  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
18  */
19 #include <mavros/mavros_plugin.h>
21 
22 #include <mavros_msgs/HilControls.h>
23 #include <mavros_msgs/HilActuatorControls.h>
24 #include <mavros_msgs/HilStateQuaternion.h>
25 #include <mavros_msgs/HilGPS.h>
26 #include <mavros_msgs/HilSensor.h>
27 #include <mavros_msgs/OpticalFlowRad.h>
28 #include <mavros_msgs/RCIn.h>
29 
30 namespace mavros {
31 namespace std_plugins {
33 static constexpr double TESLA_TO_GAUSS = 1.0e4;
35 static constexpr double PASCAL_TO_MILLIBAR = 1.0e-2;
36 
40 class HilPlugin : public plugin::PluginBase {
41 public:
42  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 
45  hil_nh("~hil")
46  { }
47 
48  void initialize(UAS &uas_)
49  {
50  PluginBase::initialize(uas_);
51 
53  hil_gps_sub = hil_nh.subscribe("gps", 10, &HilPlugin::gps_cb, this);
54  hil_sensor_sub = hil_nh.subscribe("imu_ned", 10, &HilPlugin::sensor_cb, this);
55  hil_flow_sub = hil_nh.subscribe("optical_flow", 10, &HilPlugin::optical_flow_cb, this);
56  hil_rcin_sub = hil_nh.subscribe("rc_inputs", 10, &HilPlugin::rcin_raw_cb, this);
57 
58  hil_controls_pub = hil_nh.advertise<mavros_msgs::HilControls>("controls", 10);
59  hil_actuator_controls_pub = hil_nh.advertise<mavros_msgs::HilActuatorControls>("actuator_controls", 10);
60  }
61 
63  {
64  return {
67  };
68  }
69 
70 private:
72 
75 
81 
82  Eigen::Quaterniond enu_orientation;
83 
84  /* -*- rx handlers -*- */
85 
86  void handle_hil_controls(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIL_CONTROLS &hil_controls) {
87  auto hil_controls_msg = boost::make_shared<mavros_msgs::HilControls>();
88 
89  hil_controls_msg->header.stamp = m_uas->synchronise_stamp(hil_controls.time_usec);
90  // [[[cog:
91  // for f in (
92  // 'roll_ailerons', 'pitch_elevator', 'yaw_rudder', 'throttle',
93  // 'aux1', 'aux2', 'aux3', 'aux4', 'mode', 'nav_mode'):
94  // cog.outl("hil_controls_msg->%s = hil_controls.%s;" % (f, f))
95  // ]]]
96  hil_controls_msg->roll_ailerons = hil_controls.roll_ailerons;
97  hil_controls_msg->pitch_elevator = hil_controls.pitch_elevator;
98  hil_controls_msg->yaw_rudder = hil_controls.yaw_rudder;
99  hil_controls_msg->throttle = hil_controls.throttle;
100  hil_controls_msg->aux1 = hil_controls.aux1;
101  hil_controls_msg->aux2 = hil_controls.aux2;
102  hil_controls_msg->aux3 = hil_controls.aux3;
103  hil_controls_msg->aux4 = hil_controls.aux4;
104  hil_controls_msg->mode = hil_controls.mode;
105  hil_controls_msg->nav_mode = hil_controls.nav_mode;
106  // [[[end]]] (checksum: a2c87ee8f36e7a32b08be5e0fe665b5a)
107 
108  hil_controls_pub.publish(hil_controls_msg);
109  }
110 
111  void handle_hil_actuator_controls(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIL_ACTUATOR_CONTROLS &hil_actuator_controls) {
112  auto hil_actuator_controls_msg = boost::make_shared<mavros_msgs::HilActuatorControls>();
113 
114  hil_actuator_controls_msg->header.stamp = m_uas->synchronise_stamp(hil_actuator_controls.time_usec);
115  const auto &arr = hil_actuator_controls.controls;
116  std::copy(arr.cbegin(), arr.cend(), hil_actuator_controls_msg->controls.begin());
117  hil_actuator_controls_msg->mode = hil_actuator_controls.mode;
118  hil_actuator_controls_msg->flags = hil_actuator_controls.flags;
119 
120  hil_actuator_controls_pub.publish(hil_actuator_controls_msg);
121  }
122 
123  /* -*- callbacks / low level send -*- */
124 
130  mavlink::common::msg::HIL_STATE_QUATERNION state_quat;
131 
132  state_quat.time_usec = req->header.stamp.toNSec() / 1000;
135  ftf::to_eigen(req->orientation)));
136  ftf::quaternion_to_mavlink(q, state_quat.attitude_quaternion);
137  state_quat.lat = req->geo.latitude * 1E7;
138  state_quat.lon = req->geo.longitude * 1E7;
139  // @warning geographic_msgs/GeoPoint.msg uses WGS 84 reference ellipsoid
140  // @TODO: Convert altitude to AMSL to be received by the FCU
141  // related to issue #529
142  state_quat.alt = req->geo.altitude * 1E3;
143  state_quat.ind_airspeed = req->ind_airspeed * 1E2;
144  state_quat.true_airspeed = req->true_airspeed * 1E2;
145  // WRT world frame
146  auto ang_vel = ftf::transform_frame_enu_ned(
148  ftf::to_eigen(req->angular_velocity)));
149  auto lin_vel = ftf::transform_frame_enu_ned<Eigen::Vector3d>(
150  ftf::to_eigen(req->linear_velocity)) * 1E2;
151  // linear acceleration - WRT world frame
153  ftf::to_eigen(req->linear_acceleration));
154 
155  // [[[cog:
156  // for a, b in zip(('rollspeed', 'pitchspeed', 'yawspeed'), "xyz"):
157  // cog.outl("state_quat.%s = ang_vel.%s();" % (a, b))
158  // for f in "xyz":
159  // cog.outl("state_quat.v%s = lin_vel.%s();" % (f, f))
160  // for f in "xyz":
161  // cog.outl("state_quat.%sacc = lin_acc.%s();" % (f, f))
162  // ]]]
163  state_quat.rollspeed = ang_vel.x();
164  state_quat.pitchspeed = ang_vel.y();
165  state_quat.yawspeed = ang_vel.z();
166  state_quat.vx = lin_vel.x();
167  state_quat.vy = lin_vel.y();
168  state_quat.vz = lin_vel.z();
169  state_quat.xacc = lin_acc.x();
170  state_quat.yacc = lin_acc.y();
171  state_quat.zacc = lin_acc.z();
172  // [[[end]]] (checksum: a29598b834ac1ec32ede01595aa5b3ac)
173 
174  UAS_FCU(m_uas)->send_message_ignore_drop(state_quat);
175  }
176 
182  mavlink::common::msg::HIL_GPS gps;
183 
184  gps.time_usec = req->header.stamp.toNSec() / 1000;
185  gps.fix_type = req->fix_type;
186  gps.lat = req->geo.latitude * 1E7;
187  gps.lon = req->geo.longitude * 1E7;
188  // @warning geographic_msgs/GeoPoint.msg uses WGS 84 reference ellipsoid
189  // @TODO: Convert altitude to AMSL to be received by the FCU
190  // related to issue #529
191  gps.alt = req->geo.altitude * 1E3;
192  // [[[cog:
193  // for f in (
194  // 'eph', 'epv', 'vel', 'vn', 've', 'vd', 'cog'):
195  // cog.outl("gps.%s = req->%s * 1E2;" % (f, f))
196  // ]]]
197  gps.eph = req->eph * 1E2;
198  gps.epv = req->epv * 1E2;
199  gps.vel = req->vel * 1E2;
200  gps.vn = req->vn * 1E2;
201  gps.ve = req->ve * 1E2;
202  gps.vd = req->vd * 1E2;
203  gps.cog = req->cog * 1E2;
204  // [[[end]]] (checksum: a283bcc78f496cead2e9f893200d825d)
205  gps.satellites_visible = req->satellites_visible;
206 
207  UAS_FCU(m_uas)->send_message_ignore_drop(gps);
208  }
209 
215  mavlink::common::msg::HIL_SENSOR sensor;
216 
217  sensor.time_usec = req->header.stamp.toNSec() / 1000;
218  // WRT world frame
220  ftf::to_eigen(req->acc));
222  ftf::to_eigen(req->gyro));
223  auto mag = ftf::transform_frame_baselink_aircraft<Eigen::Vector3d>(
224  ftf::to_eigen(req->mag) * TESLA_TO_GAUSS);
225 
226  // [[[cog:
227  // for a in ('acc', 'gyro', 'mag'):
228  // for b in "xyz":
229  // cog.outl("sensor.{b}{a} = {a}.{b}();".format(**locals()))
230  // for f in (('abs_pressure', 'PASCAL_TO_MILLIBAR'),
231  // ('diff_pressure', 'PASCAL_TO_MILLIBAR'),
232  // 'pressure_alt', 'temperature', 'fields_updated'):
233  // f1 = f if isinstance(f, str) else f[0]
234  // f2 = f if isinstance(f, str) else '{f[0]} * {f[1]}'.format(f=f)
235  // cog.outl("sensor.{f1} = req->{f2};".format(**locals()))
236  // ]]]
237  sensor.xacc = acc.x();
238  sensor.yacc = acc.y();
239  sensor.zacc = acc.z();
240  sensor.xgyro = gyro.x();
241  sensor.ygyro = gyro.y();
242  sensor.zgyro = gyro.z();
243  sensor.xmag = mag.x();
244  sensor.ymag = mag.y();
245  sensor.zmag = mag.z();
246  sensor.abs_pressure = req->abs_pressure * PASCAL_TO_MILLIBAR;
247  sensor.diff_pressure = req->diff_pressure * PASCAL_TO_MILLIBAR;
248  sensor.pressure_alt = req->pressure_alt;
249  sensor.temperature = req->temperature;
250  sensor.fields_updated = req->fields_updated;
251  // [[[end]]] (checksum: 316bef821ad6fc33d9726a1c8e8c5404)
252 
253  UAS_FCU(m_uas)->send_message_ignore_drop(sensor);
254  }
255 
261  mavlink::common::msg::HIL_OPTICAL_FLOW of;
262 
264  Eigen::Vector3d(
265  req->integrated_x,
266  req->integrated_y,
267  0.0));
269  Eigen::Vector3d(
270  req->integrated_xgyro,
271  req->integrated_ygyro,
272  req->integrated_zgyro));
273 
274  of.time_usec = req->header.stamp.toNSec() / 1000;
275  of.sensor_id = INT8_MAX;//while we don't find a better way of handling it
276  of.integration_time_us = req->integration_time_us;
277  // [[[cog:
278  // for f in "xy":
279  // cog.outl("of.integrated_%s = int_xy.%s();" % (f, f))
280  // for f in "xyz":
281  // cog.outl("of.integrated_%sgyro = int_gyro.%s();" % (f, f))
282  // for f in ('time_delta_distance_us', 'distance', 'quality'):
283  // cog.outl("of.%s = req->%s;" % (f, f))
284  // ]]]
285  of.integrated_x = int_xy.x();
286  of.integrated_y = int_xy.y();
287  of.integrated_xgyro = int_gyro.x();
288  of.integrated_ygyro = int_gyro.y();
289  of.integrated_zgyro = int_gyro.z();
290  of.time_delta_distance_us = req->time_delta_distance_us;
291  of.distance = req->distance;
292  of.quality = req->quality;
293  // [[[end]]] (checksum: acbfae28f4f3bb8ca135423efaaa479e)
294  of.temperature = req->temperature * 100.0f; // in centi-degrees celsius
295 
296  UAS_FCU(m_uas)->send_message_ignore_drop(of);
297  }
298 
304  mavlink::common::msg::HIL_RC_INPUTS_RAW rcin {};
305 
306  constexpr size_t MAX_CHANCNT = 12;
307 
308  std::array<uint16_t, MAX_CHANCNT> channels;
309  auto n = std::min(req->channels.size(), channels.size());
310  std::copy(req->channels.begin(), req->channels.begin() + n, channels.begin());
311  std::fill(channels.begin() + n, channels.end(), UINT16_MAX);
312 
313  rcin.time_usec = req->header.stamp.toNSec() / 100000;
314  // [[[cog:
315  // for i in range(1,13):
316  // cog.outl("rcin.chan%d_raw\t= channels[%2d];" % (i, i-1))
317  // ]]]
318  rcin.chan1_raw = channels[ 0];
319  rcin.chan2_raw = channels[ 1];
320  rcin.chan3_raw = channels[ 2];
321  rcin.chan4_raw = channels[ 3];
322  rcin.chan5_raw = channels[ 4];
323  rcin.chan6_raw = channels[ 5];
324  rcin.chan7_raw = channels[ 6];
325  rcin.chan8_raw = channels[ 7];
326  rcin.chan9_raw = channels[ 8];
327  rcin.chan10_raw = channels[ 9];
328  rcin.chan11_raw = channels[10];
329  rcin.chan12_raw = channels[11];
330  // [[[end]]] (checksum: 8d6860789d596dc39e81b351c3a50fcd)
331 
332  UAS_FCU(m_uas)->send_message_ignore_drop(rcin);
333  }
334 };
335 } // namespace std_plugins
336 } // namespace mavros
337 
static constexpr double TESLA_TO_GAUSS
Tesla to Gauss coeff.
Definition: hil.cpp:33
MAVROS Plugin base class.
Definition: mavros_plugin.h:37
std::shared_ptr< MAVConnInterface const > ConstPtr
void initialize(UAS &uas_)
Plugin initializer.
Definition: hil.cpp:48
void publish(const boost::shared_ptr< M > &message) const
void handle_hil_controls(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIL_CONTROLS &hil_controls)
Definition: hil.cpp:86
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
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:88
ros::Subscriber hil_flow_sub
Definition: hil.cpp:79
EIGEN_MAKE_ALIGNED_OPERATOR_NEW HilPlugin()
Definition: hil.cpp:44
ros::Publisher hil_controls_pub
Definition: hil.cpp:73
T transform_orientation_enu_ned(const T &in)
Transform from attitude represented WRT ENU frame to attitude represented WRT NED frame...
Definition: frame_tf.h:161
ros::Subscriber hil_sensor_sub
Definition: hil.cpp:78
ros::Publisher hil_actuator_controls_pub
Definition: hil.cpp:74
PluginBase()
Plugin constructor Should not do anything before initialize()
Definition: mavros_plugin.h:75
ros::NodeHandle hil_nh
Definition: hil.cpp:71
T transform_frame_aircraft_baselink(const T &in)
Transform data expressed in Aircraft frame to Baselink frame.
Definition: frame_tf.h:205
Eigen::Quaterniond enu_orientation
Definition: hil.cpp:82
void state_quat_cb(const mavros_msgs::HilStateQuaternion::ConstPtr &req)
Send hil_state_quaternion to FCU. Message specification: https://mavlink.io/en/messages/common.html#HIL_STATE_QUATERNION.
Definition: hil.cpp:129
UAS for plugins.
Definition: mavros_uas.h:66
void optical_flow_cb(const mavros_msgs::OpticalFlowRad::ConstPtr &req)
Send simulated optical flow to FCU. Message specification: https://mavlink.io/en/messages/common.html#HIL_OPTICAL_FLOW.
Definition: hil.cpp:260
ros::Subscriber hil_gps_sub
Definition: hil.cpp:77
#define UAS_FCU(uasobjptr)
helper accessor to FCU link interface
Definition: mavros_uas.h:41
ros::Time synchronise_stamp(uint32_t time_boot_ms)
Compute FCU message time from time_boot_ms or time_usec field.
Eigen::Vector3d to_eigen(const geometry_msgs::Point r)
Helper to convert common ROS geometry_msgs::Point to Eigen::Vector3d.
Definition: frame_tf.h:423
T transform_frame_baselink_aircraft(const T &in)
Transform data expressed in Baselink frame to Aircraft frame.
Definition: frame_tf.h:214
void sensor_cb(const mavros_msgs::HilSensor::ConstPtr &req)
Send hil_sensor to FCU. Message specification: https://mavlink.io/en/messages/common.html#HIL_SENSOR.
Definition: hil.cpp:214
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
static constexpr double PASCAL_TO_MILLIBAR
Pascal to millBar coeff.
Definition: hil.cpp:35
T transform_frame_enu_ned(const T &in)
Transform data expressed in ENU to NED frame.
Definition: frame_tf.h:196
void handle_hil_actuator_controls(const mavlink::mavlink_message_t *msg, mavlink::common::msg::HIL_ACTUATOR_CONTROLS &hil_actuator_controls)
Definition: hil.cpp:111
std::vector< HandlerInfo > Subscriptions
Subscriptions vector.
Definition: mavros_plugin.h:48
ros::Subscriber hil_state_quaternion_sub
Definition: hil.cpp:76
void quaternion_to_mavlink(const Eigen::Quaterniond &q, std::array< float, 4 > &qmsg)
Store Quaternion to MAVLink float[4] format.
Definition: frame_tf.h:344
T transform_orientation_baselink_aircraft(const T &in)
Transform from attitude represented WRT baselink frame to attitude represented WRT body frame...
Definition: frame_tf.h:179
ros::Subscriber hil_rcin_sub
Definition: hil.cpp:80
Subscriptions get_subscriptions()
Return vector of MAVLink message subscriptions (handlers)
Definition: hil.cpp:62
#define PLUGINLIB_EXPORT_CLASS(class_type, base_class_type)
void rcin_raw_cb(const mavros_msgs::RCIn::ConstPtr &req)
Send simulated received RAW values of the RC channels to the FCU. Message specification: https://mavl...
Definition: hil.cpp:303
void gps_cb(const mavros_msgs::HilGPS::ConstPtr &req)
Send hil_gps to FCU. Message specification: https://mavlink.io/en/messages/common.html#HIL_GPS.
Definition: hil.cpp:181


mavros
Author(s): Vladimir Ermakov
autogenerated on Mon Jul 8 2019 03:20:11