Paparazzi UAS  v5.14.0_stable-0-g3f680d1
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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 "-Wswitch-default"
32 #include "mavlink/paparazzi/mavlink.h"
33 #pragma GCC diagnostic pop
34 
35 #if PERIODIC_TELEMETRY
36 #define PERIODIC_C_MAVLINK
38 #include "generated/periodic_telemetry.h"
39 #ifndef TELEMETRY_MAVLINK_NB_MSG
40 #warning Using hardcoded msg periods. To customize specify a <process name="Mavlink" type="mavlink"> in your telemetry file.
41 #endif
42 #endif
43 
44 #include "generated/airframe.h"
45 #include "generated/modules.h"
46 #include "generated/settings.h"
47 
48 #include "mcu_periph/sys_time.h"
49 #include "subsystems/electrical.h"
50 #include "state.h"
51 #include "pprz_version.h"
52 #include "autopilot.h"
53 #include "autopilot_guided.h"
54 
55 #if defined RADIO_CONTROL
57 #endif
58 
60 
61 // for UINT16_MAX
62 #include <stdint.h>
63 
64 mavlink_system_t mavlink_system;
65 
66 static uint8_t mavlink_params_idx = NB_SETTING;
70 static char mavlink_param_names[NB_SETTING][16 + 1] = SETTINGS_NAMES_SHORT;
74 
75 void mavlink_common_message_handler(const mavlink_message_t *msg);
76 
77 static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev);
78 static void mavlink_send_sys_status(struct transport_tx *trans, struct link_device *dev);
79 static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev);
80 static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev);
81 static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev);
82 static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev);
83 static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev);
84 static void mavlink_send_autopilot_version(struct transport_tx *trans, struct link_device *dev);
85 static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev);
86 static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev);
87 static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev);
88 static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev);
89 static void mavlink_send_gps_global_origin(struct transport_tx *trans, struct link_device *dev);
90 static void mavlink_send_gps_status(struct transport_tx *trans, struct link_device *dev);
91 static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev);
92 
93 
98 #ifndef MAVLINK_SYSID
99 #define MAVLINK_SYSID AC_ID
100 #endif
101 
102 #if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
103 struct telemetry_cb_slots mavlink_cbs[TELEMETRY_MAVLINK_NB_MSG] = TELEMETRY_MAVLINK_CBS;
104 struct periodic_telemetry mavlink_telemetry = { TELEMETRY_MAVLINK_NB_MSG, mavlink_cbs };
105 #endif
106 
107 void mavlink_init(void)
108 {
109  mavlink_system.sysid = MAVLINK_SYSID; // System ID, 1-255
110  mavlink_system.compid = MAV_COMP_ID_MISSIONPLANNER; // Component/Subsystem ID, 1-255
111 
112  get_pprz_git_version(custom_version);
113 
114  mavlink_mission_init(&mission_mgr);
115 
116 #if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
117  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_HEARTBEAT, mavlink_send_heartbeat);
118  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_SYS_STATUS, mavlink_send_sys_status);
119  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_SYSTEM_TIME, mavlink_send_system_time);
120  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_ATTITUDE, mavlink_send_attitude);
121  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, mavlink_send_attitude_quaternion);
122  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_LOCAL_POSITION_NED, mavlink_send_local_position_ned);
123  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, mavlink_send_global_position_int);
124  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GPS_RAW_INT, mavlink_send_gps_raw_int);
125  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_RC_CHANNELS, mavlink_send_rc_channels);
126  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_BATTERY_STATUS, mavlink_send_battery_status);
127  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_AUTOPILOT_VERSION, mavlink_send_autopilot_version);
128  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, mavlink_send_gps_global_origin);
129  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GPS_STATUS, mavlink_send_gps_status);
130  register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_VFR_HUD, mavlink_send_vfr_hud);
131 #endif
132 }
133 
139 {
140 #if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
141  // send periodic mavlink messages as defined in the Mavlink process of the telemetry xml file
142  // transport and device not used here yet...
143  periodic_telemetry_send_Mavlink(&mavlink_telemetry, NULL, NULL);
144 #endif
145 }
146 
152 {
153 #if !defined (TELEMETRY_MAVLINK_NB_MSG)
154  // use these hardcoded periods if no Mavlink process in telemetry file
155  RunOnceEvery(2, mavlink_send_heartbeat(NULL, NULL));
156  RunOnceEvery(5, mavlink_send_sys_status(NULL, NULL));
157  RunOnceEvery(20, mavlink_send_system_time(NULL, NULL));
158  RunOnceEvery(10, mavlink_send_attitude(NULL, NULL));
159  RunOnceEvery(5, mavlink_send_attitude_quaternion(NULL, NULL));
160  RunOnceEvery(4, mavlink_send_local_position_ned(NULL, NULL));
161  RunOnceEvery(5, mavlink_send_global_position_int(NULL, NULL));
162  RunOnceEvery(6, mavlink_send_gps_raw_int(NULL, NULL));
163  RunOnceEvery(5, mavlink_send_rc_channels(NULL, NULL));
164  RunOnceEvery(21, mavlink_send_battery_status(NULL, NULL));
165  RunOnceEvery(32, mavlink_send_autopilot_version(NULL, NULL));
166  RunOnceEvery(33, mavlink_send_gps_global_origin(NULL, NULL));
167  RunOnceEvery(10, mavlink_send_gps_status(NULL, NULL));
168  RunOnceEvery(10, mavlink_send_vfr_hud(NULL, NULL));
169 #endif
170 
171  // if requested, send params at 5Hz until all sent
172  RunOnceEvery(2, mavlink_send_params(NULL, NULL));
173 
175 }
176 
177 static int16_t settings_idx_from_param_id(char *param_id)
178 {
179  int i, j;
180  int16_t settings_idx = -1;
181 
182  // Go trough all the settings to search the ID
183  for (i = 0; i < NB_SETTING; i++) {
184  for (j = 0; j < 16; j++) {
185  if (mavlink_param_names[i][j] != param_id[j]) {
186  break;
187  }
188 
189  if (mavlink_param_names[i][j] == '\0') {
190  settings_idx = i;
191  return settings_idx;
192  }
193  }
194 
195  if (mavlink_param_names[i][j] == '\0') {
196  break;
197  }
198  }
199  return settings_idx;
200 }
201 
205 void mavlink_event(void)
206 {
207  mavlink_message_t msg;
208  mavlink_status_t status;
209 
210  // Check uplink
211  while (MAVLinkChAvailable()) {
212  char test = MAVLinkGetch();
213 
214  // When we receive a message
215  if (mavlink_parse_char(MAVLINK_COMM_0, test, &msg, &status)) {
218  }
219  }
220 }
221 
222 void mavlink_common_message_handler(const mavlink_message_t *msg)
223 {
224  switch (msg->msgid) {
225  case MAVLINK_MSG_ID_HEARTBEAT:
226  break;
227 
228  /* When requesting data streams say we can't send them */
229  case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
230  mavlink_request_data_stream_t cmd;
231  mavlink_msg_request_data_stream_decode(msg, &cmd);
232 
233  mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0);
235  break;
236  }
237 
238  /* Override channels with RC */
239  case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: {
240  mavlink_rc_channels_override_t cmd;
241  mavlink_msg_rc_channels_override_decode(msg, &cmd);
242 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
243  uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100;
244  int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2;
245  int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2;
246  int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100;
247  parse_rc_4ch_datalink(0, thrust, roll, pitch, yaw);
248  //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n",
249  // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw);
250 #endif
251  break;
252  }
253 
254  /* When a request is made of the parameters list */
255  case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
256  mavlink_params_idx = 0;
257  break;
258  }
259 
260  /* When a request os made for a specific parameter */
261  case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
262  mavlink_param_request_read_t cmd;
263  mavlink_msg_param_request_read_decode(msg, &cmd);
264 
265  // First check param_index and search for the ID if needed
266  if (cmd.param_index == -1) {
267  cmd.param_index = settings_idx_from_param_id(cmd.param_id);
268  }
269 
270  // Send message only if the param_index was found (Coverity Scan)
271  if (cmd.param_index > -1) {
272  mavlink_msg_param_value_send(MAVLINK_COMM_0,
273  mavlink_param_names[cmd.param_index],
274  settings_get_value(cmd.param_index),
275  MAV_PARAM_TYPE_REAL32,
276  NB_SETTING,
277  cmd.param_index);
279  }
280  break;
281  }
282 
283  case MAVLINK_MSG_ID_PARAM_SET: {
284  mavlink_param_set_t set;
285  mavlink_msg_param_set_decode(msg, &set);
286 
287  // Check if this message is for this system
288  if ((uint8_t) set.target_system == AC_ID) {
289  int16_t idx = settings_idx_from_param_id(set.param_id);
290 
291  // setting found
292  if (idx >= 0) {
293  // Only write if new value is NOT "not-a-number"
294  // AND is NOT infinity
295  if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
296  !isnan(set.param_value) && !isinf(set.param_value)) {
297  DlSetting(idx, set.param_value);
298  // Report back new value
299  mavlink_msg_param_value_send(MAVLINK_COMM_0,
300  mavlink_param_names[idx],
301  settings_get_value(idx),
302  MAV_PARAM_TYPE_REAL32,
303  NB_SETTING,
304  idx);
306  }
307  }
308  }
309  }
310  break;
311 
312 #ifndef AP
313  /* only for rotorcraft */
314  case MAVLINK_MSG_ID_COMMAND_LONG: {
315  mavlink_command_long_t cmd;
316  mavlink_msg_command_long_decode(msg, &cmd);
317  // Check if this message is for this system
318  if ((uint8_t) cmd.target_system == AC_ID) {
319  uint8_t result = MAV_RESULT_UNSUPPORTED;
320  switch (cmd.command) {
321  case MAV_CMD_NAV_GUIDED_ENABLE:
322  MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1);
323  result = MAV_RESULT_FAILED;
324  if (cmd.param1 > 0.5) {
327  result = MAV_RESULT_ACCEPTED;
328  }
329  }
330  else {
331  // turn guided mode off - to what? maybe NAV? or MODE_AUTO2?
332  }
333  break;
334 
335  case MAV_CMD_COMPONENT_ARM_DISARM:
336  /* supposed to use this command to arm or SET_MODE?? */
337  MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1);
338  result = MAV_RESULT_FAILED;
339  if (cmd.param1 > 0.5) {
342  result = MAV_RESULT_ACCEPTED;
343  }
344  else {
347  result = MAV_RESULT_ACCEPTED;
348  }
349  break;
350 
351  default:
352  break;
353  }
354  // confirm command with result
355  mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result);
357  }
358  break;
359  }
360 
361  case MAVLINK_MSG_ID_SET_MODE: {
362  mavlink_set_mode_t mode;
363  mavlink_msg_set_mode_decode(msg, &mode);
364  if (mode.target_system == AC_ID) {
365  MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode);
366  if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
368  }
369  else {
371  }
372  if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
374  }
375  else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
377  }
378  }
379  break;
380  }
381 
382  case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: {
383  mavlink_set_position_target_local_ned_t target;
384  mavlink_msg_set_position_target_local_ned_decode(msg, &target);
385  // Check if this message is for this system
386  if (target.target_system == AC_ID) {
387  MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask);
388  /* if position and yaw bits are not set to ignored, use only position for now */
389  if (!(target.type_mask & 0b1110000000100000)) {
390  switch (target.coordinate_frame) {
391  case MAV_FRAME_LOCAL_NED:
392  MAVLINK_DEBUG("set position target, frame LOCAL_NED\n");
393  autopilot_guided_goto_ned(target.x, target.y, target.z, target.yaw);
394  break;
395  case MAV_FRAME_LOCAL_OFFSET_NED:
396  MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n");
397  autopilot_guided_goto_ned_relative(target.x, target.y, target.z, target.yaw);
398  break;
399  case MAV_FRAME_BODY_OFFSET_NED:
400  MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n");
401  autopilot_guided_goto_body_relative(target.x, target.y, target.z, target.yaw);
402  break;
403  default:
404  break;
405  }
406  }
407  else if (!(target.type_mask & 0b0001110000100000)) {
408  /* position is set to ignore, but velocity not */
409  switch (target.coordinate_frame) {
410  case MAV_FRAME_LOCAL_NED:
411  MAVLINK_DEBUG("set velocity target, frame LOCAL_NED\n");
412  autopilot_guided_move_ned(target.vx, target.vy, target.vz, target.yaw);
413  break;
414  default:
415  break;
416  }
417  }
418  }
419  break;
420  }
421 #endif
422 
423  default:
424  //Do nothing
425  MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid);
426  break;
427  }
428 }
429 
430 /* ignore the unused-parameter warnings */
431 #pragma GCC diagnostic push
432 #pragma GCC diagnostic ignored "-Wunused-parameter"
433 
437 static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev)
438 {
439  uint8_t mav_state = MAV_STATE_CALIBRATING;
440  uint8_t mav_mode = 0;
441 #ifdef AP
442  uint8_t mav_type = MAV_TYPE_FIXED_WING;
443  switch (autopilot_get_mode()) {
444  case AP_MODE_MANUAL:
445  mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
446  break;
447  case AP_MODE_AUTO1:
448  mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
449  break;
450  case AP_MODE_AUTO2:
451  mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
452  break;
453  case AP_MODE_HOME:
454  mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED;
455  break;
456  default:
457  break;
458  }
459 #else
460  uint8_t mav_type = MAV_TYPE_QUADROTOR;
461  switch (autopilot_get_mode()) {
462  case AP_MODE_HOME:
463  mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
464  break;
465  case AP_MODE_RATE_DIRECT:
467  case AP_MODE_RATE_Z_HOLD:
468  case AP_MODE_RC_DIRECT:
469  mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
470  break;
476  case AP_MODE_HOVER_CLIMB:
479  mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
480  break;
481  case AP_MODE_NAV:
482  mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
483  break;
484  case AP_MODE_GUIDED:
485  mav_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
486  default:
487  break;
488  }
489 #endif
490  if (stateIsAttitudeValid()) {
492  mav_state = MAV_STATE_STANDBY;
493  } else {
494  mav_state = MAV_STATE_ACTIVE;
495  mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
496  }
497  }
498  mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
499  mav_type,
500  MAV_AUTOPILOT_PPZ,
501  mav_mode,
502  0, // custom_mode
503  mav_state);
505 }
506 
510 static void mavlink_send_sys_status(struct transport_tx *trans, struct link_device *dev)
511 {
513 #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)
514 
515  mavlink_msg_sys_status_send(MAVLINK_COMM_0,
516  UAV_SENSORS, // On-board sensors: present (bitmap)
517  UAV_SENSORS, // On-board sensors: active (bitmap)
518  UAV_SENSORS, // On-board sensors: state (bitmap)
519  -1,//10*sys_mon.cpu_load, // System loadof main-loop time (0=0% to 1000=100%)
520  100 * electrical.vsupply, // Battery voltage (milivolts)
521  electrical.current / 10, // Battery current (10x miliampere)
522  -1, // Battery remaining (0-100 in %)
523  0, // Communication packet drops (0=0% to 10000=100%)
524  0, // Communication error(per packet) (0=0% to 10000=100%)
525  0, // Autopilot specific error 1
526  0, // Autopilot specific error 2
527  0, // Autopilot specific error 3
528  0); // Autopilot specific error 4
530 }
531 
537 static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev)
538 {
539  mavlink_msg_system_time_send(MAVLINK_COMM_0, 0, get_sys_time_msec());
541 }
542 
546 static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev)
547 {
548  mavlink_msg_attitude_send(MAVLINK_COMM_0,
550  stateGetNedToBodyEulers_f()->phi, // Phi
551  stateGetNedToBodyEulers_f()->theta, // Theta
552  stateGetNedToBodyEulers_f()->psi, // Psi
553  stateGetBodyRates_f()->p, // p
554  stateGetBodyRates_f()->q, // q
555  stateGetBodyRates_f()->r); // r
557 }
558 
559 static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev)
560 {
561  mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
566  stateGetSpeedNed_f()->x,
567  stateGetSpeedNed_f()->y,
568  stateGetSpeedNed_f()->z);
570 }
571 
572 static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev)
573 {
574  float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
575  if (heading < 0.) {
576  heading += 360;
577  }
578  uint16_t compass_heading = heading * 100;
581  mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
583  stateGetPositionLla_i()->lat,
584  stateGetPositionLla_i()->lon,
585  stateGetPositionLla_i()->alt,
586  relative_alt,
587  stateGetSpeedNed_f()->x * 100,
588  stateGetSpeedNed_f()->y * 100,
589  stateGetSpeedNed_f()->z * 100,
590  compass_heading);
592 }
593 
594 static void mavlink_send_gps_global_origin(struct transport_tx *trans, struct link_device *dev)
595 {
596  if (state.ned_initialized_i) {
597  mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0,
602  }
603 }
604 
608 static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev)
609 {
610  if (mavlink_params_idx >= NB_SETTING) {
611  return;
612  }
613 
614  mavlink_msg_param_value_send(MAVLINK_COMM_0,
616  settings_get_value(mavlink_params_idx),
617  MAV_PARAM_TYPE_REAL32,
618  NB_SETTING,
619  mavlink_params_idx);
621 
622  mavlink_params_idx++;
623 }
624 
625 static void mavlink_send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
626 {
628  static uint32_t ver = PPRZ_VERSION_INT;
629  static uint64_t sha;
630  get_pprz_git_version((uint8_t *)&sha);
631  mavlink_msg_autopilot_version_send(MAVLINK_COMM_0,
632  0, //uint64_t capabilities,
633  ver, //uint32_t flight_sw_version,
634  0, //uint32_t middleware_sw_version,
635  0, //uint32_t os_sw_version,
636  0, //uint32_t board_version,
637  0, //const uint8_t *flight_custom_version,
638  0, //const uint8_t *middleware_custom_version,
639  0, //const uint8_t *os_custom_version,
640  0, //uint16_t vendor_id,
641  0, //uint16_t product_id,
642  sha //uint64_t uid
643  );
645 }
646 
647 static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev)
648 {
649  mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
656  stateGetBodyRates_f()->q,
657  stateGetBodyRates_f()->r);
659 }
660 
661 #if USE_GPS
662 #include "subsystems/gps.h"
663 #endif
664 
665 static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev)
666 {
667 #if USE_GPS
668  int32_t course = DegOfRad(gps.course) / 1e5;
669  if (course < 0) {
670  course += 36000;
671  }
672  mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
674  gps.fix,
675  gps.lla_pos.lat,
676  gps.lla_pos.lon,
677  gps.lla_pos.alt,
678  gps.pdop,
679  UINT16_MAX, // VDOP
680  gps.gspeed,
681  course,
682  gps.num_sv);
684 #endif
685 }
686 
697 static void mavlink_send_gps_status(struct transport_tx *trans, struct link_device *dev)
698 {
699 #if USE_GPS
700  uint8_t nb_sats = Min(GPS_NB_CHANNELS, 20);
701  uint8_t prn[20];
702  uint8_t used[20];
703  uint8_t elevation[20];
704  uint8_t azimuth[20];
705  uint8_t snr[20];
706  for (uint8_t i = 0; i < nb_sats; i++) {
707  prn[i] = gps.svinfos[i].svid;
708  // set sat as used if cno > 0, not quite true though
709  if (gps.svinfos[i].cno > 0) {
710  used[i] = 1;
711  }
712  elevation[i] = gps.svinfos[i].elev;
713  // convert azimuth to unsigned representation: 0: 0 deg, 255: 360 deg.
714  uint8_t azim;
715  if (gps.svinfos[i].azim < 0) {
716  azim = (float)(-gps.svinfos[i].azim + 180) / 360 * 255;
717  } else {
718  azim = (float)gps.svinfos[i].azim / 360 * 255;
719  }
720  azimuth[i] = azim;
721  }
722  mavlink_msg_gps_status_send(MAVLINK_COMM_0,
723  gps.num_sv, prn, used, elevation, azimuth, snr);
725 #endif
726 }
727 
728 #if defined RADIO_CONTROL
730 // since they really want PPM values, use a hack to check if are using ppm subsystem
731 #ifdef PPM_PULSE_TYPE_POSITIVE
732 #define RC_CHANNELS RADIO_CTL_NB
733 #define PPM_PULSES(_i) ((_i) < RADIO_CTL_NB ? ppm_pulses[(_i)] : UINT16_MAX)
735 #else
736 // use radio_control.values for now...
737 #define RC_CHANNELS RADIO_CONTROL_NB_CHANNEL
738 #define PPM_OF_PPRZ(_v) ((_v) / 19.2 + 1500)
739 #define PPM_PULSES(_i) ((_i) < RADIO_CONTROL_NB_CHANNEL ? PPM_OF_PPRZ(radio_control.values[(_i)]) + MAX_PPRZ : UINT16_MAX)
740 #endif
741 #endif
742 
743 static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev)
744 {
745 #if defined RADIO_CONTROL
746  mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
748  RC_CHANNELS,
749  PPM_PULSES(0), PPM_PULSES(1), PPM_PULSES(2),
750  PPM_PULSES(3), PPM_PULSES(4), PPM_PULSES(5),
751  PPM_PULSES(6), PPM_PULSES(7), PPM_PULSES(8),
752  PPM_PULSES(9), PPM_PULSES(10), PPM_PULSES(11),
753  PPM_PULSES(12), PPM_PULSES(13), PPM_PULSES(14),
754  PPM_PULSES(15), PPM_PULSES(16), PPM_PULSES(17),
755  255); // rssi unknown
756 #else
757  mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
759  0, // zero channels available
760  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
761  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
762  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
763  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
764  UINT16_MAX, UINT16_MAX, 255);
765 #endif
767 }
768 
769 #include "subsystems/electrical.h"
770 static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev)
771 {
772  static uint16_t voltages[10];
773  // we simply only set one cell for now
774  voltages[0] = electrical.vsupply * 100;
776  mavlink_msg_battery_status_send(MAVLINK_COMM_0,
777  0, // id
778  0, // battery_function
779  0, // type
780  INT16_MAX, // unknown temperature
781  voltages, // cell voltages
782  electrical.current / 10,
784  electrical.energy, // check scaling
785  -1); // remaining percentage not estimated
787 }
788 
789 #include "subsystems/commands.h"
793 static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev)
794 {
795  /* Current heading in degrees, in compass units (0..360, 0=north) */
796  int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
797  /* Current throttle setting in integer percent, 0 to 100 */
798  // is a 16bit unsigned int but supposed to be from 0 to 100??
799  uint16_t throttle;
800 #ifdef COMMAND_THRUST
801  throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100);
802 #elif defined COMMAND_THROTTLE
803  throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);
804 #endif
805  mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
807  stateGetHorizontalSpeedNorm_f(), // groundspeed
808  heading,
809  throttle,
810  stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL
811  stateGetSpeedNed_f()->z); // climb rate
813 }
814 
815 /* end ignore unused-paramter */
816 #pragma GCC diagnostic pop
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
int32_t current
current in milliamps
Definition: electrical.h:49
unsigned short uint16_t
Definition: types.h:16
#define AP_MODE_AUTO1
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
Definition: gps.h:104
static uint32_t idx
int16_t azim
azimuth in deg
Definition: gps.h:77
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:935
#define AP_MODE_RATE_DIRECT
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1143
void mavlink_mission_message_handler(const mavlink_message_t *msg)
#define AP_MODE_HOVER_DIRECT
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.c:78
#define AP_MODE_RATE_Z_HOLD
bool autopilot_throttle_killed(void)
get kill status
Definition: autopilot.c:229
bool autopilot_get_motors_on(void)
get motors status
Definition: autopilot.c:211
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:220
Periodic telemetry structure.
#define GPS_NB_CHANNELS
Definition: gps.h:57
void mavlink_mission_periodic(void)
update current block and send if changed
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition: state.h:1407
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition: state.h:728
#define AP_MODE_RATE_RC_CLIMB
void mavlink_mission_init(mavlink_mission_mgr *mgr)
Autopilot guided mode interface.
#define AP_MODE_HOME
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition: autopilot.c:160
uint8_t svid
Satellite ID.
Definition: gps.h:72
Common tools for periodic telemetry interface Allows subsystem to register callback functions...
int32_t hmsl
Height above mean sea level in mm.
int8_t elev
elevation in deg
Definition: gps.h:76
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1131
#define FALSE
Definition: std.h:5
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define AP_MODE_ATTITUDE_RC_CLIMB
Common functions used within the mission library, blocks and waypoints cannot be send simultaneously ...
#define AP_MODE_MANUAL
AP modes.
#define TRUE
Definition: std.h:4
#define AP_MODE_ATTITUDE_CLIMB
unsigned long long uint64_t
Definition: types.h:20
static float heading
Definition: ahrs_infrared.c:45
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:75
#define AP_MODE_AUTO2
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1067
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:908
Interface for electrical status: supply voltage, current, battery status, etc.
float hmsl
Height above mean sea level in meters.
Architecture independent timing functions.
Device independent GPS code (interface)
#define AP_MODE_HOVER_Z_HOLD
uint8_t status
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:97
unsigned long uint32_t
Definition: types.h:18
#define AP_MODE_GUIDED
signed short int16_t
Definition: types.h:17
struct LlaCoor_i lla
Reference point in lla.
int32_t lon
in degrees*1e7
#define AP_MODE_RC_DIRECT
Hardware independent code for commands handling.
#define Min(x, y)
Definition: main_fbw.c:52
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1200
signed long int32_t
Definition: types.h:19
bool ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:171
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:73
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:166
Core autopilot interface common to all firmwares.
#define AP_MODE_NAV
unsigned char uint8_t
Definition: types.h:14
pprz_t commands[COMMANDS_NB]
Storage of intermediate command values.
Definition: commands.c:30
API to get/set the generic vehicle states.
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
Definition: gps.h:93
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
#define AP_MODE_HOVER_CLIMB
uint16_t vsupply
supply voltage in decivolts
Definition: electrical.h:48
static int16_t course[3]
Definition: airspeed_uADC.c:57
void 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:199
int32_t consumed
consumption in mAh
Definition: electrical.h:50
uint8_t num_sv
number of sat in fix
Definition: gps.h:98
static float p[2][2]
uint16_t gspeed
norm of 2d ground speed in cm/s
Definition: gps.h:91
float energy
consumed energy in mAh
Definition: electrical.h:51
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
Set velocity and heading setpoints in GUIDED mode.
struct Electrical electrical
Definition: electrical.c:65
#define AP_MODE_ATTITUDE_DIRECT
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
Definition: gps.h:86
#define MAX_PPRZ
Definition: paparazzi.h:8
signed char int8_t
Definition: types.h:15
struct FloatVect2 target
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.c:68
#define AP_MODE_CARE_FREE_DIRECT
int32_t lat
in degrees*1e7
uint8_t fix
status of fix
Definition: gps.h:99
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:710
#define AP_MODE_ATTITUDE_Z_HOLD
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
struct GpsState gps
global GPS state
Definition: gps.c:75
bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:683
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
struct State state
Definition: state.c:36
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition: telemetry.c:46
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition: autopilot.c:183