Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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
26
28
29// include mavlink headers, but ignore some warnings
30#pragma GCC diagnostic push
31#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
32#pragma GCC diagnostic ignored "-Wswitch-default"
33#pragma GCC diagnostic ignored "-Wuninitialized"
34#include "mavlink/ardupilotmega/mavlink.h"
35#pragma GCC diagnostic pop
36
37#if PERIODIC_TELEMETRY
38#define PERIODIC_C_MAVLINK
40#include "generated/periodic_telemetry.h"
41#ifndef TELEMETRY_MAVLINK_NB_MSG
42#warning Using hardcoded msg periods. To customize specify a <process name="Mavlink" type="mavlink"> in your telemetry file.
43#endif
44#endif
45
46#include "generated/airframe.h"
47#include "generated/modules.h"
48#include "generated/settings.h"
49#include "generated/flight_plan.h"
50
51#include "mcu.h"
52#include "mcu_periph/sys_time.h"
54#include "state.h"
55#include "pprz_version.h"
56#include "autopilot.h"
57#include "autopilot_guided.h"
58
59#if defined RADIO_CONTROL
61#endif
62
63// Change the autopilot identification code: by default identify as PPZ autopilot: alternatively as MAV_AUTOPILOT_ARDUPILOTMEGA
64#ifndef MAV_AUTOPILOT_ID
65#define MAV_AUTOPILOT_ID MAV_AUTOPILOT_PPZ
66#endif
67
69
70// for UINT16_MAX
71#include <stdint.h>
72#include <string.h>
73
74mavlink_system_t mavlink_system;
75
76static uint8_t mavlink_params_idx = NB_SETTING;
80static char mavlink_param_names[NB_SETTING][16 + 1] = SETTINGS_NAMES_SHORT;
82
84
85void mavlink_common_message_handler(const mavlink_message_t *msg);
86
87static void mavlink_send_extended_sys_state(struct transport_tx *trans, struct link_device *dev);
88static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev);
89static void mavlink_send_sys_status(struct transport_tx *trans, struct link_device *dev);
90static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev);
91static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev);
92static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev);
93static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev);
94static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev);
95static void mavlink_send_autopilot_version(struct transport_tx *trans, struct link_device *dev);
96static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev);
97static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev);
98static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev);
99static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev);
100static void mavlink_send_gps_global_origin(struct transport_tx *trans, struct link_device *dev);
101static void mavlink_send_gps_status(struct transport_tx *trans, struct link_device *dev);
102static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev);
103
104
108
109#ifndef MAVLINK_SYSID
110#define MAVLINK_SYSID AC_ID
111#endif
112
113#if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
114struct telemetry_cb_slots mavlink_cbs[TELEMETRY_MAVLINK_NB_MSG] = TELEMETRY_MAVLINK_CBS;
115struct periodic_telemetry mavlink_telemetry = { TELEMETRY_MAVLINK_NB_MSG, mavlink_cbs };
116#endif
117
118void mavlink_init(void)
119{
120 mavlink_system.sysid = MAVLINK_SYSID; // System ID, 1-255
121 mavlink_system.compid = MAV_COMP_ID_AUTOPILOT1; // Component/Subsystem ID, 1-255
122
123 get_pprz_git_version(custom_version);
124
126
127#if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
128 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_HEARTBEAT, mavlink_send_heartbeat);
129 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_SYS_STATUS, mavlink_send_sys_status);
130 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_EXTENDED_SYS_STATE, mavlink_send_extended_sys_state);
131 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_SYSTEM_TIME, mavlink_send_system_time);
132 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_ATTITUDE, mavlink_send_attitude);
133 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, mavlink_send_attitude_quaternion);
134 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_LOCAL_POSITION_NED, mavlink_send_local_position_ned);
135 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GLOBAL_POSITION_INT, mavlink_send_global_position_int);
136 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GPS_RAW_INT, mavlink_send_gps_raw_int);
137 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_RC_CHANNELS, mavlink_send_rc_channels);
138 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_BATTERY_STATUS, mavlink_send_battery_status);
139 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_AUTOPILOT_VERSION, mavlink_send_autopilot_version);
140 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, mavlink_send_gps_global_origin);
141 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_GPS_STATUS, mavlink_send_gps_status);
142 register_periodic_telemetry(&mavlink_telemetry, MAVLINK_MSG_ID_VFR_HUD, mavlink_send_vfr_hud);
143#endif
144}
145
151{
152#if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
153 // send periodic mavlink messages as defined in the Mavlink process of the telemetry xml file
154 // transport and device not used here yet...
155 periodic_telemetry_send_Mavlink(&mavlink_telemetry, NULL, NULL);
156#endif
157}
158
164{
165#if !defined (TELEMETRY_MAVLINK_NB_MSG)
166 // use these hardcoded periods if no Mavlink process in telemetry file
167 RunOnceEvery(2, mavlink_send_heartbeat(NULL, NULL));
168 RunOnceEvery(5, mavlink_send_sys_status(NULL, NULL));
169 RunOnceEvery(20, mavlink_send_system_time(NULL, NULL));
170 RunOnceEvery(10, mavlink_send_attitude(NULL, NULL));
171 RunOnceEvery(5, mavlink_send_attitude_quaternion(NULL, NULL));
172 RunOnceEvery(4, mavlink_send_local_position_ned(NULL, NULL));
173 RunOnceEvery(5, mavlink_send_global_position_int(NULL, NULL));
174 RunOnceEvery(6, mavlink_send_gps_raw_int(NULL, NULL));
175 RunOnceEvery(5, mavlink_send_rc_channels(NULL, NULL));
176 RunOnceEvery(21, mavlink_send_battery_status(NULL, NULL));
177 RunOnceEvery(32, mavlink_send_autopilot_version(NULL, NULL));
178 RunOnceEvery(33, mavlink_send_gps_global_origin(NULL, NULL));
179 RunOnceEvery(10, mavlink_send_gps_status(NULL, NULL));
180 RunOnceEvery(10, mavlink_send_vfr_hud(NULL, NULL));
181#endif
182
183 // if requested, send params at 5Hz until all sent
184 RunOnceEvery(2, mavlink_send_params(NULL, NULL));
185
187}
188
190{
191 int i, j;
192 int16_t settings_idx = -1;
193
194 // Go trough all the settings to search the ID
195 for (i = 0; i < NB_SETTING; i++) {
196 for (j = 0; j < 16; j++) {
197 if (mavlink_param_names[i][j] != param_id[j]) {
198 break;
199 }
200
201 if (mavlink_param_names[i][j] == '\0') {
202 settings_idx = i;
203 return settings_idx;
204 }
205 }
206
207 if (mavlink_param_names[i][j] == '\0') {
208 break;
209 }
210 }
211 return settings_idx;
212}
213
218{
219 mavlink_message_t msg;
220 mavlink_status_t status;
221
222 // Check uplink
223 while (MAVLinkChAvailable()) {
224 char test = MAVLinkGetch();
225
226 // When we receive a message
227 if (mavlink_parse_char(MAVLINK_COMM_0, test, &msg, &status)) {
230 }
231 }
232}
233
234void mavlink_common_message_handler(const mavlink_message_t *msg)
235{
236 switch (msg->msgid) {
237 case MAVLINK_MSG_ID_PLAY_TUNE:
238 case MAVLINK_MSG_ID_PLAY_TUNE_V2:
239 DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, strlen("PLAY TUNE"), "PLAY TUNE");
240 break;
241
242 case MAVLINK_MSG_ID_HEARTBEAT:
243 break;
244
245 /* When requesting data streams say we can't send them */
246 case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
247 mavlink_request_data_stream_t cmd;
248 mavlink_msg_request_data_stream_decode(msg, &cmd);
249
250 mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0);
252 break;
253 }
254
255 /* Override channels with RC */
256 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: {
257 mavlink_rc_channels_override_t cmd;
258 mavlink_msg_rc_channels_override_decode(msg, &cmd);
259#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
260 uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100;
261 int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2;
262 int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2;
263 int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100;
264 int8_t temp_rc[4] = {roll, pitch, yaw, thrust};
265 parse_rc_up_datalink(4, temp_rc);
266 //printf("RECEIVED: RC Channel Override for: %d/%d: throttle: %d; roll: %d; pitch: %d; yaw: %d;\r\n",
267 // cmd.target_system, cmd.target_component, thrust, roll, pitch, yaw);
268#endif
269 break;
270 }
271
272 /* When a request is made of the parameters list */
273 case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
275 break;
276 }
277
278 /* When a request os made for a specific parameter */
279 case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
280 mavlink_param_request_read_t cmd;
281 mavlink_msg_param_request_read_decode(msg, &cmd);
282
283 // First check param_index and search for the ID if needed
284 if (cmd.param_index == -1) {
285 cmd.param_index = settings_idx_from_param_id(cmd.param_id);
286 }
287
288 // Send message only if the param_index was found (Coverity Scan)
289 if (cmd.param_index > -1) {
290 mavlink_msg_param_value_send(MAVLINK_COMM_0,
291 mavlink_param_names[cmd.param_index],
292 settings_get_value(cmd.param_index),
293 MAV_PARAM_TYPE_REAL32,
294 NB_SETTING,
295 cmd.param_index);
297 }
298 break;
299 }
300
301 case MAVLINK_MSG_ID_PARAM_SET: {
302 mavlink_param_set_t set;
303 mavlink_msg_param_set_decode(msg, &set);
304
305 // Check if this message is for this system
306 if ((uint8_t) set.target_system == AC_ID) {
307 int16_t idx = settings_idx_from_param_id(set.param_id);
308
309 // setting found
310 if (idx >= 0) {
311 // Only write if new value is NOT "not-a-number"
312 // AND is NOT infinity
313 if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
314 !isnan(set.param_value) && !isinf(set.param_value)) {
315 DlSetting(idx, set.param_value);
316 // Report back new value
317 mavlink_msg_param_value_send(MAVLINK_COMM_0,
319 settings_get_value(idx),
320 MAV_PARAM_TYPE_REAL32,
321 NB_SETTING,
322 idx);
324 }
325 }
326 }
327 }
328 break;
329
330#ifdef ROTORCRAFT_FIRMWARE
331 /* only for rotorcraft */
332 case MAVLINK_MSG_ID_COMMAND_LONG: {
333 mavlink_command_long_t cmd;
334 mavlink_msg_command_long_decode(msg, &cmd);
335 // Check if this message is for this system
336 if ((uint8_t) cmd.target_system == AC_ID) {
337 uint8_t result = MAV_RESULT_UNSUPPORTED;
338 switch (cmd.command) {
339
340 case MAV_CMD_SET_MESSAGE_INTERVAL:
341 case MAV_CMD_DO_SET_MODE: {
342 char error_msg[200];
343 int rc = snprintf(error_msg, 200, "Do set %d: %0.0f %0.0f (%d)", cmd.command, cmd.param1, cmd.param2, cmd.target_system);
344 if (rc > 0) {
345 DOWNLINK_SEND_INFO_MSG(DefaultChannel, DefaultDevice, rc, error_msg);
346 }
347 result = MAV_RESULT_ACCEPTED;
348 break;
349 }
350
351 case MAV_CMD_NAV_GUIDED_ENABLE:
352 MAVLINK_DEBUG("got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1);
353 result = MAV_RESULT_FAILED;
354 if (cmd.param1 > 0.5) {
357 result = MAV_RESULT_ACCEPTED;
358 }
359 }
360 else {
361 // turn guided mode off - to what? maybe NAV? or MODE_AUTO2?
362 }
363 break;
364
365 case MAV_CMD_COMPONENT_ARM_DISARM:
366 /* supposed to use this command to arm or SET_MODE?? */
367 MAVLINK_DEBUG("got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1);
368 result = MAV_RESULT_FAILED;
369 if (cmd.param1 > 0.5) {
372 result = MAV_RESULT_ACCEPTED;
373 }
374 else {
377 result = MAV_RESULT_ACCEPTED;
378 }
379 break;
380
381 default:
382 break;
383 }
384 // confirm command with result
385 mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result, 0, UINT8_MAX, msg->sysid, msg->compid);
387 }
388 break;
389 }
390
391#ifdef WP_ML_global_target
392 case MAVLINK_MSG_ID_SET_POSITION_TARGET_GLOBAL_INT: {
393 mavlink_set_position_target_global_int_t target;
394 mavlink_msg_set_position_target_global_int_decode(msg, &target);
395
396 // Check if this message is for this system
397 //if (target.target_system == AC_ID) {
398 MAVLINK_DEBUG("SET_POSITION_TARGET_GLOBAL_INT, type_mask: %d, frame: %d\n", target.type_mask, target.coordinate_frame);
399 /* if position and yaw bits are not set to ignored, use only position for now */
400 if (target.coordinate_frame == MAV_FRAME_GLOBAL || target.coordinate_frame == MAV_FRAME_GLOBAL_INT) {
401 MAVLINK_DEBUG("set position target, frame MAV_FRAME_GLOBAL %f \n", target.alt);
402 struct NedCoor_i ned;
403 //struct NedCoor_f ned_f;
404 struct LlaCoor_i lla;
405 lla.lat = target.lat_int;
406 lla.lon = target.lon_int;
407 lla.alt = MM_OF_M(target.alt);
409 if (origin == NULL) {
410 MAVLINK_DEBUG("NED origin not set, cannot set target position\n");
411 return;
412 }
414 //NED_FLOAT_OF_BFP(ned_f, ned);
415 //autopilot_guided_goto_ned(ned_f.x, ned_f.y, ned_f.z, target.yaw);
416 waypoint_set_latlon(WP_ML_global_target, &lla);
417 waypoint_set_alt(WP_ML_global_target, target.alt);
418
419 // Downlink the new waypoint
420 uint8_t wp_id = WP_ML_global_target;
422 DOWNLINK_SEND_WP_MOVED_LLA(DefaultChannel, DefaultDevice, &wp_id,
423 &lla.lat, &lla.lon, &hmsl);
424
425 }
426 break;
427 }
428#endif
429
430 case MAVLINK_MSG_ID_SET_MODE: {
431 mavlink_set_mode_t mode;
432 mavlink_msg_set_mode_decode(msg, &mode);
433 if (mode.target_system == AC_ID) {
434 MAVLINK_DEBUG("got SET_MODE: base_mode:%d\n", mode.base_mode);
435 if (mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
437 }
438 else {
440 }
441 if (mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
443 }
444 else if (mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
446 }
447 }
448 break;
449 }
450
451 case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: {
452 mavlink_set_position_target_local_ned_t target;
453 mavlink_msg_set_position_target_local_ned_decode(msg, &target);
454 // Check if this message is for this system
455 if (target.target_system == AC_ID) {
456 MAVLINK_DEBUG("SET_POSITION_TARGET_LOCAL_NED, byte_mask: %d\n", target.type_mask);
457 /* if position and yaw bits are not set to ignored, use only position for now */
458 if (!(target.type_mask & 0b1110000000100000)) {
459 switch (target.coordinate_frame) {
460 case MAV_FRAME_LOCAL_NED:
461 MAVLINK_DEBUG("set position target, frame LOCAL_NED\n");
463 break;
464 case MAV_FRAME_LOCAL_OFFSET_NED:
465 MAVLINK_DEBUG("set position target, frame LOCAL_OFFSET_NED\n");
467 break;
468 case MAV_FRAME_BODY_OFFSET_NED:
469 MAVLINK_DEBUG("set position target, frame BODY_OFFSET_NED\n");
471 break;
472 default:
473 break;
474 }
475 }
476 else if (!(target.type_mask & 0b0001110000100000)) {
477 /* position is set to ignore, but velocity not */
478 switch (target.coordinate_frame) {
479 case MAV_FRAME_LOCAL_NED:
480 MAVLINK_DEBUG("set velocity target, frame LOCAL_NED\n");
482 break;
483 default:
484 break;
485 }
486 }
487 }
488 break;
489 }
490#endif
491
492 default:
493 //Do nothing
494 MAVLINK_DEBUG("Received message with id: %d\r\n", msg->msgid);
495 break;
496 }
497}
498
499/* ignore the unused-parameter warnings */
500#pragma GCC diagnostic push
501#pragma GCC diagnostic ignored "-Wunused-parameter"
502
506static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev)
507{
508 uint8_t mav_state = MAV_STATE_CALIBRATING;
509 uint8_t mav_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; // Ardupilot custom mode enabled
510 uint32_t custom_mode = 0;
511#if defined(FIXEDWING_FIRMWARE)
512 uint8_t mav_type = MAV_TYPE_FIXED_WING;
513 switch (autopilot_get_mode()) {
514 case AP_MODE_MANUAL:
515 mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
516 break;
517 case AP_MODE_AUTO1:
518 mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
519 break;
520 case AP_MODE_AUTO2:
521 mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
522 break;
523 case AP_MODE_HOME:
524 mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED;
525 break;
526 default:
527 break;
528 }
529#elif defined(ROTORCRAFT_FIRMWARE)
530 uint8_t mav_type = MAV_TYPE_QUADROTOR;
531 switch (autopilot_get_mode()) {
532 case AP_MODE_HOME:
533 mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
534 break;
539 mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
540 break;
549 mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
550 break;
551 case AP_MODE_NAV:
552 mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
553 custom_mode = PLANE_MODE_GUIDED;
554 break;
555 case AP_MODE_GUIDED:
556 mav_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
557 custom_mode = PLANE_MODE_GUIDED;
558 default:
559 break;
560 }
561#else
562#error "mavlink datalink: unsupported firmware"
563#endif
564 if (stateIsAttitudeValid()) {
566 mav_state = MAV_STATE_STANDBY;
567 } else {
568 mav_state = MAV_STATE_ACTIVE;
569 mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
570 }
571 }
572 mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
573 mav_type,
575 mav_mode,
576 custom_mode,
577 mav_state);
579}
580
581/*
582 * Send extended system state
583*/
584static void mavlink_send_extended_sys_state(struct transport_tx *trans, struct link_device *dev) {
585 uint8_t landed_state = autopilot_in_flight()? MAV_LANDED_STATE_IN_AIR : MAV_LANDED_STATE_ON_GROUND;
586 mavlink_msg_extended_sys_state_send(MAVLINK_COMM_0, MAV_VTOL_STATE_UNDEFINED, landed_state);
588}
589
593static void mavlink_send_sys_status(struct transport_tx *trans, struct link_device *dev)
594{
596#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)
597
598 mavlink_msg_sys_status_send(MAVLINK_COMM_0,
599 UAV_SENSORS, // On-board sensors: present (bitmap)
600 UAV_SENSORS, // On-board sensors: active (bitmap)
601 UAV_SENSORS, // On-board sensors: state (bitmap)
602 -1,//10*sys_mon.cpu_load, // System loadof main-loop time (0=0% to 1000=100%)
603 electrical.vsupply * 1000.f, // Battery voltage (milivolts)
604 electrical.current * 100.f, // Battery current (10x miliampere)
605 -1, // Battery remaining (0-100 in %)
606 0, // Communication packet drops (0=0% to 10000=100%)
607 0, // Communication error(per packet) (0=0% to 10000=100%)
608 0, // Autopilot specific error 1
609 0, // Autopilot specific error 2
610 0, // Autopilot specific error 3
611 0, // Autopilot specific error 4
612 0,
613 0,
614 0);
616}
617
623static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev)
624{
625 mavlink_msg_system_time_send(MAVLINK_COMM_0, 0, get_sys_time_msec());
627}
628
632static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev)
633{
634 mavlink_msg_attitude_send(MAVLINK_COMM_0,
636 stateGetNedToBodyEulers_f()->phi, // Phi
637 stateGetNedToBodyEulers_f()->theta, // Theta
638 stateGetNedToBodyEulers_f()->psi, // Psi
639 stateGetBodyRates_f()->p, // p
640 stateGetBodyRates_f()->q, // q
641 stateGetBodyRates_f()->r); // r
643}
644
645static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev)
646{
647 mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
654 stateGetSpeedNed_f()->z);
656}
657
658static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev)
659{
660 float heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
661 if (heading < 0.) {
662 heading += 360;
663 }
664 uint16_t compass_heading = heading * 100;
666 int32_t origin_alt = 0;
667 struct LtpDef_i *ned_origin = stateGetNedOrigin_i();
668 if (ned_origin != NULL) {
669 origin_alt = ned_origin->lla.alt;
670 }
671 int32_t hmsl_alt = stateGetHmslOrigin_i() - origin_alt;
673 mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
677 stateGetPositionLla_i()->alt + hmsl_alt,
678 relative_alt,
679 stateGetSpeedNed_f()->x * 100,
680 stateGetSpeedNed_f()->y * 100,
681 stateGetSpeedNed_f()->z * 100,
682 compass_heading);
684}
685
686static void mavlink_send_gps_global_origin(struct transport_tx *trans, struct link_device *dev)
687{
688 if (state.ned_initialized_i) {
689 mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0,
695 }
696}
697
701static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev)
702{
703 if (mavlink_params_idx >= NB_SETTING) {
704 return;
705 }
706
707 mavlink_msg_param_value_send(MAVLINK_COMM_0,
709 settings_get_value(mavlink_params_idx),
710 MAV_PARAM_TYPE_REAL32,
711 NB_SETTING,
714
716}
717
718static void mavlink_send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
719{
721 static uint32_t ver = PPRZ_VERSION_INT;
722 uint8_t sha[8];
723 uint8_t uid[18] = {0};
724 uint64_t uid64 = 0;
725 get_pprz_git_version(sha);
726 mcu_get_uid(uid, sizeof(uid));
727 memcpy(&uid64, uid, sizeof(uid64));
728 mavlink_msg_autopilot_version_send(MAVLINK_COMM_0,
729 18446744073709551615U, //uint64_t capabilities,
730 ver, //uint32_t flight_sw_version,
731 0, //uint32_t middleware_sw_version,
732 0, //uint32_t os_sw_version,
733 0, //uint32_t board_version,
734 sha, //const uint8_t *flight_custom_version,
735 0, //const uint8_t *middleware_custom_version,
736 0, //const uint8_t *os_custom_version,
737 0, //uint16_t vendor_id,
738 0, //uint16_t product_id,
739 uid64, //uint64_t uid
740 uid); //const uint8_t *uid2
742}
743
744static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev)
745{
746 float repr_offset_q[4] = {0, 0, 0, 0};
747 mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
756 repr_offset_q);
758}
759
760#if USE_GPS
761#include "modules/gps/gps.h"
762#endif
763
764static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev)
765{
766#if USE_GPS
767 int32_t course = DegOfRad(gps.course) / 1e5;
768 if (course < 0) {
769 course += 36000;
770 }
771 mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
773 gps.fix,
774 gps.lla_pos.lat,
775 gps.lla_pos.lon,
776 gps.hmsl,
777 gps.pdop,
778 UINT16_MAX, // VDOP
779 gps.gspeed,
780 course,
781 gps.num_sv,
782 gps.lla_pos.alt,
783 gps.hacc,
784 gps.vacc,
785 gps.sacc,
786 0,
787 0);
789#endif
790}
791
802static void mavlink_send_gps_status(struct transport_tx *trans, struct link_device *dev)
803{
804#if USE_GPS
805 uint8_t nb_sats = Min(GPS_NB_CHANNELS, 20);
806 uint8_t prn[20];
807 uint8_t used[20];
808 uint8_t elevation[20];
809 uint8_t azimuth[20];
810 uint8_t snr[20];
811 for (uint8_t i = 0; i < nb_sats; i++) {
812 prn[i] = gps.svinfos[i].svid;
813 // set sat as used if cno > 0, not quite true though
814 if (gps.svinfos[i].cno > 0) {
815 used[i] = 1;
816 }
817 elevation[i] = gps.svinfos[i].elev;
818 // convert azimuth to unsigned representation: 0: 0 deg, 255: 360 deg.
819 uint8_t azim;
820 if (gps.svinfos[i].azim < 0) {
821 azim = (float)(-gps.svinfos[i].azim + 180) / 360 * 255;
822 } else {
823 azim = (float)gps.svinfos[i].azim / 360 * 255;
824 }
825 azimuth[i] = azim;
826 }
827 mavlink_msg_gps_status_send(MAVLINK_COMM_0,
828 gps.num_sv, prn, used, elevation, azimuth, snr);
830#endif
831}
832
833#if defined RADIO_CONTROL
835// since they really want PPM values, use a hack to check if are using ppm subsystem
836#ifdef PPM_PULSE_TYPE_POSITIVE
837#define RC_CHANNELS RADIO_CTL_NB
839#define PPM_PULSES(_i) ((_i) < RADIO_CTL_NB ? ppm_pulses[(_i)] : UINT16_MAX)
840#else
841// use radio_control.values for now...
842#define RC_CHANNELS RADIO_CONTROL_NB_CHANNEL
843#define PPM_OF_PPRZ(_v) ((_v) / 19.2 + 1500)
844#define PPM_PULSES(_i) ((_i) < RADIO_CONTROL_NB_CHANNEL ? PPM_OF_PPRZ(radio_control.values[(_i)]) + MAX_PPRZ : UINT16_MAX)
845#endif
846#endif
847
848static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev)
849{
850#if defined RADIO_CONTROL
851 mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
857 PPM_PULSES(9), PPM_PULSES(10), PPM_PULSES(11),
858 PPM_PULSES(12), PPM_PULSES(13), PPM_PULSES(14),
859 PPM_PULSES(15), PPM_PULSES(16), PPM_PULSES(17),
860 255); // rssi unknown
861#else
862 mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
864 0, // zero channels available
865 UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
866 UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
867 UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
868 UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
869 UINT16_MAX, UINT16_MAX, 255);
870#endif
872}
873
875static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev)
876{
877 static uint16_t voltages[14] = {0};
878 // we simply only set one cell for now
879 voltages[0] = electrical.vsupply * 1000.f; // convert to mV
881 mavlink_msg_battery_status_send(MAVLINK_COMM_0,
882 0, // id
883 0, // battery_function
884 0, // type
885 INT16_MAX, // unknown temperature
886 voltages, // cell voltages
887 electrical.current * 100.f, // convert to deciA
888 electrical.charge * 1000.f, // convert to mAh
889 electrical.energy * 36, // convert to hecto Joule
890 -1, // remaining percentage not estimated
891 0,
892 MAV_BATTERY_CHARGE_STATE_UNDEFINED,
893 &voltages[10],
894 MAV_BATTERY_MODE_UNKNOWN,
895 0);
897}
898
903static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev)
904{
905 /* Current heading in degrees, in compass units (0..360, 0=north) */
906 int16_t heading = DegOfRad(stateGetNedToBodyEulers_f()->psi);
907 /* Current throttle setting in integer percent, 0 to 100 */
908 // is a 16bit unsigned int but supposed to be from 0 to 100??
909 uint16_t throttle;
910#ifdef COMMAND_THRUST
911 throttle = commands[COMMAND_THRUST] / (MAX_PPRZ / 100);
912#elif defined COMMAND_THROTTLE
913 throttle = commands[COMMAND_THROTTLE] / (MAX_PPRZ / 100);
914#endif
915 float hmsl_alt = stateGetHmslOrigin_f() - stateGetLlaOrigin_f().alt;
916 mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
917 Max(0,stateGetAirspeed_f()),
918 stateGetHorizontalSpeedNorm_f(), // groundspeed
919 heading,
920 throttle,
921 stateGetPositionLla_f()->alt + hmsl_alt,
922 stateGetSpeedNed_f()->z); // climb rate
924}
925
926/* end ignore unused-paramter */
927#pragma GCC diagnostic pop
static int16_t course[3]
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
Definition autopilot.c:200
uint8_t autopilot_get_mode(void)
get autopilot mode
Definition autopilot.c:229
bool autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly: kill_thro...
Definition autopilot.c:258
bool autopilot_get_motors_on(void)
get motors status
Definition autopilot.c:303
bool autopilot_in_flight(void)
get in_flight flag
Definition autopilot.c:351
bool autopilot_throttle_killed(void)
get kill status
Definition autopilot.c:324
Core autopilot interface common to all firmwares.
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
Set velocity and heading setpoints in GUIDED mode.
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
Autopilot guided mode interface.
static uint8_t status
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
uint32_t get_sys_time_msec(void)
Get the time in milliseconds since startup.
Hardware independent code for commands handling.
static struct uart_periph * dev
struct Electrical electrical
Definition electrical.c:92
Interface for electrical status: supply voltage, current, battery status, etc.
#define Min(x, y)
Definition esc_dshot.c:110
#define AP_MODE_HOME
#define AP_MODE_MANUAL
AP modes.
#define AP_MODE_AUTO2
#define AP_MODE_AUTO1
struct GpsState gps
global GPS state
Definition gps.c:74
Device independent GPS code (interface).
#define GPS_NB_CHANNELS
Definition gps.h:57
int32_t lat
in degrees*1e7
int32_t hmsl
Height above mean sea level in mm.
int32_t alt
in millimeters above WGS84 reference ellipsoid
struct LlaCoor_i lla
Reference point in lla.
int32_t lon
in degrees*1e7
void ned_of_lla_point_i(struct NedCoor_i *ned, struct LtpDef_i *def, struct LlaCoor_i *lla)
Convert a point from LLA to local NED.
#define MM_OF_M(_m)
vector in Latitude, Longitude and Altitude
definition of the local (flat earth) coordinate system
vector in North East Down coordinates
uint8_t mcu_get_uid(uint8_t *uid, uint8_t max_len)
Get the 96-bit unique device ID of the STM32.
Definition mcu_arch.c:252
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
Definition state.h:1243
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
Definition state.h:1325
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
Definition state.h:1313
struct State state
Definition state.c:36
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
Definition state.h:868
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
Definition state.h:823
int32_t stateGetHmslOrigin_i(void)
Get the HMSL of the frame origin (int).
Definition state.c:196
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
Definition state.c:210
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
Definition state.c:130
struct LlaCoor_f stateGetLlaOrigin_f(void)
Get the LLA position of the frame origin (float).
Definition state.c:149
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int).
Definition state.h:567
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
Definition state.h:850
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
Definition state.h:1392
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
Definition state.h:1087
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
Definition state.h:1060
static float stateGetAirspeed_f(void)
Get airspeed (float).
Definition state.h:1615
static char origin[GSM_ORIGIN_MAXLEN]
Definition gsm.c:101
static float p[2][2]
Arch independent mcu ( Micro Controller Unit ) utilities.
void mavlink_mission_init(mavlink_mission_mgr *mgr)
void mavlink_mission_message_handler(const mavlink_message_t *msg)
void mavlink_mission_periodic(void)
update current block and send if changed
Common functions used within the mission library, blocks and waypoints cannot be send simultaneously ...
void waypoint_set_latlon(uint8_t wp_id, struct LlaCoor_i *lla)
set waypoint latitude/longitude without updating altitude
Definition waypoints.c:295
void waypoint_set_alt(uint8_t wp_id, float alt)
Set altitude of waypoint in meters (above reference).
Definition waypoints.c:258
static uint32_t idx
#define MAX_PPRZ
Definition paparazzi.h:8
float alt
in meters (normally above WGS84 reference ellipsoid)
Generic interface for radio control modules.
#define AP_MODE_HOVER_DIRECT
#define AP_MODE_RATE_DIRECT
#define AP_MODE_RATE_RC_CLIMB
#define AP_MODE_HOVER_CLIMB
#define AP_MODE_HOVER_Z_HOLD
#define AP_MODE_RC_DIRECT
#define AP_MODE_ATTITUDE_DIRECT
#define AP_MODE_ATTITUDE_CLIMB
#define AP_MODE_NAV
#define AP_MODE_GUIDED
#define AP_MODE_ATTITUDE_Z_HOLD
#define AP_MODE_ATTITUDE_RC_CLIMB
#define AP_MODE_RATE_Z_HOLD
#define AP_MODE_CARE_FREE_DIRECT
static const ShellCommand commands[]
Definition shell_arch.c:77
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:65
API to get/set the generic vehicle states.
#define TRUE
Definition std.h:4
#define FALSE
Definition std.h:5
Architecture independent timing functions.
struct target_t target
Definition target_pos.c:65
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
Definition telemetry.c:51
Common tools for periodic telemetry interface Allows subsystem to register callback functions.
Periodic telemetry structure.
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
int int32_t
Typedef defining 32 bit int type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
short int16_t
Typedef defining 16 bit short type.
unsigned long long uint64_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
signed char int8_t
Typedef defining 8 bit char type.
float heading
Definition wedgebug.c:258