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"
54#include "pprz_version.h"
58#if defined RADIO_CONTROL
63#ifndef MAV_AUTOPILOT_ID
64#define MAV_AUTOPILOT_ID MAV_AUTOPILOT_PPZ
108#define MAVLINK_SYSID AC_ID
111#if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
125#if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
150#if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
163#if !defined (TELEMETRY_MAVLINK_NB_MSG)
194 for (
j = 0;
j < 16;
j++) {
234 switch (
msg->msgid) {
257#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
258 uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100;
259 int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2;
260 int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2;
261 int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100;
282 if (cmd.param_index == -1) {
287 if (cmd.param_index > -1) {
312 !
isnan(set.param_value) && !
isinf(set.param_value)) {
328#ifdef ROTORCRAFT_FIRMWARE
336 switch (cmd.command) {
341 int rc =
snprintf(
error_msg, 200,
"Do set %d: %0.0f %0.0f (%d)", cmd.command, cmd.param1, cmd.param2, cmd.target_system);
350 MAVLINK_DEBUG(
"got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1);
352 if (cmd.param1 > 0.5) {
365 MAVLINK_DEBUG(
"got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1);
367 if (cmd.param1 > 0.5) {
389#ifdef WP_ML_global_target
408 MAVLINK_DEBUG(
"NED origin not set, cannot set target position\n");
456 if (!(
target.type_mask & 0b1110000000100000)) {
457 switch (
target.coordinate_frame) {
463 MAVLINK_DEBUG(
"set position target, frame LOCAL_OFFSET_NED\n");
467 MAVLINK_DEBUG(
"set position target, frame BODY_OFFSET_NED\n");
474 else if (!(
target.type_mask & 0b0001110000100000)) {
476 switch (
target.coordinate_frame) {
498#pragma GCC diagnostic push
499#pragma GCC diagnostic ignored "-Wunused-parameter"
509#if defined(FIXEDWING_FIRMWARE)
527#elif defined(ROTORCRAFT_FIRMWARE)
560#error "mavlink datalink: unsupported firmware"
594#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)
723 18446744073709551615U,
827#if defined RADIO_CONTROL
830#ifdef PPM_PULSE_TYPE_POSITIVE
831#define RC_CHANNELS RADIO_CTL_NB
833#define PPM_PULSES(_i) ((_i) < RADIO_CTL_NB ? ppm_pulses[(_i)] : UINT16_MAX)
836#define RC_CHANNELS RADIO_CONTROL_NB_CHANNEL
837#define PPM_OF_PPRZ(_v) ((_v) / 19.2 + 1500)
838#define PPM_PULSES(_i) ((_i) < RADIO_CONTROL_NB_CHANNEL ? PPM_OF_PPRZ(radio_control.values[(_i)]) + MAX_PPRZ : UINT16_MAX)
844#if defined RADIO_CONTROL
906#elif defined COMMAND_THROTTLE
921#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 FIXME is i...
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.
struct Electrical electrical
Interface for electrical status: supply voltage, current, battery status, etc.
float energy
consumed energy in Wh
float current
current in A
float charge
consumed electric charge in Ah
float vsupply
supply voltage in V
#define AP_MODE_MANUAL
AP modes.
struct GpsState gps
global GPS state
Device independent GPS code (interface)
int16_t azim
azimuth in deg
int32_t hmsl
height above mean sea level (MSL) in mm
int8_t elev
elevation in deg
struct LlaCoor_i lla_pos
position in LLA (lat,lon: deg*1e7; alt: mm over ellipsoid)
uint32_t sacc
speed accuracy in cm/s
int32_t course
GPS course over ground in rad*1e7, [0, 2*Pi]*1e7 (CW/north)
uint8_t cno
Carrier to Noise Ratio (Signal Strength) in dbHz.
uint32_t hacc
horizontal accuracy in cm
uint16_t pdop
position dilution of precision scaled by 100
uint8_t svid
Satellite ID.
uint16_t gspeed
norm of 2d ground speed in cm/s
uint32_t vacc
vertical accuracy in cm
struct SVinfo svinfos[GPS_NB_CHANNELS]
holds information from the Space Vehicles (Satellites)
uint8_t num_sv
number of sat in fix
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
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).
bool ned_initialized_i
true if local int coordinate frame is initialsed
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]
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
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_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()
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.
static const struct usb_device_descriptor dev
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.