Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
mavlink.c File Reference

Basic MAVLink datalink implementation. More...

#include "modules/datalink/mavlink.h"
#include "mavlink/paparazzi/mavlink.h"
#include "modules/datalink/telemetry_common.h"
#include "generated/periodic_telemetry.h"
#include "generated/airframe.h"
#include "generated/modules.h"
#include "generated/settings.h"
#include "mcu_periph/sys_time.h"
#include "modules/energy/electrical.h"
#include "state.h"
#include "pprz_version.h"
#include "autopilot.h"
#include "autopilot_guided.h"
#include "modules/radio_control/radio_control.h"
#include "modules/datalink/missionlib/mission_manager.h"
#include <stdint.h>
#include "modules/gps/gps.h"
#include "modules/core/commands.h"
+ Include dependency graph for mavlink.c:

Go to the source code of this file.

Macros

#define PERIODIC_C_MAVLINK
 
#define MAV_AUTOPILOT_ID   MAV_AUTOPILOT_PPZ
 
#define MAVLINK_SYSID   AC_ID
 MAVLink initialization. More...
 
#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)
 
#define RC_CHANNELS   RADIO_CONTROL_NB_CHANNEL
 
#define PPM_OF_PPRZ(_v)   ((_v) / 19.2 + 1500)
 
#define PPM_PULSES(_i)   ((_i) < RADIO_CONTROL_NB_CHANNEL ? PPM_OF_PPRZ(radio_control.values[(_i)]) + MAX_PPRZ : UINT16_MAX)
 

Functions

void mavlink_common_message_handler (const mavlink_message_t *msg)
 
static void mavlink_send_heartbeat (struct transport_tx *trans, struct link_device *dev)
 Send a heartbeat. More...
 
static void mavlink_send_sys_status (struct transport_tx *trans, struct link_device *dev)
 Send the system status. More...
 
static void mavlink_send_system_time (struct transport_tx *trans, struct link_device *dev)
 Send SYSTEM_TIME. More...
 
static void mavlink_send_attitude (struct transport_tx *trans, struct link_device *dev)
 Send the attitude. More...
 
static void mavlink_send_local_position_ned (struct transport_tx *trans, struct link_device *dev)
 
static void mavlink_send_global_position_int (struct transport_tx *trans, struct link_device *dev)
 
static void mavlink_send_params (struct transport_tx *trans, struct link_device *dev)
 Send the parameters. More...
 
static void mavlink_send_autopilot_version (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_gps_raw_int (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_battery_status (struct transport_tx *trans, struct link_device *dev)
 
static void mavlink_send_gps_global_origin (struct transport_tx *trans, struct link_device *dev)
 
static void mavlink_send_gps_status (struct transport_tx *trans, struct link_device *dev)
 Send gps status. More...
 
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. More...
 
void mavlink_init (void)
 Module functions. More...
 
void mavlink_periodic_telemetry (void)
 Send periodic mavlink messages as defined in Mavlink process of telemetry xml file. More...
 
void mavlink_periodic (void)
 Periodic MAVLink calls. More...
 
static int16_t settings_idx_from_param_id (char *param_id)
 
void mavlink_event (void)
 Event MAVLink calls. More...
 

Variables

mavlink_system_t mavlink_system
 
static uint8_t mavlink_params_idx = NB_SETTING
 Transmitting parameters index. More...
 
static char mavlink_param_names [NB_SETTING][16+1] = SETTINGS_NAMES_SHORT
 mavlink parameter names. More...
 
static uint8_t custom_version [8]
 first 8 bytes (16 chars) of GIT SHA1 More...
 
mavlink_mission_mgr mission_mgr
 

Detailed Description

Basic MAVLink datalink implementation.

Definition in file mavlink.c.

Macro Definition Documentation

◆ MAV_AUTOPILOT_ID

#define MAV_AUTOPILOT_ID   MAV_AUTOPILOT_PPZ

Definition at line 62 of file mavlink.c.

◆ MAVLINK_SYSID

#define MAVLINK_SYSID   AC_ID

MAVLink initialization.

Definition at line 105 of file mavlink.c.

◆ PERIODIC_C_MAVLINK

#define PERIODIC_C_MAVLINK

Definition at line 37 of file mavlink.c.

◆ PPM_OF_PPRZ

#define PPM_OF_PPRZ (   _v)    ((_v) / 19.2 + 1500)

Definition at line 759 of file mavlink.c.

◆ PPM_PULSES

#define PPM_PULSES (   _i)    ((_i) < RADIO_CONTROL_NB_CHANNEL ? PPM_OF_PPRZ(radio_control.values[(_i)]) + MAX_PPRZ : UINT16_MAX)

Definition at line 760 of file mavlink.c.

◆ RC_CHANNELS

#define RC_CHANNELS   RADIO_CONTROL_NB_CHANNEL

Definition at line 758 of file mavlink.c.

◆ UAV_SENSORS

#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)

Function Documentation

◆ mavlink_common_message_handler()

◆ mavlink_event()

void mavlink_event ( void  )

Event MAVLink calls.

Definition at line 211 of file mavlink.c.

References mavlink_common_message_handler(), mavlink_mission_message_handler(), MAVLinkChAvailable, MAVLinkGetch, msg, and status.

+ Here is the call graph for this function:

◆ mavlink_init()

◆ mavlink_periodic()

◆ mavlink_periodic_telemetry()

void mavlink_periodic_telemetry ( void  )

Send periodic mavlink messages as defined in Mavlink process of telemetry xml file.

Called at TELEMETRY_FREQUENCY

Definition at line 144 of file mavlink.c.

◆ mavlink_send_attitude()

static void mavlink_send_attitude ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Send the attitude.

Definition at line 557 of file mavlink.c.

References get_sys_time_msec(), MAVLinkSendMessage, p, stateGetBodyRates_f(), and stateGetNedToBodyEulers_f().

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ mavlink_send_attitude_quaternion()

static void mavlink_send_attitude_quaternion ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 660 of file mavlink.c.

References get_sys_time_msec(), MAVLinkSendMessage, p, stateGetBodyRates_f(), and stateGetNedToBodyQuat_f().

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ mavlink_send_autopilot_version()

static void mavlink_send_autopilot_version ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

TODO: fill in versions correctly, how should they be encoded?

Definition at line 638 of file mavlink.c.

References MAVLinkSendMessage.

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the caller graph for this function:

◆ mavlink_send_battery_status()

static void mavlink_send_battery_status ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

TODO: check what all these fields are supposed to represent

Definition at line 791 of file mavlink.c.

References Electrical::charge, Electrical::current, electrical, Electrical::energy, MAVLinkSendMessage, and Electrical::vsupply.

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the caller graph for this function:

◆ mavlink_send_global_position_int()

static void mavlink_send_global_position_int ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

TODO: check/ask what coordinate system vel is supposed to be in, not clear from docs

Definition at line 583 of file mavlink.c.

References LlaCoor_i::alt, get_sys_time_msec(), heading, LtpDef_i::hmsl, LtpDef_i::lla, MAVLinkSendMessage, State::ned_origin_i, state, stateGetNedToBodyEulers_f(), stateGetPositionLla_i(), and stateGetSpeedNed_f().

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ mavlink_send_gps_global_origin()

static void mavlink_send_gps_global_origin ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 606 of file mavlink.c.

References get_sys_time_usec(), LtpDef_i::hmsl, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, MAVLinkSendMessage, State::ned_initialized_i, State::ned_origin_i, and state.

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ mavlink_send_gps_raw_int()

static void mavlink_send_gps_raw_int ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 680 of file mavlink.c.

References LlaCoor_i::alt, GpsState::course, course, GpsState::fix, get_sys_time_usec(), gps, GpsState::gspeed, GpsState::hacc, GpsState::hmsl, LlaCoor_i::lat, GpsState::lla_pos, LlaCoor_i::lon, MAVLinkSendMessage, GpsState::num_sv, GpsState::pdop, GpsState::sacc, and GpsState::vacc.

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ mavlink_send_gps_status()

static void mavlink_send_gps_status ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Send gps status.

The GPS status message consists of:

  • satellites visible
  • satellite pnr[] (ignored)
  • satellite used[] (ignored)
  • satellite elevation[] (ignored)
  • satellite azimuth[] (ignored)
  • satellite snr[] (ignored)

Definition at line 718 of file mavlink.c.

References SVinfo::azim, SVinfo::cno, SVinfo::elev, gps, GPS_NB_CHANNELS, MAVLinkSendMessage, Min, GpsState::num_sv, SVinfo::svid, and GpsState::svinfos.

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the caller graph for this function:

◆ mavlink_send_heartbeat()

static void mavlink_send_heartbeat ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

◆ mavlink_send_local_position_ned()

static void mavlink_send_local_position_ned ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 570 of file mavlink.c.

References get_sys_time_msec(), MAVLinkSendMessage, stateGetPositionNed_f(), and stateGetSpeedNed_f().

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ mavlink_send_params()

static void mavlink_send_params ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Send the parameters.

Definition at line 621 of file mavlink.c.

References mavlink_param_names, mavlink_params_idx, and MAVLinkSendMessage.

Referenced by mavlink_periodic().

+ Here is the caller graph for this function:

◆ mavlink_send_rc_channels()

static void mavlink_send_rc_channels ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 764 of file mavlink.c.

References get_sys_time_msec(), MAVLinkSendMessage, PPM_PULSES, and RC_CHANNELS.

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ mavlink_send_sys_status()

static void mavlink_send_sys_status ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Send the system status.

TODO: FIXME

Definition at line 518 of file mavlink.c.

References Electrical::current, electrical, MAVLinkSendMessage, UAV_SENSORS, and Electrical::vsupply.

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the caller graph for this function:

◆ mavlink_send_system_time()

static void mavlink_send_system_time ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Send SYSTEM_TIME.

  • time_unix_usec
  • time_boot_ms

Definition at line 548 of file mavlink.c.

References get_sys_time_msec(), and MAVLinkSendMessage.

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ mavlink_send_vfr_hud()

static void mavlink_send_vfr_hud ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Send Metrics typically displayed on a HUD for fixed wing aircraft.

Definition at line 819 of file mavlink.c.

References LlaCoor_f::alt, commands, heading, LtpDef_f::hmsl, LtpDef_f::lla, MAVLinkSendMessage, MAX_PPRZ, State::ned_origin_f, state, stateGetAirspeed_f(), stateGetHorizontalSpeedNorm_f(), stateGetNedToBodyEulers_f(), stateGetPositionLla_f(), and stateGetSpeedNed_f().

Referenced by mavlink_init(), and mavlink_periodic().

+ Here is the call graph for this function:
+ Here is the caller graph for this function:

◆ settings_idx_from_param_id()

static int16_t settings_idx_from_param_id ( char *  param_id)
static

Definition at line 183 of file mavlink.c.

References mavlink_param_names.

Referenced by mavlink_common_message_handler().

+ Here is the caller graph for this function:

Variable Documentation

◆ custom_version

uint8_t custom_version[8]
static

first 8 bytes (16 chars) of GIT SHA1

Definition at line 77 of file mavlink.c.

Referenced by mavlink_init().

◆ mavlink_param_names

char mavlink_param_names[NB_SETTING][16+1] = SETTINGS_NAMES_SHORT
static

mavlink parameter names.

16 chars + 1 NULL termination.

Definition at line 76 of file mavlink.c.

Referenced by mavlink_common_message_handler(), mavlink_send_params(), and settings_idx_from_param_id().

◆ mavlink_params_idx

uint8_t mavlink_params_idx = NB_SETTING
static

Transmitting parameters index.

Definition at line 72 of file mavlink.c.

Referenced by mavlink_common_message_handler(), and mavlink_send_params().

◆ mavlink_system

mavlink_system_t mavlink_system

◆ mission_mgr