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 "mavlink.h"
32 
33 #include "board.h"
34 
35 #include <cstdint>
36 
37 namespace rosflight_firmware
38 {
39 Mavlink::Mavlink(Board &board) : board_(board) {}
40 
41 void Mavlink::init(uint32_t baud_rate, uint32_t dev)
42 {
43  board_.serial_init(baud_rate, dev);
44  initialized_ = true;
45 }
46 
47 void Mavlink::receive(void)
48 {
50  {
53  }
54 }
55 
56 void Mavlink::send_attitude_quaternion(uint8_t system_id,
57  uint64_t timestamp_us,
58  const turbomath::Quaternion &attitude,
59  const turbomath::Vector &angular_velocity)
60 {
61  mavlink_message_t msg;
62  mavlink_msg_attitude_quaternion_pack(system_id, compid_, &msg, timestamp_us / 1000, attitude.w, attitude.x,
63  attitude.y, attitude.z, angular_velocity.x, angular_velocity.y,
64  angular_velocity.z);
65  send_message(msg);
66 }
67 
68 void Mavlink::send_baro(uint8_t system_id, float altitude, float pressure, float temperature)
69 {
70  mavlink_message_t msg;
71  mavlink_msg_small_baro_pack(system_id, compid_, &msg, altitude, pressure, temperature);
72  send_message(msg);
73 }
74 
75 void Mavlink::send_command_ack(uint8_t system_id, Command command, bool success)
76 {
77  ROSFLIGHT_CMD rosflight_cmd = ROSFLIGHT_CMD_ENUM_END;
78  switch (command)
79  {
81  rosflight_cmd = ROSFLIGHT_CMD_READ_PARAMS;
82  break;
84  rosflight_cmd = ROSFLIGHT_CMD_WRITE_PARAMS;
85  break;
87  rosflight_cmd = ROSFLIGHT_CMD_SET_PARAM_DEFAULTS;
88  break;
90  rosflight_cmd = ROSFLIGHT_CMD_ACCEL_CALIBRATION;
91  break;
93  rosflight_cmd = ROSFLIGHT_CMD_GYRO_CALIBRATION;
94  break;
96  rosflight_cmd = ROSFLIGHT_CMD_BARO_CALIBRATION;
97  break;
99  rosflight_cmd = ROSFLIGHT_CMD_AIRSPEED_CALIBRATION;
100  break;
102  rosflight_cmd = ROSFLIGHT_CMD_RC_CALIBRATION;
103  break;
105  rosflight_cmd = ROSFLIGHT_CMD_REBOOT;
106  break;
108  rosflight_cmd = ROSFLIGHT_CMD_REBOOT_TO_BOOTLOADER;
109  break;
111  rosflight_cmd = ROSFLIGHT_CMD_SEND_VERSION;
112  break;
113  }
114 
115  mavlink_message_t msg;
116  mavlink_msg_rosflight_cmd_ack_pack(system_id, compid_, &msg, rosflight_cmd,
118  send_message(msg);
119 }
120 
121 void Mavlink::send_diff_pressure(uint8_t system_id, float velocity, float pressure, float temperature)
122 {
123  mavlink_message_t msg;
124  mavlink_msg_diff_pressure_pack(system_id, compid_, &msg, velocity, pressure, temperature);
125  send_message(msg);
126 }
127 
128 void Mavlink::send_heartbeat(uint8_t system_id, bool fixed_wing)
129 {
130  mavlink_message_t msg;
131  mavlink_msg_heartbeat_pack(system_id, compid_, &msg, fixed_wing ? MAV_TYPE_FIXED_WING : MAV_TYPE_QUADROTOR, 0, 0, 0,
132  0);
133  send_message(msg);
134 }
135 
136 void Mavlink::send_imu(uint8_t system_id,
137  uint64_t timestamp_us,
138  const turbomath::Vector &accel,
139  const turbomath::Vector &gyro,
140  float temperature)
141 {
142  mavlink_message_t msg;
143  mavlink_msg_small_imu_pack(system_id, compid_, &msg, timestamp_us, accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z,
144  temperature);
145  send_message(msg);
146 }
147 void Mavlink::send_gnss(uint8_t system_id, const GNSSData &data)
148 {
149  mavlink_message_t msg;
150  mavlink_msg_rosflight_gnss_pack(system_id, compid_, &msg, data.time_of_week, data.fix_type, data.time, data.nanos,
151  data.lat, data.lon, data.height, data.vel_n, data.vel_e, data.vel_d, data.h_acc,
152  data.v_acc, data.ecef.x, data.ecef.y, data.ecef.z, data.ecef.p_acc, data.ecef.vx,
153  data.ecef.vy, data.ecef.vz, data.ecef.s_acc, data.rosflight_timestamp);
154  send_message(msg);
155 }
156 
157 void Mavlink::send_gnss_full(uint8_t system_id, const GNSSFull &full)
158 {
159  mavlink_message_t msg;
161  data.time_of_week = full.time_of_week;
162  data.year = full.year;
163  data.month = full.month;
164  data.day = full.day;
165  data.hour = full.hour;
166  data.min = full.min;
167  data.sec = full.sec;
168  data.valid = full.valid;
169  data.t_acc = full.t_acc;
170  data.nano = full.nano;
171  data.fix_type = full.fix_type;
172  data.num_sat = full.num_sat;
173  data.lon = full.lon;
174  data.lat = full.lat;
175  data.height = full.height;
176  data.height_msl = full.height_msl;
177  data.h_acc = full.h_acc;
178  data.v_acc = full.v_acc;
179  data.vel_n = full.vel_n;
180  data.vel_e = full.vel_e;
181  data.vel_d = full.vel_d;
182  data.g_speed = full.g_speed;
183  data.head_mot = full.head_mot;
184  data.s_acc = full.s_acc;
185  data.head_acc = full.head_acc;
186  data.p_dop = full.p_dop;
188  mavlink_msg_rosflight_gnss_full_encode(system_id, compid_, &msg, &data);
189  send_message(msg);
190 }
191 
192 void Mavlink::send_log_message(uint8_t system_id, LogSeverity severity, const char *text)
193 {
194  MAV_SEVERITY mavlink_severity = MAV_SEVERITY_ENUM_END;
195  switch (severity)
196  {
198  mavlink_severity = MAV_SEVERITY_INFO;
199  break;
201  mavlink_severity = MAV_SEVERITY_WARNING;
202  break;
204  mavlink_severity = MAV_SEVERITY_ERROR;
205  break;
207  mavlink_severity = MAV_SEVERITY_CRITICAL;
208  break;
209  }
210 
211  mavlink_message_t msg;
212  mavlink_msg_statustext_pack(system_id, compid_, &msg, static_cast<uint8_t>(mavlink_severity), text);
213  send_message(msg);
214 }
215 
216 void Mavlink::send_mag(uint8_t system_id, const turbomath::Vector &mag)
217 {
218  mavlink_message_t msg;
219  mavlink_msg_small_mag_pack(system_id, compid_, &msg, mag.x, mag.y, mag.z);
220  send_message(msg);
221 }
222 
223 void Mavlink::send_named_value_int(uint8_t system_id, uint32_t timestamp_ms, const char *const name, int32_t value)
224 {
225  mavlink_message_t msg;
226  mavlink_msg_named_value_int_pack(system_id, compid_, &msg, timestamp_ms, name, value);
227  send_message(msg);
228 }
229 
230 void Mavlink::send_named_value_float(uint8_t system_id, uint32_t timestamp_ms, const char *const name, float value)
231 {
232  mavlink_message_t msg;
233  mavlink_msg_named_value_float_pack(system_id, compid_, &msg, timestamp_ms, name, value);
234  send_message(msg);
235 }
236 
237 void Mavlink::send_output_raw(uint8_t system_id, uint32_t timestamp_ms, const float raw_outputs[14])
238 {
239  mavlink_message_t msg;
240  mavlink_msg_rosflight_output_raw_pack(system_id, compid_, &msg, timestamp_ms, raw_outputs);
241  send_message(msg);
242 }
243 
244 void Mavlink::send_param_value_int(uint8_t system_id,
245  uint16_t index,
246  const char *const name,
247  int32_t value,
248  uint16_t param_count)
249 {
250  mavlink_param_union_t param;
251  param.param_int32 = value;
252 
253  mavlink_message_t msg;
254  mavlink_msg_param_value_pack(system_id, 0, &msg, name, param.param_float, MAV_PARAM_TYPE_INT32, param_count, index);
255  send_message(msg);
256 }
257 
258 void Mavlink::send_param_value_float(uint8_t system_id,
259  uint16_t index,
260  const char *const name,
261  float value,
262  uint16_t param_count)
263 {
264  mavlink_message_t msg;
265  mavlink_msg_param_value_pack(system_id, 0, &msg, name, value, MAV_PARAM_TYPE_REAL32, param_count, index);
266  send_message(msg);
267 }
268 
269 void Mavlink::send_rc_raw(uint8_t system_id, uint32_t timestamp_ms, const uint16_t channels[8])
270 {
271  mavlink_message_t msg;
272  mavlink_msg_rc_channels_pack(system_id, compid_, &msg, timestamp_ms, 0, channels[0], channels[1], channels[2],
273  channels[3], channels[4], channels[5], channels[6], channels[7], 0, 0, 0, 0, 0, 0, 0, 0,
274  0, 0, 0);
275  send_message(msg);
276 }
277 
278 void Mavlink::send_sonar(uint8_t system_id,
279  /* TODO enum type*/ uint8_t type,
280  float range,
281  float max_range,
282  float min_range)
283 {
284  (void)type;
285  mavlink_message_t msg;
286  mavlink_msg_small_range_pack(system_id, compid_, &msg, /* TODO */ ROSFLIGHT_RANGE_SONAR, range, max_range, min_range);
287  send_message(msg);
288 }
289 
290 void Mavlink::send_status(uint8_t system_id,
291  bool armed,
292  bool failsafe,
293  bool rc_override,
294  bool offboard,
295  uint8_t error_code,
296  uint8_t control_mode,
297  int16_t num_errors,
298  int16_t loop_time_us)
299 {
300  mavlink_message_t msg;
301  mavlink_msg_rosflight_status_pack(system_id, compid_, &msg, armed, failsafe, rc_override, offboard, error_code,
302  control_mode, num_errors, loop_time_us);
303  send_message(msg);
304 }
305 
306 void Mavlink::send_timesync(uint8_t system_id, int64_t tc1, int64_t ts1)
307 {
308  mavlink_message_t msg;
309  mavlink_msg_timesync_pack(system_id, compid_, &msg, tc1, ts1);
310  send_message(msg);
311 }
312 
313 void Mavlink::send_version(uint8_t system_id, const char *const version)
314 {
315  mavlink_message_t msg;
316  mavlink_msg_rosflight_version_pack(system_id, compid_, &msg, version);
317  send_message(msg);
318 }
319 void Mavlink::send_error_data(uint8_t system_id, const StateManager::BackupData &error_data)
320 {
321  mavlink_message_t msg;
322  bool rearm = (error_data.arm_flag == StateManager::BackupData::ARM_MAGIC);
323  mavlink_msg_rosflight_hard_error_pack(system_id, compid_, &msg, error_data.error_code, error_data.debug.pc,
324  error_data.reset_count, rearm);
325  send_message(msg);
326 }
327 void Mavlink::send_battery_status(uint8_t system_id, float voltage, float current)
328 {
329  mavlink_message_t msg;
330  mavlink_msg_rosflight_battery_status_pack(system_id, compid_, &msg, voltage, current);
331  send_message(msg);
332 }
333 
334 void Mavlink::send_message(const mavlink_message_t &msg)
335 {
336  if (initialized_)
337  {
338  uint8_t data[MAVLINK_MAX_PACKET_LEN];
339  uint16_t len = mavlink_msg_to_send_buffer(data, &msg);
340  board_.serial_write(data, len);
341  }
342 }
343 
344 void Mavlink::handle_msg_param_request_list(const mavlink_message_t *const msg)
345 {
348 
349  if (listener_ != nullptr)
351 }
352 
353 void Mavlink::handle_msg_param_request_read(const mavlink_message_t *const msg)
354 {
357 
358  if (listener_ != nullptr)
360 }
361 
362 void Mavlink::handle_msg_param_set(const mavlink_message_t *const msg)
363 {
365  mavlink_msg_param_set_decode(msg, &set);
366 
367  mavlink_param_union_t param;
368  param.param_float = set.param_value;
369  param.type = set.param_type;
370 
371  switch (param.type)
372  {
374  if (listener_ != nullptr)
375  listener_->param_set_int_callback(set.target_system, set.param_id, param.param_int32);
376  break;
378  if (listener_ != nullptr)
379  listener_->param_set_float_callback(set.target_system, set.param_id, param.param_float);
380  break;
381  default:
382  // unsupported parameter type
383  break;
384  }
385 }
386 
387 void Mavlink::handle_msg_rosflight_cmd(const mavlink_message_t *const msg)
388 {
391 
393  switch (cmd.command)
394  {
397  break;
400  break;
403  break;
406  break;
409  break;
412  break;
415  break;
418  break;
421  break;
424  break;
427  break;
428  default: // unsupported command; report failure then return without calling command callback
429  mavlink_message_t out_msg;
431  send_message(out_msg);
432  // log(LogSeverity::LOG_ERROR, "Unsupported ROSFLIGHT CMD %d", command);
433  return;
434  }
435 
436  if (listener_ != nullptr)
437  listener_->command_callback(command);
438 }
439 
440 void Mavlink::handle_msg_rosflight_aux_cmd(const mavlink_message_t *const msg)
441 {
444 
446  // Repack mavlink message into CommLinkInterface::AuxCommand
447  for (int i = 0; i < 14; i++)
448  {
449  switch (cmd.type_array[i])
450  {
451  case DISABLED:
453  break;
454  case SERVO:
456  break;
457  case MOTOR:
459  break;
460  default:
461  // Invalid channel mode; log an error and return with calling callback
462  // log(CommLinkInterface::LogSeverity::LOG_ERROR, "Unsupported AUX_CMD_CHANNEL_MODE %d",
463  // cmd.type_array[i]);
464  return;
465  }
466 
467  command.cmd_array[i].value = cmd.aux_cmd_array[i];
468  }
469 
470  // call callback after all channels have been repacked
471  if (listener_ != nullptr)
473 }
474 
475 void Mavlink::handle_msg_timesync(const mavlink_message_t *const msg)
476 {
477  mavlink_timesync_t tsync;
478  mavlink_msg_timesync_decode(msg, &tsync);
479 
480  if (listener_ != nullptr)
481  listener_->timesync_callback(tsync.tc1, tsync.ts1);
482 }
483 
484 void Mavlink::handle_msg_offboard_control(const mavlink_message_t *const msg)
485 {
488 
490  switch (ctrl.mode)
491  {
492  case MODE_PASS_THROUGH:
494  break;
497  break;
500  break;
501  default:
502  // invalid mode; ignore message and return without calling callback
503  return;
504  }
505 
506  control.x.value = ctrl.x;
507  control.y.value = ctrl.y;
508  control.z.value = ctrl.z;
509  control.F.value = ctrl.F;
510 
511  control.x.valid = !(ctrl.ignore & IGNORE_VALUE1);
512  control.y.valid = !(ctrl.ignore & IGNORE_VALUE2);
513  control.z.valid = !(ctrl.ignore & IGNORE_VALUE3);
514  control.F.valid = !(ctrl.ignore & IGNORE_VALUE4);
515 
516  if (listener_ != nullptr)
518 }
519 
520 void Mavlink::handle_msg_external_attitude(const mavlink_message_t *const msg)
521 {
524 
525  turbomath::Quaternion q_extatt;
526  q_extatt.w = q_msg.qw;
527  q_extatt.x = q_msg.qx;
528  q_extatt.y = q_msg.qy;
529  q_extatt.z = q_msg.qz;
530 
531  if (listener_ != nullptr)
533 }
534 
535 void Mavlink::handle_msg_heartbeat(const mavlink_message_t *const msg)
536 {
537  // none of the information from the heartbeat is used
538  (void)msg;
539 
540  if (listener_ != nullptr)
542 }
543 
545 {
546  switch (in_buf_.msgid)
547  {
550  break;
553  break;
556  break;
559  break;
562  break;
565  break;
568  break;
571  break;
574  break;
575  default:
576  break;
577  }
578 }
579 
580 } // namespace rosflight_firmware
virtual void external_attitude_callback(const turbomath::Quaternion &q)=0
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
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
virtual void serial_init(uint32_t baud_rate, uint32_t dev)=0
virtual uint8_t serial_read()=0


rosflight_firmware
Author(s): Daniel Koch , James Jackson
autogenerated on Fri Oct 9 2020 03:17:15