Paparazzi UAS  v5.8.2_stable-0-g6260b7c
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  mavlink_msg_param_value_send(MAVLINK_COMM_0,
269  mavlink_param_names[cmd.param_index],
270  settings_get_value(cmd.param_index),
271  MAV_PARAM_TYPE_REAL32,
272  NB_SETTING,
273  cmd.param_index);
275 
276  break;
277  }
278 
279  case MAVLINK_MSG_ID_PARAM_SET: {
280  mavlink_param_set_t set;
281  mavlink_msg_param_set_decode(msg, &set);
282 
283  // Check if this message is for this system
284  if ((uint8_t) set.target_system == AC_ID) {
285  int16_t idx = settings_idx_from_param_id(set.param_id);
286 
287  // setting found
288  if (idx >= 0) {
289  // Only write if new value is NOT "not-a-number"
290  // AND is NOT infinity
291  if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
292  !isnan(set.param_value) && !isinf(set.param_value)) {
293  DlSetting(idx, set.param_value);
294  // Report back new value
295  mavlink_msg_param_value_send(MAVLINK_COMM_0,
296  mavlink_param_names[idx],
297  settings_get_value(idx),
298  MAV_PARAM_TYPE_REAL32,
299  NB_SETTING,
300  idx);
302  }
303  }
304  }
305  }
306  break;
307 
308 #ifndef AP
309  /* only for rotorcraft */
310  case MAVLINK_MSG_ID_COMMAND_LONG: {
311  mavlink_command_long_t cmd;
312  mavlink_msg_command_long_decode(msg, &cmd);
313  // Check if this message is for this system
314  if ((uint8_t) cmd.target_system == AC_ID) {
315  uint8_t result = MAV_RESULT_UNSUPPORTED;
316  switch (cmd.command) {
317  case MAV_CMD_NAV_GUIDED_ENABLE:
318  MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1);
319  result = MAV_RESULT_FAILED;
320  if (cmd.param1 > 0.5) {
323  result = MAV_RESULT_ACCEPTED;
324  }
325  }
326  else {
327  // turn guided mode off - to what? maybe NAV? or MODE_AUTO2?
328  }
329  break;
330 
331  case MAV_CMD_COMPONENT_ARM_DISARM:
332  /* supposed to use this command to arm or SET_MODE?? */
333  MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1);
334  result = MAV_RESULT_FAILED;
335  if (cmd.param1 > 0.5) {
338  result = MAV_RESULT_ACCEPTED;
339  }
340  else {
342  if (!autopilot_motors_on)
343  result = MAV_RESULT_ACCEPTED;
344  }
345  break;
346 
347  default:
348  break;
349  }
350  // confirm command with result
351  mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result);
353  }
354  break;
355  }
356 
357  case MAVLINK_MSG_ID_SET_MODE: {
358  mavlink_set_mode_t mode;
359  mavlink_msg_set_mode_decode(msg, &mode);
360  if (mode.target_system == AC_ID) {
361  MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode);
362  if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
364  }
365  else {
367  }
368  if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
370  }
371  else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
373  }
374  }
375  break;
376  }
377 
378  case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: {
379  mavlink_set_position_target_local_ned_t target;
380  mavlink_msg_set_position_target_local_ned_decode(msg, &target);
381  // Check if this message is for this system
382  if (target.target_system == AC_ID) {
383  MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask);
384  /* only accept inputs where position and yaw bits are not set to ignored */
385  if (!(target.type_mask & 0b1110000000100000)) {
386  switch (target.coordinate_frame) {
387  case MAV_FRAME_LOCAL_NED:
388  MAVLINK_DEBUG("set position target, frame LOCAL_NED\n");
389  autopilot_guided_goto_ned(target.x, target.y, target.z, target.yaw);
390  break;
391  case MAV_FRAME_LOCAL_OFFSET_NED:
392  MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n");
393  autopilot_guided_goto_ned_relative(target.x, target.y, target.z, target.yaw);
394  break;
395  case MAV_FRAME_BODY_OFFSET_NED:
396  MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n");
397  autopilot_guided_goto_body_relative(target.x, target.y, target.z, target.yaw);
398  break;
399  default:
400  break;
401  }
402  }
403  }
404  break;
405  }
406 #endif
407 
408  default:
409  //Do nothing
410  MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid);
411  break;
412  }
413 }
414 
415 /* ignore the unused-parameter warnings */
416 #pragma GCC diagnostic push
417 #pragma GCC diagnostic ignored "-Wunused-parameter"
418 
422 static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev)
423 {
424  uint8_t mav_state = MAV_STATE_CALIBRATING;
425  uint8_t mav_mode = 0;
426 #ifdef AP
427  uint8_t mav_type = MAV_TYPE_FIXED_WING;
428  switch (pprz_mode) {
429  case PPRZ_MODE_MANUAL:
430  mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
431  break;
432  case PPRZ_MODE_AUTO1:
433  mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
434  break;
435  case PPRZ_MODE_AUTO2:
436  mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
437  break;
438  case PPRZ_MODE_HOME:
439  mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED;
440  break;
441  default:
442  break;
443  }
444 #else
445  uint8_t mav_type = MAV_TYPE_QUADROTOR;
446  switch (autopilot_mode) {
447  case AP_MODE_HOME:
448  mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
449  break;
450  case AP_MODE_RATE_DIRECT:
452  case AP_MODE_RATE_Z_HOLD:
453  case AP_MODE_RC_DIRECT:
454  mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
455  break;
461  case AP_MODE_HOVER_CLIMB:
464  mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
465  break;
466  case AP_MODE_NAV:
467  mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
468  break;
469  case AP_MODE_GUIDED:
470  mav_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
471  default:
472  break;
473  }
474 #endif
475  if (stateIsAttitudeValid()) {
476  if (kill_throttle) {
477  mav_state = MAV_STATE_STANDBY;
478  } else {
479  mav_state = MAV_STATE_ACTIVE;
480  mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
481  }
482  }
483  mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
484  mav_type,
485  MAV_AUTOPILOT_PPZ,
486  mav_mode,
487  0, // custom_mode
488  mav_state);
490 }
491 
495 static void mavlink_send_sys_status(struct transport_tx *trans, struct link_device *dev)
496 {
498 #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)
499 
500  mavlink_msg_sys_status_send(MAVLINK_COMM_0,
501  UAV_SENSORS, // On-board sensors: present (bitmap)
502  UAV_SENSORS, // On-board sensors: active (bitmap)
503  UAV_SENSORS, // On-board sensors: state (bitmap)
504  -1,//10*sys_mon.cpu_load, // System loadof main-loop time (0=0% to 1000=100%)
505  100 * electrical.vsupply, // Battery voltage (milivolts)
506  electrical.current / 10, // Battery current (10x miliampere)
507  -1, // Battery remaining (0-100 in %)
508  0, // Communication packet drops (0=0% to 10000=100%)
509  0, // Communication error(per packet) (0=0% to 10000=100%)
510  0, // Autopilot specific error 1
511  0, // Autopilot specific error 2
512  0, // Autopilot specific error 3
513  0); // Autopilot specific error 4
515 }
516 
522 static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev)
523 {
524  mavlink_msg_system_time_send(MAVLINK_COMM_0, 0, get_sys_time_msec());
526 }
527 
531 static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev)
532 {
533  mavlink_msg_attitude_send(MAVLINK_COMM_0,
535  stateGetNedToBodyEulers_f()->phi, // Phi
536  stateGetNedToBodyEulers_f()->theta, // Theta
537  stateGetNedToBodyEulers_f()->psi, // Psi
538  stateGetBodyRates_f()->p, // p
539  stateGetBodyRates_f()->q, // q
540  stateGetBodyRates_f()->r); // r
542 }
543 
544 static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev)
545 {
546  mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
551  stateGetSpeedNed_f()->x,
552  stateGetSpeedNed_f()->y,
553  stateGetSpeedNed_f()->z);
555 }
556 
558 {
559  float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
560  if (heading < 0.) {
561  heading += 360;
562  }
563  uint16_t compass_heading = heading * 100;
566  mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
568  stateGetPositionLla_i()->lat,
569  stateGetPositionLla_i()->lon,
570  stateGetPositionLla_i()->alt,
571  relative_alt,
572  stateGetSpeedNed_f()->x * 100,
573  stateGetSpeedNed_f()->y * 100,
574  stateGetSpeedNed_f()->z * 100,
575  compass_heading);
577 }
578 
579 static void mavlink_send_gps_global_origin(struct transport_tx *trans, struct link_device *dev)
580 {
581  if (state.ned_initialized_i) {
582  mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0,
587  }
588 }
589 
593 static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev)
594 {
595  if (mavlink_params_idx >= NB_SETTING) {
596  return;
597  }
598 
599  mavlink_msg_param_value_send(MAVLINK_COMM_0,
601  settings_get_value(mavlink_params_idx),
602  MAV_PARAM_TYPE_REAL32,
603  NB_SETTING,
604  mavlink_params_idx);
606 
607  mavlink_params_idx++;
608 }
609 
610 static void mavlink_send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
611 {
613  static uint32_t ver = PPRZ_VERSION_INT;
614  static uint64_t sha;
615  get_pprz_git_version((uint8_t *)&sha);
616  mavlink_msg_autopilot_version_send(MAVLINK_COMM_0,
617  0, //uint64_t capabilities,
618  ver, //uint32_t flight_sw_version,
619  0, //uint32_t middleware_sw_version,
620  0, //uint32_t os_sw_version,
621  0, //uint32_t board_version,
622  0, //const uint8_t *flight_custom_version,
623  0, //const uint8_t *middleware_custom_version,
624  0, //const uint8_t *os_custom_version,
625  0, //uint16_t vendor_id,
626  0, //uint16_t product_id,
627  sha //uint64_t uid
628  );
630 }
631 
633 {
634  mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
641  stateGetBodyRates_f()->q,
642  stateGetBodyRates_f()->r);
644 }
645 
646 #if USE_GPS
647 #include "subsystems/gps.h"
648 #endif
649 
650 static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev)
651 {
652 #if USE_GPS
653  int32_t course = DegOfRad(gps.course) / 1e5;
654  if (course < 0) {
655  course += 36000;
656  }
657  mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
659  gps.fix,
660  gps.lla_pos.lat,
661  gps.lla_pos.lon,
662  gps.lla_pos.alt,
663  gps.pdop,
664  UINT16_MAX, // VDOP
665  gps.gspeed,
666  course,
667  gps.num_sv);
669 #endif
670 }
671 
682 static void mavlink_send_gps_status(struct transport_tx *trans, struct link_device *dev)
683 {
684 #if USE_GPS
685  uint8_t nb_sats = Min(GPS_NB_CHANNELS, 20);
686  uint8_t prn[20];
687  uint8_t used[20];
688  uint8_t elevation[20];
689  uint8_t azimuth[20];
690  uint8_t snr[20];
691  for (uint8_t i = 0; i < nb_sats; i++) {
692  prn[i] = gps.svinfos[i].svid;
693  // set sat as used if cno > 0, not quite true though
694  if (gps.svinfos[i].cno > 0) {
695  used[i] = 1;
696  }
697  elevation[i] = gps.svinfos[i].elev;
698  // convert azimuth to unsigned representation: 0: 0 deg, 255: 360 deg.
699  uint8_t azim;
700  if (gps.svinfos[i].azim < 0) {
701  azim = (float)(-gps.svinfos[i].azim + 180) / 360 * 255;
702  } else {
703  azim = (float)gps.svinfos[i].azim / 360 * 255;
704  }
705  azimuth[i] = azim;
706  }
707  mavlink_msg_gps_status_send(MAVLINK_COMM_0,
708  gps.num_sv, prn, used, elevation, azimuth, snr);
710 #endif
711 }
712 
713 #if defined RADIO_CONTROL
715 // since they really want PPM values, use a hack to check if are using ppm subsystem
716 #ifdef PPM_PULSE_TYPE_POSITIVE
717 #define RC_CHANNELS RADIO_CTL_NB
718 #define PPM_PULSES(_i) ((_i) < RADIO_CTL_NB ? ppm_pulses[(_i)] : UINT16_MAX)
720 #else
721 // use radio_control.values for now...
722 #define RC_CHANNELS RADIO_CONTROL_NB_CHANNEL
723 #define PPM_OF_PPRZ(_v) ((_v) / 19.2 + 1500)
724 #define PPM_PULSES(_i) ((_i) < RADIO_CONTROL_NB_CHANNEL ? PPM_OF_PPRZ(radio_control.values[(_i)]) + MAX_PPRZ : UINT16_MAX)
725 #endif
726 #endif
727 
728 static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev)
729 {
730 #if defined RADIO_CONTROL
731  mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
733  RC_CHANNELS,
734  PPM_PULSES(0), PPM_PULSES(1), PPM_PULSES(2),
735  PPM_PULSES(3), PPM_PULSES(4), PPM_PULSES(5),
736  PPM_PULSES(6), PPM_PULSES(7), PPM_PULSES(8),
737  PPM_PULSES(9), PPM_PULSES(10), PPM_PULSES(11),
738  PPM_PULSES(12), PPM_PULSES(13), PPM_PULSES(14),
739  PPM_PULSES(15), PPM_PULSES(16), PPM_PULSES(17),
740  255); // rssi unknown
741 #else
742  mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
744  0, // zero channels available
745  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
746  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
747  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
748  UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
749  UINT16_MAX, UINT16_MAX, 255);
750 #endif
752 }
753 
754 #include "subsystems/electrical.h"
755 static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev)
756 {
757  static uint16_t voltages[10];
758  // we simply only set one cell for now
759  voltages[0] = electrical.vsupply * 100;
761  mavlink_msg_battery_status_send(MAVLINK_COMM_0,
762  0, // id
763  0, // battery_function
764  0, // type
765  INT16_MAX, // unknown temperature
766  voltages, // cell voltages
767  electrical.current / 10,
769  electrical.energy, // check scaling
770  -1); // remaining percentage not estimated
772 }
773 
774 #include "subsystems/commands.h"
778 static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev)
779 {
780  /* Current heading in degrees, in compass units (0..360, 0=north) */
781  int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
782  /* Current throttle setting in integer percent, 0 to 100 */
783  // is a 16bit unsigned int but supposed to be from 0 to 100??
784  uint16_t throttle;
785 #ifdef COMMAND_THRUST
786  throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100);
787 #elif defined COMMAND_THROTTLE
788  throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);
789 #endif
790  mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
792  stateGetHorizontalSpeedNorm_f(), // groundspeed
793  heading,
794  throttle,
795  stateGetPositionLla_f()->alt, // altitude, FIXME: should be MSL
796  stateGetSpeedNed_f()->z); // climb rate
798 }
799 
800 /* end ignore unused-paramter */
801 #pragma GCC diagnostic pop
static uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Definition: sys_time_arch.h:49
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:87
int16_t azim
azimuth in deg
Definition: gps.h:63
#define AP_MODE_ATTITUDE_DIRECT
Definition: autopilot.h:40
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition: state.h:912
#define Min(x, y)
Generic transmission transport header.
Definition: transport.h:89
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition: state.h:1114
uint8_t autopilot_mode
Definition: autopilot.c:65
void mavlink_mission_message_handler(const mavlink_message_t *msg)
bool_t autopilot_motors_on
Definition: autopilot.c:72
#define AP_MODE_HOVER_DIRECT
Definition: autopilot.h:46
static bool_t stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition: state.h:1038
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
Definition: state.h:216
uint8_t pprz_mode
Definition: autopilot.c:41
Periodic telemetry structure.
#define AP_MODE_RATE_RC_CLIMB
Definition: autopilot.h:41
#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:1310
#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:711
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:58
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:62
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition: state.h:1102
#define FALSE
Definition: std.h:5
int32_t alt
in millimeters above WGS84 reference ellipsoid
#define AP_MODE_RATE_DIRECT
Definition: autopilot.h:39
static uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
Definition: sys_time_arch.h:39
Common functions used within the mission library, blocks and waypoints cannot be send simultaneously ...
bool_t autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
Definition: autopilot.c:516
#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
void autopilot_set_motors_on(bool_t motors_on)
Definition: autopilot.c:587
bool_t autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
Definition: autopilot.c:527
#define AP_MODE_CARE_FREE_DIRECT
Definition: autopilot.h:51
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
Definition: gps.h:61
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition: state.h:885
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
Device independent GPS code (interface)
uint16_t pdop
position dilution of precision scaled by 100
Definition: gps.h:80
unsigned long uint32_t
Definition: types.h:18
signed short int16_t
Definition: types.h:17
struct LlaCoor_i lla
Reference point in lla.
bool_t kill_throttle
Definition: autopilot.c:42
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
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition: state.h:1171
signed long int32_t
Definition: types.h:19
static const struct usb_device_descriptor dev
Definition: usb_ser_hw.c:69
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
Definition: state.h:162
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:76
uint16_t vsupply
supply voltage in decivolts
Definition: electrical.h:48
#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:81
#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:74
float energy
consumed energy in mAh
Definition: electrical.h:51
#define AP_MODE_HOME
Definition: autopilot.h:38
bool_t autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
Definition: autopilot.c:539
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:69
#define MAX_PPRZ
Definition: paparazzi.h:8
bool_t ned_initialized_i
true if local int coordinate frame is initialsed
Definition: state.h:167
signed char int8_t
Definition: types.h:15
struct FloatVect2 target
#define PPRZ_MODE_MANUAL
AP modes.
Definition: autopilot.h:50
int32_t lat
in degrees*1e7
uint8_t fix
status of fix
Definition: gps.h:82
void autopilot_set_mode(uint8_t new_autopilot_mode)
Definition: autopilot.c:395
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition: state.h:693
#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:41
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition: state.h:666
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