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