30 #pragma GCC diagnostic push
31 #pragma GCC diagnostic ignored "-Waddress-of-packed-member"
32 #pragma GCC diagnostic ignored "-Wswitch-default"
33 #include "mavlink/paparazzi/mavlink.h"
34 #pragma GCC diagnostic pop
36 #if PERIODIC_TELEMETRY
37 #define PERIODIC_C_MAVLINK
39 #include "generated/periodic_telemetry.h"
40 #ifndef TELEMETRY_MAVLINK_NB_MSG
41 #warning Using hardcoded msg periods. To customize specify a <process name="Mavlink" type="mavlink"> in your telemetry file.
45 #include "generated/airframe.h"
46 #include "generated/modules.h"
47 #include "generated/settings.h"
52 #include "pprz_version.h"
56 #if defined RADIO_CONTROL
61 #ifndef MAV_AUTOPILOT_ID
62 #define MAV_AUTOPILOT_ID MAV_AUTOPILOT_PPZ
104 #ifndef MAVLINK_SYSID
105 #define MAVLINK_SYSID AC_ID
108 #if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
109 struct telemetry_cb_slots mavlink_cbs[TELEMETRY_MAVLINK_NB_MSG] = TELEMETRY_MAVLINK_CBS;
110 struct periodic_telemetry mavlink_telemetry = { TELEMETRY_MAVLINK_NB_MSG, mavlink_cbs };
122 #if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
146 #if PERIODIC_TELEMETRY && defined TELEMETRY_MAVLINK_NB_MSG
149 periodic_telemetry_send_Mavlink(&mavlink_telemetry, NULL, NULL);
159 #if !defined (TELEMETRY_MAVLINK_NB_MSG)
189 for (i = 0; i < NB_SETTING; i++) {
190 for (j = 0; j < 16; j++) {
213 mavlink_message_t
msg;
221 if (mavlink_parse_char(MAVLINK_COMM_0, test, &
msg, &
status)) {
230 switch (
msg->msgid) {
231 case MAVLINK_MSG_ID_HEARTBEAT:
235 case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: {
236 mavlink_request_data_stream_t cmd;
237 mavlink_msg_request_data_stream_decode(
msg, &cmd);
239 mavlink_msg_data_stream_send(MAVLINK_COMM_0, cmd.req_stream_id, 0, 0);
245 case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: {
246 mavlink_rc_channels_override_t cmd;
247 mavlink_msg_rc_channels_override_decode(
msg, &cmd);
248 #if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
249 uint8_t thrust = (cmd.chan3_raw - 950) * 127 / 1100;
250 int8_t roll = -(cmd.chan1_raw - 1500) * 255 / 1100 / 2;
251 int8_t pitch = -(cmd.chan2_raw - 1500) * 255 / 1100 / 2;
252 int8_t yaw = -(cmd.chan4_raw - 1500) * 255 / 1100;
261 case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
267 case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
268 mavlink_param_request_read_t cmd;
269 mavlink_msg_param_request_read_decode(
msg, &cmd);
272 if (cmd.param_index == -1) {
277 if (cmd.param_index > -1) {
278 mavlink_msg_param_value_send(MAVLINK_COMM_0,
280 settings_get_value(cmd.param_index),
281 MAV_PARAM_TYPE_REAL32,
289 case MAVLINK_MSG_ID_PARAM_SET: {
290 mavlink_param_set_t set;
291 mavlink_msg_param_set_decode(
msg, &set);
294 if ((
uint8_t) set.target_system == AC_ID) {
301 if (set.param_type == MAV_PARAM_TYPE_REAL32 &&
302 !isnan(set.param_value) && !isinf(set.param_value)) {
303 DlSetting(
idx, set.param_value);
305 mavlink_msg_param_value_send(MAVLINK_COMM_0,
307 settings_get_value(
idx),
308 MAV_PARAM_TYPE_REAL32,
318 #ifdef ROTORCRAFT_FIRMWARE
320 case MAVLINK_MSG_ID_COMMAND_LONG: {
321 mavlink_command_long_t cmd;
322 mavlink_msg_command_long_decode(
msg, &cmd);
324 if ((
uint8_t) cmd.target_system == AC_ID) {
325 uint8_t result = MAV_RESULT_UNSUPPORTED;
326 switch (cmd.command) {
327 case MAV_CMD_NAV_GUIDED_ENABLE:
328 MAVLINK_DEBUG(
"got cmd NAV_GUIDED_ENABLE: %f\n", cmd.param1);
329 result = MAV_RESULT_FAILED;
330 if (cmd.param1 > 0.5) {
333 result = MAV_RESULT_ACCEPTED;
341 case MAV_CMD_COMPONENT_ARM_DISARM:
343 MAVLINK_DEBUG(
"got cmd COMPONENT_ARM_DISARM: %f\n", cmd.param1);
344 result = MAV_RESULT_FAILED;
345 if (cmd.param1 > 0.5) {
348 result = MAV_RESULT_ACCEPTED;
353 result = MAV_RESULT_ACCEPTED;
361 mavlink_msg_command_ack_send(MAVLINK_COMM_0, cmd.command, result, 0, UINT8_MAX,
msg->sysid,
msg->compid);
367 case MAVLINK_MSG_ID_SET_MODE: {
368 mavlink_set_mode_t
mode;
369 mavlink_msg_set_mode_decode(
msg, &
mode);
370 if (
mode.target_system == AC_ID) {
372 if (
mode.base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
378 if (
mode.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
381 else if (
mode.base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
388 case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED: {
389 mavlink_set_position_target_local_ned_t
target;
390 mavlink_msg_set_position_target_local_ned_decode(
msg, &
target);
392 if (
target.target_system == AC_ID) {
395 if (!(
target.type_mask & 0b1110000000100000)) {
396 switch (
target.coordinate_frame) {
397 case MAV_FRAME_LOCAL_NED:
401 case MAV_FRAME_LOCAL_OFFSET_NED:
402 MAVLINK_DEBUG(
"set position target, frame LOCAL_OFFSET_NED\n");
405 case MAV_FRAME_BODY_OFFSET_NED:
406 MAVLINK_DEBUG(
"set position target, frame BODY_OFFSET_NED\n");
413 else if (!(
target.type_mask & 0b0001110000100000)) {
415 switch (
target.coordinate_frame) {
416 case MAV_FRAME_LOCAL_NED:
437 #pragma GCC diagnostic push
438 #pragma GCC diagnostic ignored "-Wunused-parameter"
445 uint8_t mav_state = MAV_STATE_CALIBRATING;
447 #if defined(FIXEDWING_FIRMWARE)
448 uint8_t mav_type = MAV_TYPE_FIXED_WING;
451 mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
454 mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
457 mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
460 mav_mode |= MAV_MODE_FLAG_GUIDED_ENABLED | MAV_MODE_FLAG_AUTO_ENABLED;
465 #elif defined(ROTORCRAFT_FIRMWARE)
466 uint8_t mav_type = MAV_TYPE_QUADROTOR;
469 mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
475 mav_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
485 mav_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
488 mav_mode |= MAV_MODE_FLAG_AUTO_ENABLED;
491 mav_mode = MAV_MODE_FLAG_GUIDED_ENABLED;
496 #error "mavlink datalink: unsupported firmware"
500 mav_state = MAV_STATE_STANDBY;
502 mav_state = MAV_STATE_ACTIVE;
503 mav_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
506 mavlink_msg_heartbeat_send(MAVLINK_COMM_0,
521 #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)
523 mavlink_msg_sys_status_send(MAVLINK_COMM_0,
559 mavlink_msg_attitude_send(MAVLINK_COMM_0,
572 mavlink_msg_local_position_ned_send(MAVLINK_COMM_0,
593 mavlink_msg_global_position_int_send(MAVLINK_COMM_0,
609 mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0,
627 mavlink_msg_param_value_send(MAVLINK_COMM_0,
630 MAV_PARAM_TYPE_REAL32,
641 static uint32_t ver = PPRZ_VERSION_INT;
643 get_pprz_git_version((
uint8_t *)&sha);
644 mavlink_msg_autopilot_version_send(MAVLINK_COMM_0,
662 float repr_offset_q[4] = {0, 0, 0, 0};
663 mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0,
687 mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0,
727 for (
uint8_t i = 0; i < nb_sats; i++) {
743 mavlink_msg_gps_status_send(MAVLINK_COMM_0,
744 gps.
num_sv, prn, used, elevation, azimuth, snr);
749 #if defined RADIO_CONTROL
752 #ifdef PPM_PULSE_TYPE_POSITIVE
753 #define RC_CHANNELS RADIO_CTL_NB
755 #define PPM_PULSES(_i) ((_i) < RADIO_CTL_NB ? ppm_pulses[(_i)] : UINT16_MAX)
758 #define RC_CHANNELS RADIO_CONTROL_NB_CHANNEL
759 #define PPM_OF_PPRZ(_v) ((_v) / 19.2 + 1500)
760 #define PPM_PULSES(_i) ((_i) < RADIO_CONTROL_NB_CHANNEL ? PPM_OF_PPRZ(radio_control.values[(_i)]) + MAX_PPRZ : UINT16_MAX)
766 #if defined RADIO_CONTROL
767 mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
778 mavlink_msg_rc_channels_send(MAVLINK_COMM_0,
781 UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
782 UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
783 UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
784 UINT16_MAX, UINT16_MAX, UINT16_MAX, UINT16_MAX,
785 UINT16_MAX, UINT16_MAX, 255);
797 mavlink_msg_battery_status_send(MAVLINK_COMM_0,
808 MAV_BATTERY_CHARGE_STATE_UNDEFINED,
810 MAV_BATTERY_MODE_UNKNOWN,
826 #ifdef COMMAND_THRUST
828 #elif defined COMMAND_THROTTLE
832 mavlink_msg_vfr_hud_send(MAVLINK_COMM_0,
843 #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_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.
pprz_t commands[COMMANDS_NB]
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
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).
struct LtpDef_f ned_origin_f
Definition of the local (flat earth) coordinate system.
struct LtpDef_i ned_origin_i
Definition of the local (flat earth) coordinate system.
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
static struct LlaCoor_f * stateGetPositionLla_f(void)
Get position in LLA coordinates (float).
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).
uint8_t msg[10]
Buffer used for general comunication over SPI (out buffer)
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_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()
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 ...
float alt
in meters (normally above WGS84 reference ellipsoid)
float hmsl
Height above mean sea level in meters.
struct LlaCoor_f lla
origin of local frame in LLA
Generic interface for radio control modules.
void parse_rc_4ch_datalink(uint8_t mode, uint8_t throttle, int8_t roll, int8_t pitch, int8_t yaw)
Decode datalink message to get rc values with RC_4CH 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 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.
int8_t register_periodic_telemetry(struct periodic_telemetry *_pt, uint8_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.