mavlink.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2017, James Jackson and Daniel Koch, BYU MAGICC Lab
3  *
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright notice, this
10  * list of conditions and the following disclaimer.
11  *
12  * * Redistributions in binary form must reproduce the above copyright notice,
13  * this list of conditions and the following disclaimer in the documentation
14  * and/or other materials provided with the distribution.
15  *
16  * * Neither the name of the copyright holder nor the names of its
17  * contributors may be used to endorse or promote products derived from
18  * this software without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
21  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
23  * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
26  * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
27  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
28  * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
29  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
30  */
31 #include <cstdint>
32 
33 #include "board.h"
34 
35 #include "mavlink.h"
36 
37 namespace rosflight_firmware
38 {
39 
41  board_(board)
42 {}
43 
44 void Mavlink::init(uint32_t baud_rate, uint32_t dev)
45 {
46  board_.serial_init(baud_rate, dev);
47  initialized_ = true;
48 }
49 
50 void Mavlink::receive(void)
51 {
53  {
56  }
57 }
58 
59 void Mavlink::send_attitude_quaternion(uint8_t system_id,
60  uint64_t timestamp_us,
61  const turbomath::Quaternion &attitude,
62  const turbomath::Vector &angular_velocity)
63 {
64  mavlink_message_t msg;
66  timestamp_us / 1000,
67  attitude.w,
68  attitude.x,
69  attitude.y,
70  attitude.z,
71  angular_velocity.x,
72  angular_velocity.y,
73  angular_velocity.z);
74  send_message(msg);
75 }
76 
77 void Mavlink::send_baro(uint8_t system_id, float altitude, float pressure, float temperature)
78 {
79  mavlink_message_t msg;
80  mavlink_msg_small_baro_pack(system_id, compid_, &msg, altitude, pressure, temperature);
81  send_message(msg);
82 }
83 
84 void Mavlink::send_command_ack(uint8_t system_id, Command command, bool success)
85 {
86  ROSFLIGHT_CMD rosflight_cmd = ROSFLIGHT_CMD_ENUM_END;
87  switch (command)
88  {
90  rosflight_cmd = ROSFLIGHT_CMD_READ_PARAMS;
91  break;
93  rosflight_cmd = ROSFLIGHT_CMD_WRITE_PARAMS;
94  break;
96  rosflight_cmd = ROSFLIGHT_CMD_SET_PARAM_DEFAULTS;
97  break;
99  rosflight_cmd = ROSFLIGHT_CMD_ACCEL_CALIBRATION;
100  break;
102  rosflight_cmd = ROSFLIGHT_CMD_GYRO_CALIBRATION;
103  break;
105  rosflight_cmd = ROSFLIGHT_CMD_BARO_CALIBRATION;
106  break;
108  rosflight_cmd = ROSFLIGHT_CMD_AIRSPEED_CALIBRATION;
109  break;
111  rosflight_cmd = ROSFLIGHT_CMD_RC_CALIBRATION;
112  break;
114  rosflight_cmd = ROSFLIGHT_CMD_REBOOT;
115  break;
117  rosflight_cmd = ROSFLIGHT_CMD_REBOOT_TO_BOOTLOADER;
118  break;
120  rosflight_cmd = ROSFLIGHT_CMD_SEND_VERSION;
121  break;
122  }
123 
124  mavlink_message_t msg;
126  rosflight_cmd, (success) ? ROSFLIGHT_CMD_SUCCESS : ROSFLIGHT_CMD_FAILED);
127  send_message(msg);
128 }
129 
130 void Mavlink::send_diff_pressure(uint8_t system_id, float velocity, float pressure, float temperature)
131 {
132  mavlink_message_t msg;
133  mavlink_msg_diff_pressure_pack(system_id, compid_, &msg, velocity, pressure, temperature);
134  send_message(msg);
135 }
136 
137 void Mavlink::send_heartbeat(uint8_t system_id, bool fixed_wing)
138 {
139  mavlink_message_t msg;
140  mavlink_msg_heartbeat_pack(system_id, compid_, &msg,
142  0, 0, 0, 0);
143  send_message(msg);
144 }
145 
146 void Mavlink::send_imu(uint8_t system_id, uint64_t timestamp_us,
147  const turbomath::Vector &accel,
148  const turbomath::Vector &gyro,
149  float temperature)
150 {
151  mavlink_message_t msg;
152  mavlink_msg_small_imu_pack(system_id, compid_, &msg,
153  timestamp_us,
154  accel.x,
155  accel.y,
156  accel.z,
157  gyro.x,
158  gyro.y,
159  gyro.z,
160  temperature);
161  send_message(msg);
162 }
163 void Mavlink::send_gnss(uint8_t system_id, const GNSSData& data)
164 {
165  mavlink_message_t msg;
166  mavlink_msg_rosflight_gnss_pack(system_id, compid_, &msg,
167  data.time_of_week, data.fix_type, data.time, data.nanos,
168  data.lat, data.lon, data.height,
169  data.vel_n, data.vel_e, data.vel_d,
170  data.h_acc, data.v_acc,
171  data.ecef.x, data.ecef.y, data.ecef.z, data.ecef.p_acc,
172  data.ecef.vx, data.ecef.vy, data.ecef.vz, data.ecef.s_acc,
173  data.rosflight_timestamp);
174  send_message(msg);
175 }
176 
177 void Mavlink::send_gnss_raw(uint8_t system_id, const GNSSRaw& raw)
178 {
179  mavlink_message_t msg;
181  data.time_of_week = raw.time_of_week;
182  data.year = raw.year;
183  data.month = raw.month;
184  data.day = raw.day;
185  data.hour = raw.hour;
186  data.min = raw.min;
187  data.sec = raw.sec;
188  data.valid = raw.valid;
189  data.t_acc = raw.t_acc;
190  data.nano = raw.nano;
191  data.fix_type = raw.fix_type;
192  data.num_sat = raw.num_sat;
193  data.lon = raw.lon;
194  data.lat = raw.lat;
195  data.height = raw.height;
196  data.height_msl = raw.height_msl;
197  data.h_acc = raw.h_acc;
198  data.v_acc = raw.v_acc;
199  data.vel_n = raw.vel_n;
200  data.vel_e = raw.vel_e;
201  data.vel_d = raw.vel_d;
202  data.g_speed = raw.g_speed;
203  data.head_mot = raw.head_mot;
204  data.s_acc = raw.s_acc;
205  data.head_acc = raw.head_acc;
206  data.p_dop = raw.p_dop;
208  mavlink_msg_rosflight_gnss_raw_encode(system_id, compid_, &msg, &data);
209  send_message(msg);
210 }
211 
212 void Mavlink::send_log_message(uint8_t system_id, LogSeverity severity, const char *text)
213 {
214  MAV_SEVERITY mavlink_severity = MAV_SEVERITY_ENUM_END;
215  switch (severity)
216  {
218  mavlink_severity = MAV_SEVERITY_INFO;
219  break;
221  mavlink_severity = MAV_SEVERITY_WARNING;
222  break;
224  mavlink_severity = MAV_SEVERITY_ERROR;
225  break;
227  mavlink_severity = MAV_SEVERITY_CRITICAL;
228  break;
229  }
230 
231  mavlink_message_t msg;
232  mavlink_msg_statustext_pack(system_id, compid_, &msg, static_cast<uint8_t>(mavlink_severity), text);
233  send_message(msg);
234 }
235 
236 void Mavlink::send_mag(uint8_t system_id, const turbomath::Vector &mag)
237 {
238  mavlink_message_t msg;
239  mavlink_msg_small_mag_pack(system_id, compid_, &msg, mag.x, mag.y, mag.z);
240  send_message(msg);
241 }
242 
243 void Mavlink::send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, const char *const name, int32_t value)
244 {
245  mavlink_message_t msg;
246  mavlink_msg_named_value_int_pack(system_id, compid_, &msg, timestamp_ms, name, value);
247  send_message(msg);
248 }
249 
250 void Mavlink::send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, const char *const name, float value)
251 {
252  mavlink_message_t msg;
253  mavlink_msg_named_value_float_pack(system_id, compid_, &msg, timestamp_ms, name, value);
254  send_message(msg);
255 }
256 
257 void Mavlink::send_output_raw(uint8_t system_id, uint32_t timestamp_ms, const float raw_outputs[14])
258 {
259  mavlink_message_t msg;
260  mavlink_msg_rosflight_output_raw_pack(system_id, compid_, &msg, timestamp_ms, raw_outputs);
261  send_message(msg);
262 }
263 
264 void Mavlink::send_param_value_int(uint8_t system_id,
265  uint16_t index,
266  const char *const name,
267  int32_t value,
268  uint16_t param_count)
269 {
270  mavlink_param_union_t param;
271  param.param_int32 = value;
272 
273  mavlink_message_t msg;
274  mavlink_msg_param_value_pack(system_id, 0, &msg, name, param.param_float, MAV_PARAM_TYPE_INT32, param_count, index);
275  send_message(msg);
276 }
277 
278 void Mavlink::send_param_value_float(uint8_t system_id,
279  uint16_t index,
280  const char *const name,
281  float value,
282  uint16_t param_count)
283 {
284  mavlink_message_t msg;
285  mavlink_msg_param_value_pack(system_id, 0, &msg, name, value, MAV_PARAM_TYPE_REAL32, param_count, index);
286  send_message(msg);
287 }
288 
289 void Mavlink::send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, const uint16_t channels[8])
290 {
291  mavlink_message_t msg;
292  mavlink_msg_rc_channels_pack(system_id, compid_, &msg,
293  timestamp_ms,
294  0,
295  channels[0],
296  channels[1],
297  channels[2],
298  channels[3],
299  channels[4],
300  channels[5],
301  channels[6],
302  channels[7],
303  0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
304  send_message(msg);
305 }
306 
307 void Mavlink::send_sonar(uint8_t system_id, /* TODO enum type*/uint8_t type, float range, float max_range,
308  float min_range)
309 {
310  (void) type;
311  mavlink_message_t msg;
312  mavlink_msg_small_range_pack(system_id, compid_, &msg, /* TODO */ROSFLIGHT_RANGE_SONAR, range, max_range, min_range);
313  send_message(msg);
314 }
315 
316 void Mavlink::send_status(uint8_t system_id,
317  bool armed,
318  bool failsafe,
319  bool rc_override,
320  bool offboard,
321  uint8_t error_code,
322  uint8_t control_mode,
323  int16_t num_errors,
324  int16_t loop_time_us)
325 {
326  mavlink_message_t msg;
328  armed,
329  failsafe,
330  rc_override,
331  offboard,
332  error_code,
333  control_mode,
334  num_errors,
335  loop_time_us);
336  send_message(msg);
337 }
338 
339 void Mavlink::send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1)
340 {
341  mavlink_message_t msg;
342  mavlink_msg_timesync_pack(system_id, compid_, &msg, tc1, ts1);
343  send_message(msg);
344 }
345 
346 void Mavlink::send_version(uint8_t system_id, const char *const version)
347 {
348  mavlink_message_t msg;
349  mavlink_msg_rosflight_version_pack(system_id, compid_, &msg, version);
350  send_message(msg);
351 }
352 void Mavlink::send_error_data(uint8_t system_id, const BackupData &error_data)
353 {
354  mavlink_message_t msg;
355  bool rearm = (error_data.state.armed && error_data.arm_status==rosflight_firmware::ARM_MAGIC);
356  mavlink_msg_rosflight_hard_error_pack(system_id,compid_, &msg, error_data.error_code, error_data.debug_info.pc,
357  error_data.reset_count, rearm);
358  send_message(msg);
359 }
360 
361 void Mavlink::send_message(const mavlink_message_t &msg)
362 {
363  if (initialized_)
364  {
365  uint8_t data[MAVLINK_MAX_PACKET_LEN];
366  uint16_t len = mavlink_msg_to_send_buffer(data, &msg);
367  board_.serial_write(data, len);
368  }
369 }
370 
371 void Mavlink::handle_msg_param_request_list(const mavlink_message_t *const msg)
372 {
375 
376  if (listener_ != nullptr)
378 }
379 
380 void Mavlink::handle_msg_param_request_read(const mavlink_message_t *const msg)
381 {
384 
385  if (listener_ != nullptr)
387 }
388 
389 void Mavlink::handle_msg_param_set(const mavlink_message_t *const msg)
390 {
392  mavlink_msg_param_set_decode(msg, &set);
393 
394  mavlink_param_union_t param;
395  param.param_float = set.param_value;
396  param.type = set.param_type;
397 
398  switch (param.type)
399  {
401  if (listener_ != nullptr)
402  listener_->param_set_int_callback(set.target_system, set.param_id, param.param_int32);
403  break;
405  if (listener_ != nullptr)
406  listener_->param_set_float_callback(set.target_system, set.param_id, param.param_float);
407  break;
408  default:
409  // unsupported parameter type
410  break;
411  }
412 }
413 
414 void Mavlink::handle_msg_rosflight_cmd(const mavlink_message_t *const msg)
415 {
418 
420  switch (cmd.command)
421  {
424  break;
427  break;
430  break;
433  break;
436  break;
439  break;
442  break;
445  break;
448  break;
451  break;
454  break;
455  default: // unsupported command; report failure then return without calling command callback
456  mavlink_message_t out_msg;
458  send_message(out_msg);
459  // log(LogSeverity::LOG_ERROR, "Unsupported ROSFLIGHT CMD %d", command);
460  return;
461  }
462 
463  if (listener_ != nullptr)
464  listener_->command_callback(command);
465 }
466 
467 void Mavlink::handle_msg_rosflight_aux_cmd(const mavlink_message_t *const msg)
468 {
471 
473  // Repack mavlink message into CommLinkInterface::AuxCommand
474  for (int i = 0; i < 14; i++)
475  {
476  switch (cmd.type_array[i])
477  {
478  case DISABLED:
480  break;
481  case SERVO:
483  break;
484  case MOTOR:
486  break;
487  default:
488  // Invalid channel mode; log an error and return with calling callback
489  // log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unsupported AUX_CMD_CHANNEL_MODE %d", cmd.type_array[i]);
490  return;
491  }
492 
493  command.cmd_array[i].value = cmd.aux_cmd_array[i];
494  }
495 
496  // call callback after all channels have been repacked
497  if (listener_ != nullptr)
499 }
500 
501 void Mavlink::handle_msg_timesync(const mavlink_message_t *const msg)
502 {
503  mavlink_timesync_t tsync;
504  mavlink_msg_timesync_decode(msg, &tsync);
505 
506  if (listener_ != nullptr)
507  listener_->timesync_callback(tsync.tc1, tsync.ts1);
508 }
509 
510 void Mavlink::handle_msg_offboard_control(const mavlink_message_t *const msg)
511 {
514 
516  switch (ctrl.mode)
517  {
518  case MODE_PASS_THROUGH:
520  break;
523  break;
526  break;
527  default:
528  // invalid mode; ignore message and return without calling callback
529  return;
530  }
531 
532  control.x.value = ctrl.x;
533  control.y.value = ctrl.y;
534  control.z.value = ctrl.z;
535  control.F.value = ctrl.F;
536 
537  control.x.valid = !(ctrl.ignore & IGNORE_VALUE1);
538  control.y.valid = !(ctrl.ignore & IGNORE_VALUE2);
539  control.z.valid = !(ctrl.ignore & IGNORE_VALUE3);
540  control.F.valid = !(ctrl.ignore & IGNORE_VALUE4);
541 
542  if (listener_ != nullptr)
544 }
545 
546 void Mavlink::handle_msg_attitude_correction(const mavlink_message_t *const msg)
547 {
550 
551  turbomath::Quaternion q_correction;
552  q_correction.w = q_msg.qw;
553  q_correction.x = q_msg.qx;
554  q_correction.y = q_msg.qy;
555  q_correction.z = q_msg.qz;
556 
557  if (listener_ != nullptr)
559 }
560 void Mavlink::handle_msg_heartbeat(const mavlink_message_t *const msg)
561 {
562  //none of the information from the heartbeat is used
563  (void)msg;
564 
565  if (listener_ != nullptr)
567 }
568 
570 {
571  switch (in_buf_.msgid)
572  {
575  break;
578  break;
581  break;
584  break;
587  break;
590  break;
593  break;
596  break;
599  break;
600  default:
601  break;
602  }
603 }
604 
605 } // namespace rosflight_firmware
static volatile int16_t gyro[3]
Definition: drv_mpu6050.c:277
bool param(const std::string &param_name, T &param_val, const T &default_val)
virtual void param_set_float_callback(uint8_t target_system, const char *const param_name, float param_value)=0
virtual void offboard_control_callback(const OffboardControl &control)=0
float altitude
Definition: ms4525.c:41
float pressure
Definition: ms4525.c:41
static uint8_t cmd
Definition: drv_hmc5883l.c:79
debug_info_t debug_info
Definition: board.h:60
virtual void serial_write(const uint8_t *src, size_t len)=0
float temperature
Definition: ms4525.c:41
virtual void param_set_int_callback(uint8_t target_system, const char *const param_name, int32_t param_value)=0
virtual uint16_t serial_bytes_available()=0
static volatile int16_t accel[3]
Definition: drv_mpu6050.c:276
ROSLIB_DECL std::string command(const std::string &cmd)
static const channelConfig_t channels[CC_CHANNELS_PER_TIMER]
Definition: drv_timer.c:111
virtual void param_request_read_callback(uint8_t target_system, const char *const param_name, int16_t param_index)=0
virtual void param_request_list_callback(uint8_t target_system)=0
virtual void aux_command_callback(const AuxCommand &command)=0
virtual void timesync_callback(int64_t tc1, int64_t ts1)=0
StateManager::State state
Definition: board.h:64
virtual void attitude_correction_callback(const turbomath::Quaternion &q)=0
virtual void serial_init(uint32_t baud_rate, uint32_t dev)=0
const uint32_t ARM_MAGIC
Definition: board.h:69
uint8_t raw[15]
Definition: mpu6000.cpp:34
virtual uint8_t serial_read()=0


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Thu Oct 24 2019 03:17:18