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
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.
46#include "generated/airframe.h"
47#include "generated/modules.h"
48#include "generated/settings.h"
49#include "generated/flight_plan.h"
55#include "pprz_version.h"
59#if defined RADIO_CONTROL
64#ifndef MAV_AUTOPILOT_ID
65#define MAV_AUTOPILOT_ID MAV_AUTOPILOT_PPZ
110#define MAVLINK_SYSID AC_ID
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 };
127#if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
152#if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
155 periodic_telemetry_send_Mavlink(&mavlink_telemetry, NULL, NULL);
165#if !defined (TELEMETRY_MAVLINK_NB_MSG)
195 for (i = 0; i < NB_SETTING; i++) {
196 for (j = 0; j < 16; j++) {
219 mavlink_message_t msg;
227 if (mavlink_parse_char(MAVLINK_COMM_0, test, &msg, &
status)) {
236 switch (msg->msgid) {
237 case MAVLINK_MSG_ID_PLAY_TUNE:
238 case MAVLINK_MSG_ID_PLAY_TUNE_V2:
242 case MAVLINK_MSG_ID_HEARTBEAT:
246 case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
247 mavlink_request_data_stream_t cmd;
248 mavlink_msg_request_data_stream_decode(msg, &cmd);
250 mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0);
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};
273 case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
279 case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
280 mavlink_param_request_read_t cmd;
281 mavlink_msg_param_request_read_decode(msg, &cmd);
284 if (cmd.param_index == -1) {
289 if (cmd.param_index > -1) {
290 mavlink_msg_param_value_send(MAVLINK_COMM_0,
292 settings_get_value(cmd.param_index),
293 MAV_PARAM_TYPE_REAL32,
301 case MAVLINK_MSG_ID_PARAM_SET: {
302 mavlink_param_set_t set;
303 mavlink_msg_param_set_decode(msg, &set);
306 if ((
uint8_t) set.target_system == AC_ID) {
313 if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
314 !isnan(set.param_value) && !isinf(set.param_value)) {
315 DlSetting(
idx, set.param_value);
317 mavlink_msg_param_value_send(MAVLINK_COMM_0,
319 settings_get_value(
idx),
320 MAV_PARAM_TYPE_REAL32,
330#ifdef ROTORCRAFT_FIRMWARE
332 case MAVLINK_MSG_ID_COMMAND_LONG: {
333 mavlink_command_long_t cmd;
334 mavlink_msg_command_long_decode(msg, &cmd);
336 if ((
uint8_t) cmd.target_system == AC_ID) {
337 uint8_t result = MAV_RESULT_UNSUPPORTED;
338 switch (cmd.command) {
340 case MAV_CMD_SET_MESSAGE_INTERVAL:
341 case MAV_CMD_DO_SET_MODE: {
343 int rc = snprintf(error_msg, 200,
"Do set %d: %0.0f %0.0f (%d)", cmd.command, cmd.param1, cmd.param2, cmd.target_system);
347 result = MAV_RESULT_ACCEPTED;
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;
365 case MAV_CMD_COMPONENT_ARM_DISARM:
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;
377 result = MAV_RESULT_ACCEPTED;
385 mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result, 0, UINT8_MAX, msg->sysid, msg->compid);
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);
400 if (
target.coordinate_frame == MAV_FRAME_GLOBAL ||
target.coordinate_frame == MAV_FRAME_GLOBAL_INT) {
410 MAVLINK_DEBUG(
"NED origin not set, cannot set target position\n");
420 uint8_t wp_id = WP_ML_global_target;
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) {
435 if (
mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
441 if (
mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
444 else if (
mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
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);
455 if (
target.target_system == AC_ID) {
458 if (!(
target.type_mask & 0b1110000000100000)) {
459 switch (
target.coordinate_frame) {
460 case MAV_FRAME_LOCAL_NED:
464 case MAV_FRAME_LOCAL_OFFSET_NED:
465 MAVLINK_DEBUG(
"set position target, frame LOCAL_OFFSET_NED\n");
468 case MAV_FRAME_BODY_OFFSET_NED:
469 MAVLINK_DEBUG(
"set position target, frame BODY_OFFSET_NED\n");
476 else if (!(
target.type_mask & 0b0001110000100000)) {
478 switch (
target.coordinate_frame) {
479 case MAV_FRAME_LOCAL_NED:
494 MAVLINK_DEBUG(
"Received message with id: %d\r\n", msg->msgid);
500#pragma GCC diagnostic push
501#pragma GCC diagnostic ignored "-Wunused-parameter"
508 uint8_t mav_state = MAV_STATE_CALIBRATING;
509 uint8_t mav_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
511#if defined(FIXEDWING_FIRMWARE)
512 uint8_t mav_type = MAV_TYPE_FIXED_WING;
515 mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
518 mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
521 mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
524 mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED;
529#elif defined(ROTORCRAFT_FIRMWARE)
530 uint8_t mav_type = MAV_TYPE_QUADROTOR;
533 mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
539 mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
549 mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
552 mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
553 custom_mode = PLANE_MODE_GUIDED;
556 mav_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
557 custom_mode = PLANE_MODE_GUIDED;
562#error "mavlink datalink: unsupported firmware"
566 mav_state = MAV_STATE_STANDBY;
568 mav_state = MAV_STATE_ACTIVE;
569 mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
572 mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
586 mavlink_msg_extended_sys_state_send(MAVLINK_COMM_0, MAV_VTOL_STATE_UNDEFINED, landed_state);
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)
598 mavlink_msg_sys_status_send(MAVLINK_COMM_0,
634 mavlink_msg_attitude_send(MAVLINK_COMM_0,
647 mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
668 if (ned_origin != NULL) {
669 origin_alt = ned_origin->
lla.
alt;
673 mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
688 if (
state.ned_initialized_i) {
689 mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0,
707 mavlink_msg_param_value_send(MAVLINK_COMM_0,
710 MAV_PARAM_TYPE_REAL32,
721 static uint32_t ver = PPRZ_VERSION_INT;
725 get_pprz_git_version(sha);
727 memcpy(&uid64, uid,
sizeof(uid64));
728 mavlink_msg_autopilot_version_send(MAVLINK_COMM_0,
729 18446744073709551615U,
746 float repr_offset_q[4] = {0, 0, 0, 0};
747 mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
771 mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
811 for (
uint8_t i = 0; i < nb_sats; i++) {
812 prn[i] =
gps.svinfos[i].svid;
814 if (
gps.svinfos[i].cno > 0) {
817 elevation[i] =
gps.svinfos[i].elev;
820 if (
gps.svinfos[i].azim < 0) {
821 azim = (float)(-
gps.svinfos[i].azim + 180) / 360 * 255;
823 azim = (float)
gps.svinfos[i].azim / 360 * 255;
827 mavlink_msg_gps_status_send(MAVLINK_COMM_0,
828 gps.num_sv, prn, used, elevation, azimuth, snr);
833#if defined RADIO_CONTROL
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)
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)
850#if defined RADIO_CONTROL
851 mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
862 mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
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);
881 mavlink_msg_battery_status_send(MAVLINK_COMM_0,
892 MAV_BATTERY_CHARGE_STATE_UNDEFINED,
894 MAV_BATTERY_MODE_UNKNOWN,
912#elif defined COMMAND_THROTTLE
916 mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
927#pragma GCC diagnostic pop
bool autopilot_set_mode(uint8_t new_autopilot_mode)
set autopilot mode
uint8_t autopilot_get_mode(void)
get autopilot mode
bool autopilot_set_motors_on(bool motors_on)
turn motors on/off, eventually depending of the current mode set kill_throttle accordingly: kill_thro...
bool autopilot_get_motors_on(void)
get motors status
bool autopilot_in_flight(void)
get in_flight flag
bool autopilot_throttle_killed(void)
get kill status
Core autopilot interface common to all firmwares.
bool autopilot_guided_move_ned(float vx, float vy, float vz, float heading)
Set velocity and heading setpoints in GUIDED mode.
bool autopilot_guided_goto_ned_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
bool autopilot_guided_goto_body_relative(float dx, float dy, float dz, float dyaw)
Set position and heading setpoints wrt.
bool autopilot_guided_goto_ned(float x, float y, float z, float heading)
Set position and heading setpoints in GUIDED mode.
Autopilot guided mode interface.
uint32_t get_sys_time_usec(void)
Get the time in microseconds since startup.
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
Interface for electrical status: supply voltage, current, battery status, etc.
#define AP_MODE_MANUAL
AP modes.
struct GpsState gps
global GPS state
Device independent GPS code (interface).
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.
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.
static bool stateIsAttitudeValid(void)
Test if attitudes are valid.
static struct FloatEulers * stateGetNedToBodyEulers_f(void)
Get vehicle body attitude euler angles (float).
static struct FloatQuat * stateGetNedToBodyQuat_f(void)
Get vehicle body attitude quaternion (float).
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
static struct LlaCoor_i * stateGetPositionLla_i(void)
Get position in LLA coordinates (int).
int32_t stateGetHmslOrigin_i(void)
Get the HMSL of the frame origin (int).
float stateGetHmslOrigin_f(void)
Get the HMSL of the frame origin (float).
struct LlaCoor_i stateGetLlaOrigin_i(void)
Get the LLA position of the frame origin (int).
struct LlaCoor_f stateGetLlaOrigin_f(void)
Get the LLA position of the frame origin (float).
static struct LtpDef_i * stateGetNedOrigin_i(void)
Get the coordinate NED frame origin (int).
static struct NedCoor_f * stateGetPositionNed_f(void)
Get position in local NED coordinates (float).
static struct FloatRates * stateGetBodyRates_f(void)
Get vehicle body angular rate (float).
static float stateGetHorizontalSpeedNorm_f(void)
Get norm of horizontal ground speed (float).
static struct NedCoor_f * stateGetSpeedNed_f(void)
Get ground speed in local NED coordinates (float).
static float stateGetAirspeed_f(void)
Get airspeed (float).
static char origin[GSM_ORIGIN_MAXLEN]
static void mavlink_send_attitude(void)
static void mavlink_send_heartbeat(void)
static int16_t settings_idx_from_param_id(char *param_id)
static uint8_t custom_version[8]
first 8 bytes (16 chars) of GIT SHA1
static void mavlink_send_autopilot_version(struct transport_tx *trans, struct link_device *dev)
static void mavlink_send_system_time(struct transport_tx *trans, struct link_device *dev)
Send SYSTEM_TIME.
static char mavlink_param_names[NB_SETTING][16+1]
mavlink parameter names.
static uint8_t mavlink_params_idx
Transmitting parameters index.
static void mavlink_send_gps_global_origin(struct transport_tx *trans, struct link_device *dev)
static void mavlink_send_gps_raw_int(struct transport_tx *trans, struct link_device *dev)
void mavlink_periodic(void)
Periodic MAVLink calls.
mavlink_system_t mavlink_system
mavlink_mission_mgr mission_mgr
#define MAVLINK_SYSID
MAVLink initialization.
static void mavlink_send_global_position_int(struct transport_tx *trans, struct link_device *dev)
static void mavlink_send_battery_status(struct transport_tx *trans, struct link_device *dev)
static void mavlink_send_attitude_quaternion(struct transport_tx *trans, struct link_device *dev)
static void mavlink_send_vfr_hud(struct transport_tx *trans, struct link_device *dev)
Send Metrics typically displayed on a HUD for fixed wing aircraft.
void mavlink_common_message_handler(const mavlink_message_t *msg)
static void mavlink_send_extended_sys_state(struct transport_tx *trans, struct link_device *dev)
static void mavlink_send_rc_channels(struct transport_tx *trans, struct link_device *dev)
static void mavlink_send_params(struct transport_tx *trans, struct link_device *dev)
Send the parameters.
static void mavlink_send_gps_status(struct transport_tx *trans, struct link_device *dev)
Send gps status.
void mavlink_periodic_telemetry(void)
Send periodic mavlink messages as defined in Mavlink process of telemetry xml file.
void mavlink_event(void)
Event MAVLink calls.
void mavlink_init(void)
Module functions.
static void mavlink_send_local_position_ned(struct transport_tx *trans, struct link_device *dev)
static void mavlink_send_attitude(struct transport_tx *trans, struct link_device *dev)
Send the attitude.
static void mavlink_send_heartbeat(struct transport_tx *trans, struct link_device *dev)
Send a heartbeat.
static void mavlink_send_sys_status(struct transport_tx *trans, struct link_device *dev)
Send the system status.
Basic MAVLink datalink implementation.
#define MAVLINK_DEBUG(...)
#define MAVLinkSendMessage()
#define MAVLinkChAvailable()
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
void waypoint_set_alt(uint8_t wp_id, float alt)
Set altitude of waypoint in meters (above reference).
float alt
in meters (normally above WGS84 reference ellipsoid)
Generic interface for radio control modules.
void parse_rc_up_datalink(int8_t n, int8_t *channels)
Decode datalink message to get rc values with RC_UP message.
#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_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[]
static uint8_t mode
mode holds the current sonar mode mode = 0 used at high altitude, uses 16 wave patterns mode = 1 used...
API to get/set the generic vehicle states.
Architecture independent timing functions.
int16_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint16_t _id, telemetry_cb _cb)
Register a telemetry callback function.
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.