Commit d950361a authored by LM's avatar LM

Updated MAVLink v1.0 release candidate sources

parent bdcfb523
*~
doc/html
doc/*.log
missionlib/testing/*.xcodeproj/*
/** @file
* @brief MAVLink comm protocol.
* @see http://qgroundcontrol.org/mavlink/
* Generated on Sunday, July 31 2011, 15:11 UTC
* Generated on Tuesday, August 9 2011, 16:16 UTC
*/
#ifndef ARDUPILOTMEGA_H
#define ARDUPILOTMEGA_H
......@@ -33,6 +33,13 @@ extern "C" {
// MESSAGE DEFINITIONS
// MESSAGE CRC KEYS
#undef MAVLINK_MESSAGE_KEYS
#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 165, 44, 249, 133, 0, 0, 0, 0, 0, 0, 0, 112, 34, 81, 152, 0, 223, 28, 99, 28, 21, 243, 240, 204, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 157, 125, 0, 0, 0, 85, 211, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 53, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 43, 141, 211, 166 }
#ifdef __cplusplus
}
#endif
......
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Sunday, July 31 2011, 15:11 UTC
* Generated on Tuesday, August 9 2011, 16:16 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H
#pragma pack(push,1)
#include "mavlink_options.h"
#include "ardupilotmega.h"
#ifdef MAVLINK_CHECK_LENGTH
#include "lengths.h"
#ifdef MAVLINK_DATA
#include "mavlink_data.h"
#endif
#pragma pack(pop)
#endif
This diff is collapsed.
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Sunday, July 31 2011, 15:12 UTC
* Generated on Tuesday, August 9 2011, 16:16 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H
#pragma pack(push,1)
#include "mavlink_options.h"
#include "common.h"
#ifdef MAVLINK_CHECK_LENGTH
#include "lengths.h"
#ifdef MAVLINK_DATA
#include "mavlink_data.h"
#endif
#pragma pack(pop)
#endif
......@@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_action_encode(uint8_t system_id, uint8_t comp
* @param target_component The component executing the action
* @param action The action id
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_action_t *p = (mavlink_action_t *)&msg.payload[0];
p->target = target; // uint8_t:The system executing the action
p->target_component = target_component; // uint8_t:The component executing the action
p->action = action; // uint8_t:The action id
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_ACTION_LEN;
msg.msgid = MAVLINK_MSG_ID_ACTION;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_action_send(mavlink_channel_t chan, uint8_t target, uint8_t target_component, uint8_t action)
{
mavlink_header_t hdr;
......
......@@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_action_ack_encode(uint8_t system_id, uint8_t
* @param action The action id
* @param result 0: Action DENIED, 1: Action executed
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_action_ack_t *p = (mavlink_action_ack_t *)&msg.payload[0];
p->action = action; // uint8_t:The action id
p->result = result; // uint8_t:0: Action DENIED, 1: Action executed
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_ACTION_ACK_LEN;
msg.msgid = MAVLINK_MSG_ID_ACTION_ACK;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_action_ack_send(mavlink_channel_t chan, uint8_t action, uint8_t result)
{
mavlink_header_t hdr;
......
......@@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_attitude_encode(uint8_t system_id, uint8_t co
* @param pitchspeed Pitch angular speed (rad/s)
* @param yawspeed Yaw angular speed (rad/s)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_attitude_t *p = (mavlink_attitude_t *)&msg.payload[0];
p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
p->roll = roll; // float:Roll angle (rad)
p->pitch = pitch; // float:Pitch angle (rad)
p->yaw = yaw; // float:Yaw angle (rad)
p->rollspeed = rollspeed; // float:Roll angular speed (rad/s)
p->pitchspeed = pitchspeed; // float:Pitch angular speed (rad/s)
p->yawspeed = yawspeed; // float:Yaw angular speed (rad/s)
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_ATTITUDE_LEN;
msg.msgid = MAVLINK_MSG_ID_ATTITUDE;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_attitude_send(mavlink_channel_t chan, uint64_t usec, float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed)
{
mavlink_header_t hdr;
......
......@@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_attitude_controller_output_encode(uint8_t sys
* @param yaw Attitude yaw: -128: -100%, 127: +100%
* @param thrust Attitude thrust: -128: -100%, 127: +100%
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_attitude_controller_output_t *p = (mavlink_attitude_controller_output_t *)&msg.payload[0];
p->enabled = enabled; // uint8_t:1: enabled, 0: disabled
p->roll = roll; // int8_t:Attitude roll: -128: -100%, 127: +100%
p->pitch = pitch; // int8_t:Attitude pitch: -128: -100%, 127: +100%
p->yaw = yaw; // int8_t:Attitude yaw: -128: -100%, 127: +100%
p->thrust = thrust; // int8_t:Attitude thrust: -128: -100%, 127: +100%
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT_LEN;
msg.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROLLER_OUTPUT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_attitude_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t roll, int8_t pitch, int8_t yaw, int8_t thrust)
{
mavlink_header_t hdr;
......
......@@ -68,32 +68,9 @@ static inline uint16_t mavlink_msg_auth_key_encode(uint8_t system_id, uint8_t co
*
* @param key key
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_auth_key_t *p = (mavlink_auth_key_t *)&msg.payload[0];
memcpy(p->key, key, sizeof(p->key)); // char[32]:key
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_AUTH_KEY_LEN;
msg.msgid = MAVLINK_MSG_ID_AUTH_KEY;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_auth_key_send(mavlink_channel_t chan, const char* key)
{
mavlink_header_t hdr;
......
......@@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_boot_encode(uint8_t system_id, uint8_t compon
*
* @param version The onboard software version
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_boot_t *p = (mavlink_boot_t *)&msg.payload[0];
p->version = version; // uint32_t:The onboard software version
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_BOOT_LEN;
msg.msgid = MAVLINK_MSG_ID_BOOT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_boot_send(mavlink_channel_t chan, uint32_t version)
{
mavlink_header_t hdr;
......
......@@ -86,35 +86,9 @@ static inline uint16_t mavlink_msg_change_operator_control_encode(uint8_t system
* @param version 0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
* @param passkey Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_change_operator_control_t *p = (mavlink_change_operator_control_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System the GCS requests control for
p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV
p->version = version; // uint8_t:0: key as plaintext, 1-255: future, different hashing/encryption variants. The GCS should in general use the safest mode possible initially and then gradually move down the encryption level if it gets a NACK message indicating an encryption mismatch.
memcpy(p->passkey, passkey, sizeof(p->passkey)); // char[25]:Password / Key, depending on version plaintext or encrypted. 25 or less characters, NULL terminated. The characters may involve A-Z, a-z, 0-9, and "!?,.-"
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_LEN;
msg.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_change_operator_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t control_request, uint8_t version, const char* passkey)
{
mavlink_header_t hdr;
......
......@@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_change_operator_control_ack_encode(uint8_t sy
* @param control_request 0: request control of this MAV, 1: Release control of this MAV
* @param ack 0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_change_operator_control_ack_t *p = (mavlink_change_operator_control_ack_t *)&msg.payload[0];
p->gcs_system_id = gcs_system_id; // uint8_t:ID of the GCS this message
p->control_request = control_request; // uint8_t:0: request control of this MAV, 1: Release control of this MAV
p->ack = ack; // uint8_t:0: ACK, 1: NACK: Wrong passkey, 2: NACK: Unsupported passkey encryption method, 3: NACK: Already under control
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK_LEN;
msg.msgid = MAVLINK_MSG_ID_CHANGE_OPERATOR_CONTROL_ACK;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_change_operator_control_ack_send(mavlink_channel_t chan, uint8_t gcs_system_id, uint8_t control_request, uint8_t ack)
{
mavlink_header_t hdr;
......
......@@ -6,14 +6,14 @@
typedef struct __mavlink_command_t
{
uint8_t target_system; ///< System which should execute the command
uint8_t target_component; ///< Component which should execute the command, 0 for all components
uint8_t command; ///< Command ID, as defined by MAV_CMD enum.
uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
float param1; ///< Parameter 1, as defined by MAV_CMD enum.
float param2; ///< Parameter 2, as defined by MAV_CMD enum.
float param3; ///< Parameter 3, as defined by MAV_CMD enum.
float param4; ///< Parameter 4, as defined by MAV_CMD enum.
uint8_t target_system; ///< System which should execute the command
uint8_t target_component; ///< Component which should execute the command, 0 for all components
uint8_t command; ///< Command ID, as defined by MAV_CMD enum.
uint8_t confirmation; ///< 0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
} mavlink_command_t;
......@@ -109,39 +109,9 @@ static inline uint16_t mavlink_msg_command_encode(uint8_t system_id, uint8_t com
* @param param3 Parameter 3, as defined by MAV_CMD enum.
* @param param4 Parameter 4, as defined by MAV_CMD enum.
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_command_t *p = (mavlink_command_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System which should execute the command
p->target_component = target_component; // uint8_t:Component which should execute the command, 0 for all components
p->command = command; // uint8_t:Command ID, as defined by MAV_CMD enum.
p->confirmation = confirmation; // uint8_t:0: First transmission of this command. 1-255: Confirmation transmissions (e.g. for kill command)
p->param1 = param1; // float:Parameter 1, as defined by MAV_CMD enum.
p->param2 = param2; // float:Parameter 2, as defined by MAV_CMD enum.
p->param3 = param3; // float:Parameter 3, as defined by MAV_CMD enum.
p->param4 = param4; // float:Parameter 4, as defined by MAV_CMD enum.
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_COMMAND_LEN;
msg.msgid = MAVLINK_MSG_ID_COMMAND;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_command_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t command, uint8_t confirmation, float param1, float param2, float param3, float param4)
{
mavlink_header_t hdr;
......
......@@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t
* @param command Current airspeed in m/s
* @param result 1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_command_ack_t *p = (mavlink_command_ack_t *)&msg.payload[0];
p->command = command; // float:Current airspeed in m/s
p->result = result; // float:1: Action ACCEPTED and EXECUTED, 1: Action TEMPORARY REJECTED/DENIED, 2: Action PERMANENTLY DENIED, 3: Action UNKNOWN/UNSUPPORTED, 4: Requesting CONFIRMATION
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_COMMAND_ACK_LEN;
msg.msgid = MAVLINK_MSG_ID_COMMAND_ACK;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, float command, float result)
{
mavlink_header_t hdr;
......
......@@ -109,39 +109,9 @@ static inline uint16_t mavlink_msg_control_status_encode(uint8_t system_id, uint
* @param control_pos_z 0: Z position control disabled, 1: enabled
* @param control_pos_yaw 0: Yaw angle control disabled, 1: enabled
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_control_status_t *p = (mavlink_control_status_t *)&msg.payload[0];
p->position_fix = position_fix; // uint8_t:Position fix: 0: lost, 2: 2D position fix, 3: 3D position fix
p->vision_fix = vision_fix; // uint8_t:Vision position fix: 0: lost, 1: 2D local position hold, 2: 2D global position fix, 3: 3D global position fix
p->gps_fix = gps_fix; // uint8_t:GPS position fix: 0: no reception, 1: Minimum 1 satellite, but no position fix, 2: 2D position fix, 3: 3D position fix
p->ahrs_health = ahrs_health; // uint8_t:Attitude estimation health: 0: poor, 255: excellent
p->control_att = control_att; // uint8_t:0: Attitude control disabled, 1: enabled
p->control_pos_xy = control_pos_xy; // uint8_t:0: X, Y position control disabled, 1: enabled
p->control_pos_z = control_pos_z; // uint8_t:0: Z position control disabled, 1: enabled
p->control_pos_yaw = control_pos_yaw; // uint8_t:0: Yaw angle control disabled, 1: enabled
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_CONTROL_STATUS_LEN;
msg.msgid = MAVLINK_MSG_ID_CONTROL_STATUS;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_control_status_send(mavlink_channel_t chan, uint8_t position_fix, uint8_t vision_fix, uint8_t gps_fix, uint8_t ahrs_health, uint8_t control_att, uint8_t control_pos_xy, uint8_t control_pos_z, uint8_t control_pos_yaw)
{
mavlink_header_t hdr;
......
......@@ -6,8 +6,8 @@
typedef struct __mavlink_debug_t
{
uint8_t ind; ///< index of debug variable
float value; ///< DEBUG value
uint8_t ind; ///< index of debug variable
} mavlink_debug_t;
......@@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_debug_encode(uint8_t system_id, uint8_t compo
* @param ind index of debug variable
* @param value DEBUG value
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_debug_t *p = (mavlink_debug_t *)&msg.payload[0];
p->ind = ind; // uint8_t:index of debug variable
p->value = value; // float:DEBUG value
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_DEBUG_LEN;
msg.msgid = MAVLINK_MSG_ID_DEBUG;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_debug_send(mavlink_channel_t chan, uint8_t ind, float value)
{
mavlink_header_t hdr;
......
......@@ -6,11 +6,11 @@
typedef struct __mavlink_debug_vect_t
{
char name[10]; ///< Name
uint64_t usec; ///< Timestamp
float x; ///< x
float y; ///< y
float z; ///< z
char name[10]; ///< Name
} mavlink_debug_vect_t;
#define MAVLINK_MSG_DEBUG_VECT_FIELD_NAME_LEN 10
......@@ -92,36 +92,9 @@ static inline uint16_t mavlink_msg_debug_vect_encode(uint8_t system_id, uint8_t
* @param y y
* @param z z
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_debug_vect_t *p = (mavlink_debug_vect_t *)&msg.payload[0];
memcpy(p->name, name, sizeof(p->name)); // char[10]:Name
p->usec = usec; // uint64_t:Timestamp
p->x = x; // float:x
p->y = y; // float:y
p->z = z; // float:z
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_DEBUG_VECT_LEN;
msg.msgid = MAVLINK_MSG_ID_DEBUG_VECT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_debug_vect_send(mavlink_channel_t chan, const char* name, uint64_t usec, float x, float y, float z)
{
mavlink_header_t hdr;
......
This diff is collapsed.
......@@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_global_position_encode(uint8_t system_id, uin
* @param vy Y Speed (in Longitude direction, positive: going east)
* @param vz Z Speed (in Altitude direction, positive: going up)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_global_position_t *p = (mavlink_global_position_t *)&msg.payload[0];
p->usec = usec; // uint64_t:Timestamp (microseconds since unix epoch)
p->lat = lat; // float:Latitude, in degrees
p->lon = lon; // float:Longitude, in degrees
p->alt = alt; // float:Absolute altitude, in meters
p->vx = vx; // float:X Speed (in Latitude direction, positive: going north)
p->vy = vy; // float:Y Speed (in Longitude direction, positive: going east)
p->vz = vz; // float:Z Speed (in Altitude direction, positive: going up)
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_GLOBAL_POSITION_LEN;
msg.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_global_position_send(mavlink_channel_t chan, uint64_t usec, float lat, float lon, float alt, float vx, float vy, float vz)
{
mavlink_header_t hdr;
......
......@@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_global_position_int_encode(uint8_t system_id,
* @param vy Ground Y Speed (Longitude), expressed as m/s * 100
* @param vz Ground Z Speed (Altitude), expressed as m/s * 100
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_global_position_int_t *p = (mavlink_global_position_int_t *)&msg.payload[0];
p->lat = lat; // int32_t:Latitude, expressed as * 1E7
p->lon = lon; // int32_t:Longitude, expressed as * 1E7
p->alt = alt; // int32_t:Altitude in meters, expressed as * 1000 (millimeters)
p->vx = vx; // int16_t:Ground X Speed (Latitude), expressed as m/s * 100
p->vy = vy; // int16_t:Ground Y Speed (Longitude), expressed as m/s * 100
p->vz = vz; // int16_t:Ground Z Speed (Altitude), expressed as m/s * 100
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_GLOBAL_POSITION_INT_LEN;
msg.msgid = MAVLINK_MSG_ID_GLOBAL_POSITION_INT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_global_position_int_send(mavlink_channel_t chan, int32_t lat, int32_t lon, int32_t alt, int16_t vx, int16_t vy, int16_t vz)
{
mavlink_header_t hdr;
......
......@@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_gps_local_origin_set_encode(uint8_t system_id
* @param longitude Longitude (WGS84), expressed as * 1E7
* @param altitude Altitude(WGS84), expressed as * 1000
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_gps_local_origin_set_t *p = (mavlink_gps_local_origin_set_t *)&msg.payload[0];
p->latitude = latitude; // int32_t:Latitude (WGS84), expressed as * 1E7
p->longitude = longitude; // int32_t:Longitude (WGS84), expressed as * 1E7
p->altitude = altitude; // int32_t:Altitude(WGS84), expressed as * 1000
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET_LEN;
msg.msgid = MAVLINK_MSG_ID_GPS_LOCAL_ORIGIN_SET;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_local_origin_set_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude)
{
mavlink_header_t hdr;
......
......@@ -7,7 +7,6 @@
typedef struct __mavlink_gps_raw_t
{
uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
float lat; ///< Latitude in degrees
float lon; ///< Longitude in degrees
float alt; ///< Altitude in meters
......@@ -15,6 +14,7 @@ typedef struct __mavlink_gps_raw_t
float epv; ///< GPS VDOP
float v; ///< GPS ground speed
float hdg; ///< Compass heading in degrees, 0..360 degrees
uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
} mavlink_gps_raw_t;
......@@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_gps_raw_encode(uint8_t system_id, uint8_t com
* @param v GPS ground speed
* @param hdg Compass heading in degrees, 0..360 degrees
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_gps_raw_t *p = (mavlink_gps_raw_t *)&msg.payload[0];
p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
p->lat = lat; // float:Latitude in degrees
p->lon = lon; // float:Longitude in degrees
p->alt = alt; // float:Altitude in meters
p->eph = eph; // float:GPS HDOP
p->epv = epv; // float:GPS VDOP
p->v = v; // float:GPS ground speed
p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_GPS_RAW_LEN;
msg.msgid = MAVLINK_MSG_ID_GPS_RAW;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_raw_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, float lat, float lon, float alt, float eph, float epv, float v, float hdg)
{
mavlink_header_t hdr;
......
......@@ -7,7 +7,6 @@
typedef struct __mavlink_gps_raw_int_t
{
uint64_t usec; ///< Timestamp (microseconds since UNIX epoch or microseconds since system boot)
uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
int32_t lat; ///< Latitude in 1E7 degrees
int32_t lon; ///< Longitude in 1E7 degrees
int32_t alt; ///< Altitude in 1E3 meters (millimeters)
......@@ -15,6 +14,7 @@ typedef struct __mavlink_gps_raw_int_t
float epv; ///< GPS VDOP
float v; ///< GPS ground speed (m/s)
float hdg; ///< Compass heading in degrees, 0..360 degrees
uint8_t fix_type; ///< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
} mavlink_gps_raw_int_t;
......@@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t
* @param v GPS ground speed (m/s)
* @param hdg Compass heading in degrees, 0..360 degrees
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_gps_raw_int_t *p = (mavlink_gps_raw_int_t *)&msg.payload[0];
p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
p->fix_type = fix_type; // uint8_t:0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.
p->lat = lat; // int32_t:Latitude in 1E7 degrees
p->lon = lon; // int32_t:Longitude in 1E7 degrees
p->alt = alt; // int32_t:Altitude in 1E3 meters (millimeters)
p->eph = eph; // float:GPS HDOP
p->epv = epv; // float:GPS VDOP
p->v = v; // float:GPS ground speed (m/s)
p->hdg = hdg; // float:Compass heading in degrees, 0..360 degrees
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_GPS_RAW_INT_LEN;
msg.msgid = MAVLINK_MSG_ID_GPS_RAW_INT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, float eph, float epv, float v, float hdg)
{
mavlink_header_t hdr;
......
......@@ -6,11 +6,11 @@
typedef struct __mavlink_gps_set_global_origin_t
{
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
int32_t latitude; ///< global position * 1E7
int32_t longitude; ///< global position * 1E7
int32_t altitude; ///< global position * 1000
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
} mavlink_gps_set_global_origin_t;
......@@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_gps_set_global_origin_encode(uint8_t system_i
* @param longitude global position * 1E7
* @param altitude global position * 1000
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_gps_set_global_origin_t *p = (mavlink_gps_set_global_origin_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->latitude = latitude; // int32_t:global position * 1E7
p->longitude = longitude; // int32_t:global position * 1E7
p->altitude = altitude; // int32_t:global position * 1000
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN_LEN;
msg.msgid = MAVLINK_MSG_ID_GPS_SET_GLOBAL_ORIGIN;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_set_global_origin_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int32_t latitude, int32_t longitude, int32_t altitude)
{
mavlink_header_t hdr;
......
......@@ -102,37 +102,9 @@ static inline uint16_t mavlink_msg_gps_status_encode(uint8_t system_id, uint8_t
* @param satellite_azimuth Direction of satellite, 0: 0 deg, 255: 360 deg.
* @param satellite_snr Signal to noise ratio of satellite
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_gps_status_t *p = (mavlink_gps_status_t *)&msg.payload[0];
p->satellites_visible = satellites_visible; // uint8_t:Number of satellites visible
memcpy(p->satellite_prn, satellite_prn, sizeof(p->satellite_prn)); // uint8_t[20]:Global satellite ID
memcpy(p->satellite_used, satellite_used, sizeof(p->satellite_used)); // uint8_t[20]:0: Satellite not used, 1: used for localization
memcpy(p->satellite_elevation, satellite_elevation, sizeof(p->satellite_elevation)); // uint8_t[20]:Elevation (0: right on top of receiver, 90: on the horizon) of satellite
memcpy(p->satellite_azimuth, satellite_azimuth, sizeof(p->satellite_azimuth)); // uint8_t[20]:Direction of satellite, 0: 0 deg, 255: 360 deg.
memcpy(p->satellite_snr, satellite_snr, sizeof(p->satellite_snr)); // uint8_t[20]:Signal to noise ratio of satellite
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_GPS_STATUS_LEN;
msg.msgid = MAVLINK_MSG_ID_GPS_STATUS;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_gps_status_send(mavlink_channel_t chan, uint8_t satellites_visible, const uint8_t* satellite_prn, const uint8_t* satellite_used, const uint8_t* satellite_elevation, const uint8_t* satellite_azimuth, const uint8_t* satellite_snr)
{
mavlink_header_t hdr;
......
......@@ -76,34 +76,9 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c
* @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
* @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg.payload[0];
p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN;
msg.msgid = MAVLINK_MSG_ID_HEARTBEAT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot)
{
mavlink_header_t hdr;
......
......@@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_local_position_encode(uint8_t system_id, uint
* @param vy Y Speed
* @param vz Z Speed
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float vx, float vy, float vz)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_local_position_t *p = (mavlink_local_position_t *)&msg.payload[0];
p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
p->x = x; // float:X Position
p->y = y; // float:Y Position
p->z = z; // float:Z Position
p->vx = vx; // float:X Speed
p->vy = vy; // float:Y Speed
p->vz = vz; // float:Z Speed
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_LEN;
msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_local_position_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float vx, float vy, float vz)
{
mavlink_header_t hdr;
......
......@@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_local_position_setpoint_encode(uint8_t system
* @param z z position
* @param yaw Desired yaw angle
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_local_position_setpoint_t *p = (mavlink_local_position_setpoint_t *)&msg.payload[0];
p->x = x; // float:x position
p->y = y; // float:y position
p->z = z; // float:z position
p->yaw = yaw; // float:Desired yaw angle
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN;
msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_local_position_setpoint_send(mavlink_channel_t chan, float x, float y, float z, float yaw)
{
mavlink_header_t hdr;
......
......@@ -6,12 +6,12 @@
typedef struct __mavlink_local_position_setpoint_set_t
{
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
float x; ///< x position
float y; ///< y position
float z; ///< z position
float yaw; ///< Desired yaw angle
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
} mavlink_local_position_setpoint_set_t;
......@@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_local_position_setpoint_set_encode(uint8_t sy
* @param z z position
* @param yaw Desired yaw angle
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_local_position_setpoint_set_t *p = (mavlink_local_position_setpoint_set_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->x = x; // float:x position
p->y = y; // float:y position
p->z = z; // float:z position
p->yaw = yaw; // float:Desired yaw angle
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET_LEN;
msg.msgid = MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_local_position_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float x, float y, float z, float yaw)
{
mavlink_header_t hdr;
......
......@@ -6,11 +6,11 @@
typedef struct __mavlink_manual_control_t
{
uint8_t target; ///< The system to be controlled
float roll; ///< roll
float pitch; ///< pitch
float yaw; ///< yaw
float thrust; ///< thrust
uint8_t target; ///< The system to be controlled
uint8_t roll_manual; ///< roll control enabled auto:0, manual:1
uint8_t pitch_manual; ///< pitch auto:0, manual:1
uint8_t yaw_manual; ///< yaw auto:0, manual:1
......@@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_manual_control_encode(uint8_t system_id, uint
* @param yaw_manual yaw auto:0, manual:1
* @param thrust_manual thrust auto:0, manual:1
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_manual_control_t *p = (mavlink_manual_control_t *)&msg.payload[0];
p->target = target; // uint8_t:The system to be controlled
p->roll = roll; // float:roll
p->pitch = pitch; // float:pitch
p->yaw = yaw; // float:yaw
p->thrust = thrust; // float:thrust
p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1
p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1
p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1
p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_MANUAL_CONTROL_LEN;
msg.msgid = MAVLINK_MSG_ID_MANUAL_CONTROL;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_manual_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual)
{
mavlink_header_t hdr;
......
......@@ -6,8 +6,8 @@
typedef struct __mavlink_named_value_float_t
{
char name[10]; ///< Name of the debug variable
float value; ///< Floating point value
char name[10]; ///< Name of the debug variable
} mavlink_named_value_float_t;
#define MAVLINK_MSG_NAMED_VALUE_FLOAT_FIELD_NAME_LEN 10
......@@ -74,33 +74,9 @@ static inline uint16_t mavlink_msg_named_value_float_encode(uint8_t system_id, u
* @param name Name of the debug variable
* @param value Floating point value
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_named_value_float_t *p = (mavlink_named_value_float_t *)&msg.payload[0];
memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable
p->value = value; // float:Floating point value
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT_LEN;
msg.msgid = MAVLINK_MSG_ID_NAMED_VALUE_FLOAT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_named_value_float_send(mavlink_channel_t chan, const char* name, float value)
{
mavlink_header_t hdr;
......
......@@ -6,8 +6,8 @@
typedef struct __mavlink_named_value_int_t
{
char name[10]; ///< Name of the debug variable
int32_t value; ///< Signed integer value
char name[10]; ///< Name of the debug variable
} mavlink_named_value_int_t;
#define MAVLINK_MSG_NAMED_VALUE_INT_FIELD_NAME_LEN 10
......@@ -74,33 +74,9 @@ static inline uint16_t mavlink_msg_named_value_int_encode(uint8_t system_id, uin
* @param name Name of the debug variable
* @param value Signed integer value
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_named_value_int_t *p = (mavlink_named_value_int_t *)&msg.payload[0];
memcpy(p->name, name, sizeof(p->name)); // char[10]:Name of the debug variable
p->value = value; // int32_t:Signed integer value
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_NAMED_VALUE_INT_LEN;
msg.msgid = MAVLINK_MSG_ID_NAMED_VALUE_INT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_named_value_int_send(mavlink_channel_t chan, const char* name, int32_t value)
{
mavlink_header_t hdr;
......
......@@ -8,12 +8,12 @@ typedef struct __mavlink_nav_controller_output_t
{
float nav_roll; ///< Current desired roll in degrees
float nav_pitch; ///< Current desired pitch in degrees
int16_t nav_bearing; ///< Current desired heading in degrees
int16_t target_bearing; ///< Bearing to current waypoint/target in degrees
uint16_t wp_dist; ///< Distance to active waypoint in meters
float alt_error; ///< Current altitude error in meters
float aspd_error; ///< Current airspeed error in meters/second
float xtrack_error; ///< Current crosstrack error on x-y plane in meters
int16_t nav_bearing; ///< Current desired heading in degrees
int16_t target_bearing; ///< Bearing to current waypoint/target in degrees
uint16_t wp_dist; ///< Distance to active waypoint in meters
} mavlink_nav_controller_output_t;
......@@ -109,39 +109,9 @@ static inline uint16_t mavlink_msg_nav_controller_output_encode(uint8_t system_i
* @param aspd_error Current airspeed error in meters/second
* @param xtrack_error Current crosstrack error on x-y plane in meters
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_nav_controller_output_t *p = (mavlink_nav_controller_output_t *)&msg.payload[0];
p->nav_roll = nav_roll; // float:Current desired roll in degrees
p->nav_pitch = nav_pitch; // float:Current desired pitch in degrees
p->nav_bearing = nav_bearing; // int16_t:Current desired heading in degrees
p->target_bearing = target_bearing; // int16_t:Bearing to current waypoint/target in degrees
p->wp_dist = wp_dist; // uint16_t:Distance to active waypoint in meters
p->alt_error = alt_error; // float:Current altitude error in meters
p->aspd_error = aspd_error; // float:Current airspeed error in meters/second
p->xtrack_error = xtrack_error; // float:Current crosstrack error on x-y plane in meters
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT_LEN;
msg.msgid = MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_nav_controller_output_send(mavlink_channel_t chan, float nav_roll, float nav_pitch, int16_t nav_bearing, int16_t target_bearing, uint16_t wp_dist, float alt_error, float aspd_error, float xtrack_error)
{
mavlink_header_t hdr;
......
......@@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_param_request_list_encode(uint8_t system_id,
* @param target_system System ID
* @param target_component Component ID
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_param_request_list_t *p = (mavlink_param_request_list_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_PARAM_REQUEST_LIST_LEN;
msg.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_LIST;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_param_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
{
mavlink_header_t hdr;
......
// MESSAGE PARAM_REQUEST_READ PACKING
#define MAVLINK_MSG_ID_PARAM_REQUEST_READ 20
#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 19
#define MAVLINK_MSG_20_LEN 19
#define MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN 20
#define MAVLINK_MSG_20_LEN 20
typedef struct __mavlink_param_request_read_t
{
int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
char param_id[15]; ///< Onboard parameter id
int16_t param_index; ///< Parameter index. Send -1 to use the param ID field as identifier
uint8_t param_id[16]; ///< Onboard parameter id
} mavlink_param_request_read_t;
#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 15
#define MAVLINK_MSG_PARAM_REQUEST_READ_FIELD_PARAM_ID_LEN 16
/**
* @brief Pack a param_request_read message
......@@ -26,14 +26,14 @@ typedef struct __mavlink_param_request_read_t
* @param param_index Parameter index. Send -1 to use the param ID field as identifier
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index)
static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, int16_t param_index)
{
mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ;
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id
p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
......@@ -51,14 +51,14 @@ static inline uint16_t mavlink_msg_param_request_read_pack(uint8_t system_id, ui
* @param param_index Parameter index. Send -1 to use the param ID field as identifier
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index)
static inline uint16_t mavlink_msg_param_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, int16_t param_index)
{
mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ;
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id
p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN);
......@@ -86,36 +86,10 @@ static inline uint16_t mavlink_msg_param_request_read_encode(uint8_t system_id,
* @param param_id Onboard parameter id
* @param param_index Parameter index. Send -1 to use the param ID field as identifier
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_PARAM_REQUEST_READ_LEN;
msg.msgid = MAVLINK_MSG_ID_PARAM_REQUEST_READ;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, int16_t param_index)
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, int16_t param_index)
{
mavlink_header_t hdr;
mavlink_param_request_read_t payload;
......@@ -124,7 +98,7 @@ static inline void mavlink_msg_param_request_read_send(mavlink_channel_t chan, u
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id
p->param_index = param_index; // int16_t:Parameter index. Send -1 to use the param ID field as identifier
hdr.STX = MAVLINK_STX;
......@@ -176,7 +150,7 @@ static inline uint8_t mavlink_msg_param_request_read_get_target_component(const
*
* @return Onboard parameter id
*/
static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, char* param_id)
static inline uint16_t mavlink_msg_param_request_read_get_param_id(const mavlink_message_t* msg, uint8_t* param_id)
{
mavlink_param_request_read_t *p = (mavlink_param_request_read_t *)&msg->payload[0];
......
// MESSAGE PARAM_SET PACKING
#define MAVLINK_MSG_ID_PARAM_SET 23
#define MAVLINK_MSG_ID_PARAM_SET_LEN 21
#define MAVLINK_MSG_23_LEN 21
#define MAVLINK_MSG_ID_PARAM_SET_LEN 22
#define MAVLINK_MSG_23_LEN 22
typedef struct __mavlink_param_set_t
{
float param_value; ///< Onboard parameter value
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
char param_id[15]; ///< Onboard parameter id
float param_value; ///< Onboard parameter value
uint8_t param_id[16]; ///< Onboard parameter id
} mavlink_param_set_t;
#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 15
#define MAVLINK_MSG_PARAM_SET_FIELD_PARAM_ID_LEN 16
/**
* @brief Pack a param_set message
......@@ -26,14 +26,14 @@ typedef struct __mavlink_param_set_t
* @param param_value Onboard parameter value
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value)
static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, float param_value)
{
mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_PARAM_SET;
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id
p->param_value = param_value; // float:Onboard parameter value
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_SET_LEN);
......@@ -51,14 +51,14 @@ static inline uint16_t mavlink_msg_param_set_pack(uint8_t system_id, uint8_t com
* @param param_value Onboard parameter value
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value)
static inline uint16_t mavlink_msg_param_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, float param_value)
{
mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_PARAM_SET;
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id
p->param_value = param_value; // float:Onboard parameter value
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_SET_LEN);
......@@ -86,36 +86,10 @@ static inline uint16_t mavlink_msg_param_set_encode(uint8_t system_id, uint8_t c
* @param param_id Onboard parameter id
* @param param_value Onboard parameter value
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_param_set_t *p = (mavlink_param_set_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
p->param_value = param_value; // float:Onboard parameter value
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_PARAM_SET_LEN;
msg.msgid = MAVLINK_MSG_ID_PARAM_SET;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char* param_id, float param_value)
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t* param_id, float param_value)
{
mavlink_header_t hdr;
mavlink_param_set_t payload;
......@@ -124,7 +98,7 @@ static inline void mavlink_msg_param_set_send(mavlink_channel_t chan, uint8_t ta
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id
p->param_value = param_value; // float:Onboard parameter value
hdr.STX = MAVLINK_STX;
......@@ -176,7 +150,7 @@ static inline uint8_t mavlink_msg_param_set_get_target_component(const mavlink_m
*
* @return Onboard parameter id
*/
static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, char* param_id)
static inline uint16_t mavlink_msg_param_set_get_param_id(const mavlink_message_t* msg, uint8_t* param_id)
{
mavlink_param_set_t *p = (mavlink_param_set_t *)&msg->payload[0];
......
// MESSAGE PARAM_VALUE PACKING
#define MAVLINK_MSG_ID_PARAM_VALUE 22
#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 23
#define MAVLINK_MSG_22_LEN 23
#define MAVLINK_MSG_ID_PARAM_VALUE_LEN 24
#define MAVLINK_MSG_22_LEN 24
typedef struct __mavlink_param_value_t
{
char param_id[15]; ///< Onboard parameter id
float param_value; ///< Onboard parameter value
uint16_t param_count; ///< Total number of onboard parameters
uint16_t param_index; ///< Index of this onboard parameter
uint8_t param_id[16]; ///< Onboard parameter id
} mavlink_param_value_t;
#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 15
#define MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN 16
/**
* @brief Pack a param_value message
......@@ -26,12 +26,12 @@ typedef struct __mavlink_param_value_t
* @param param_index Index of this onboard parameter
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index)
static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const uint8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index)
{
mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE;
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id
p->param_value = param_value; // float:Onboard parameter value
p->param_count = param_count; // uint16_t:Total number of onboard parameters
p->param_index = param_index; // uint16_t:Index of this onboard parameter
......@@ -51,12 +51,12 @@ static inline uint16_t mavlink_msg_param_value_pack(uint8_t system_id, uint8_t c
* @param param_index Index of this onboard parameter
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index)
static inline uint16_t mavlink_msg_param_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const uint8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index)
{
mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_PARAM_VALUE;
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id
p->param_value = param_value; // float:Onboard parameter value
p->param_count = param_count; // uint16_t:Total number of onboard parameters
p->param_index = param_index; // uint16_t:Index of this onboard parameter
......@@ -86,43 +86,17 @@ static inline uint16_t mavlink_msg_param_value_encode(uint8_t system_id, uint8_t
* @param param_count Total number of onboard parameters
* @param param_index Index of this onboard parameter
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_param_value_t *p = (mavlink_param_value_t *)&msg.payload[0];
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
p->param_value = param_value; // float:Onboard parameter value
p->param_count = param_count; // uint16_t:Total number of onboard parameters
p->param_index = param_index; // uint16_t:Index of this onboard parameter
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_PARAM_VALUE_LEN;
msg.msgid = MAVLINK_MSG_ID_PARAM_VALUE;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const char* param_id, float param_value, uint16_t param_count, uint16_t param_index)
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const uint8_t* param_id, float param_value, uint16_t param_count, uint16_t param_index)
{
mavlink_header_t hdr;
mavlink_param_value_t payload;
uint16_t checksum;
mavlink_param_value_t *p = &payload;
memcpy(p->param_id, param_id, sizeof(p->param_id)); // char[15]:Onboard parameter id
memcpy(p->param_id, param_id, sizeof(p->param_id)); // uint8_t[16]:Onboard parameter id
p->param_value = param_value; // float:Onboard parameter value
p->param_count = param_count; // uint16_t:Total number of onboard parameters
p->param_index = param_index; // uint16_t:Index of this onboard parameter
......@@ -154,7 +128,7 @@ static inline void mavlink_msg_param_value_send(mavlink_channel_t chan, const ch
*
* @return Onboard parameter id
*/
static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, char* param_id)
static inline uint16_t mavlink_msg_param_value_get_param_id(const mavlink_message_t* msg, uint8_t* param_id)
{
mavlink_param_value_t *p = (mavlink_param_value_t *)&msg->payload[0];
......
......@@ -6,10 +6,10 @@
typedef struct __mavlink_ping_t
{
uint64_t time; ///< Unix timestamp in microseconds
uint32_t seq; ///< PING sequence
uint8_t target_system; ///< 0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system
uint8_t target_component; ///< 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system
uint64_t time; ///< Unix timestamp in microseconds
} mavlink_ping_t;
......@@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_ping_encode(uint8_t system_id, uint8_t compon
* @param target_component 0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system
* @param time Unix timestamp in microseconds
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_ping_t *p = (mavlink_ping_t *)&msg.payload[0];
p->seq = seq; // uint32_t:PING sequence
p->target_system = target_system; // uint8_t:0: request ping from all receiving systems, if greater than 0: message is a ping response and number is the system id of the requesting system
p->target_component = target_component; // uint8_t:0: request ping from all receiving components, if greater than 0: message is a ping response and number is the system id of the requesting system
p->time = time; // uint64_t:Unix timestamp in microseconds
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_PING_LEN;
msg.msgid = MAVLINK_MSG_ID_PING;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_ping_send(mavlink_channel_t chan, uint32_t seq, uint8_t target_system, uint8_t target_component, uint64_t time)
{
mavlink_header_t hdr;
......
......@@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_position_controller_output_encode(uint8_t sys
* @param z Position z: -128: -100%, 127: +100%
* @param yaw Position yaw: -128: -100%, 127: +100%
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_position_controller_output_t *p = (mavlink_position_controller_output_t *)&msg.payload[0];
p->enabled = enabled; // uint8_t:1: enabled, 0: disabled
p->x = x; // int8_t:Position x: -128: -100%, 127: +100%
p->y = y; // int8_t:Position y: -128: -100%, 127: +100%
p->z = z; // int8_t:Position z: -128: -100%, 127: +100%
p->yaw = yaw; // int8_t:Position yaw: -128: -100%, 127: +100%
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT_LEN;
msg.msgid = MAVLINK_MSG_ID_POSITION_CONTROLLER_OUTPUT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_position_controller_output_send(mavlink_channel_t chan, uint8_t enabled, int8_t x, int8_t y, int8_t z, int8_t yaw)
{
mavlink_header_t hdr;
......
......@@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_position_target_encode(uint8_t system_id, uin
* @param z z position
* @param yaw yaw orientation in radians, 0 = NORTH
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_position_target_t *p = (mavlink_position_target_t *)&msg.payload[0];
p->x = x; // float:x position
p->y = y; // float:y position
p->z = z; // float:z position
p->yaw = yaw; // float:yaw orientation in radians, 0 = NORTH
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_POSITION_TARGET_LEN;
msg.msgid = MAVLINK_MSG_ID_POSITION_TARGET;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_position_target_send(mavlink_channel_t chan, float x, float y, float z, float yaw)
{
mavlink_header_t hdr;
......
......@@ -121,41 +121,9 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com
* @param ymag Y Magnetic field (raw)
* @param zmag Z Magnetic field (raw)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_raw_imu_t *p = (mavlink_raw_imu_t *)&msg.payload[0];
p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
p->xacc = xacc; // int16_t:X acceleration (raw)
p->yacc = yacc; // int16_t:Y acceleration (raw)
p->zacc = zacc; // int16_t:Z acceleration (raw)
p->xgyro = xgyro; // int16_t:Angular speed around X axis (raw)
p->ygyro = ygyro; // int16_t:Angular speed around Y axis (raw)
p->zgyro = zgyro; // int16_t:Angular speed around Z axis (raw)
p->xmag = xmag; // int16_t:X Magnetic field (raw)
p->ymag = ymag; // int16_t:Y Magnetic field (raw)
p->zmag = zmag; // int16_t:Z Magnetic field (raw)
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_RAW_IMU_LEN;
msg.msgid = MAVLINK_MSG_ID_RAW_IMU;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
{
mavlink_header_t hdr;
......
......@@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_raw_pressure_encode(uint8_t system_id, uint8_
* @param press_diff2 Differential pressure 2 (raw)
* @param temperature Raw Temperature measurement (raw)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_raw_pressure_t *p = (mavlink_raw_pressure_t *)&msg.payload[0];
p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
p->press_abs = press_abs; // int16_t:Absolute pressure (raw)
p->press_diff1 = press_diff1; // int16_t:Differential pressure 1 (raw)
p->press_diff2 = press_diff2; // int16_t:Differential pressure 2 (raw)
p->temperature = temperature; // int16_t:Raw Temperature measurement (raw)
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_RAW_PRESSURE_LEN;
msg.msgid = MAVLINK_MSG_ID_RAW_PRESSURE;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_raw_pressure_send(mavlink_channel_t chan, uint64_t usec, int16_t press_abs, int16_t press_diff1, int16_t press_diff2, int16_t temperature)
{
mavlink_header_t hdr;
......
......@@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_rc_channels_raw_encode(uint8_t system_id, uin
* @param chan8_raw RC channel 8 value, in microseconds
* @param rssi Receive signal strength indicator, 0: 0%, 255: 100%
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_rc_channels_raw_t *p = (mavlink_rc_channels_raw_t *)&msg.payload[0];
p->chan1_raw = chan1_raw; // uint16_t:RC channel 1 value, in microseconds
p->chan2_raw = chan2_raw; // uint16_t:RC channel 2 value, in microseconds
p->chan3_raw = chan3_raw; // uint16_t:RC channel 3 value, in microseconds
p->chan4_raw = chan4_raw; // uint16_t:RC channel 4 value, in microseconds
p->chan5_raw = chan5_raw; // uint16_t:RC channel 5 value, in microseconds
p->chan6_raw = chan6_raw; // uint16_t:RC channel 6 value, in microseconds
p->chan7_raw = chan7_raw; // uint16_t:RC channel 7 value, in microseconds
p->chan8_raw = chan8_raw; // uint16_t:RC channel 8 value, in microseconds
p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100%
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_RC_CHANNELS_RAW_LEN;
msg.msgid = MAVLINK_MSG_ID_RC_CHANNELS_RAW;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_rc_channels_raw_send(mavlink_channel_t chan, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint8_t rssi)
{
mavlink_header_t hdr;
......
......@@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_rc_channels_scaled_encode(uint8_t system_id,
* @param chan8_scaled RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000
* @param rssi Receive signal strength indicator, 0: 0%, 255: 100%
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_rc_channels_scaled_t *p = (mavlink_rc_channels_scaled_t *)&msg.payload[0];
p->chan1_scaled = chan1_scaled; // int16_t:RC channel 1 value scaled, (-100%) -10000, (0%) 0, (100%) 10000
p->chan2_scaled = chan2_scaled; // int16_t:RC channel 2 value scaled, (-100%) -10000, (0%) 0, (100%) 10000
p->chan3_scaled = chan3_scaled; // int16_t:RC channel 3 value scaled, (-100%) -10000, (0%) 0, (100%) 10000
p->chan4_scaled = chan4_scaled; // int16_t:RC channel 4 value scaled, (-100%) -10000, (0%) 0, (100%) 10000
p->chan5_scaled = chan5_scaled; // int16_t:RC channel 5 value scaled, (-100%) -10000, (0%) 0, (100%) 10000
p->chan6_scaled = chan6_scaled; // int16_t:RC channel 6 value scaled, (-100%) -10000, (0%) 0, (100%) 10000
p->chan7_scaled = chan7_scaled; // int16_t:RC channel 7 value scaled, (-100%) -10000, (0%) 0, (100%) 10000
p->chan8_scaled = chan8_scaled; // int16_t:RC channel 8 value scaled, (-100%) -10000, (0%) 0, (100%) 10000
p->rssi = rssi; // uint8_t:Receive signal strength indicator, 0: 0%, 255: 100%
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_RC_CHANNELS_SCALED_LEN;
msg.msgid = MAVLINK_MSG_ID_RC_CHANNELS_SCALED;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_rc_channels_scaled_send(mavlink_channel_t chan, int16_t chan1_scaled, int16_t chan2_scaled, int16_t chan3_scaled, int16_t chan4_scaled, int16_t chan5_scaled, int16_t chan6_scaled, int16_t chan7_scaled, int16_t chan8_scaled, uint8_t rssi)
{
mavlink_header_t hdr;
......
......@@ -6,10 +6,10 @@
typedef struct __mavlink_request_data_stream_t
{
uint16_t req_message_rate; ///< The requested interval between two messages of this type
uint8_t target_system; ///< The target requested to send the message stream.
uint8_t target_component; ///< The target requested to send the message stream.
uint8_t req_stream_id; ///< The ID of the requested message type
uint16_t req_message_rate; ///< The requested interval between two messages of this type
uint8_t start_stop; ///< 1 to start sending, 0 to stop sending.
} mavlink_request_data_stream_t;
......@@ -91,36 +91,9 @@ static inline uint16_t mavlink_msg_request_data_stream_encode(uint8_t system_id,
* @param req_message_rate The requested interval between two messages of this type
* @param start_stop 1 to start sending, 0 to stop sending.
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_request_data_stream_t *p = (mavlink_request_data_stream_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:The target requested to send the message stream.
p->target_component = target_component; // uint8_t:The target requested to send the message stream.
p->req_stream_id = req_stream_id; // uint8_t:The ID of the requested message type
p->req_message_rate = req_message_rate; // uint16_t:The requested interval between two messages of this type
p->start_stop = start_stop; // uint8_t:1 to start sending, 0 to stop sending.
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_REQUEST_DATA_STREAM_LEN;
msg.msgid = MAVLINK_MSG_ID_REQUEST_DATA_STREAM;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_request_data_stream_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t req_stream_id, uint16_t req_message_rate, uint8_t start_stop)
{
mavlink_header_t hdr;
......
......@@ -6,13 +6,13 @@
typedef struct __mavlink_safety_allowed_area_t
{
uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
float p1x; ///< x position 1 / Latitude 1
float p1y; ///< y position 1 / Longitude 1
float p1z; ///< z position 1 / Altitude 1
float p2x; ///< x position 2 / Latitude 2
float p2y; ///< y position 2 / Longitude 2
float p2z; ///< z position 2 / Altitude 2
uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
} mavlink_safety_allowed_area_t;
......@@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_safety_allowed_area_encode(uint8_t system_id,
* @param p2y y position 2 / Longitude 2
* @param p2z z position 2 / Altitude 2
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_safety_allowed_area_t *p = (mavlink_safety_allowed_area_t *)&msg.payload[0];
p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
p->p1x = p1x; // float:x position 1 / Latitude 1
p->p1y = p1y; // float:y position 1 / Longitude 1
p->p1z = p1z; // float:z position 1 / Altitude 1
p->p2x = p2x; // float:x position 2 / Latitude 2
p->p2y = p2y; // float:y position 2 / Longitude 2
p->p2z = p2z; // float:z position 2 / Altitude 2
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA_LEN;
msg.msgid = MAVLINK_MSG_ID_SAFETY_ALLOWED_AREA;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_safety_allowed_area_send(mavlink_channel_t chan, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z)
{
mavlink_header_t hdr;
......
......@@ -6,15 +6,15 @@
typedef struct __mavlink_safety_set_allowed_area_t
{
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
float p1x; ///< x position 1 / Latitude 1
float p1y; ///< y position 1 / Longitude 1
float p1z; ///< z position 1 / Altitude 1
float p2x; ///< x position 2 / Latitude 2
float p2y; ///< y position 2 / Longitude 2
float p2z; ///< z position 2 / Altitude 2
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
} mavlink_safety_set_allowed_area_t;
......@@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_safety_set_allowed_area_encode(uint8_t system
* @param p2y y position 2 / Longitude 2
* @param p2z z position 2 / Altitude 2
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_safety_set_allowed_area_t *p = (mavlink_safety_set_allowed_area_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->frame = frame; // uint8_t:Coordinate frame, as defined by MAV_FRAME enum in mavlink_types.h. Can be either global, GPS, right-handed with Z axis up or local, right handed, Z axis down.
p->p1x = p1x; // float:x position 1 / Latitude 1
p->p1y = p1y; // float:y position 1 / Longitude 1
p->p1z = p1z; // float:z position 1 / Altitude 1
p->p2x = p2x; // float:x position 2 / Latitude 2
p->p2y = p2y; // float:y position 2 / Longitude 2
p->p2z = p2z; // float:z position 2 / Altitude 2
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA_LEN;
msg.msgid = MAVLINK_MSG_ID_SAFETY_SET_ALLOWED_AREA;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_safety_set_allowed_area_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t frame, float p1x, float p1y, float p1z, float p2x, float p2y, float p2z)
{
mavlink_header_t hdr;
......
......@@ -121,41 +121,9 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t
* @param ymag Y Magnetic field (milli tesla)
* @param zmag Z Magnetic field (milli tesla)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_scaled_imu_t *p = (mavlink_scaled_imu_t *)&msg.payload[0];
p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
p->xacc = xacc; // int16_t:X acceleration (mg)
p->yacc = yacc; // int16_t:Y acceleration (mg)
p->zacc = zacc; // int16_t:Z acceleration (mg)
p->xgyro = xgyro; // int16_t:Angular speed around X axis (millirad /sec)
p->ygyro = ygyro; // int16_t:Angular speed around Y axis (millirad /sec)
p->zgyro = zgyro; // int16_t:Angular speed around Z axis (millirad /sec)
p->xmag = xmag; // int16_t:X Magnetic field (milli tesla)
p->ymag = ymag; // int16_t:Y Magnetic field (milli tesla)
p->zmag = zmag; // int16_t:Z Magnetic field (milli tesla)
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SCALED_IMU_LEN;
msg.msgid = MAVLINK_MSG_ID_SCALED_IMU;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint64_t usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag)
{
mavlink_header_t hdr;
......
......@@ -85,35 +85,9 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin
* @param press_diff Differential pressure 1 (hectopascal)
* @param temperature Temperature measurement (0.01 degrees celsius)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint64_t usec, float press_abs, float press_diff, int16_t temperature)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_scaled_pressure_t *p = (mavlink_scaled_pressure_t *)&msg.payload[0];
p->usec = usec; // uint64_t:Timestamp (microseconds since UNIX epoch or microseconds since system boot)
p->press_abs = press_abs; // float:Absolute pressure (hectopascal)
p->press_diff = press_diff; // float:Differential pressure 1 (hectopascal)
p->temperature = temperature; // int16_t:Temperature measurement (0.01 degrees celsius)
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SCALED_PRESSURE_LEN;
msg.msgid = MAVLINK_MSG_ID_SCALED_PRESSURE;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint64_t usec, float press_abs, float press_diff, int16_t temperature)
{
mavlink_header_t hdr;
......
......@@ -109,39 +109,9 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui
* @param servo7_raw Servo output 7 value, in microseconds
* @param servo8_raw Servo output 8 value, in microseconds
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_servo_output_raw_t *p = (mavlink_servo_output_raw_t *)&msg.payload[0];
p->servo1_raw = servo1_raw; // uint16_t:Servo output 1 value, in microseconds
p->servo2_raw = servo2_raw; // uint16_t:Servo output 2 value, in microseconds
p->servo3_raw = servo3_raw; // uint16_t:Servo output 3 value, in microseconds
p->servo4_raw = servo4_raw; // uint16_t:Servo output 4 value, in microseconds
p->servo5_raw = servo5_raw; // uint16_t:Servo output 5 value, in microseconds
p->servo6_raw = servo6_raw; // uint16_t:Servo output 6 value, in microseconds
p->servo7_raw = servo7_raw; // uint16_t:Servo output 7 value, in microseconds
p->servo8_raw = servo8_raw; // uint16_t:Servo output 8 value, in microseconds
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN;
msg.msgid = MAVLINK_MSG_ID_SERVO_OUTPUT_RAW;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw)
{
mavlink_header_t hdr;
......
......@@ -6,8 +6,8 @@
typedef struct __mavlink_set_altitude_t
{
uint8_t target; ///< The system setting the altitude
uint32_t mode; ///< The new altitude in meters
uint8_t target; ///< The system setting the altitude
} mavlink_set_altitude_t;
......@@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_set_altitude_encode(uint8_t system_id, uint8_
* @param target The system setting the altitude
* @param mode The new altitude in meters
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_set_altitude_t *p = (mavlink_set_altitude_t *)&msg.payload[0];
p->target = target; // uint8_t:The system setting the altitude
p->mode = mode; // uint32_t:The new altitude in meters
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SET_ALTITUDE_LEN;
msg.msgid = MAVLINK_MSG_ID_SET_ALTITUDE;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_set_altitude_send(mavlink_channel_t chan, uint8_t target, uint32_t mode)
{
mavlink_header_t hdr;
......
......@@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_set_mode_encode(uint8_t system_id, uint8_t co
* @param target The system setting the mode
* @param mode The new mode
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_set_mode_t *p = (mavlink_set_mode_t *)&msg.payload[0];
p->target = target; // uint8_t:The system setting the mode
p->mode = mode; // uint8_t:The new mode
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SET_MODE_LEN;
msg.msgid = MAVLINK_MSG_ID_SET_MODE;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_set_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t mode)
{
mavlink_header_t hdr;
......
......@@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_set_nav_mode_encode(uint8_t system_id, uint8_
* @param target The system setting the mode
* @param nav_mode The new navigation mode
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_set_nav_mode_t *p = (mavlink_set_nav_mode_t *)&msg.payload[0];
p->target = target; // uint8_t:The system setting the mode
p->nav_mode = nav_mode; // uint8_t:The new navigation mode
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SET_NAV_MODE_LEN;
msg.msgid = MAVLINK_MSG_ID_SET_NAV_MODE;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_set_nav_mode_send(mavlink_channel_t chan, uint8_t target, uint8_t nav_mode)
{
mavlink_header_t hdr;
......
// MESSAGE SET_ROLL_PITCH_YAW PACKING
#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW 55
#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN 14
#define MAVLINK_MSG_55_LEN 14
typedef struct __mavlink_set_roll_pitch_yaw_t
{
float roll; ///< Desired roll angle in radians
float pitch; ///< Desired pitch angle in radians
float yaw; ///< Desired yaw angle in radians
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
} mavlink_set_roll_pitch_yaw_t;
/**
* @brief Pack a set_roll_pitch_yaw message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param target_system System ID
* @param target_component Component ID
* @param roll Desired roll angle in radians
* @param pitch Desired pitch angle in radians
* @param yaw Desired yaw angle in radians
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw)
{
mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW;
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->roll = roll; // float:Desired roll angle in radians
p->pitch = pitch; // float:Desired pitch angle in radians
p->yaw = yaw; // float:Desired yaw angle in radians
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN);
}
/**
* @brief Pack a set_roll_pitch_yaw message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @param roll Desired roll angle in radians
* @param pitch Desired pitch angle in radians
* @param yaw Desired yaw angle in radians
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw)
{
mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW;
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->roll = roll; // float:Desired roll angle in radians
p->pitch = pitch; // float:Desired pitch angle in radians
p->yaw = yaw; // float:Desired yaw angle in radians
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN);
}
/**
* @brief Encode a set_roll_pitch_yaw struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param set_roll_pitch_yaw C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw)
{
return mavlink_msg_set_roll_pitch_yaw_pack(system_id, component_id, msg, set_roll_pitch_yaw->target_system, set_roll_pitch_yaw->target_component, set_roll_pitch_yaw->roll, set_roll_pitch_yaw->pitch, set_roll_pitch_yaw->yaw);
}
/**
* @brief Send a set_roll_pitch_yaw message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
* @param roll Desired roll angle in radians
* @param pitch Desired pitch angle in radians
* @param yaw Desired yaw angle in radians
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_set_roll_pitch_yaw_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll, float pitch, float yaw)
{
mavlink_header_t hdr;
mavlink_set_roll_pitch_yaw_t payload;
uint16_t checksum;
mavlink_set_roll_pitch_yaw_t *p = &payload;
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->roll = roll; // float:Desired roll angle in radians
p->pitch = pitch; // float:Desired pitch angle in radians
p->yaw = yaw; // float:Desired yaw angle in radians
hdr.STX = MAVLINK_STX;
hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_LEN;
hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW;
hdr.sysid = mavlink_system.sysid;
hdr.compid = mavlink_system.compid;
hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;
mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );
crc_init(&checksum);
checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);
checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );
hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);
mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);
}
#endif
// MESSAGE SET_ROLL_PITCH_YAW UNPACKING
/**
* @brief Get field target_system from set_roll_pitch_yaw message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_system(const mavlink_message_t* msg)
{
mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0];
return (uint8_t)(p->target_system);
}
/**
* @brief Get field target_component from set_roll_pitch_yaw message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_set_roll_pitch_yaw_get_target_component(const mavlink_message_t* msg)
{
mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0];
return (uint8_t)(p->target_component);
}
/**
* @brief Get field roll from set_roll_pitch_yaw message
*
* @return Desired roll angle in radians
*/
static inline float mavlink_msg_set_roll_pitch_yaw_get_roll(const mavlink_message_t* msg)
{
mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0];
return (float)(p->roll);
}
/**
* @brief Get field pitch from set_roll_pitch_yaw message
*
* @return Desired pitch angle in radians
*/
static inline float mavlink_msg_set_roll_pitch_yaw_get_pitch(const mavlink_message_t* msg)
{
mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0];
return (float)(p->pitch);
}
/**
* @brief Get field yaw from set_roll_pitch_yaw message
*
* @return Desired yaw angle in radians
*/
static inline float mavlink_msg_set_roll_pitch_yaw_get_yaw(const mavlink_message_t* msg)
{
mavlink_set_roll_pitch_yaw_t *p = (mavlink_set_roll_pitch_yaw_t *)&msg->payload[0];
return (float)(p->yaw);
}
/**
* @brief Decode a set_roll_pitch_yaw message into a struct
*
* @param msg The message to decode
* @param set_roll_pitch_yaw C-struct to decode the message contents into
*/
static inline void mavlink_msg_set_roll_pitch_yaw_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_t* set_roll_pitch_yaw)
{
memcpy( set_roll_pitch_yaw, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_t));
}
// MESSAGE SET_ROLL_PITCH_YAW_SPEED PACKING
#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED 56
#define MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN 14
#define MAVLINK_MSG_56_LEN 14
typedef struct __mavlink_set_roll_pitch_yaw_speed_t
{
float roll_speed; ///< Desired roll angular speed in rad/s
float pitch_speed; ///< Desired pitch angular speed in rad/s
float yaw_speed; ///< Desired yaw angular speed in rad/s
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
} mavlink_set_roll_pitch_yaw_speed_t;
/**
* @brief Pack a set_roll_pitch_yaw_speed message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
*
* @param target_system System ID
* @param target_component Component ID
* @param roll_speed Desired roll angular speed in rad/s
* @param pitch_speed Desired pitch angular speed in rad/s
* @param yaw_speed Desired yaw angular speed in rad/s
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
{
mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED;
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s
p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s
p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN);
}
/**
* @brief Pack a set_roll_pitch_yaw_speed message
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param chan The MAVLink channel this message was sent over
* @param msg The MAVLink message to compress the data into
* @param target_system System ID
* @param target_component Component ID
* @param roll_speed Desired roll angular speed in rad/s
* @param pitch_speed Desired pitch angular speed in rad/s
* @param yaw_speed Desired yaw angular speed in rad/s
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
{
mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED;
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s
p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s
p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN);
}
/**
* @brief Encode a set_roll_pitch_yaw_speed struct into a message
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param set_roll_pitch_yaw_speed C-struct to read the message contents from
*/
static inline uint16_t mavlink_msg_set_roll_pitch_yaw_speed_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed)
{
return mavlink_msg_set_roll_pitch_yaw_speed_pack(system_id, component_id, msg, set_roll_pitch_yaw_speed->target_system, set_roll_pitch_yaw_speed->target_component, set_roll_pitch_yaw_speed->roll_speed, set_roll_pitch_yaw_speed->pitch_speed, set_roll_pitch_yaw_speed->yaw_speed);
}
/**
* @brief Send a set_roll_pitch_yaw_speed message
* @param chan MAVLink channel to send the message
*
* @param target_system System ID
* @param target_component Component ID
* @param roll_speed Desired roll angular speed in rad/s
* @param pitch_speed Desired pitch angular speed in rad/s
* @param yaw_speed Desired yaw angular speed in rad/s
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_set_roll_pitch_yaw_speed_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, float roll_speed, float pitch_speed, float yaw_speed)
{
mavlink_header_t hdr;
mavlink_set_roll_pitch_yaw_speed_t payload;
uint16_t checksum;
mavlink_set_roll_pitch_yaw_speed_t *p = &payload;
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->roll_speed = roll_speed; // float:Desired roll angular speed in rad/s
p->pitch_speed = pitch_speed; // float:Desired pitch angular speed in rad/s
p->yaw_speed = yaw_speed; // float:Desired yaw angular speed in rad/s
hdr.STX = MAVLINK_STX;
hdr.len = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED_LEN;
hdr.msgid = MAVLINK_MSG_ID_SET_ROLL_PITCH_YAW_SPEED;
hdr.sysid = mavlink_system.sysid;
hdr.compid = mavlink_system.compid;
hdr.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = hdr.seq + 1;
mavlink_send_mem(chan, (uint8_t *)&hdr.STX, MAVLINK_NUM_HEADER_BYTES );
crc_init(&checksum);
checksum = crc_calculate_mem((uint8_t *)&hdr.len, &checksum, MAVLINK_CORE_HEADER_LEN);
checksum = crc_calculate_mem((uint8_t *)&payload, &checksum, hdr.len );
hdr.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
hdr.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_mem(chan, (uint8_t *)&payload, hdr.len);
mavlink_send_mem(chan, (uint8_t *)&hdr.ck_a, MAVLINK_NUM_CHECKSUM_BYTES);
}
#endif
// MESSAGE SET_ROLL_PITCH_YAW_SPEED UNPACKING
/**
* @brief Get field target_system from set_roll_pitch_yaw_speed message
*
* @return System ID
*/
static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_system(const mavlink_message_t* msg)
{
mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0];
return (uint8_t)(p->target_system);
}
/**
* @brief Get field target_component from set_roll_pitch_yaw_speed message
*
* @return Component ID
*/
static inline uint8_t mavlink_msg_set_roll_pitch_yaw_speed_get_target_component(const mavlink_message_t* msg)
{
mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0];
return (uint8_t)(p->target_component);
}
/**
* @brief Get field roll_speed from set_roll_pitch_yaw_speed message
*
* @return Desired roll angular speed in rad/s
*/
static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_roll_speed(const mavlink_message_t* msg)
{
mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0];
return (float)(p->roll_speed);
}
/**
* @brief Get field pitch_speed from set_roll_pitch_yaw_speed message
*
* @return Desired pitch angular speed in rad/s
*/
static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_pitch_speed(const mavlink_message_t* msg)
{
mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0];
return (float)(p->pitch_speed);
}
/**
* @brief Get field yaw_speed from set_roll_pitch_yaw_speed message
*
* @return Desired yaw angular speed in rad/s
*/
static inline float mavlink_msg_set_roll_pitch_yaw_speed_get_yaw_speed(const mavlink_message_t* msg)
{
mavlink_set_roll_pitch_yaw_speed_t *p = (mavlink_set_roll_pitch_yaw_speed_t *)&msg->payload[0];
return (float)(p->yaw_speed);
}
/**
* @brief Decode a set_roll_pitch_yaw_speed message into a struct
*
* @param msg The message to decode
* @param set_roll_pitch_yaw_speed C-struct to decode the message contents into
*/
static inline void mavlink_msg_set_roll_pitch_yaw_speed_decode(const mavlink_message_t* msg, mavlink_set_roll_pitch_yaw_speed_t* set_roll_pitch_yaw_speed)
{
memcpy( set_roll_pitch_yaw_speed, msg->payload, sizeof(mavlink_set_roll_pitch_yaw_speed_t));
}
......@@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_state_correction_encode(uint8_t system_id, ui
* @param vyErr y velocity
* @param vzErr z velocity
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_state_correction_t *p = (mavlink_state_correction_t *)&msg.payload[0];
p->xErr = xErr; // float:x position error
p->yErr = yErr; // float:y position error
p->zErr = zErr; // float:z position error
p->rollErr = rollErr; // float:roll error (radians)
p->pitchErr = pitchErr; // float:pitch error (radians)
p->yawErr = yawErr; // float:yaw error (radians)
p->vxErr = vxErr; // float:x velocity
p->vyErr = vyErr; // float:y velocity
p->vzErr = vzErr; // float:z velocity
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_STATE_CORRECTION_LEN;
msg.msgid = MAVLINK_MSG_ID_STATE_CORRECTION;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_state_correction_send(mavlink_channel_t chan, float xErr, float yErr, float zErr, float rollErr, float pitchErr, float yawErr, float vxErr, float vyErr, float vzErr)
{
mavlink_header_t hdr;
......
......@@ -7,7 +7,7 @@
typedef struct __mavlink_statustext_t
{
uint8_t severity; ///< Severity of status, 0 = info message, 255 = critical fault
char text[50]; ///< Status text message, without null termination character
int8_t text[50]; ///< Status text message, without null termination character
} mavlink_statustext_t;
#define MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 50
......@@ -22,13 +22,13 @@ typedef struct __mavlink_statustext_t
* @param text Status text message, without null termination character
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t severity, const char* text)
static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, uint8_t severity, const int8_t* text)
{
mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_STATUSTEXT;
p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault
memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character
memcpy(p->text, text, sizeof(p->text)); // int8_t[50]:Status text message, without null termination character
return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STATUSTEXT_LEN);
}
......@@ -43,13 +43,13 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co
* @param text Status text message, without null termination character
* @return length of the message in bytes (excluding serial stream start sign)
*/
static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t severity, const char* text)
static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, uint8_t severity, const int8_t* text)
{
mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0];
msg->msgid = MAVLINK_MSG_ID_STATUSTEXT;
p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault
memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character
memcpy(p->text, text, sizeof(p->text)); // int8_t[50]:Status text message, without null termination character
return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STATUSTEXT_LEN);
}
......@@ -74,34 +74,10 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t
* @param severity Severity of status, 0 = info message, 255 = critical fault
* @param text Status text message, without null termination character
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_statustext_t *p = (mavlink_statustext_t *)&msg.payload[0];
p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault
memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_STATUSTEXT_LEN;
msg.msgid = MAVLINK_MSG_ID_STATUSTEXT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char* text)
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const int8_t* text)
{
mavlink_header_t hdr;
mavlink_statustext_t payload;
......@@ -109,7 +85,7 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s
mavlink_statustext_t *p = &payload;
p->severity = severity; // uint8_t:Severity of status, 0 = info message, 255 = critical fault
memcpy(p->text, text, sizeof(p->text)); // char[50]:Status text message, without null termination character
memcpy(p->text, text, sizeof(p->text)); // int8_t[50]:Status text message, without null termination character
hdr.STX = MAVLINK_STX;
hdr.len = MAVLINK_MSG_ID_STATUSTEXT_LEN;
......@@ -149,7 +125,7 @@ static inline uint8_t mavlink_msg_statustext_get_severity(const mavlink_message_
*
* @return Status text message, without null termination character
*/
static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, char* text)
static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* msg, int8_t* text)
{
mavlink_statustext_t *p = (mavlink_statustext_t *)&msg->payload[0];
......
......@@ -6,13 +6,13 @@
typedef struct __mavlink_sys_status_t
{
uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h
uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM
uint8_t status; ///< System status flag, see MAV_STATUS ENUM
uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
uint16_t vbat; ///< Battery voltage, in millivolts (1 = 1 millivolt)
uint16_t battery_remaining; ///< Remaining battery energy: (0%: 0, 100%: 1000)
uint16_t packet_drop; ///< Dropped packets (packets that were corrupted on reception on the MAV)
uint8_t mode; ///< System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h
uint8_t nav_mode; ///< Navigation mode, see MAV_NAV_MODE ENUM
uint8_t status; ///< System status flag, see MAV_STATUS ENUM
} mavlink_sys_status_t;
......@@ -103,38 +103,9 @@ static inline uint16_t mavlink_msg_sys_status_encode(uint8_t system_id, uint8_t
* @param battery_remaining Remaining battery energy: (0%: 0, 100%: 1000)
* @param packet_drop Dropped packets (packets that were corrupted on reception on the MAV)
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_sys_status_t *p = (mavlink_sys_status_t *)&msg.payload[0];
p->mode = mode; // uint8_t:System mode, see MAV_MODE ENUM in mavlink/include/mavlink_types.h
p->nav_mode = nav_mode; // uint8_t:Navigation mode, see MAV_NAV_MODE ENUM
p->status = status; // uint8_t:System status flag, see MAV_STATUS ENUM
p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000
p->vbat = vbat; // uint16_t:Battery voltage, in millivolts (1 = 1 millivolt)
p->battery_remaining = battery_remaining; // uint16_t:Remaining battery energy: (0%: 0, 100%: 1000)
p->packet_drop = packet_drop; // uint16_t:Dropped packets (packets that were corrupted on reception on the MAV)
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SYS_STATUS_LEN;
msg.msgid = MAVLINK_MSG_ID_SYS_STATUS;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t status, uint16_t load, uint16_t vbat, uint16_t battery_remaining, uint16_t packet_drop)
{
mavlink_header_t hdr;
......
......@@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_system_time_encode(uint8_t system_id, uint8_t
*
* @param time_usec Timestamp of the master clock in microseconds since UNIX epoch.
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_system_time_t *p = (mavlink_system_time_t *)&msg.payload[0];
p->time_usec = time_usec; // uint64_t:Timestamp of the master clock in microseconds since UNIX epoch.
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SYSTEM_TIME_LEN;
msg.msgid = MAVLINK_MSG_ID_SYSTEM_TIME;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_system_time_send(mavlink_channel_t chan, uint64_t time_usec)
{
mavlink_header_t hdr;
......
......@@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_system_time_utc_encode(uint8_t system_id, uin
* @param utc_date GPS UTC date ddmmyy
* @param utc_time GPS UTC time hhmmss
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_system_time_utc_t *p = (mavlink_system_time_utc_t *)&msg.payload[0];
p->utc_date = utc_date; // uint32_t:GPS UTC date ddmmyy
p->utc_time = utc_time; // uint32_t:GPS UTC time hhmmss
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_SYSTEM_TIME_UTC_LEN;
msg.msgid = MAVLINK_MSG_ID_SYSTEM_TIME_UTC;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_system_time_utc_send(mavlink_channel_t chan, uint32_t utc_date, uint32_t utc_time)
{
mavlink_header_t hdr;
......
......@@ -8,10 +8,10 @@ typedef struct __mavlink_vfr_hud_t
{
float airspeed; ///< Current airspeed in m/s
float groundspeed; ///< Current ground speed in m/s
int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north)
uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100
float alt; ///< Current altitude (MSL), in meters
float climb; ///< Current climb rate in meters/second
int16_t heading; ///< Current heading in degrees, in compass units (0..360, 0=north)
uint16_t throttle; ///< Current throttle setting in integer percent, 0 to 100
} mavlink_vfr_hud_t;
......@@ -97,37 +97,9 @@ static inline uint16_t mavlink_msg_vfr_hud_encode(uint8_t system_id, uint8_t com
* @param alt Current altitude (MSL), in meters
* @param climb Current climb rate in meters/second
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_vfr_hud_t *p = (mavlink_vfr_hud_t *)&msg.payload[0];
p->airspeed = airspeed; // float:Current airspeed in m/s
p->groundspeed = groundspeed; // float:Current ground speed in m/s
p->heading = heading; // int16_t:Current heading in degrees, in compass units (0..360, 0=north)
p->throttle = throttle; // uint16_t:Current throttle setting in integer percent, 0 to 100
p->alt = alt; // float:Current altitude (MSL), in meters
p->climb = climb; // float:Current climb rate in meters/second
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_VFR_HUD_LEN;
msg.msgid = MAVLINK_MSG_ID_VFR_HUD;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_vfr_hud_send(mavlink_channel_t chan, float airspeed, float groundspeed, int16_t heading, uint16_t throttle, float alt, float climb)
{
mavlink_header_t hdr;
......
......@@ -6,13 +6,6 @@
typedef struct __mavlink_waypoint_t
{
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint16_t seq; ///< Sequence
uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h
uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
float param1; ///< PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
......@@ -20,6 +13,13 @@ typedef struct __mavlink_waypoint_t
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h
uint8_t command; ///< The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
} mavlink_waypoint_t;
......@@ -145,45 +145,9 @@ static inline uint16_t mavlink_msg_waypoint_encode(uint8_t system_id, uint8_t co
* @param y PARAM6 / y position: global: longitude
* @param z PARAM7 / z position: global: altitude
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_waypoint_t *p = (mavlink_waypoint_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->seq = seq; // uint16_t:Sequence
p->frame = frame; // uint8_t:The coordinate system of the waypoint. see MAV_FRAME in mavlink_types.h
p->command = command; // uint8_t:The scheduled action for the waypoint. see MAV_COMMAND in common.xml MAVLink specs
p->current = current; // uint8_t:false:0, true:1
p->autocontinue = autocontinue; // uint8_t:autocontinue to next wp
p->param1 = param1; // float:PARAM1 / For NAV command waypoints: Radius in which the waypoint is accepted as reached, in meters
p->param2 = param2; // float:PARAM2 / For NAV command waypoints: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
p->param3 = param3; // float:PARAM3 / For LOITER command waypoints: Orbit to circle around the waypoint, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
p->param4 = param4; // float:PARAM4 / For NAV and LOITER command waypoints: Yaw orientation in degrees, [0..360] 0 = NORTH
p->x = x; // float:PARAM5 / local: x position, global: latitude
p->y = y; // float:PARAM6 / y position: global: longitude
p->z = z; // float:PARAM7 / z position: global: altitude
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_WAYPOINT_LEN;
msg.msgid = MAVLINK_MSG_ID_WAYPOINT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint8_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z)
{
mavlink_header_t hdr;
......
......@@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_waypoint_ack_encode(uint8_t system_id, uint8_
* @param target_component Component ID
* @param type 0: OK, 1: Error
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_waypoint_ack_t *p = (mavlink_waypoint_ack_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->type = type; // uint8_t:0: OK, 1: Error
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_WAYPOINT_ACK_LEN;
msg.msgid = MAVLINK_MSG_ID_WAYPOINT_ACK;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type)
{
mavlink_header_t hdr;
......
......@@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_waypoint_clear_all_encode(uint8_t system_id,
* @param target_system System ID
* @param target_component Component ID
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_waypoint_clear_all_t *p = (mavlink_waypoint_clear_all_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL_LEN;
msg.msgid = MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
{
mavlink_header_t hdr;
......
......@@ -6,9 +6,9 @@
typedef struct __mavlink_waypoint_count_t
{
uint16_t count; ///< Number of Waypoints in the Sequence
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint16_t count; ///< Number of Waypoints in the Sequence
} mavlink_waypoint_count_t;
......@@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_waypoint_count_encode(uint8_t system_id, uint
* @param target_component Component ID
* @param count Number of Waypoints in the Sequence
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_waypoint_count_t *p = (mavlink_waypoint_count_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->count = count; // uint16_t:Number of Waypoints in the Sequence
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_WAYPOINT_COUNT_LEN;
msg.msgid = MAVLINK_MSG_ID_WAYPOINT_COUNT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count)
{
mavlink_header_t hdr;
......
......@@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_waypoint_current_encode(uint8_t system_id, ui
*
* @param seq Sequence
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_waypoint_current_t *p = (mavlink_waypoint_current_t *)&msg.payload[0];
p->seq = seq; // uint16_t:Sequence
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_WAYPOINT_CURRENT_LEN;
msg.msgid = MAVLINK_MSG_ID_WAYPOINT_CURRENT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_current_send(mavlink_channel_t chan, uint16_t seq)
{
mavlink_header_t hdr;
......
......@@ -67,32 +67,9 @@ static inline uint16_t mavlink_msg_waypoint_reached_encode(uint8_t system_id, ui
*
* @param seq Sequence
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_waypoint_reached_t *p = (mavlink_waypoint_reached_t *)&msg.payload[0];
p->seq = seq; // uint16_t:Sequence
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_WAYPOINT_REACHED_LEN;
msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REACHED;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_reached_send(mavlink_channel_t chan, uint16_t seq)
{
mavlink_header_t hdr;
......
......@@ -6,9 +6,9 @@
typedef struct __mavlink_waypoint_request_t
{
uint16_t seq; ///< Sequence
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint16_t seq; ///< Sequence
} mavlink_waypoint_request_t;
......@@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_waypoint_request_encode(uint8_t system_id, ui
* @param target_component Component ID
* @param seq Sequence
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_waypoint_request_t *p = (mavlink_waypoint_request_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->seq = seq; // uint16_t:Sequence
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LEN;
msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq)
{
mavlink_header_t hdr;
......
......@@ -73,33 +73,9 @@ static inline uint16_t mavlink_msg_waypoint_request_list_encode(uint8_t system_i
* @param target_system System ID
* @param target_component Component ID
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_waypoint_request_list_t *p = (mavlink_waypoint_request_list_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST_LEN;
msg.msgid = MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component)
{
mavlink_header_t hdr;
......
......@@ -6,9 +6,9 @@
typedef struct __mavlink_waypoint_set_current_t
{
uint16_t seq; ///< Sequence
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint16_t seq; ///< Sequence
} mavlink_waypoint_set_current_t;
......@@ -79,34 +79,9 @@ static inline uint16_t mavlink_msg_waypoint_set_current_encode(uint8_t system_id
* @param target_component Component ID
* @param seq Sequence
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_waypoint_set_current_t *p = (mavlink_waypoint_set_current_t *)&msg.payload[0];
p->target_system = target_system; // uint8_t:System ID
p->target_component = target_component; // uint8_t:Component ID
p->seq = seq; // uint16_t:Sequence
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT_LEN;
msg.msgid = MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_waypoint_set_current_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq)
{
mavlink_header_t hdr;
......
/** @file
* @brief MAVLink comm protocol checksum routines.
* @see http://qgroundcontrol.org/mavlink/
* Edited on Monday, August 8 2011
*/
#ifdef __cplusplus
extern "C" {
#endif
#ifndef _CHECKSUM_H_
#define _CHECKSUM_H_
#include "inttypes.h"
/**
*
* CALCULATE THE CHECKSUM
*
*/
#define X25_INIT_CRC 0xffff
#define X25_VALIDATE_CRC 0xf0b8
/**
* @brief Accumulate the X.25 CRC by adding one char at a time.
*
* The checksum function adds the hash of one char at a time to the
* 16 bit checksum (uint16_t).
*
* @param data new char to hash
* @param crcAccum the already accumulated checksum
**/
static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum)
{
/*Accumulate one byte of data into the CRC*/
uint8_t tmp;
tmp=data ^ (uint8_t)(*crcAccum &0xff);
tmp^= (tmp<<4);
*crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4);
// *crcAccum += data; // super simple to test
}
/**
* @brief Initiliaze the buffer for the X.25 CRC
*
* @param crcAccum the 16 bit X.25 CRC
*/
static inline void crc_init(uint16_t* crcAccum)
{
*crcAccum = X25_INIT_CRC;
}
/**
* @brief Initiliaze the buffer for the X.25 CRC to a specified value
*
* @param crcAccum the 16 bit X.25 CRC
*/
static inline void crc_init2(uint16_t* crcAccum, uint16_t crcValue)
{
*crcAccum = crcValue;
}
/**
* @brief Calculates the X.25 checksum on a byte buffer
*
* @param pBuffer buffer containing the byte array to hash
* @param length length of the byte array
* @return the checksum over the buffer bytes
**/
static inline uint16_t crc_calculate(uint8_t* pBuffer, int length)
{
// For a "message" of length bytes contained in the unsigned char array
// pointed to by pBuffer, calculate the CRC
// crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed
uint16_t crcTmp;
//uint16_t tmp;
uint8_t* pTmp;
int i;
pTmp=pBuffer;
/* init crcTmp */
crc_init(&crcTmp);
for (i = 0; i < length; i++){
crc_accumulate(*pTmp++, &crcTmp);
}
/* This is currently not needed, as only the checksum over payload should be computed
tmp = crcTmp;
crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp);
crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp);
*checkConst = tmp;
*/
return(crcTmp);
}
/**
* @brief Calculates the X.25 checksum on a msg buffer
*
* @param pMSG buffer containing the msg to hash
* @param length number of bytes to hash
* @return the checksum over the buffer bytes
**/
static inline uint16_t crc_calculate_msg(mavlink_message_t* pMSG, int length)
{
// For a "message" of length bytes contained in the unsigned char array
// pointed to by pBuffer, calculate the CRC
// crcCalculate(unsigned char* pBuffer, int length, unsigned short* checkConst) < not needed
uint16_t crcTmp;
//uint16_t tmp;
uint8_t* pTmp;
int i;
pTmp=&pMSG->len;
/* init crcTmp */
crc_init(&crcTmp);
for (i = 0; i < 5; i++){
crc_accumulate(*pTmp++, &crcTmp);
}
pTmp=&pMSG->payload[0];
for (; i < length; i++){
crc_accumulate(*pTmp++, &crcTmp);
}
/* This is currently not needed, as only the checksum over payload should be computed
tmp = crcTmp;
crcAccumulate((unsigned char)(~crcTmp & 0xff),&tmp);
crcAccumulate((unsigned char)((~crcTmp>>8)&0xff),&tmp);
*checkConst = tmp;
*/
return(crcTmp);
}
#endif /* _CHECKSUM_H_ */
#ifdef __cplusplus
}
#endif
/** @file
* @brief Main MAVLink comm protocol data.
* @see http://qgroundcontrol.org/mavlink/
* Edited on Monday, August 8 2011
*/
#ifndef _ML_DATA_H_
#define _ML_DATA_H_
#include "mavlink_types.h"
#ifdef MAVLINK_CHECK_LENGTH
const uint8_t MAVLINK_CONST mavlink_msg_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
#endif
const uint8_t MAVLINK_CONST mavlink_msg_keys[256] = MAVLINK_MESSAGE_KEYS;
mavlink_status_t m_mavlink_status[MAVLINK_COMM_NB];
mavlink_message_t m_mavlink_message[MAVLINK_COMM_NB];
#endif
\ No newline at end of file
/** @file
* @brief MAVLink comm protocol option constants.
* @see http://qgroundcontrol.org/mavlink/
* Edited on Monday, August 8 2011
*/
#ifdef __cplusplus
extern "C" {
#endif
#ifndef _ML_OPTIONS_H_
#define _ML_OPTIONS_H_
/**
*
* Receive message length check option. On receive verify the length field
* as soon as the message ID field is received. Requires a 256 byte const
* table. Comment out the define to leave out the table and code to check it.
*
*/
#define MAVLINK_CHECK_LENGTH
/**
*
* Receive message buffer option. This option should be used only when the
* side effects are understood but allows the underlying program access to
* the internal recieve buffer - eliminating the usual double buffering. It
* also REQUIRES changes in the return type of mavlink_parse_char so only
* enable if you make the changes required. Default DISABLED.
*
*/
//#define MAVLINK_STAIC_BUFFER
/**
*
* Receive message buffers option. This option defines how many msg buffers
* mavlink will define, and thereby how many links it can support. A default
* will be supplied if the symbol is not pre-defined, dependant on the make
* envionment. The default is 16 for a recognised OS envionment and 1
* otherwise.
*
*/
#if !((defined MAVLINK_COMM_NB) | (MAVLINK_COMM_NB < 1))
#undef MAVLINK_COMM_NB
#if (defined linux) | (defined __linux) | (defined __MACH__) | (defined _WIN32) | (defined __APPLE__)
#define MAVLINK_COMM_NB 16
#else
#define MAVLINK_COMM_NB 1
#endif
#endif
/**
*
* Data relization option. This option controls inclusion of the file
* mavlink_data.h in the current compile unit - thus defining mavlink's
* variables. Default is ON (not defined) because typically mavlink.h is only
* included once in a system but if it was used in two files there would
* be duplicate variables at link time. Normal practice would be to define
* this symbol outside of this file as defining it here will cause missing
* symbols at link time. In other words in the first file to include mavlink.h
* do not define this sybol, then define this symbol in all other files before
* including mavlink.h
*
*/
//#define MAVLINK_NO_DATA
#ifdef MAVLINK_NO_DATA
#undef MAVLINK_DATA
#else
#define MAVLINK_DATA
#endif
/**
*
* Custom data const data relization and access options.
* This define is placed in the form
* const uint8_t MAVLINK_CONST name[] = { ... };
* for the keys table and (if applicable) lengths table to tell the compiler
* were to put the data. The access option is placed in the form
* variable = MAVLINK_CONST_READ( name[i] );
* in order to allow custom read function's or accessors.
* By default MAVLINK_CONST is defined as nothing and MAVLINK_CONST_READ as
* MAVLINK_CONST_READ( a ) a
* These symbols are only defined if not already defined allowing this file
* to remain unchanged while the actual definitions are maintained in external
* files.
*
*/
#ifndef MAVLINK_CONST
#define MAVLINK_CONST
#endif
#ifndef MAVLINK_CONST_READ
#define MAVLINK_CONST_READ( a ) a
#endif
#endif /* _ML_OPTIONS_H_ */
#ifdef __cplusplus
}
#endif
This diff is collapsed.
This diff is collapsed.
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Sunday, July 31 2011, 15:11 UTC
* Generated on Tuesday, August 9 2011, 16:16 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H
#pragma pack(push,1)
#include "mavlink_options.h"
#include "minimal.h"
#ifdef MAVLINK_CHECK_LENGTH
#include "lengths.h"
#ifdef MAVLINK_DATA
#include "mavlink_data.h"
#endif
#pragma pack(pop)
#endif
......@@ -76,34 +76,9 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c
* @param type Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
* @param autopilot Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg.payload[0];
p->type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM)
p->autopilot = autopilot; // uint8_t:Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM
p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_HEARTBEAT_LEN;
msg.msgid = MAVLINK_MSG_ID_HEARTBEAT;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t type, uint8_t autopilot)
{
mavlink_header_t hdr;
......
/** @file
* @brief MAVLink comm protocol.
* @see http://qgroundcontrol.org/mavlink/
* Generated on Sunday, July 31 2011, 15:11 UTC
* Generated on Tuesday, August 9 2011, 16:16 UTC
*/
#ifndef MINIMAL_H
#define MINIMAL_H
......@@ -32,6 +32,13 @@ extern "C" {
// MESSAGE DEFINITIONS
#include "./mavlink_msg_heartbeat.h"
// MESSAGE CRC KEYS
#undef MAVLINK_MESSAGE_KEYS
#define MAVLINK_MESSAGE_KEYS { }
#ifdef __cplusplus
}
#endif
......
/** @file
* @brief MAVLink comm protocol.
* @see http://pixhawk.ethz.ch/software/mavlink
* Generated on Sunday, July 31 2011, 15:11 UTC
* Generated on Tuesday, August 9 2011, 16:16 UTC
*/
#ifndef MAVLINK_H
#define MAVLINK_H
#pragma pack(push,1)
#include "mavlink_options.h"
#include "pixhawk.h"
#ifdef MAVLINK_CHECK_LENGTH
#include "lengths.h"
#ifdef MAVLINK_DATA
#include "mavlink_data.h"
#endif
#pragma pack(pop)
#endif
......@@ -6,11 +6,11 @@
typedef struct __mavlink_attitude_control_t
{
uint8_t target; ///< The system to be controlled
float roll; ///< roll
float pitch; ///< pitch
float yaw; ///< yaw
float thrust; ///< thrust
uint8_t target; ///< The system to be controlled
uint8_t roll_manual; ///< roll control enabled auto:0, manual:1
uint8_t pitch_manual; ///< pitch auto:0, manual:1
uint8_t yaw_manual; ///< yaw auto:0, manual:1
......@@ -115,40 +115,9 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui
* @param yaw_manual yaw auto:0, manual:1
* @param thrust_manual thrust auto:0, manual:1
*/
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual)
{
mavlink_message_t msg;
uint16_t checksum;
mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg.payload[0];
p->target = target; // uint8_t:The system to be controlled
p->roll = roll; // float:roll
p->pitch = pitch; // float:pitch
p->yaw = yaw; // float:yaw
p->thrust = thrust; // float:thrust
p->roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1
p->pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1
p->yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1
p->thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1
msg.STX = MAVLINK_STX;
msg.len = MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN;
msg.msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL;
msg.sysid = mavlink_system.sysid;
msg.compid = mavlink_system.compid;
msg.seq = mavlink_get_channel_status(chan)->current_tx_seq;
mavlink_get_channel_status(chan)->current_tx_seq = msg.seq + 1;
checksum = crc_calculate_msg(&msg, msg.len + MAVLINK_CORE_HEADER_LEN);
msg.ck_a = (uint8_t)(checksum & 0xFF); ///< Low byte
msg.ck_b = (uint8_t)(checksum >> 8); ///< High byte
mavlink_send_msg(chan, &msg);
}
#endif
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS_SMALL
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uint8_t target, float roll, float pitch, float yaw, float thrust, uint8_t roll_manual, uint8_t pitch_manual, uint8_t yaw_manual, uint8_t thrust_manual)
{
mavlink_header_t hdr;
......
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
This diff is collapsed.
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment