Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
mavlink.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Freek van Tienen <freek.v.tienen@gmail.com>
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 
28 
29 // include mavlink headers, but ignore some warnings
30 #pragma GCC diagnostic push
31 #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
32 #pragma GCC diagnostic ignored "-Wswitch-default"
33 #include "mavlink/paparazzi/mavlink.h"
34 #pragma GCC diagnostic pop
35 
36 #if PERIODIC_TELEMETRY
37 #define PERIODIC_C_MAVLINK
39 #include "generated/periodic_telemetry.h"
40 #ifndef TELEMETRY_MAVLINK_NB_MSG
41 #warning Using hardcoded msg periods. To customize specify a <process name="Mavlink" type="mavlink"> in your telemetry file.
42 #endif
43 #endif
44 
45 #include "generated/airframe.h"
46 #include "generated/modules.h"
47 #include "generated/settings.h"
48 
49 #include "mcu_periph/sys_time.h"
51 #include "state.h"
52 #include "pprz_version.h"
53 #include "autopilot.h"
54 #include "autopilot_guided.h"
55 
56 #if defined RADIO_CONTROL
58 #endif
59 
60 // Change the autopilot identification code: by default identify as PPZ autopilot: alternatively as MAV_AUTOPILOT_ARDUPILOTMEGA
61 #ifndef MAV_AUTOPILOT_ID
62 #define MAV_AUTOPILOT_ID MAV_AUTOPILOT_PPZ
63 #endif
64 
66 
67 // for UINT16_MAX
68 #include <stdint.h>
69 
70 mavlink_system_t mavlink_system;
71 
72 static uint8_t mavlink_params_idx = NB_SETTING;
76 static char mavlink_param_names[NB_SETTING][16 + 1] = SETTINGS_NAMES_SHORT;
80 
81 void mavlink_common_message_handler(const mavlink_message_t *msg);
82 
83 static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev);
84 static void mavlink_send_sys_status(struct transport_tx *trans, struct link_device *dev);
85 static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev);
86 static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev);
87 static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev);
88 static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev);
89 static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev);
90 static void mavlink_send_autopilot_version(struct transport_tx *trans, struct link_device *dev);
91 static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev);
92 static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev);
93 static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev);
94 static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev);
95 static void mavlink_send_gps_global_origin(struct transport_tx *trans, struct link_device *dev);
96 static void mavlink_send_gps_status(struct transport_tx *trans, struct link_device *dev);
97 static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev);
98 
99 
104 #ifndef MAVLINK_SYSID
105 #define MAVLINK_SYSID AC_ID
106 #endif
107 
108 #if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
109 struct telemetry_cb_slots mavlink_cbs[TELEMETRY_MAVLINK_NB_MSG] = TELEMETRY_MAVLINK_CBS;
110 struct periodic_telemetry mavlink_telemetry = { TELEMETRY_MAVLINK_NB_MSG, mavlink_cbs };
111 #endif
112 
113 void mavlink_init(void)
114 {
115  mavlink_system.sysid = MAVLINK_SYSID; // System ID, 1-255
116  mavlink_system.compid = MAV_COMP_ID_AUTOPILOT1; // Component/Subsystem ID, 1-255
117 
118  get_pprz_git_version(custom_version);
119 
121 
122 #if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
123  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_HEARTBEAT, mavlink_send_heartbeat);
124  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_SYS_STATUS, mavlink_send_sys_status);
125  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_SYSTEM_TIME, mavlink_send_system_time);
126  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_ATTITUDE, mavlink_send_attitude);
127  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, mavlink_send_attitude_quaternion);
128  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_LOCAL_POSITION_NED, mavlink_send_local_position_ned);
129  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, mavlink_send_global_position_int);
130  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GPS_RAW_INT, mavlink_send_gps_raw_int);
131  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_RC_CHANNELS, mavlink_send_rc_channels);
132  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_BATTERY_STATUS, mavlink_send_battery_status);
133  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_AUTOPILOT_VERSION, mavlink_send_autopilot_version);
134  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, mavlink_send_gps_global_origin);
135  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GPS_STATUS, mavlink_send_gps_status);
136  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_VFR_HUD, mavlink_send_vfr_hud);
137 #endif
138 }
139 
145 {
146 #if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
147  // send periodic mavlink messages as defined in the Mavlink process of the telemetry xml file
148  // transport and device not used here yet...
149  periodic_telemetry_send_Mavlink(&mavlink_telemetry, NULL, NULL);
150 #endif
151 }
152 
158 {
159 #if !defined (TELEMETRY_MAVLINK_NB_MSG)
160  // use these hardcoded periods if no Mavlink process in telemetry file
161  RunOnceEvery(2, mavlink_send_heartbeat(NULL, NULL));
162  RunOnceEvery(5, mavlink_send_sys_status(NULL, NULL));
163  RunOnceEvery(20, mavlink_send_system_time(NULL, NULL));
164  RunOnceEvery(10, mavlink_send_attitude(NULL, NULL));
165  RunOnceEvery(5, mavlink_send_attitude_quaternion(NULL, NULL));
166  RunOnceEvery(4, mavlink_send_local_position_ned(NULL, NULL));
167  RunOnceEvery(5, mavlink_send_global_position_int(NULL, NULL));
168  RunOnceEvery(6, mavlink_send_gps_raw_int(NULL, NULL));
169  RunOnceEvery(5, mavlink_send_rc_channels(NULL, NULL));
170  RunOnceEvery(21, mavlink_send_battery_status(NULL, NULL));
171  RunOnceEvery(32, mavlink_send_autopilot_version(NULL, NULL));
172  RunOnceEvery(33, mavlink_send_gps_global_origin(NULL, NULL));
173  RunOnceEvery(10, mavlink_send_gps_status(NULL, NULL));
174  RunOnceEvery(10, mavlink_send_vfr_hud(NULL, NULL));
175 #endif
176 
177  // if requested, send params at 5Hz until all sent
178  RunOnceEvery(2, mavlink_send_params(NULL, NULL));
179 
181 }
182 
183 static int16_t settings_idx_from_param_id(char *param_id)
184 {
185  int i, j;
186  int16_t settings_idx = -1;
187 
188  // Go trough all the settings to search the ID
189  for (i = 0; i < NB_SETTING; i++) {
190  for (j = 0; j < 16; j++) {
191  if (mavlink_param_names[i][j] != param_id[j]) {
192  break;
193  }
194 
195  if (mavlink_param_names[i][j] == '\0') {
196  settings_idx = i;
197  return settings_idx;
198  }
199  }
200 
201  if (mavlink_param_names[i][j] == '\0') {
202  break;
203  }
204  }
205  return settings_idx;
206 }
207 
211 void mavlink_event(void)
212 {
213  mavlink_message_t msg;
214  mavlink_status_t status;
215 
216  // Check uplink
217  while (MAVLinkChAvailable()) {
218  char test = MAVLinkGetch();
219 
220  // When we receive a message
221  if (mavlink_parse_char(MAVLINK_COMM_0, test, &msg, &status)) {
224  }
225  }
226 }
227 
228 void mavlink_common_message_handler(const mavlink_message_t *msg)
229 {
230  switch (msg->msgid) {
231  case MAVLINK_MSG_ID_HEARTBEAT:
232  break;
233 
234  /* When requesting data streams say we can't send them */
235  case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
236  mavlink_request_data_stream_t cmd;
237  mavlink_msg_request_data_stream_decode(msg, &cmd);
238 
239  mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0);
241  break;
242  }
243 
244  /* Override channels with RC */
245  case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: {
246  mavlink_rc_channels_override_t cmd;
247  mavlink_msg_rc_channels_override_decode(msg, &cmd);
248 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
249  uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100;
250  int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2;
251  int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2;
252  int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100;
253  parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw);
254  //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n",
255  // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw);
256 #endif
257  break;
258  }
259 
260  /* When a request is made of the parameters list */
261  case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
262  mavlink_params_idx = 0;
263  break;
264  }
265 
266  /* When a request os made for a specific parameter */
267  case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
268  mavlink_param_request_read_t cmd;
269  mavlink_msg_param_request_read_decode(msg, &cmd);
270 
271  // First check param_index and search for the ID if needed
272  if (cmd.param_index == -1) {
273  cmd.param_index = settings_idx_from_param_id(cmd.param_id);
274  }
275 
276  // Send message only if the param_index was found (Coverity Scan)
277  if (cmd.param_index > -1) {
278  mavlink_msg_param_value_send(MAVLINK_COMM_0,
279  mavlink_param_names[cmd.param_index],
280  settings_get_value(cmd.param_index),
281  MAV_PARAM_TYPE_REAL32,
282  NB_SETTING,
283  cmd.param_index);
285  }
286  break;
287  }
288 
289  case MAVLINK_MSG_ID_PARAM_SET: {
290  mavlink_param_set_t set;
291  mavlink_msg_param_set_decode(msg, &set);
292 
293  // Check if this message is for this system
294  if ((uint8_t) set.target_system == AC_ID) {
295  int16_t idx = settings_idx_from_param_id(set.param_id);
296 
297  // setting found
298  if (idx >= 0) {
299  // Only write if new value is NOT "not-a-number"
300  // AND is NOT infinity
301  if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
302  !isnan(set.param_value) && !isinf(set.param_value)) {
303  DlSetting(idx, set.param_value);
304  // Report back new value
305  mavlink_msg_param_value_send(MAVLINK_COMM_0,
307  settings_get_value(idx),
308  MAV_PARAM_TYPE_REAL32,
309  NB_SETTING,
310  idx);
312  }
313  }
314  }
315  }
316  break;
317 
318 #ifdef ROTORCRAFT_FIRMWARE
319  /* only for rotorcraft */
320  case MAVLINK_MSG_ID_COMMAND_LONG: {
321  mavlink_command_long_t cmd;
322  mavlink_msg_command_long_decode(msg, &cmd);
323  // Check if this message is for this system
324  if ((uint8_t) cmd.target_system == AC_ID) {
325  uint8_t result = MAV_RESULT_UNSUPPORTED;
326  switch (cmd.command) {
327  case MAV_CMD_NAV_GUIDED_ENABLE:
328  MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1);
329  result = MAV_RESULT_FAILED;
330  if (cmd.param1 > 0.5) {
333  result = MAV_RESULT_ACCEPTED;
334  }
335  }
336  else {
337  // turn guided mode off - to what? maybe NAV? or MODE_AUTO2?
338  }
339  break;
340 
341  case MAV_CMD_COMPONENT_ARM_DISARM:
342  /* supposed to use this command to arm or SET_MODE?? */
343  MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1);
344  result = MAV_RESULT_FAILED;
345  if (cmd.param1 > 0.5) {
348  result = MAV_RESULT_ACCEPTED;
349  }
350  else {
353  result = MAV_RESULT_ACCEPTED;
354  }
355  break;
356 
357  default:
358  break;
359  }
360  // confirm command with result
361  mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result, 0, UINT8_MAX, msg->sysid, msg->compid);
363  }
364  break;
365  }
366 
367  case MAVLINK_MSG_ID_SET_MODE: {
368  mavlink_set_mode_t mode;
369  mavlink_msg_set_mode_decode(msg, &mode);
370  if (mode.target_system == AC_ID) {
371  MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode);
372  if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
374  }
375  else {
377  }
378  if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
380  }
381  else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
383  }
384  }
385  break;
386  }
387 
388  case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: {
389  mavlink_set_position_target_local_ned_t target;
390  mavlink_msg_set_position_target_local_ned_decode(msg, &target);
391  // Check if this message is for this system
392  if (target.target_system == AC_ID) {
393  MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask);
394  /* if position and yaw bits are not set to ignored, use only position for now */
395  if (!(target.type_mask & 0b1110000000100000)) {
396  switch (target.coordinate_frame) {
397  case MAV_FRAME_LOCAL_NED:
398  MAVLINK_DEBUG("set position target, frame LOCAL_NED\n");
400  break;
401  case MAV_FRAME_LOCAL_OFFSET_NED:
402  MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n");
404  break;
405  case MAV_FRAME_BODY_OFFSET_NED:
406  MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n");
408  break;
409  default:
410  break;
411  }
412  }
413  else if (!(target.type_mask & 0b0001110000100000)) {
414  /* position is set to ignore, but velocity not */
415  switch (target.coordinate_frame) {
416  case MAV_FRAME_LOCAL_NED:
417  MAVLINK_DEBUG("set velocity target, frame LOCAL_NED\n");
419  break;
420  default:
421  break;
422  }
423  }
424  }
425  break;
426  }
427 #endif
428 
429  default:
430  //Do nothing
431  MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid);
432  break;
433  }
434 }
435 
436 /* ignore the unused-parameter warnings */
437 #pragma GCC diagnostic push
438 #pragma GCC diagnostic ignored "-Wunused-parameter"
439 
443 static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev)
444 {
445  uint8_t mav_state = MAV_STATE_CALIBRATING;
446  uint8_t mav_mode = 0;
447 #if defined(FIXEDWING_FIRMWARE)
448  uint8_t mav_type = MAV_TYPE_FIXED_WING;
449  switch (autopilot_get_mode()) {
450  case AP_MODE_MANUAL:
451  mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
452  break;
453  case AP_MODE_AUTO1:
454  mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
455  break;
456  case AP_MODE_AUTO2:
457  mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
458  break;
459  case AP_MODE_HOME:
460  mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED;
461  break;
462  default:
463  break;
464  }
465 #elif defined(ROTORCRAFT_FIRMWARE)
466  uint8_t mav_type = MAV_TYPE_QUADROTOR;
467  switch (autopilot_get_mode()) {
468  case AP_MODE_HOME:
469  mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
470  break;
471  case AP_MODE_RATE_DIRECT:
473  case AP_MODE_RATE_Z_HOLD:
474  case AP_MODE_RC_DIRECT:
475  mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
476  break;
482  case AP_MODE_HOVER_CLIMB:
485  mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
486  break;
487  case AP_MODE_NAV:
488  mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
489  break;
490  case AP_MODE_GUIDED:
491  mav_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
492  default:
493  break;
494  }
495 #else
496 #error "mavlink datalink: unsupported firmware"
497 #endif
498  if (stateIsAttitudeValid()) {
500  mav_state = MAV_STATE_STANDBY;
501  } else {
502  mav_state = MAV_STATE_ACTIVE;
503  mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
504  }
505  }
506  mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
507  mav_type,
509  mav_mode,
510  0, // custom_mode
511  mav_state);
513 }
514 
518 static void mavlink_send_sys_status(struct transport_tx *trans, struct link_device *dev)
519 {
521 #define UAV_SENSORS (MAV_SYS_STATUS_SENSOR_3D_GYRO|MAV_SYS_STATUS_SENSOR_3D_ACCEL|MAV_SYS_STATUS_SENSOR_3D_MAG|MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE)
522 
523  mavlink_msg_sys_status_send(MAVLINK_COMM_0,
524  UAV_SENSORS, // On-board sensors: present (bitmap)
525  UAV_SENSORS, // On-board sensors: active (bitmap)
526  UAV_SENSORS, // On-board sensors: state (bitmap)
527  -1,//10*sys_mon.cpu_load, // System loadof main-loop time (0=0% to 1000=100%)
528  electrical.vsupply * 1000.f, // Battery voltage (milivolts)
529  electrical.current * 100.f, // Battery current (10x miliampere)
530  -1, // Battery remaining (0-100 in %)
531  0, // Communication packet drops (0=0% to 10000=100%)
532  0, // Communication error(per packet) (0=0% to 10000=100%)
533  0, // Autopilot specific error 1
534  0, // Autopilot specific error 2
535  0, // Autopilot specific error 3
536  0, // Autopilot specific error 4
537  0,
538  0,
539  0);
541 }
542 
548 static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev)
549 {
550  mavlink_msg_system_time_send(MAVLINK_COMM_0, 0, get_sys_time_msec());
552 }
553 
557 static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev)
558 {
559  mavlink_msg_attitude_send(MAVLINK_COMM_0,
561  stateGetNedToBodyEulers_f()->phi, // Phi
562  stateGetNedToBodyEulers_f()->theta, // Theta
563  stateGetNedToBodyEulers_f()->psi, // Psi
564  stateGetBodyRates_f()->p, // p
565  stateGetBodyRates_f()->q, // q
566  stateGetBodyRates_f()->r); // r
568 }
569 
570 static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev)
571 {
572  mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
577  stateGetSpeedNed_f()->x,
578  stateGetSpeedNed_f()->y,
579  stateGetSpeedNed_f()->z);
581 }
582 
583 static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev)
584 {
585  float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
586  if (heading < 0.) {
587  heading += 360;
588  }
589  uint16_t compass_heading = heading * 100;
593  mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
595  stateGetPositionLla_i()->lat,
596  stateGetPositionLla_i()->lon,
597  stateGetPositionLla_i()->alt + hmsl_alt,
598  relative_alt,
599  stateGetSpeedNed_f()->x * 100,
600  stateGetSpeedNed_f()->y * 100,
601  stateGetSpeedNed_f()->z * 100,
602  compass_heading);
604 }
605 
606 static void mavlink_send_gps_global_origin(struct transport_tx *trans, struct link_device *dev)
607 {
608  if (state.ned_initialized_i) {
609  mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0,
615  }
616 }
617 
621 static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev)
622 {
623  if (mavlink_params_idx >= NB_SETTING) {
624  return;
625  }
626 
627  mavlink_msg_param_value_send(MAVLINK_COMM_0,
629  settings_get_value(mavlink_params_idx),
630  MAV_PARAM_TYPE_REAL32,
631  NB_SETTING,
634 
636 }
637 
638 static void mavlink_send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
639 {
641  static uint32_t ver = PPRZ_VERSION_INT;
642  static uint64_t sha;
643  get_pprz_git_version((uint8_t *)&sha);
644  mavlink_msg_autopilot_version_send(MAVLINK_COMM_0,
645  0, //uint64_t capabilities,
646  ver, //uint32_t flight_sw_version,
647  0, //uint32_t middleware_sw_version,
648  0, //uint32_t os_sw_version,
649  0, //uint32_t board_version,
650  0, //const uint8_t *flight_custom_version,
651  0, //const uint8_t *middleware_custom_version,
652  0, //const uint8_t *os_custom_version,
653  0, //uint16_t vendor_id,
654  0, //uint16_t product_id,
655  sha, //uint64_t uid
656  0);
658 }
659 
660 static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev)
661 {
662  float repr_offset_q[4] = {0, 0, 0, 0};
663  mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
670  stateGetBodyRates_f()->q,
671  stateGetBodyRates_f()->r,
672  repr_offset_q);
674 }
675 
676 #if USE_GPS
677 #include "modules/gps/gps.h"
678 #endif
679 
680 static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev)
681 {
682 #if USE_GPS
683  int32_t course = DegOfRad(gps.course) / 1e5;
684  if (course < 0) {
685  course += 36000;
686  }
687  mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
689  gps.fix,
690  gps.lla_pos.lat,
691  gps.lla_pos.lon,
692  gps.hmsl,
693  gps.pdop,
694  UINT16_MAX, // VDOP
695  gps.gspeed,
696  course,
697  gps.num_sv,
698  gps.lla_pos.alt,
699  gps.hacc,
700  gps.vacc,
701  gps.sacc,
702  0,
703  0);
705 #endif
706 }
707 
718 static void mavlink_send_gps_status(struct transport_tx *trans, struct link_device *dev)
719 {
720 #if USE_GPS
721  uint8_t nb_sats = Min(GPS_NB_CHANNELS, 20);
722  uint8_t prn[20];
723  uint8_t used[20];
724  uint8_t elevation[20];
725  uint8_t azimuth[20];
726  uint8_t snr[20];
727  for (uint8_t i = 0; i < nb_sats; i++) {
728  prn[i] = gps.svinfos[i].svid;
729  // set sat as used if cno > 0, not quite true though
730  if (gps.svinfos[i].cno > 0) {
731  used[i] = 1;
732  }
733  elevation[i] = gps.svinfos[i].elev;
734  // convert azimuth to unsigned representation: 0: 0 deg, 255: 360 deg.
735  uint8_t azim;
736  if (gps.svinfos[i].azim < 0) {
737  azim = (float)(-gps.svinfos[i].azim + 180) / 360 * 255;
738  } else {
739  azim = (float)gps.svinfos[i].azim / 360 * 255;
740  }
741  azimuth[i] = azim;
742  }
743  mavlink_msg_gps_status_send(MAVLINK_COMM_0,
744  gps.num_sv, prn, used, elevation, azimuth, snr);
746 #endif
747 }
748 
749 #if defined RADIO_CONTROL
751 // since they really want PPM values, use a hack to check if are using ppm subsystem
752 #ifdef PPM_PULSE_TYPE_POSITIVE
753 #define RC_CHANNELS RADIO_CTL_NB
755 #define PPM_PULSES(_i) ((_i) < RADIO_CTL_NB ? ppm_pulses[(_i)] : UINT16_MAX)
756 #else
757 // use radio_control.values for now...
758 #define RC_CHANNELS RADIO_CONTROL_NB_CHANNEL
759 #define PPM_OF_PPRZ(_v) ((_v) / 19.2 + 1500)
760 #define PPM_PULSES(_i) ((_i) < RADIO_CONTROL_NB_CHANNEL ? PPM_OF_PPRZ(radio_control.values[(_i)]) + MAX_PPRZ : UINT16_MAX)
761 #endif
762 #endif
763 
764 static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev)
765 {
766 #if defined RADIO_CONTROL
767  mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
769  RC_CHANNELS,
770  PPM_PULSES(0), PPM_PULSES(1), PPM_PULSES(2),
771  PPM_PULSES(3), PPM_PULSES(4), PPM_PULSES(5),
772  PPM_PULSES(6), PPM_PULSES(7), PPM_PULSES(8),
773  PPM_PULSES(9), PPM_PULSES(10), PPM_PULSES(11),
774  PPM_PULSES(12), PPM_PULSES(13), PPM_PULSES(14),
775  PPM_PULSES(15), PPM_PULSES(16), PPM_PULSES(17),
776  255); // rssi unknown
777 #else
778  mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
780  0, // zero channels available
781  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
782  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
783  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
784  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
785  UINT16_MAX, UINT16_MAX, 255);
786 #endif
788 }
789 
791 static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev)
792 {
793  static uint16_t voltages[14] = {0};
794  // we simply only set one cell for now
795  voltages[0] = electrical.vsupply * 1000.f; // convert to mV
797  mavlink_msg_battery_status_send(MAVLINK_COMM_0,
798  0, // id
799  0, // battery_function
800  0, // type
801  INT16_MAX, // unknown temperature
802  voltages, // cell voltages
803  electrical.current * 100.f, // convert to deciA
804  electrical.charge * 1000.f, // convert to mAh
805  electrical.energy * 36, // convert to hecto Joule
806  -1, // remaining percentage not estimated
807  0,
808  MAV_BATTERY_CHARGE_STATE_UNDEFINED,
809  &voltages[10],
810  MAV_BATTERY_MODE_UNKNOWN,
811  0);
813 }
814 
815 #include "modules/core/commands.h"
819 static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev)
820 {
821  /* Current heading in degrees, in compass units (0..360, 0=north) */
822  int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
823  /* Current throttle setting in integer percent, 0 to 100 */
824  // is a 16bit unsigned int but supposed to be from 0 to 100??
825  uint16_t throttle;
826 #ifdef COMMAND_THRUST
827  throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100);
828 #elif defined COMMAND_THROTTLE
829  throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);
830 #endif
831  float hmsl_alt = state.ned_origin_f.hmsl - state.ned_origin_f.lla.alt;
832  mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
833  Max(0,stateGetAirspeed_f()),
834  stateGetHorizontalSpeedNorm_f(), // groundspeed
835  heading,
836  throttle,
837  stateGetPositionLla_f()->alt + hmsl_alt,
838  stateGetSpeedNed_f()->z); // climb rate
840 }
841 
842 /* end ignore unused-paramter */
843 #pragma GCC diagnostic pop
static int16_t course[3]
Definition: airspeed_uADC.c:58
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:193
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:217
bool autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly FIXME is i...
Definition: autopilot.c:245
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:290
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:308
Core autopilot interface common to all firmwares.
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
Set velocity and heading setpoints in GUIDED mode.
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
Autopilot guided mode interface.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:71
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:98
pprz_t commands[COMMANDS_NB]
Definition: commands.c:30
Hardware independent code for commands handling.
struct Electrical electrical
Definition: electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
float energy
consumed energy in Wh
Definition: electrical.h:49
float current
current in A
Definition: electrical.h:47
float charge
consumed electric charge in Ah
Definition: electrical.h:48
float vsupply
supply voltage in V
Definition: electrical.h:45
#define Min(x, y)
Definition: esc_dshot.c:101
#define AP_MODE_HOME
#define AP_MODE_MANUAL
AP modes.
#define AP_MODE_AUTO2
#define AP_MODE_AUTO1
struct GpsState gps
global GPS state
Definition: gps.c:69
Device independent GPS code (interface)
int16_t azim
azimuth in deg
Definition: gps.h:81
int32_t hmsl
height above mean sea level (MSL) in mm
Definition: gps.h:92
int8_t elev
elevation in deg
Definition: gps.h:80
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:90
uint32_t sacc
speed accuracy in cm/s
Definition: gps.h:101
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:97
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:79
uint32_t hacc
horizontal accuracy in cm
Definition: gps.h:99
#define GPS_NB_CHANNELS
Definition: gps.h:55
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:103
uint8_t svid
Satellite ID.
Definition: gps.h:76
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:95
uint32_t vacc
vertical accuracy in cm
Definition: gps.h:100
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:110
uint8_t num_sv
number of sat in fix
Definition: gps.h:104
uint8_t fix
status of fix
Definition: gps.h:105
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
int32_t lon
in degrees*1e7
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1067
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
struct State state
Definition: state.c:36
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:728
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
static float p[2][2]
void mavlink_mission_init(mavlink_mission_mgr *mgr)
void mavlink_mission_message_handler(const mavlink_message_t *msg)
void mavlink_mission_periodic(void)
update current block and send if changed
Common functions used within the mission library, blocks and waypoints cannot be send simultaneously ...
static uint32_t idx
uint8_t status
#define MAX_PPRZ
Definition: paparazzi.h:8
float alt
in meters (normally above WGS84 reference ellipsoid)
float hmsl
Height above mean sea level in meters.
struct LlaCoor_f lla
origin of local frame in LLA
Generic interface for radio control modules.
#define AP_MODE_HOVER_DIRECT
#define AP_MODE_RATE_DIRECT
#define AP_MODE_RATE_RC_CLIMB
#define AP_MODE_HOVER_CLIMB
#define AP_MODE_HOVER_Z_HOLD
#define AP_MODE_RC_DIRECT
#define AP_MODE_ATTITUDE_DIRECT
#define AP_MODE_ATTITUDE_CLIMB
#define AP_MODE_NAV
#define AP_MODE_GUIDED
#define AP_MODE_ATTITUDE_Z_HOLD
#define AP_MODE_ATTITUDE_RC_CLIMB
#define AP_MODE_RATE_Z_HOLD
#define AP_MODE_CARE_FREE_DIRECT
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
Definition: sonar_bebop.c:69
API to get/set the generic vehicle states.
#define TRUE
Definition: std.h:4
#define FALSE
Definition: std.h:5
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:74
Architecture independent timing functions.
struct target_t target
Definition: target_pos.c:64
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:51
Common tools for periodic telemetry interface Allows subsystem to register callback functions.
Periodic telemetry structure.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
int int32_t
Typedef defining 32 bit int type.
Definition: vl53l1_types.h:83
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
short int16_t
Typedef defining 16 bit short type.
Definition: vl53l1_types.h:93
unsigned long long uint64_t
Definition: vl53l1_types.h:72
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
signed char int8_t
Typedef defining 8 bit char type.
Definition: vl53l1_types.h:103
float heading
Definition: wedgebug.c:258