diff --git a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h index 774e3e48dfca526ec1fd9b64a078585272b0bb9d..2b2107389445c6c23df203086ebb3ae222525b6c 100644 --- a/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h +++ b/thirdParty/mavlink/include/ardupilotmega/ardupilotmega.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef ARDUPILOTMEGA_H #define ARDUPILOTMEGA_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_ARDUPILOTMEGA @@ -38,7 +38,12 @@ extern "C" { // 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 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 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, 255, 73, 239, 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, 138, 43, 141, 211, 144 } + +// MESSAGE LENGTHS + +#undef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 0, 2, 2, 0, 0, 0, 0, 0, 0, 0, 20, 2, 24, 22, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 20, 20, 20, 8, 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, 36, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/ardupilotmega/mavlink.h b/thirdParty/mavlink/include/ardupilotmega/mavlink.h index 933149099f58246694eeb34b4214081f6a7ed829..e0e17b063e809bfb0611f734703a8dd3477f5867 100644 --- a/thirdParty/mavlink/include/ardupilotmega/mavlink.h +++ b/thirdParty/mavlink/include/ardupilotmega/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/common/common.h b/thirdParty/mavlink/include/common/common.h index 2364eb3ecdebc4c3a1f1319c742e4c7f06352899..b14f77c31a0210c4c46da483702060bd095697e7 100644 --- a/thirdParty/mavlink/include/common/common.h +++ b/thirdParty/mavlink/include/common/common.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Friday, August 12 2011, 20:25 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef COMMON_H #define COMMON_H @@ -300,7 +300,7 @@ enum MAV_CMD // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 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, 138, 43, 141, 211, 144 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 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, 255, 73, 239, 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, 138, 43, 141, 211, 144 } // MESSAGE LENGTHS diff --git a/thirdParty/mavlink/include/common/mavlink.h b/thirdParty/mavlink/include/common/mavlink.h index 9c3399f4558f1db482cfc05756520e89d1897185..b7cbe96da44dd442d81d3ee795164cac9a3a746c 100644 --- a/thirdParty/mavlink/include/common/mavlink.h +++ b/thirdParty/mavlink/include/common/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Friday, August 12 2011, 20:25 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/mavlink_protocol.h b/thirdParty/mavlink/include/mavlink_protocol.h index 596b5ffcf12b99cf2095ca46409240cbf860ec65..8cf62f115d81ae5ba93b14ad863d43839d4361f3 100644 --- a/thirdParty/mavlink/include/mavlink_protocol.h +++ b/thirdParty/mavlink/include/mavlink_protocol.h @@ -31,26 +31,26 @@ extern mavlink_system_t mavlink_system; */ static void mavlink_parse_state_initialize(mavlink_status_t* initStatus) { - if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) - { - initStatus->ck_a = 0; - initStatus->ck_b = 0; - initStatus->msg_received = 0; - initStatus->buffer_overrun = 0; - initStatus->parse_error = 0; - initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; - initStatus->packet_idx = 0; - initStatus->packet_rx_drop_count = 0; - initStatus->packet_rx_success_count = 0; - initStatus->current_rx_seq = 0; - initStatus->current_tx_seq = 0; - } + if ((initStatus->parse_state <= MAVLINK_PARSE_STATE_UNINIT) || (initStatus->parse_state > MAVLINK_PARSE_STATE_GOT_CRC1)) + { + initStatus->ck_a = 0; + initStatus->ck_b = 0; + initStatus->msg_received = 0; + initStatus->buffer_overrun = 0; + initStatus->parse_error = 0; + initStatus->parse_state = MAVLINK_PARSE_STATE_UNINIT; + initStatus->packet_idx = 0; + initStatus->packet_rx_drop_count = 0; + initStatus->packet_rx_success_count = 0; + initStatus->current_rx_seq = 0; + initStatus->current_tx_seq = 0; + } } static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) { - return &m_mavlink_status[chan]; + return &m_mavlink_status[chan]; } /** @@ -76,19 +76,19 @@ static inline mavlink_status_t* mavlink_get_channel_status(uint8_t chan) */ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint16_t length) { - // This code part is the same for all messages; - uint8_t key; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; - mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; - msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); - key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); - crc_accumulate( key, &msg->ck ); /// include key in X25 checksum - - return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; + // This code part is the same for all messages; + uint8_t key; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq; + mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq = mavlink_get_channel_status(MAVLINK_COMM_0)->current_tx_seq+1; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; } /** @@ -105,19 +105,19 @@ static inline uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t */ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t chan, uint16_t length) { - // This code part is the same for all messages; - uint8_t key; - msg->len = length; - msg->sysid = system_id; - msg->compid = component_id; - // One sequence number per component - msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); - key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); - crc_accumulate( key, &msg->ck ); /// include key in X25 checksum - - return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; + // This code part is the same for all messages; + uint8_t key; + msg->len = length; + msg->sysid = system_id; + msg->compid = component_id; + // One sequence number per component + msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; + mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; + msg->ck = crc_calculate_msg(msg, length + MAVLINK_CORE_HEADER_LEN); + key = MAVLINK_CONST_READ( mavlink_msg_keys[msg->msgid] ); + crc_accumulate( key, &msg->ck ); /// include key in X25 checksum + + return length + MAVLINK_NUM_NON_STX_PAYLOAD_BYTES; } /** @@ -125,14 +125,14 @@ static inline uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uin */ static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink_message_t* msg) { - *(buffer+0) = MAVLINK_STX; ///< Start transmit - // memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload + *(buffer+0) = MAVLINK_STX; ///< Start transmit +// memcpy((buffer+1), msg, msg->len + MAVLINK_CORE_HEADER_LEN); ///< Core header plus payload memcpy((buffer+1), &msg->len, MAVLINK_CORE_HEADER_LEN); ///< Core header memcpy((buffer+1+MAVLINK_CORE_HEADER_LEN), &msg->payload[0], msg->len); ///< payload *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 1) = msg->ck_a; - *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; - // return 0; + *(buffer + msg->len + MAVLINK_CORE_HEADER_LEN + 2) = msg->ck_b; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; +// return 0; } /** @@ -140,22 +140,22 @@ static inline uint16_t mavlink_msg_to_send_buffer(uint8_t* buffer, const mavlink */ static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) { - return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES; } union checksum_ { - uint16_t s; - uint8_t c[2]; + uint16_t s; + uint8_t c[2]; }; static inline void mavlink_start_checksum(mavlink_message_t* msg) { - crc_init(&msg->ck); + crc_init(&msg->ck); } static inline void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) { - crc_accumulate(c, &msg->ck); + crc_accumulate(c, &msg->ck); } /** @@ -200,172 +200,167 @@ static inline mavlink_message_t* mavlink_parse_char(uint8_t chan, uint8_t c, mav static inline int16_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) #endif { - // Initializes only once, values keep unchanged after first initialization - mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); - - mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message - mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status - int bufferIndex = 0; - - status->msg_received = 0; - - switch (status->parse_state) - { - case MAVLINK_PARSE_STATE_UNINIT: - case MAVLINK_PARSE_STATE_IDLE: - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - break; - - case MAVLINK_PARSE_STATE_GOT_STX: - if (status->msg_received) - { - status->buffer_overrun++; - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - } - else - { - // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 - rxmsg->len = c; - status->packet_idx = 0; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; - } - break; - - case MAVLINK_PARSE_STATE_GOT_LENGTH: - rxmsg->seq = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; - break; - - case MAVLINK_PARSE_STATE_GOT_SEQ: - rxmsg->sysid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; - break; - - case MAVLINK_PARSE_STATE_GOT_SYSID: - rxmsg->compid = c; - mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; - break; - - case MAVLINK_PARSE_STATE_GOT_COMPID: - rxmsg->msgid = c; - mavlink_update_checksum(rxmsg, c); + // Initializes only once, values keep unchanged after first initialization + mavlink_parse_state_initialize(mavlink_get_channel_status(chan)); + + mavlink_message_t* rxmsg = &m_mavlink_message[chan]; ///< The currently decoded message + mavlink_status_t* status = mavlink_get_channel_status(chan); ///< The current decode status + int bufferIndex = 0; + + status->msg_received = 0; + + switch (status->parse_state) + { + case MAVLINK_PARSE_STATE_UNINIT: + case MAVLINK_PARSE_STATE_IDLE: + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + break; + + case MAVLINK_PARSE_STATE_GOT_STX: + if (status->msg_received) + { + status->buffer_overrun++; + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + } + else + { + // NOT counting STX, LENGTH, SEQ, SYSID, COMPID, MSGID, CRC1 and CRC2 + rxmsg->len = c; + status->packet_idx = 0; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } + break; + + case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->seq = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; + break; + + case MAVLINK_PARSE_STATE_GOT_SEQ: + rxmsg->sysid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_SYSID; + break; + + case MAVLINK_PARSE_STATE_GOT_SYSID: + rxmsg->compid = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPID: + rxmsg->msgid = c; + mavlink_update_checksum(rxmsg, c); #ifdef MAVLINK_CHECK_LENGTH - if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) - { - status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway - break; - } else ; + if (rxmsg->len != MAVLINK_CONST_READ( mavlink_msg_lengths[c] ) ) + { + status->parse_state = MAVLINK_PARSE_STATE_IDLE; // abort, not going to understand it anyway + break; + } else ; #endif - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; - } - break; - - case MAVLINK_PARSE_STATE_GOT_MSGID: - rxmsg->payload[status->packet_idx++] = c; - mavlink_update_checksum(rxmsg, c); - if (status->packet_idx == rxmsg->len) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - mavlink_update_checksum(rxmsg, - MAVLINK_CONST_READ( mavlink_msg_keys[rxmsg->msgid] )); - } - break; - - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: - if (c != rxmsg->ck_a) - { - // Check first checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; - } - break; - - case MAVLINK_PARSE_STATE_GOT_CRC1: - if (c != rxmsg->ck_b) - {// Check second checksum byte - status->parse_error++; - status->msg_received = 0; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if (c == MAVLINK_STX) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; - mavlink_start_checksum(rxmsg); - } - } - else - { - // Successfully got message - status->msg_received = 1; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - if ( r_message != NULL ) - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); - else ; - } - break; - } - - bufferIndex++; - // If a message has been sucessfully decoded, check index - if (status->msg_received == 1) - { - //while(status->current_seq != rxmsg->seq) - //{ - // status->packet_rx_drop_count++; - // status->current_seq++; - //} - status->current_rx_seq = rxmsg->seq; - // Initial condition: If no packet has been received so far, drop count is undefined - if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; - // Count this packet as received - status->packet_rx_success_count++; - } - - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; + if (rxmsg->len == 0) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + } + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID: + rxmsg->payload[status->packet_idx++] = c; + mavlink_update_checksum(rxmsg, c); + if (status->packet_idx == rxmsg->len) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + mavlink_update_checksum(rxmsg, + MAVLINK_CONST_READ( mavlink_msg_keys[rxmsg->msgid] )); + } + break; + + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: + if (c != rxmsg->ck_a) + { + // Check first checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; + } + break; + + case MAVLINK_PARSE_STATE_GOT_CRC1: + if (c != rxmsg->ck_b) + {// Check second checksum byte + status->parse_error++; + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (c == MAVLINK_STX) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + mavlink_start_checksum(rxmsg); + } + } + else + { + // Successfully got message + status->msg_received = 1; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if ( r_message != NULL ) + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + else ; + } + break; + } + + bufferIndex++; + // If a message has been sucessfully decoded, check index + if (status->msg_received == 1) + { + //while(status->current_seq != rxmsg->seq) + //{ + // status->packet_rx_drop_count++; + // status->current_seq++; + //} + status->current_rx_seq = rxmsg->seq; + // Initial condition: If no packet has been received so far, drop count is undefined + if (status->packet_rx_success_count == 0) status->packet_rx_drop_count = 0; + // Count this packet as received + status->packet_rx_success_count++; + } + + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + status->parse_error = 0; #ifdef MAVLINK_STATIC_BUFFER - if (status->msg_received == 1) - { - if ( r_message != NULL ) - { - return r_message; - } - else - { - return rxmsg; - } - } else return NULL; + if (status->msg_received == 1) + { + if ( r_message != NULL ) + return r_message; + else return rxmsg; + } else return NULL; #else - if (status->msg_received == 1) - return 1; - else return 0; + if (status->msg_received == 1) + return 1; + else return 0; #endif } @@ -385,39 +380,39 @@ void comm_send_ch(mavlink_channel_t chan, uint8_t ch) } if (chan == MAVLINK_COMM_1) { - uart1_transmit(ch); + uart1_transmit(ch); } } static inline void mavlink_send_msg(mavlink_channel_t chan, mavlink_message_t* msg) { - // ARM7 MCU board implementation - // Create pointer on message struct - // Send STX - comm_send_ch(chan, MAVLINK_STX); - comm_send_ch(chan, msg->len); - comm_send_ch(chan, msg->seq); - comm_send_ch(chan, msg->sysid); - comm_send_ch(chan, msg->compid); - comm_send_ch(chan, msg->msgid); - for(uint16_t i = 0; i < msg->len; i++) - { - comm_send_ch(chan, msg->payload[i]); - } - comm_send_ch(chan, msg->ck_a); - comm_send_ch(chan, msg->ck_b); + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + comm_send_ch(chan, MAVLINK_STX); + comm_send_ch(chan, msg->len); + comm_send_ch(chan, msg->seq); + comm_send_ch(chan, msg->sysid); + comm_send_ch(chan, msg->compid); + comm_send_ch(chan, msg->msgid); + for(uint16_t i = 0; i < msg->len; i++) + { + comm_send_ch(chan, msg->payload[i]); + } + comm_send_ch(chan, msg->ck_a); + comm_send_ch(chan, msg->ck_b); } static inline void mavlink_send_mem(mavlink_channel_t chan, (uint8_t *)mem, uint8_t num) { - // ARM7 MCU board implementation - // Create pointer on message struct - // Send STX - for(uint16_t i = 0; i < num; i++) - { - comm_send_ch( chan, mem[i] ); - } + // ARM7 MCU board implementation + // Create pointer on message struct + // Send STX + for(uint16_t i = 0; i < num; i++) + { + comm_send_ch( chan, mem[i] ); + } } */ static inline void mavlink_send_uart(mavlink_channel_t chan, mavlink_message_t* msg); diff --git a/thirdParty/mavlink/include/minimal/mavlink.h b/thirdParty/mavlink/include/minimal/mavlink.h index c8cd1940e27e8ed6ae4a1da255b3590e1edb9ae0..3727dd55d3f1f475110830b06f6ebe4ea341f7e9 100644 --- a/thirdParty/mavlink/include/minimal/mavlink.h +++ b/thirdParty/mavlink/include/minimal/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h index 7c686831eee88b7e0cb4e92e8ffd8f771c7a51ae..466f8a783d30f6fa7dd7214dfbb0c2707b5d7edd 100644 --- a/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h +++ b/thirdParty/mavlink/include/minimal/mavlink_msg_heartbeat.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_HEARTBEAT 0 #define MAVLINK_MSG_ID_HEARTBEAT_LEN 3 #define MAVLINK_MSG_0_LEN 3 +#define MAVLINK_MSG_ID_HEARTBEAT_KEY 0x47 +#define MAVLINK_MSG_0_KEY 0x47 typedef struct __mavlink_heartbeat_t { - uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) - uint8_t autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM - uint8_t mavlink_version; ///< MAVLink version + uint8_t type; ///< Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + uint8_t autopilot; ///< Type of the Autopilot: 0: Generic, 1: PIXHAWK, 2: SLUGS, 3: Ardupilot (up to 15 types), defined in MAV_AUTOPILOT_TYPE ENUM + uint8_t mavlink_version; ///< MAVLink version } mavlink_heartbeat_t; @@ -27,10 +29,10 @@ static inline uint16_t mavlink_msg_heartbeat_pack(uint8_t system_id, uint8_t com mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - 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->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 + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_HEARTBEAT_LEN); } @@ -49,10 +51,10 @@ static inline uint16_t mavlink_msg_heartbeat_pack_chan(uint8_t system_id, uint8_ mavlink_heartbeat_t *p = (mavlink_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_HEARTBEAT; - 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->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 + p->mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_HEARTBEAT_LEN); } @@ -69,6 +71,8 @@ static inline uint16_t mavlink_msg_heartbeat_encode(uint8_t system_id, uint8_t c return mavlink_msg_heartbeat_pack(system_id, component_id, msg, heartbeat->type, heartbeat->autopilot); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a heartbeat message * @param chan MAVLink channel to send the message @@ -76,20 +80,16 @@ 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_header_t hdr; mavlink_heartbeat_t payload; - uint16_t checksum; - mavlink_heartbeat_t *p = &payload; - 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 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_HEARTBEAT_LEN ) + payload.type = type; // uint8_t:Type of the MAV (quadrotor, helicopter, etc., up to 15 types, defined in MAV_TYPE ENUM) + payload.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 + payload.mavlink_version = MAVLINK_VERSION; // uint8_t_mavlink_version:MAVLink version hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_HEARTBEAT_LEN; hdr.msgid = MAVLINK_MSG_ID_HEARTBEAT; @@ -99,14 +99,12 @@ static inline void mavlink_msg_heartbeat_send(mavlink_channel_t chan, uint8_t ty 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x47, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/minimal/minimal.h b/thirdParty/mavlink/include/minimal/minimal.h index c3d7e2a1bf9a894711febe2ae471dc87b2389449..455b0735a813ebbd609ca2743921810da62a3042 100644 --- a/thirdParty/mavlink/include/minimal/minimal.h +++ b/thirdParty/mavlink/include/minimal/minimal.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:20 UTC */ #ifndef MINIMAL_H #define MINIMAL_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_MINIMAL @@ -39,6 +39,11 @@ extern "C" { #undef MAVLINK_MESSAGE_KEYS #define MAVLINK_MESSAGE_KEYS { } +// MESSAGE LENGTHS + +#undef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS { } + #ifdef __cplusplus } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink.h b/thirdParty/mavlink/include/pixhawk/mavlink.h index b14edfc4d057db2186964ff76d9ed44e34f149c0..c6ab3b1c07e6f7a9cb16d11db14fdd57ea344cc1 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:49 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h index fd8503d974ee85d912fd0fe8f19e34824f2efd1c..3da14afabfaa615f350e63dc46bcefdf1164362a 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_attitude_control.h @@ -3,18 +3,20 @@ #define MAVLINK_MSG_ID_ATTITUDE_CONTROL 85 #define MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN 21 #define MAVLINK_MSG_85_LEN 21 +#define MAVLINK_MSG_ID_ATTITUDE_CONTROL_KEY 0x7F +#define MAVLINK_MSG_85_KEY 0x7F typedef struct __mavlink_attitude_control_t { - 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 - uint8_t thrust_manual; ///< thrust auto:0, manual:1 + 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 + uint8_t thrust_manual; ///< thrust auto:0, manual:1 } mavlink_attitude_control_t; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_attitude_control_pack(uint8_t system_id, uint mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - 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 + 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 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_attitude_control_pack_chan(uint8_t system_id, mavlink_attitude_control_t *p = (mavlink_attitude_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ATTITUDE_CONTROL; - 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 + 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 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN); } @@ -101,6 +103,8 @@ static inline uint16_t mavlink_msg_attitude_control_encode(uint8_t system_id, ui return mavlink_msg_attitude_control_pack(system_id, component_id, msg, attitude_control->target, attitude_control->roll, attitude_control->pitch, attitude_control->yaw, attitude_control->thrust, attitude_control->roll_manual, attitude_control->pitch_manual, attitude_control->yaw_manual, attitude_control->thrust_manual); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a attitude_control message * @param chan MAVLink channel to send the message @@ -115,25 +119,21 @@ 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_header_t hdr; mavlink_attitude_control_t payload; - uint16_t checksum; - mavlink_attitude_control_t *p = &payload; - 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 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN ) + payload.target = target; // uint8_t:The system to be controlled + payload.roll = roll; // float:roll + payload.pitch = pitch; // float:pitch + payload.yaw = yaw; // float:yaw + payload.thrust = thrust; // float:thrust + payload.roll_manual = roll_manual; // uint8_t:roll control enabled auto:0, manual:1 + payload.pitch_manual = pitch_manual; // uint8_t:pitch auto:0, manual:1 + payload.yaw_manual = yaw_manual; // uint8_t:yaw auto:0, manual:1 + payload.thrust_manual = thrust_manual; // uint8_t:thrust auto:0, manual:1 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_ATTITUDE_CONTROL_LEN; @@ -144,14 +144,12 @@ static inline void mavlink_msg_attitude_control_send(mavlink_channel_t chan, uin 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x7F, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h index 2fb3314c931a70bcfd1bf23f620e39bf89658d60..4f470bd84a724cba02dc3d86dc6d9e7569a01c72 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_aux_status.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_AUX_STATUS 142 #define MAVLINK_MSG_ID_AUX_STATUS_LEN 12 #define MAVLINK_MSG_142_LEN 12 +#define MAVLINK_MSG_ID_AUX_STATUS_KEY 0x7A +#define MAVLINK_MSG_142_KEY 0x7A typedef struct __mavlink_aux_status_t { - uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - uint16_t i2c0_err_count; ///< Number of I2C errors since startup - uint16_t i2c1_err_count; ///< Number of I2C errors since startup - uint16_t spi0_err_count; ///< Number of I2C errors since startup - uint16_t spi1_err_count; ///< Number of I2C errors since startup - uint16_t uart_total_err_count; ///< Number of I2C errors since startup + uint16_t load; ///< Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + uint16_t i2c0_err_count; ///< Number of I2C errors since startup + uint16_t i2c1_err_count; ///< Number of I2C errors since startup + uint16_t spi0_err_count; ///< Number of I2C errors since startup + uint16_t spi1_err_count; ///< Number of I2C errors since startup + uint16_t uart_total_err_count; ///< Number of I2C errors since startup } mavlink_aux_status_t; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_aux_status_pack(uint8_t system_id, uint8_t co mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup - p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup - p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup - p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup - p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUX_STATUS_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_aux_status_pack_chan(uint8_t system_id, uint8 mavlink_aux_status_t *p = (mavlink_aux_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AUX_STATUS; - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup - p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup - p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup - p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup - p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUX_STATUS_LEN); } @@ -86,6 +88,8 @@ static inline uint16_t mavlink_msg_aux_status_encode(uint8_t system_id, uint8_t return mavlink_msg_aux_status_pack(system_id, component_id, msg, aux_status->load, aux_status->i2c0_err_count, aux_status->i2c1_err_count, aux_status->spi0_err_count, aux_status->spi1_err_count, aux_status->uart_total_err_count); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a aux_status message * @param chan MAVLink channel to send the message @@ -97,22 +101,18 @@ static inline uint16_t mavlink_msg_aux_status_encode(uint8_t system_id, uint8_t * @param spi1_err_count Number of I2C errors since startup * @param uart_total_err_count Number of I2C errors since startup */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t load, uint16_t i2c0_err_count, uint16_t i2c1_err_count, uint16_t spi0_err_count, uint16_t spi1_err_count, uint16_t uart_total_err_count) { mavlink_header_t hdr; mavlink_aux_status_t payload; - uint16_t checksum; - mavlink_aux_status_t *p = &payload; - p->load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 - p->i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup - p->i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup - p->spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup - p->spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup - p->uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_AUX_STATUS_LEN ) + payload.load = load; // uint16_t:Maximum usage in percent of the mainloop time, (0%: 0, 100%: 1000) should be always below 1000 + payload.i2c0_err_count = i2c0_err_count; // uint16_t:Number of I2C errors since startup + payload.i2c1_err_count = i2c1_err_count; // uint16_t:Number of I2C errors since startup + payload.spi0_err_count = spi0_err_count; // uint16_t:Number of I2C errors since startup + payload.spi1_err_count = spi1_err_count; // uint16_t:Number of I2C errors since startup + payload.uart_total_err_count = uart_total_err_count; // uint16_t:Number of I2C errors since startup hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_AUX_STATUS_LEN; @@ -123,14 +123,12 @@ static inline void mavlink_msg_aux_status_send(mavlink_channel_t chan, uint16_t 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x7A, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h index 4e6f6b83bc2ae7e9c0f580ea14f9e677e8b5b53a..cc0e71398b8545d62c94111b5064836fe1d26445 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_brief_feature.h @@ -3,17 +3,19 @@ #define MAVLINK_MSG_ID_BRIEF_FEATURE 172 #define MAVLINK_MSG_ID_BRIEF_FEATURE_LEN 53 #define MAVLINK_MSG_172_LEN 53 +#define MAVLINK_MSG_ID_BRIEF_FEATURE_KEY 0xD9 +#define MAVLINK_MSG_172_KEY 0xD9 typedef struct __mavlink_brief_feature_t { - float x; ///< x position in m - float y; ///< y position in m - float z; ///< z position in m - float response; ///< Harris operator response at this location - uint16_t size; ///< Size in pixels - uint16_t orientation; ///< Orientation - uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true - uint8_t descriptor[32]; ///< Descriptor + float x; ///< x position in m + float y; ///< y position in m + float z; ///< z position in m + float response; ///< Harris operator response at this location + uint16_t size; ///< Size in pixels + uint16_t orientation; ///< Orientation + uint8_t orientation_assignment; ///< Orientation assignment 0: false, 1:true + uint8_t descriptor[32]; ///< Descriptor } mavlink_brief_feature_t; #define MAVLINK_MSG_BRIEF_FEATURE_FIELD_DESCRIPTOR_LEN 32 @@ -39,14 +41,14 @@ static inline uint16_t mavlink_msg_brief_feature_pack(uint8_t system_id, uint8_t mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - p->x = x; // float:x position in m - p->y = y; // float:y position in m - p->z = z; // float:z position in m - p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true - p->size = size; // uint16_t:Size in pixels - p->orientation = orientation; // uint16_t:Orientation - memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor - p->response = response; // float:Harris operator response at this location + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor + p->response = response; // float:Harris operator response at this location return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); } @@ -72,14 +74,14 @@ static inline uint16_t mavlink_msg_brief_feature_pack_chan(uint8_t system_id, ui mavlink_brief_feature_t *p = (mavlink_brief_feature_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_BRIEF_FEATURE; - p->x = x; // float:x position in m - p->y = y; // float:y position in m - p->z = z; // float:z position in m - p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true - p->size = size; // uint16_t:Size in pixels - p->orientation = orientation; // uint16_t:Orientation - memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor - p->response = response; // float:Harris operator response at this location + p->x = x; // float:x position in m + p->y = y; // float:y position in m + p->z = z; // float:z position in m + p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + p->size = size; // uint16_t:Size in pixels + p->orientation = orientation; // uint16_t:Orientation + memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor + p->response = response; // float:Harris operator response at this location return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN); } @@ -97,6 +99,8 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 return mavlink_msg_brief_feature_pack(system_id, component_id, msg, brief_feature->x, brief_feature->y, brief_feature->z, brief_feature->orientation_assignment, brief_feature->size, brief_feature->orientation, brief_feature->descriptor, brief_feature->response); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a brief_feature message * @param chan MAVLink channel to send the message @@ -110,24 +114,20 @@ static inline uint16_t mavlink_msg_brief_feature_encode(uint8_t system_id, uint8 * @param descriptor Descriptor * @param response Harris operator response at this location */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float x, float y, float z, uint8_t orientation_assignment, uint16_t size, uint16_t orientation, const uint8_t* descriptor, float response) { mavlink_header_t hdr; mavlink_brief_feature_t payload; - uint16_t checksum; - mavlink_brief_feature_t *p = &payload; - - p->x = x; // float:x position in m - p->y = y; // float:y position in m - p->z = z; // float:z position in m - p->orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true - p->size = size; // uint16_t:Size in pixels - p->orientation = orientation; // uint16_t:Orientation - memcpy(p->descriptor, descriptor, sizeof(p->descriptor)); // uint8_t[32]:Descriptor - p->response = response; // float:Harris operator response at this location + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_BRIEF_FEATURE_LEN ) + payload.x = x; // float:x position in m + payload.y = y; // float:y position in m + payload.z = z; // float:z position in m + payload.orientation_assignment = orientation_assignment; // uint8_t:Orientation assignment 0: false, 1:true + payload.size = size; // uint16_t:Size in pixels + payload.orientation = orientation; // uint16_t:Orientation + memcpy(payload.descriptor, descriptor, sizeof(payload.descriptor)); // uint8_t[32]:Descriptor + payload.response = response; // float:Harris operator response at this location hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_BRIEF_FEATURE_LEN; @@ -138,14 +138,12 @@ static inline void mavlink_msg_brief_feature_send(mavlink_channel_t chan, float 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xD9, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h index 54d2070a987464f289a4fb0551d7c743588a1dfe..e329ac10252b57e6a817c2a79a78f60f52f243be 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_data_transmission_handshake.h @@ -3,14 +3,16 @@ #define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE 170 #define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN 8 #define MAVLINK_MSG_170_LEN 8 +#define MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_KEY 0xC8 +#define MAVLINK_MSG_170_KEY 0xC8 typedef struct __mavlink_data_transmission_handshake_t { - uint32_t size; ///< total data size in bytes (set on ACK only) - uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - uint8_t packets; ///< number of packets beeing sent (set on ACK only) - uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - uint8_t jpg_quality; ///< JPEG quality out of [1,100] + uint32_t size; ///< total data size in bytes (set on ACK only) + uint8_t type; ///< type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + uint8_t packets; ///< number of packets beeing sent (set on ACK only) + uint8_t payload; ///< payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + uint8_t jpg_quality; ///< JPEG quality out of [1,100] } mavlink_data_transmission_handshake_t; @@ -32,11 +34,11 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack(uint8_t syst mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - p->size = size; // uint32_t:total data size in bytes (set on ACK only) - p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) - p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); } @@ -59,11 +61,11 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_pack_chan(uint8_t mavlink_data_transmission_handshake_t *p = (mavlink_data_transmission_handshake_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE; - p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - p->size = size; // uint32_t:total data size in bytes (set on ACK only) - p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) - p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] + p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + p->size = size; // uint32_t:total data size in bytes (set on ACK only) + p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN); } @@ -81,6 +83,8 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy return mavlink_msg_data_transmission_handshake_pack(system_id, component_id, msg, data_transmission_handshake->type, data_transmission_handshake->size, data_transmission_handshake->packets, data_transmission_handshake->payload, data_transmission_handshake->jpg_quality); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a data_transmission_handshake message * @param chan MAVLink channel to send the message @@ -91,21 +95,17 @@ static inline uint16_t mavlink_msg_data_transmission_handshake_encode(uint8_t sy * @param payload payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) * @param jpg_quality JPEG quality out of [1,100] */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_t chan, uint8_t type, uint32_t size, uint8_t packets, uint8_t payload, uint8_t jpg_quality) { mavlink_header_t hdr; mavlink_data_transmission_handshake_t payload; - uint16_t checksum; - mavlink_data_transmission_handshake_t *p = &payload; - p->type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) - p->size = size; // uint32_t:total data size in bytes (set on ACK only) - p->packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) - p->payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) - p->jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN ) + payload.type = type; // uint8_t:type of requested/acknowledged data (as defined in ENUM DATA_TYPES in mavlink/include/mavlink_types.h) + payload.size = size; // uint32_t:total data size in bytes (set on ACK only) + payload.packets = packets; // uint8_t:number of packets beeing sent (set on ACK only) + payload.payload = payload; // uint8_t:payload size per packet (normally 253 byte, see DATA field size in message ENCAPSULATED_DATA) (set on ACK only) + payload.jpg_quality = jpg_quality; // uint8_t:JPEG quality out of [1,100] hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_DATA_TRANSMISSION_HANDSHAKE_LEN; @@ -116,14 +116,12 @@ static inline void mavlink_msg_data_transmission_handshake_send(mavlink_channel_ 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xC8, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h index a5354fddedae8612a5426b27b02cd7767e1d125d..4fc69dfb9813a95c09e3fbb91c3e901ccb28ad8c 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_encapsulated_data.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_ENCAPSULATED_DATA 171 #define MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN 255 #define MAVLINK_MSG_171_LEN 255 +#define MAVLINK_MSG_ID_ENCAPSULATED_DATA_KEY 0x87 +#define MAVLINK_MSG_171_KEY 0x87 typedef struct __mavlink_encapsulated_data_t { - uint16_t seqnr; ///< sequence number (starting with 0 on every transmission) - uint8_t data[253]; ///< image data bytes + uint16_t seqnr; ///< sequence number (starting with 0 on every transmission) + uint8_t data[253]; ///< image data bytes } mavlink_encapsulated_data_t; #define MAVLINK_MSG_ENCAPSULATED_DATA_FIELD_DATA_LEN 253 @@ -27,8 +29,8 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack(uint8_t system_id, uin mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) - memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); } @@ -48,8 +50,8 @@ static inline uint16_t mavlink_msg_encapsulated_data_pack_chan(uint8_t system_id mavlink_encapsulated_data_t *p = (mavlink_encapsulated_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_ENCAPSULATED_DATA; - p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) - memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes + p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN); } @@ -67,6 +69,8 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u return mavlink_msg_encapsulated_data_pack(system_id, component_id, msg, encapsulated_data->seqnr, encapsulated_data->data); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a encapsulated_data message * @param chan MAVLink channel to send the message @@ -74,18 +78,14 @@ static inline uint16_t mavlink_msg_encapsulated_data_encode(uint8_t system_id, u * @param seqnr sequence number (starting with 0 on every transmission) * @param data image data bytes */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, uint16_t seqnr, const uint8_t* data) { mavlink_header_t hdr; mavlink_encapsulated_data_t payload; - uint16_t checksum; - mavlink_encapsulated_data_t *p = &payload; - p->seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) - memcpy(p->data, data, sizeof(p->data)); // uint8_t[253]:image data bytes + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN ) + payload.seqnr = seqnr; // uint16_t:sequence number (starting with 0 on every transmission) + memcpy(payload.data, data, sizeof(payload.data)); // uint8_t[253]:image data bytes hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_ENCAPSULATED_DATA_LEN; @@ -96,14 +96,12 @@ static inline void mavlink_msg_encapsulated_data_send(mavlink_channel_t chan, ui 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x87, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h index 0b4c9c8eff8ffb40a4025a6bef0dda500a0ea6f3..2bb279c2185d23127ec7c14d6f57576c6937be57 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_available.h @@ -3,32 +3,34 @@ #define MAVLINK_MSG_ID_IMAGE_AVAILABLE 103 #define MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN 92 #define MAVLINK_MSG_103_LEN 92 +#define MAVLINK_MSG_ID_IMAGE_AVAILABLE_KEY 0xA5 +#define MAVLINK_MSG_103_KEY 0xA5 typedef struct __mavlink_image_available_t { - uint64_t cam_id; ///< Camera id - uint64_t timestamp; ///< Timestamp - uint64_t valid_until; ///< Until which timestamp this buffer will stay valid - uint32_t img_seq; ///< The image sequence number - uint32_t img_buf_index; ///< Position of the image in the buffer, starts with 0 - uint32_t key; ///< Shared memory area key - uint32_t exposure; ///< Exposure time, in microseconds - float gain; ///< Camera gain - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - float local_z; ///< Local frame Z coordinate (height over ground) - float lat; ///< GPS X coordinate - float lon; ///< GPS Y coordinate - float alt; ///< Global frame altitude - float ground_x; ///< Ground truth X - float ground_y; ///< Ground truth Y - float ground_z; ///< Ground truth Z - uint16_t width; ///< Image width - uint16_t height; ///< Image height - uint16_t depth; ///< Image depth - uint8_t cam_no; ///< Camera # (starts with 0) - uint8_t channels; ///< Image channels + uint64_t cam_id; ///< Camera id + uint64_t timestamp; ///< Timestamp + uint64_t valid_until; ///< Until which timestamp this buffer will stay valid + uint32_t img_seq; ///< The image sequence number + uint32_t img_buf_index; ///< Position of the image in the buffer, starts with 0 + uint32_t key; ///< Shared memory area key + uint32_t exposure; ///< Exposure time, in microseconds + float gain; ///< Camera gain + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad + float local_z; ///< Local frame Z coordinate (height over ground) + float lat; ///< GPS X coordinate + float lon; ///< GPS Y coordinate + float alt; ///< Global frame altitude + float ground_x; ///< Ground truth X + float ground_y; ///< Ground truth Y + float ground_z; ///< Ground truth Z + uint16_t width; ///< Image width + uint16_t height; ///< Image height + uint16_t depth; ///< Image depth + uint8_t cam_no; ///< Camera # (starts with 0) + uint8_t channels; ///< Image channels } mavlink_image_available_t; @@ -68,29 +70,29 @@ static inline uint16_t mavlink_msg_image_available_pack(uint8_t system_id, uint8 mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - p->cam_id = cam_id; // uint64_t:Camera id - p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) - p->timestamp = timestamp; // uint64_t:Timestamp - p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid - p->img_seq = img_seq; // uint32_t:The image sequence number - p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 - p->width = width; // uint16_t:Image width - p->height = height; // uint16_t:Image height - p->depth = depth; // uint16_t:Image depth - p->channels = channels; // uint8_t:Image channels - p->key = key; // uint32_t:Shared memory area key - p->exposure = exposure; // uint32_t:Exposure time, in microseconds - p->gain = gain; // float:Camera gain - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - p->local_z = local_z; // float:Local frame Z coordinate (height over ground) - p->lat = lat; // float:GPS X coordinate - p->lon = lon; // float:GPS Y coordinate - p->alt = alt; // float:Global frame altitude - p->ground_x = ground_x; // float:Ground truth X - p->ground_y = ground_y; // float:Ground truth Y - p->ground_z = ground_z; // float:Ground truth Z + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); } @@ -131,29 +133,29 @@ static inline uint16_t mavlink_msg_image_available_pack_chan(uint8_t system_id, mavlink_image_available_t *p = (mavlink_image_available_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_AVAILABLE; - p->cam_id = cam_id; // uint64_t:Camera id - p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) - p->timestamp = timestamp; // uint64_t:Timestamp - p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid - p->img_seq = img_seq; // uint32_t:The image sequence number - p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 - p->width = width; // uint16_t:Image width - p->height = height; // uint16_t:Image height - p->depth = depth; // uint16_t:Image depth - p->channels = channels; // uint8_t:Image channels - p->key = key; // uint32_t:Shared memory area key - p->exposure = exposure; // uint32_t:Exposure time, in microseconds - p->gain = gain; // float:Camera gain - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - p->local_z = local_z; // float:Local frame Z coordinate (height over ground) - p->lat = lat; // float:GPS X coordinate - p->lon = lon; // float:GPS Y coordinate - p->alt = alt; // float:Global frame altitude - p->ground_x = ground_x; // float:Ground truth X - p->ground_y = ground_y; // float:Ground truth Y - p->ground_z = ground_z; // float:Ground truth Z + p->cam_id = cam_id; // uint64_t:Camera id + p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) + p->timestamp = timestamp; // uint64_t:Timestamp + p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + p->img_seq = img_seq; // uint32_t:The image sequence number + p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + p->width = width; // uint16_t:Image width + p->height = height; // uint16_t:Image height + p->depth = depth; // uint16_t:Image depth + p->channels = channels; // uint8_t:Image channels + p->key = key; // uint32_t:Shared memory area key + p->exposure = exposure; // uint32_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN); } @@ -171,6 +173,8 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin return mavlink_msg_image_available_pack(system_id, component_id, msg, image_available->cam_id, image_available->cam_no, image_available->timestamp, image_available->valid_until, image_available->img_seq, image_available->img_buf_index, image_available->width, image_available->height, image_available->depth, image_available->channels, image_available->key, image_available->exposure, image_available->gain, image_available->roll, image_available->pitch, image_available->yaw, image_available->local_z, image_available->lat, image_available->lon, image_available->alt, image_available->ground_x, image_available->ground_y, image_available->ground_z); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a image_available message * @param chan MAVLink channel to send the message @@ -199,39 +203,35 @@ static inline uint16_t mavlink_msg_image_available_encode(uint8_t system_id, uin * @param ground_y Ground truth Y * @param ground_z Ground truth Z */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint64_t cam_id, uint8_t cam_no, uint64_t timestamp, uint64_t valid_until, uint32_t img_seq, uint32_t img_buf_index, uint16_t width, uint16_t height, uint16_t depth, uint8_t channels, uint32_t key, uint32_t exposure, float gain, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { mavlink_header_t hdr; mavlink_image_available_t payload; - uint16_t checksum; - mavlink_image_available_t *p = &payload; - - p->cam_id = cam_id; // uint64_t:Camera id - p->cam_no = cam_no; // uint8_t:Camera # (starts with 0) - p->timestamp = timestamp; // uint64_t:Timestamp - p->valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid - p->img_seq = img_seq; // uint32_t:The image sequence number - p->img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 - p->width = width; // uint16_t:Image width - p->height = height; // uint16_t:Image height - p->depth = depth; // uint16_t:Image depth - p->channels = channels; // uint8_t:Image channels - p->key = key; // uint32_t:Shared memory area key - p->exposure = exposure; // uint32_t:Exposure time, in microseconds - p->gain = gain; // float:Camera gain - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - p->local_z = local_z; // float:Local frame Z coordinate (height over ground) - p->lat = lat; // float:GPS X coordinate - p->lon = lon; // float:GPS Y coordinate - p->alt = alt; // float:Global frame altitude - p->ground_x = ground_x; // float:Ground truth X - p->ground_y = ground_y; // float:Ground truth Y - p->ground_z = ground_z; // float:Ground truth Z + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN ) + payload.cam_id = cam_id; // uint64_t:Camera id + payload.cam_no = cam_no; // uint8_t:Camera # (starts with 0) + payload.timestamp = timestamp; // uint64_t:Timestamp + payload.valid_until = valid_until; // uint64_t:Until which timestamp this buffer will stay valid + payload.img_seq = img_seq; // uint32_t:The image sequence number + payload.img_buf_index = img_buf_index; // uint32_t:Position of the image in the buffer, starts with 0 + payload.width = width; // uint16_t:Image width + payload.height = height; // uint16_t:Image height + payload.depth = depth; // uint16_t:Image depth + payload.channels = channels; // uint8_t:Image channels + payload.key = key; // uint32_t:Shared memory area key + payload.exposure = exposure; // uint32_t:Exposure time, in microseconds + payload.gain = gain; // float:Camera gain + payload.roll = roll; // float:Roll angle in rad + payload.pitch = pitch; // float:Pitch angle in rad + payload.yaw = yaw; // float:Yaw angle in rad + payload.local_z = local_z; // float:Local frame Z coordinate (height over ground) + payload.lat = lat; // float:GPS X coordinate + payload.lon = lon; // float:GPS Y coordinate + payload.alt = alt; // float:Global frame altitude + payload.ground_x = ground_x; // float:Ground truth X + payload.ground_y = ground_y; // float:Ground truth Y + payload.ground_z = ground_z; // float:Ground truth Z hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_IMAGE_AVAILABLE_LEN; @@ -242,14 +242,12 @@ static inline void mavlink_msg_image_available_send(mavlink_channel_t chan, uint 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xA5, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h index 99027fea5bced7caff56823021cec2c109748d97..4ab1dad96f2e3f1c532ccc96b7e05a64289f0fa6 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_trigger_control.h @@ -3,10 +3,12 @@ #define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL 102 #define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN 1 #define MAVLINK_MSG_102_LEN 1 +#define MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_KEY 0xEE +#define MAVLINK_MSG_102_KEY 0xEE typedef struct __mavlink_image_trigger_control_t { - uint8_t enable; ///< 0 to disable, 1 to enable + uint8_t enable; ///< 0 to disable, 1 to enable } mavlink_image_trigger_control_t; @@ -24,7 +26,7 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack(uint8_t system_id, mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - p->enable = enable; // uint8_t:0 to disable, 1 to enable + p->enable = enable; // uint8_t:0 to disable, 1 to enable return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); } @@ -43,7 +45,7 @@ static inline uint16_t mavlink_msg_image_trigger_control_pack_chan(uint8_t syste mavlink_image_trigger_control_t *p = (mavlink_image_trigger_control_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL; - p->enable = enable; // uint8_t:0 to disable, 1 to enable + p->enable = enable; // uint8_t:0 to disable, 1 to enable return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN); } @@ -61,23 +63,21 @@ static inline uint16_t mavlink_msg_image_trigger_control_encode(uint8_t system_i return mavlink_msg_image_trigger_control_pack(system_id, component_id, msg, image_trigger_control->enable); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a image_trigger_control message * @param chan MAVLink channel to send the message * * @param enable 0 to disable, 1 to enable */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan, uint8_t enable) { mavlink_header_t hdr; mavlink_image_trigger_control_t payload; - uint16_t checksum; - mavlink_image_trigger_control_t *p = &payload; - p->enable = enable; // uint8_t:0 to disable, 1 to enable + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN ) + payload.enable = enable; // uint8_t:0 to disable, 1 to enable hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_IMAGE_TRIGGER_CONTROL_LEN; @@ -88,14 +88,12 @@ static inline void mavlink_msg_image_trigger_control_send(mavlink_channel_t chan 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xEE, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h index 250bee3a1b0c6b9d8265cddd0dcd3ae14326bd2f..6fca6a1cf781032e6192a318241eb2413fc088ca 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_image_triggered.h @@ -3,21 +3,23 @@ #define MAVLINK_MSG_ID_IMAGE_TRIGGERED 101 #define MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN 52 #define MAVLINK_MSG_101_LEN 52 +#define MAVLINK_MSG_ID_IMAGE_TRIGGERED_KEY 0x8 +#define MAVLINK_MSG_101_KEY 0x8 typedef struct __mavlink_image_triggered_t { - uint64_t timestamp; ///< Timestamp - uint32_t seq; ///< IMU seq - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad - float local_z; ///< Local frame Z coordinate (height over ground) - float lat; ///< GPS X coordinate - float lon; ///< GPS Y coordinate - float alt; ///< Global frame altitude - float ground_x; ///< Ground truth X - float ground_y; ///< Ground truth Y - float ground_z; ///< Ground truth Z + uint64_t timestamp; ///< Timestamp + uint32_t seq; ///< IMU seq + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad + float local_z; ///< Local frame Z coordinate (height over ground) + float lat; ///< GPS X coordinate + float lon; ///< GPS Y coordinate + float alt; ///< Global frame altitude + float ground_x; ///< Ground truth X + float ground_y; ///< Ground truth Y + float ground_z; ///< Ground truth Z } mavlink_image_triggered_t; @@ -46,18 +48,18 @@ static inline uint16_t mavlink_msg_image_triggered_pack(uint8_t system_id, uint8 mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - p->timestamp = timestamp; // uint64_t:Timestamp - p->seq = seq; // uint32_t:IMU seq - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - p->local_z = local_z; // float:Local frame Z coordinate (height over ground) - p->lat = lat; // float:GPS X coordinate - p->lon = lon; // float:GPS Y coordinate - p->alt = alt; // float:Global frame altitude - p->ground_x = ground_x; // float:Ground truth X - p->ground_y = ground_y; // float:Ground truth Y - p->ground_z = ground_z; // float:Ground truth Z + p->timestamp = timestamp; // uint64_t:Timestamp + p->seq = seq; // uint32_t:IMU seq + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); } @@ -87,18 +89,18 @@ static inline uint16_t mavlink_msg_image_triggered_pack_chan(uint8_t system_id, mavlink_image_triggered_t *p = (mavlink_image_triggered_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_IMAGE_TRIGGERED; - p->timestamp = timestamp; // uint64_t:Timestamp - p->seq = seq; // uint32_t:IMU seq - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - p->local_z = local_z; // float:Local frame Z coordinate (height over ground) - p->lat = lat; // float:GPS X coordinate - p->lon = lon; // float:GPS Y coordinate - p->alt = alt; // float:Global frame altitude - p->ground_x = ground_x; // float:Ground truth X - p->ground_y = ground_y; // float:Ground truth Y - p->ground_z = ground_z; // float:Ground truth Z + p->timestamp = timestamp; // uint64_t:Timestamp + p->seq = seq; // uint32_t:IMU seq + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad + p->local_z = local_z; // float:Local frame Z coordinate (height over ground) + p->lat = lat; // float:GPS X coordinate + p->lon = lon; // float:GPS Y coordinate + p->alt = alt; // float:Global frame altitude + p->ground_x = ground_x; // float:Ground truth X + p->ground_y = ground_y; // float:Ground truth Y + p->ground_z = ground_z; // float:Ground truth Z return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN); } @@ -116,6 +118,8 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin return mavlink_msg_image_triggered_pack(system_id, component_id, msg, image_triggered->timestamp, image_triggered->seq, image_triggered->roll, image_triggered->pitch, image_triggered->yaw, image_triggered->local_z, image_triggered->lat, image_triggered->lon, image_triggered->alt, image_triggered->ground_x, image_triggered->ground_y, image_triggered->ground_z); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a image_triggered message * @param chan MAVLink channel to send the message @@ -133,28 +137,24 @@ static inline uint16_t mavlink_msg_image_triggered_encode(uint8_t system_id, uin * @param ground_y Ground truth Y * @param ground_z Ground truth Z */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint64_t timestamp, uint32_t seq, float roll, float pitch, float yaw, float local_z, float lat, float lon, float alt, float ground_x, float ground_y, float ground_z) { mavlink_header_t hdr; mavlink_image_triggered_t payload; - uint16_t checksum; - mavlink_image_triggered_t *p = &payload; - p->timestamp = timestamp; // uint64_t:Timestamp - p->seq = seq; // uint32_t:IMU seq - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad - p->local_z = local_z; // float:Local frame Z coordinate (height over ground) - p->lat = lat; // float:GPS X coordinate - p->lon = lon; // float:GPS Y coordinate - p->alt = alt; // float:Global frame altitude - p->ground_x = ground_x; // float:Ground truth X - p->ground_y = ground_y; // float:Ground truth Y - p->ground_z = ground_z; // float:Ground truth Z + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN ) + payload.timestamp = timestamp; // uint64_t:Timestamp + payload.seq = seq; // uint32_t:IMU seq + payload.roll = roll; // float:Roll angle in rad + payload.pitch = pitch; // float:Pitch angle in rad + payload.yaw = yaw; // float:Yaw angle in rad + payload.local_z = local_z; // float:Local frame Z coordinate (height over ground) + payload.lat = lat; // float:GPS X coordinate + payload.lon = lon; // float:GPS Y coordinate + payload.alt = alt; // float:Global frame altitude + payload.ground_x = ground_x; // float:Ground truth X + payload.ground_y = ground_y; // float:Ground truth Y + payload.ground_z = ground_z; // float:Ground truth Z hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_IMAGE_TRIGGERED_LEN; @@ -165,14 +165,12 @@ static inline void mavlink_msg_image_triggered_send(mavlink_channel_t chan, uint 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x8, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h index 34ebf635172a102d27739c4ed7cc77863c094f99..a5074c706d3f663062eee5e8a52c6b272842f38e 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_marker.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_MARKER 130 #define MAVLINK_MSG_ID_MARKER_LEN 26 #define MAVLINK_MSG_130_LEN 26 +#define MAVLINK_MSG_ID_MARKER_KEY 0xDD +#define MAVLINK_MSG_130_KEY 0xDD typedef struct __mavlink_marker_t { - float x; ///< x position - float y; ///< y position - float z; ///< z position - float roll; ///< roll orientation - float pitch; ///< pitch orientation - float yaw; ///< yaw orientation - uint16_t id; ///< ID + float x; ///< x position + float y; ///< y position + float z; ///< z position + float roll; ///< roll orientation + float pitch; ///< pitch orientation + float yaw; ///< yaw orientation + uint16_t id; ///< ID } mavlink_marker_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_marker_pack(uint8_t system_id, uint8_t compon mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MARKER; - p->id = id; // uint16_t:ID - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->roll = roll; // float:roll orientation - p->pitch = pitch; // float:pitch orientation - p->yaw = yaw; // float:yaw orientation + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MARKER_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_marker_pack_chan(uint8_t system_id, uint8_t c mavlink_marker_t *p = (mavlink_marker_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MARKER; - p->id = id; // uint16_t:ID - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->roll = roll; // float:roll orientation - p->pitch = pitch; // float:pitch orientation - p->yaw = yaw; // float:yaw orientation + p->id = id; // uint16_t:ID + p->x = x; // float:x position + p->y = y; // float:y position + p->z = z; // float:z position + p->roll = roll; // float:roll orientation + p->pitch = pitch; // float:pitch orientation + p->yaw = yaw; // float:yaw orientation return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MARKER_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp return mavlink_msg_marker_pack(system_id, component_id, msg, marker->id, marker->x, marker->y, marker->z, marker->roll, marker->pitch, marker->yaw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a marker message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_marker_encode(uint8_t system_id, uint8_t comp * @param pitch pitch orientation * @param yaw yaw orientation */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float roll, float pitch, float yaw) { mavlink_header_t hdr; mavlink_marker_t payload; - uint16_t checksum; - mavlink_marker_t *p = &payload; - p->id = id; // uint16_t:ID - p->x = x; // float:x position - p->y = y; // float:y position - p->z = z; // float:z position - p->roll = roll; // float:roll orientation - p->pitch = pitch; // float:pitch orientation - p->yaw = yaw; // float:yaw orientation + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_MARKER_LEN ) + payload.id = id; // uint16_t:ID + payload.x = x; // float:x position + payload.y = y; // float:y position + payload.z = z; // float:z position + payload.roll = roll; // float:roll orientation + payload.pitch = pitch; // float:pitch orientation + payload.yaw = yaw; // float:yaw orientation hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_MARKER_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_marker_send(mavlink_channel_t chan, uint16_t id, 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xDD, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h index d5d803bd0cfe66c3491a845fb947ac664e6e3d3f..2ea0fc79c014556f68ea9bf48d9b0ec552cca7c3 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_pattern_detected.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_PATTERN_DETECTED 160 #define MAVLINK_MSG_ID_PATTERN_DETECTED_LEN 106 #define MAVLINK_MSG_160_LEN 106 +#define MAVLINK_MSG_ID_PATTERN_DETECTED_KEY 0x34 +#define MAVLINK_MSG_160_KEY 0x34 typedef struct __mavlink_pattern_detected_t { - float confidence; ///< Confidence of detection - uint8_t type; ///< 0: Pattern, 1: Letter - char file[100]; ///< Pattern file name - uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes + float confidence; ///< Confidence of detection + uint8_t type; ///< 0: Pattern, 1: Letter + char file[100]; ///< Pattern file name + uint8_t detected; ///< Accepted as true detection, 0 no, 1 yes } mavlink_pattern_detected_t; #define MAVLINK_MSG_PATTERN_DETECTED_FIELD_FILE_LEN 100 @@ -31,10 +33,10 @@ static inline uint16_t mavlink_msg_pattern_detected_pack(uint8_t system_id, uint mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - p->type = type; // uint8_t:0: Pattern, 1: Letter - p->confidence = confidence; // float:Confidence of detection - memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name - p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); } @@ -56,10 +58,10 @@ static inline uint16_t mavlink_msg_pattern_detected_pack_chan(uint8_t system_id, mavlink_pattern_detected_t *p = (mavlink_pattern_detected_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_PATTERN_DETECTED; - p->type = type; // uint8_t:0: Pattern, 1: Letter - p->confidence = confidence; // float:Confidence of detection - memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name - p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes + p->type = type; // uint8_t:0: Pattern, 1: Letter + p->confidence = confidence; // float:Confidence of detection + memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name + p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN); } @@ -77,6 +79,8 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui return mavlink_msg_pattern_detected_pack(system_id, component_id, msg, pattern_detected->type, pattern_detected->confidence, pattern_detected->file, pattern_detected->detected); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a pattern_detected message * @param chan MAVLink channel to send the message @@ -86,20 +90,16 @@ static inline uint16_t mavlink_msg_pattern_detected_encode(uint8_t system_id, ui * @param file Pattern file name * @param detected Accepted as true detection, 0 no, 1 yes */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uint8_t type, float confidence, const char* file, uint8_t detected) { mavlink_header_t hdr; mavlink_pattern_detected_t payload; - uint16_t checksum; - mavlink_pattern_detected_t *p = &payload; - p->type = type; // uint8_t:0: Pattern, 1: Letter - p->confidence = confidence; // float:Confidence of detection - memcpy(p->file, file, sizeof(p->file)); // char[100]:Pattern file name - p->detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_PATTERN_DETECTED_LEN ) + payload.type = type; // uint8_t:0: Pattern, 1: Letter + payload.confidence = confidence; // float:Confidence of detection + memcpy(payload.file, file, sizeof(payload.file)); // char[100]:Pattern file name + payload.detected = detected; // uint8_t:Accepted as true detection, 0 no, 1 yes hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_PATTERN_DETECTED_LEN; @@ -110,14 +110,12 @@ static inline void mavlink_msg_pattern_detected_send(mavlink_channel_t chan, uin 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x34, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h index 148d8f784e7e636b7390b65a41e21e1ff0bb7daf..f3ae2efca458426a5778a544abd23cbcb6e8126f 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest.h @@ -3,17 +3,19 @@ #define MAVLINK_MSG_ID_POINT_OF_INTEREST 161 #define MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN 43 #define MAVLINK_MSG_161_LEN 43 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_KEY 0xA3 +#define MAVLINK_MSG_161_KEY 0xA3 typedef struct __mavlink_point_of_interest_t { - float x; ///< X Position - float y; ///< Y Position - float z; ///< Z Position - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - char name[26]; ///< POI name + float x; ///< X Position + float y; ///< Y Position + float z; ///< Z Position + uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds + uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + uint8_t coordinate_system; ///< 0: global, 1:local + char name[26]; ///< POI name } mavlink_point_of_interest_t; #define MAVLINK_MSG_POINT_OF_INTEREST_FIELD_NAME_LEN 26 @@ -39,14 +41,14 @@ static inline uint16_t mavlink_msg_point_of_interest_pack(uint8_t system_id, uin mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - p->x = x; // float:X Position - p->y = y; // float:Y Position - p->z = z; // float:Z Position - memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } @@ -72,14 +74,14 @@ static inline uint16_t mavlink_msg_point_of_interest_pack_chan(uint8_t system_id mavlink_point_of_interest_t *p = (mavlink_point_of_interest_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST; - p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - p->x = x; // float:X Position - p->y = y; // float:Y Position - p->z = z; // float:Z Position - memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->x = x; // float:X Position + p->y = y; // float:Y Position + p->z = z; // float:Z Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN); } @@ -97,6 +99,8 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u return mavlink_msg_point_of_interest_pack(system_id, component_id, msg, point_of_interest->type, point_of_interest->color, point_of_interest->coordinate_system, point_of_interest->timeout, point_of_interest->x, point_of_interest->y, point_of_interest->z, point_of_interest->name); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a point_of_interest message * @param chan MAVLink channel to send the message @@ -110,24 +114,20 @@ static inline uint16_t mavlink_msg_point_of_interest_encode(uint8_t system_id, u * @param z Z Position * @param name POI name */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float x, float y, float z, const char* name) { mavlink_header_t hdr; mavlink_point_of_interest_t payload; - uint16_t checksum; - mavlink_point_of_interest_t *p = &payload; - - p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - p->x = x; // float:X Position - p->y = y; // float:Y Position - p->z = z; // float:Z Position - memcpy(p->name, name, sizeof(p->name)); // char[26]:POI name + + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN ) + payload.type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + payload.color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + payload.coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + payload.timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + payload.x = x; // float:X Position + payload.y = y; // float:Y Position + payload.z = z; // float:Z Position + memcpy(payload.name, name, sizeof(payload.name)); // char[26]:POI name hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_LEN; @@ -138,14 +138,12 @@ static inline void mavlink_msg_point_of_interest_send(mavlink_channel_t chan, ui 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xA3, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h index 7accdbc9c33f2fbd6b00215aea3a2a1f77cead57..75f26249f8212782755c8bd6768d5a12c2213f5e 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_point_of_interest_connection.h @@ -3,20 +3,22 @@ #define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION 162 #define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN 55 #define MAVLINK_MSG_162_LEN 55 +#define MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_KEY 0x10 +#define MAVLINK_MSG_162_KEY 0x10 typedef struct __mavlink_point_of_interest_connection_t { - float xp1; ///< X1 Position - float yp1; ///< Y1 Position - float zp1; ///< Z1 Position - float xp2; ///< X2 Position - float yp2; ///< Y2 Position - float zp2; ///< Z2 Position - uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds - uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - uint8_t coordinate_system; ///< 0: global, 1:local - char name[26]; ///< POI connection name + float xp1; ///< X1 Position + float yp1; ///< Y1 Position + float zp1; ///< Z1 Position + float xp2; ///< X2 Position + float yp2; ///< Y2 Position + float zp2; ///< Z2 Position + uint16_t timeout; ///< 0: no timeout, >1: timeout in seconds + uint8_t type; ///< 0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + uint8_t color; ///< 0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + uint8_t coordinate_system; ///< 0: global, 1:local + char name[26]; ///< POI connection name } mavlink_point_of_interest_connection_t; #define MAVLINK_MSG_POINT_OF_INTEREST_CONNECTION_FIELD_NAME_LEN 26 @@ -45,17 +47,17 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack(uint8_t sys mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - p->xp1 = xp1; // float:X1 Position - p->yp1 = yp1; // float:Y1 Position - p->zp1 = zp1; // float:Z1 Position - p->xp2 = xp2; // float:X2 Position - p->yp2 = yp2; // float:Y2 Position - p->zp2 = zp2; // float:Z2 Position - memcpy(p->name, name, sizeof(p->name)); // char[26]:POI connection name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->xp1 = xp1; // float:X1 Position + p->yp1 = yp1; // float:Y1 Position + p->zp1 = zp1; // float:Z1 Position + p->xp2 = xp2; // float:X2 Position + p->yp2 = yp2; // float:Y2 Position + p->zp2 = zp2; // float:Z2 Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI connection name return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } @@ -84,17 +86,17 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_pack_chan(uint8_ mavlink_point_of_interest_connection_t *p = (mavlink_point_of_interest_connection_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION; - p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - p->xp1 = xp1; // float:X1 Position - p->yp1 = yp1; // float:Y1 Position - p->zp1 = zp1; // float:Z1 Position - p->xp2 = xp2; // float:X2 Position - p->yp2 = yp2; // float:Y2 Position - p->zp2 = zp2; // float:Z2 Position - memcpy(p->name, name, sizeof(p->name)); // char[26]:POI connection name + p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + p->xp1 = xp1; // float:X1 Position + p->yp1 = yp1; // float:Y1 Position + p->zp1 = zp1; // float:Z1 Position + p->xp2 = xp2; // float:X2 Position + p->yp2 = yp2; // float:Y2 Position + p->zp2 = zp2; // float:Z2 Position + memcpy(p->name, name, sizeof(p->name)); // char[26]:POI connection name return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN); } @@ -112,6 +114,8 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s return mavlink_msg_point_of_interest_connection_pack(system_id, component_id, msg, point_of_interest_connection->type, point_of_interest_connection->color, point_of_interest_connection->coordinate_system, point_of_interest_connection->timeout, point_of_interest_connection->xp1, point_of_interest_connection->yp1, point_of_interest_connection->zp1, point_of_interest_connection->xp2, point_of_interest_connection->yp2, point_of_interest_connection->zp2, point_of_interest_connection->name); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a point_of_interest_connection message * @param chan MAVLink channel to send the message @@ -128,27 +132,23 @@ static inline uint16_t mavlink_msg_point_of_interest_connection_encode(uint8_t s * @param zp2 Z2 Position * @param name POI connection name */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel_t chan, uint8_t type, uint8_t color, uint8_t coordinate_system, uint16_t timeout, float xp1, float yp1, float zp1, float xp2, float yp2, float zp2, const char* name) { mavlink_header_t hdr; mavlink_point_of_interest_connection_t payload; - uint16_t checksum; - mavlink_point_of_interest_connection_t *p = &payload; - p->type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug - p->color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta - p->coordinate_system = coordinate_system; // uint8_t:0: global, 1:local - p->timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds - p->xp1 = xp1; // float:X1 Position - p->yp1 = yp1; // float:Y1 Position - p->zp1 = zp1; // float:Z1 Position - p->xp2 = xp2; // float:X2 Position - p->yp2 = yp2; // float:Y2 Position - p->zp2 = zp2; // float:Z2 Position - memcpy(p->name, name, sizeof(p->name)); // char[26]:POI connection name + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN ) + payload.type = type; // uint8_t:0: Notice, 1: Warning, 2: Critical, 3: Emergency, 4: Debug + payload.color = color; // uint8_t:0: blue, 1: yellow, 2: red, 3: orange, 4: green, 5: magenta + payload.coordinate_system = coordinate_system; // uint8_t:0: global, 1:local + payload.timeout = timeout; // uint16_t:0: no timeout, >1: timeout in seconds + payload.xp1 = xp1; // float:X1 Position + payload.yp1 = yp1; // float:Y1 Position + payload.zp1 = zp1; // float:Z1 Position + payload.xp2 = xp2; // float:X2 Position + payload.yp2 = yp2; // float:Y2 Position + payload.zp2 = zp2; // float:Z2 Position + memcpy(payload.name, name, sizeof(payload.name)); // char[26]:POI connection name hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POINT_OF_INTEREST_CONNECTION_LEN; @@ -159,14 +159,12 @@ static inline void mavlink_msg_point_of_interest_connection_send(mavlink_channel 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x10, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h index b5446931f4dce3cda1d518540bb9f46c0e569dfb..3ae1a4107da01c29ec05adf169afcaa52f705e83 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_offset_set.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET 154 #define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN 18 #define MAVLINK_MSG_154_LEN 18 +#define MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_KEY 0xA +#define MAVLINK_MSG_154_KEY 0xA typedef struct __mavlink_position_control_offset_set_t { - float x; ///< x position offset - float y; ///< y position offset - float z; ///< z position offset - float yaw; ///< yaw orientation offset in radians, 0 = NORTH - uint8_t target_system; ///< System ID - uint8_t target_component; ///< Component ID + float x; ///< x position offset + float y; ///< y position offset + float z; ///< z position offset + float yaw; ///< yaw orientation offset in radians, 0 = NORTH + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_position_control_offset_set_t; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_position_control_offset_set_pack(uint8_t syst mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->x = x; // float:x position offset - p->y = y; // float:y position offset - p->z = z; // float:z position offset - p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_position_control_offset_set_pack_chan(uint8_t mavlink_position_control_offset_set_t *p = (mavlink_position_control_offset_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->x = x; // float:x position offset - p->y = y; // float:y position offset - p->z = z; // float:z position offset - p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->x = x; // float:x position offset + p->y = y; // float:y position offset + p->z = z; // float:z position offset + p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN); } @@ -86,6 +88,8 @@ static inline uint16_t mavlink_msg_position_control_offset_set_encode(uint8_t sy return mavlink_msg_position_control_offset_set_pack(system_id, component_id, msg, position_control_offset_set->target_system, position_control_offset_set->target_component, position_control_offset_set->x, position_control_offset_set->y, position_control_offset_set->z, position_control_offset_set->yaw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a position_control_offset_set message * @param chan MAVLink channel to send the message @@ -97,22 +101,18 @@ static inline uint16_t mavlink_msg_position_control_offset_set_encode(uint8_t sy * @param z z position offset * @param yaw yaw orientation offset in radians, 0 = NORTH */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_control_offset_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; mavlink_position_control_offset_set_t payload; - uint16_t checksum; - mavlink_position_control_offset_set_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->x = x; // float:x position offset - p->y = y; // float:y position offset - p->z = z; // float:z position offset - p->yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.x = x; // float:x position offset + payload.y = y; // float:y position offset + payload.z = z; // float:z position offset + payload.yaw = yaw; // float:yaw orientation offset in radians, 0 = NORTH hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_OFFSET_SET_LEN; @@ -123,14 +123,12 @@ static inline void mavlink_msg_position_control_offset_set_send(mavlink_channel_ 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xA, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h index dd3b92ca8cb78540540608cd20870cb884ba604b..a40eea0c9f5afc7a1a3e3f797927702045d880a9 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint.h @@ -3,14 +3,16 @@ #define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT 121 #define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN 18 #define MAVLINK_MSG_121_LEN 18 +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_KEY 0x72 +#define MAVLINK_MSG_121_KEY 0x72 typedef struct __mavlink_position_control_setpoint_t { - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< yaw orientation in radians, 0 = NORTH - uint16_t id; ///< ID of waypoint, 0 for plain position + float x; ///< x position + float y; ///< y position + float z; ///< z position + float yaw; ///< yaw orientation in radians, 0 = NORTH + uint16_t id; ///< ID of waypoint, 0 for plain position } mavlink_position_control_setpoint_t; @@ -32,11 +34,11 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack(uint8_t system mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - p->id = id; // uint16_t:ID of waypoint, 0 for plain position - 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 + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + 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 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); } @@ -59,11 +61,11 @@ static inline uint16_t mavlink_msg_position_control_setpoint_pack_chan(uint8_t s mavlink_position_control_setpoint_t *p = (mavlink_position_control_setpoint_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT; - p->id = id; // uint16_t:ID of waypoint, 0 for plain position - 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 + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + 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 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN); } @@ -81,6 +83,8 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst return mavlink_msg_position_control_setpoint_pack(system_id, component_id, msg, position_control_setpoint->id, position_control_setpoint->x, position_control_setpoint->y, position_control_setpoint->z, position_control_setpoint->yaw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a position_control_setpoint message * @param chan MAVLink channel to send the message @@ -91,21 +95,17 @@ static inline uint16_t mavlink_msg_position_control_setpoint_encode(uint8_t syst * @param z z position * @param yaw yaw orientation in radians, 0 = NORTH */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t chan, uint16_t id, float x, float y, float z, float yaw) { mavlink_header_t hdr; mavlink_position_control_setpoint_t payload; - uint16_t checksum; - mavlink_position_control_setpoint_t *p = &payload; - p->id = id; // uint16_t:ID of waypoint, 0 for plain position - 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 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN ) + payload.id = id; // uint16_t:ID of waypoint, 0 for plain position + payload.x = x; // float:x position + payload.y = y; // float:y position + payload.z = z; // float:z position + payload.yaw = yaw; // float:yaw orientation in radians, 0 = NORTH hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_LEN; @@ -116,14 +116,12 @@ static inline void mavlink_msg_position_control_setpoint_send(mavlink_channel_t 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x72, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h index f14a31c467889ecc03b85d71f9478532c363b8bd..e22c10bf6d5d71f8c85b75fbc89b374ac9a9ec1b 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_position_control_setpoint_set.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET 120 #define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN 20 #define MAVLINK_MSG_120_LEN 20 +#define MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_KEY 0xE1 +#define MAVLINK_MSG_120_KEY 0xE1 typedef struct __mavlink_position_control_setpoint_set_t { - float x; ///< x position - float y; ///< y position - float z; ///< z position - float yaw; ///< yaw orientation in radians, 0 = NORTH - uint16_t id; ///< ID of waypoint, 0 for plain position - 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; ///< yaw orientation in radians, 0 = NORTH + uint16_t id; ///< ID of waypoint, 0 for plain position + uint8_t target_system; ///< System ID + uint8_t target_component; ///< Component ID } mavlink_position_control_setpoint_set_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack(uint8_t sy mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->id = id; // uint16_t:ID of waypoint, 0 for plain position - 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + 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 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_pack_chan(uint8 mavlink_position_control_setpoint_set_t *p = (mavlink_position_control_setpoint_set_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->id = id; // uint16_t:ID of waypoint, 0 for plain position - 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 + p->target_system = target_system; // uint8_t:System ID + p->target_component = target_component; // uint8_t:Component ID + p->id = id; // uint16_t:ID of waypoint, 0 for plain position + 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 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_encode(uint8_t return mavlink_msg_position_control_setpoint_set_pack(system_id, component_id, msg, position_control_setpoint_set->target_system, position_control_setpoint_set->target_component, position_control_setpoint_set->id, position_control_setpoint_set->x, position_control_setpoint_set->y, position_control_setpoint_set->z, position_control_setpoint_set->yaw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a position_control_setpoint_set message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_position_control_setpoint_set_encode(uint8_t * @param z z position * @param yaw yaw orientation in radians, 0 = NORTH */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t id, float x, float y, float z, float yaw) { mavlink_header_t hdr; mavlink_position_control_setpoint_set_t payload; - uint16_t checksum; - mavlink_position_control_setpoint_set_t *p = &payload; - p->target_system = target_system; // uint8_t:System ID - p->target_component = target_component; // uint8_t:Component ID - p->id = id; // uint16_t:ID of waypoint, 0 for plain position - 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 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN ) + payload.target_system = target_system; // uint8_t:System ID + payload.target_component = target_component; // uint8_t:Component ID + payload.id = id; // uint16_t:ID of waypoint, 0 for plain position + payload.x = x; // float:x position + payload.y = y; // float:y position + payload.z = z; // float:z position + payload.yaw = yaw; // float:yaw orientation in radians, 0 = NORTH hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_POSITION_CONTROL_SETPOINT_SET_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_position_control_setpoint_set_send(mavlink_channe 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xE1, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h index 7bafeb4c99b9c77b1046d5873bac3815e936afaa..917935346bfd23db67992a5c57ac6453ecc0d513 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_raw_aux.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_RAW_AUX 141 #define MAVLINK_MSG_ID_RAW_AUX_LEN 16 #define MAVLINK_MSG_141_LEN 16 +#define MAVLINK_MSG_ID_RAW_AUX_KEY 0xAB +#define MAVLINK_MSG_141_KEY 0xAB typedef struct __mavlink_raw_aux_t { - int32_t baro; ///< Barometric pressure (hecto Pascal) - uint16_t adc1; ///< ADC1 (J405 ADC3, LPC2148 AD0.6) - uint16_t adc2; ///< ADC2 (J405 ADC5, LPC2148 AD0.2) - uint16_t adc3; ///< ADC3 (J405 ADC6, LPC2148 AD0.1) - uint16_t adc4; ///< ADC4 (J405 ADC7, LPC2148 AD1.3) - uint16_t vbat; ///< Battery voltage - int16_t temp; ///< Temperature (degrees celcius) + int32_t baro; ///< Barometric pressure (hecto Pascal) + uint16_t adc1; ///< ADC1 (J405 ADC3, LPC2148 AD0.6) + uint16_t adc2; ///< ADC2 (J405 ADC5, LPC2148 AD0.2) + uint16_t adc3; ///< ADC3 (J405 ADC6, LPC2148 AD0.1) + uint16_t adc4; ///< ADC4 (J405 ADC7, LPC2148 AD1.3) + uint16_t vbat; ///< Battery voltage + int16_t temp; ///< Temperature (degrees celcius) } mavlink_raw_aux_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_raw_aux_pack(uint8_t system_id, uint8_t compo mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) - p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) - p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) - p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) - p->vbat = vbat; // uint16_t:Battery voltage - p->temp = temp; // int16_t:Temperature (degrees celcius) - p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_AUX_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_raw_aux_pack_chan(uint8_t system_id, uint8_t mavlink_raw_aux_t *p = (mavlink_raw_aux_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RAW_AUX; - p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) - p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) - p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) - p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) - p->vbat = vbat; // uint16_t:Battery voltage - p->temp = temp; // int16_t:Temperature (degrees celcius) - p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) + p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + p->vbat = vbat; // uint16_t:Battery voltage + p->temp = temp; // int16_t:Temperature (degrees celcius) + p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_AUX_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com return mavlink_msg_raw_aux_pack(system_id, component_id, msg, raw_aux->adc1, raw_aux->adc2, raw_aux->adc3, raw_aux->adc4, raw_aux->vbat, raw_aux->temp, raw_aux->baro); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a raw_aux message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_raw_aux_encode(uint8_t system_id, uint8_t com * @param temp Temperature (degrees celcius) * @param baro Barometric pressure (hecto Pascal) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc1, uint16_t adc2, uint16_t adc3, uint16_t adc4, uint16_t vbat, int16_t temp, int32_t baro) { mavlink_header_t hdr; mavlink_raw_aux_t payload; - uint16_t checksum; - mavlink_raw_aux_t *p = &payload; - p->adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) - p->adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) - p->adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) - p->adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) - p->vbat = vbat; // uint16_t:Battery voltage - p->temp = temp; // int16_t:Temperature (degrees celcius) - p->baro = baro; // int32_t:Barometric pressure (hecto Pascal) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RAW_AUX_LEN ) + payload.adc1 = adc1; // uint16_t:ADC1 (J405 ADC3, LPC2148 AD0.6) + payload.adc2 = adc2; // uint16_t:ADC2 (J405 ADC5, LPC2148 AD0.2) + payload.adc3 = adc3; // uint16_t:ADC3 (J405 ADC6, LPC2148 AD0.1) + payload.adc4 = adc4; // uint16_t:ADC4 (J405 ADC7, LPC2148 AD1.3) + payload.vbat = vbat; // uint16_t:Battery voltage + payload.temp = temp; // int16_t:Temperature (degrees celcius) + payload.baro = baro; // int32_t:Barometric pressure (hecto Pascal) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_RAW_AUX_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_raw_aux_send(mavlink_channel_t chan, uint16_t adc 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xAB, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h index 5b14662365bd32633880816326a7533f34a57f51..0cc93ff58c1f3b07dd8d84b617ee35e542d51305 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_set_cam_shutter.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_SET_CAM_SHUTTER 100 #define MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN 11 #define MAVLINK_MSG_100_LEN 11 +#define MAVLINK_MSG_ID_SET_CAM_SHUTTER_KEY 0x24 +#define MAVLINK_MSG_100_KEY 0x24 typedef struct __mavlink_set_cam_shutter_t { - float gain; ///< Camera gain - uint16_t interval; ///< Shutter interval, in microseconds - uint16_t exposure; ///< Exposure time, in microseconds - uint8_t cam_no; ///< Camera id - uint8_t cam_mode; ///< Camera mode: 0 = auto, 1 = manual - uint8_t trigger_pin; ///< Trigger pin, 0-3 for PtGrey FireFly + float gain; ///< Camera gain + uint16_t interval; ///< Shutter interval, in microseconds + uint16_t exposure; ///< Exposure time, in microseconds + uint8_t cam_no; ///< Camera id + uint8_t cam_mode; ///< Camera mode: 0 = auto, 1 = manual + uint8_t trigger_pin; ///< Trigger pin, 0-3 for PtGrey FireFly } mavlink_set_cam_shutter_t; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack(uint8_t system_id, uint8 mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - p->cam_no = cam_no; // uint8_t:Camera id - p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual - p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly - p->interval = interval; // uint16_t:Shutter interval, in microseconds - p->exposure = exposure; // uint16_t:Exposure time, in microseconds - p->gain = gain; // float:Camera gain + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_set_cam_shutter_pack_chan(uint8_t system_id, mavlink_set_cam_shutter_t *p = (mavlink_set_cam_shutter_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SET_CAM_SHUTTER; - p->cam_no = cam_no; // uint8_t:Camera id - p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual - p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly - p->interval = interval; // uint16_t:Shutter interval, in microseconds - p->exposure = exposure; // uint16_t:Exposure time, in microseconds - p->gain = gain; // float:Camera gain + p->cam_no = cam_no; // uint8_t:Camera id + p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + p->interval = interval; // uint16_t:Shutter interval, in microseconds + p->exposure = exposure; // uint16_t:Exposure time, in microseconds + p->gain = gain; // float:Camera gain return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN); } @@ -86,6 +88,8 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin return mavlink_msg_set_cam_shutter_pack(system_id, component_id, msg, set_cam_shutter->cam_no, set_cam_shutter->cam_mode, set_cam_shutter->trigger_pin, set_cam_shutter->interval, set_cam_shutter->exposure, set_cam_shutter->gain); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a set_cam_shutter message * @param chan MAVLink channel to send the message @@ -97,22 +101,18 @@ static inline uint16_t mavlink_msg_set_cam_shutter_encode(uint8_t system_id, uin * @param exposure Exposure time, in microseconds * @param gain Camera gain */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint8_t cam_no, uint8_t cam_mode, uint8_t trigger_pin, uint16_t interval, uint16_t exposure, float gain) { mavlink_header_t hdr; mavlink_set_cam_shutter_t payload; - uint16_t checksum; - mavlink_set_cam_shutter_t *p = &payload; - p->cam_no = cam_no; // uint8_t:Camera id - p->cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual - p->trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly - p->interval = interval; // uint16_t:Shutter interval, in microseconds - p->exposure = exposure; // uint16_t:Exposure time, in microseconds - p->gain = gain; // float:Camera gain + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN ) + payload.cam_no = cam_no; // uint8_t:Camera id + payload.cam_mode = cam_mode; // uint8_t:Camera mode: 0 = auto, 1 = manual + payload.trigger_pin = trigger_pin; // uint8_t:Trigger pin, 0-3 for PtGrey FireFly + payload.interval = interval; // uint16_t:Shutter interval, in microseconds + payload.exposure = exposure; // uint16_t:Exposure time, in microseconds + payload.gain = gain; // float:Camera gain hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SET_CAM_SHUTTER_LEN; @@ -123,14 +123,12 @@ static inline void mavlink_msg_set_cam_shutter_send(mavlink_channel_t chan, uint 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x24, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h index 862ea8cbef57172cdebd8464d79474fd2ae50d51..462e3801fb1d1fa6a552e5ec66963aa1aac274bc 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vicon_position_estimate.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE 112 #define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32 #define MAVLINK_MSG_112_LEN 32 +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_KEY 0xDA +#define MAVLINK_MSG_112_KEY 0xDA typedef struct __mavlink_vicon_position_estimate_t { - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad + uint64_t usec; ///< Timestamp (milliseconds) + float x; ///< Global X position + float y; ///< Global Y position + float z; ///< Global Z position + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad } mavlink_vicon_position_estimate_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X position - p->y = y; // float:Global Y position - p->z = z; // float:Global Z position - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys mavlink_vicon_position_estimate_t *p = (mavlink_vicon_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X position - p->y = y; // float:Global Y position - p->z = z; // float:Global Z position - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a vicon_position_estimate message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system * @param pitch Pitch angle in rad * @param yaw Yaw angle in rad */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { mavlink_header_t hdr; mavlink_vicon_position_estimate_t payload; - uint16_t checksum; - mavlink_vicon_position_estimate_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X position - p->y = y; // float:Global Y position - p->z = z; // float:Global Z position - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN ) + payload.usec = usec; // uint64_t:Timestamp (milliseconds) + payload.x = x; // float:Global X position + payload.y = y; // float:Global Y position + payload.z = z; // float:Global Z position + payload.roll = roll; // float:Roll angle in rad + payload.pitch = pitch; // float:Pitch angle in rad + payload.yaw = yaw; // float:Yaw angle in rad hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xDA, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h index cfd0b15f7cad035f2d4044702d0c067727064184..1bd2ca2506d1f70390957413332d4cafbff0fb0e 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_position_estimate.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE 111 #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 #define MAVLINK_MSG_111_LEN 32 +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_KEY 0xDA +#define MAVLINK_MSG_111_KEY 0xDA typedef struct __mavlink_vision_position_estimate_t { - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X position - float y; ///< Global Y position - float z; ///< Global Z position - float roll; ///< Roll angle in rad - float pitch; ///< Pitch angle in rad - float yaw; ///< Yaw angle in rad + uint64_t usec; ///< Timestamp (milliseconds) + float x; ///< Global X position + float y; ///< Global Y position + float z; ///< Global Z position + float roll; ///< Roll angle in rad + float pitch; ///< Pitch angle in rad + float yaw; ///< Yaw angle in rad } mavlink_vision_position_estimate_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X position - p->y = y; // float:Global Y position - p->z = z; // float:Global Z position - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy mavlink_vision_position_estimate_t *p = (mavlink_vision_position_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X position - p->y = y; // float:Global Y position - p->z = z; // float:Global Z position - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X position + p->y = y; // float:Global Y position + p->z = z; // float:Global Z position + p->roll = roll; // float:Roll angle in rad + p->pitch = pitch; // float:Pitch angle in rad + p->yaw = yaw; // float:Yaw angle in rad return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a vision_position_estimate message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste * @param pitch Pitch angle in rad * @param yaw Yaw angle in rad */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) { mavlink_header_t hdr; mavlink_vision_position_estimate_t payload; - uint16_t checksum; - mavlink_vision_position_estimate_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X position - p->y = y; // float:Global Y position - p->z = z; // float:Global Z position - p->roll = roll; // float:Roll angle in rad - p->pitch = pitch; // float:Pitch angle in rad - p->yaw = yaw; // float:Yaw angle in rad + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN ) + payload.usec = usec; // uint64_t:Timestamp (milliseconds) + payload.x = x; // float:Global X position + payload.y = y; // float:Global Y position + payload.z = z; // float:Global Z position + payload.roll = roll; // float:Roll angle in rad + payload.pitch = pitch; // float:Pitch angle in rad + payload.yaw = yaw; // float:Yaw angle in rad hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xDA, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h index e3d0755de3d699b39c5c392bcb63b3cbebdf8239..d9bfe7b6349b710e4e8c472ae535d536f6a836d5 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_vision_speed_estimate.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE 113 #define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20 #define MAVLINK_MSG_113_LEN 20 +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_KEY 0xEB +#define MAVLINK_MSG_113_KEY 0xEB typedef struct __mavlink_vision_speed_estimate_t { - uint64_t usec; ///< Timestamp (milliseconds) - float x; ///< Global X speed - float y; ///< Global Y speed - float z; ///< Global Z speed + uint64_t usec; ///< Timestamp (milliseconds) + float x; ///< Global X speed + float y; ///< Global Y speed + float z; ///< Global Z speed } mavlink_vision_speed_estimate_t; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X speed - p->y = y; // float:Global Y speed - p->z = z; // float:Global Z speed + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste mavlink_vision_speed_estimate_t *p = (mavlink_vision_speed_estimate_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X speed - p->y = y; // float:Global Y speed - p->z = z; // float:Global Z speed + p->usec = usec; // uint64_t:Timestamp (milliseconds) + p->x = x; // float:Global X speed + p->y = y; // float:Global Y speed + p->z = z; // float:Global Z speed return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); } @@ -76,6 +78,8 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a vision_speed_estimate message * @param chan MAVLink channel to send the message @@ -85,20 +89,16 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i * @param y Global Y speed * @param z Global Z speed */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) { mavlink_header_t hdr; mavlink_vision_speed_estimate_t payload; - uint16_t checksum; - mavlink_vision_speed_estimate_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (milliseconds) - p->x = x; // float:Global X speed - p->y = y; // float:Global Y speed - p->z = z; // float:Global Z speed + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN ) + payload.usec = usec; // uint64_t:Timestamp (milliseconds) + payload.x = x; // float:Global X speed + payload.y = y; // float:Global Y speed + payload.z = z; // float:Global Z speed hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN; @@ -109,14 +109,12 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xEB, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h index dee5bcf0aaf99d366d81d9a21eac37a53919daf2..8acb62e2da071b3ac19c38d4a17182bb46d362c7 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_command.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_WATCHDOG_COMMAND 153 #define MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN 6 #define MAVLINK_MSG_153_LEN 6 +#define MAVLINK_MSG_ID_WATCHDOG_COMMAND_KEY 0xA9 +#define MAVLINK_MSG_153_KEY 0xA9 typedef struct __mavlink_watchdog_command_t { - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - uint8_t target_system_id; ///< Target system ID - uint8_t command_id; ///< Command ID + uint16_t watchdog_id; ///< Watchdog ID + uint16_t process_id; ///< Process ID + uint8_t target_system_id; ///< Target system ID + uint8_t command_id; ///< Command ID } mavlink_watchdog_command_t; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_watchdog_command_pack(uint8_t system_id, uint mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - p->target_system_id = target_system_id; // uint8_t:Target system ID - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - p->command_id = command_id; // uint8_t:Command ID + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_watchdog_command_pack_chan(uint8_t system_id, mavlink_watchdog_command_t *p = (mavlink_watchdog_command_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_COMMAND; - p->target_system_id = target_system_id; // uint8_t:Target system ID - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - p->command_id = command_id; // uint8_t:Command ID + p->target_system_id = target_system_id; // uint8_t:Target system ID + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->command_id = command_id; // uint8_t:Command ID return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN); } @@ -76,6 +78,8 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui return mavlink_msg_watchdog_command_pack(system_id, component_id, msg, watchdog_command->target_system_id, watchdog_command->watchdog_id, watchdog_command->process_id, watchdog_command->command_id); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a watchdog_command message * @param chan MAVLink channel to send the message @@ -85,20 +89,16 @@ static inline uint16_t mavlink_msg_watchdog_command_encode(uint8_t system_id, ui * @param process_id Process ID * @param command_id Command ID */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uint8_t target_system_id, uint16_t watchdog_id, uint16_t process_id, uint8_t command_id) { mavlink_header_t hdr; mavlink_watchdog_command_t payload; - uint16_t checksum; - mavlink_watchdog_command_t *p = &payload; - p->target_system_id = target_system_id; // uint8_t:Target system ID - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - p->command_id = command_id; // uint8_t:Command ID + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN ) + payload.target_system_id = target_system_id; // uint8_t:Target system ID + payload.watchdog_id = watchdog_id; // uint16_t:Watchdog ID + payload.process_id = process_id; // uint16_t:Process ID + payload.command_id = command_id; // uint8_t:Command ID hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WATCHDOG_COMMAND_LEN; @@ -109,14 +109,12 @@ static inline void mavlink_msg_watchdog_command_send(mavlink_channel_t chan, uin 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xA9, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h index e31a060eadb009b3fd7db5838e20cdf140e47821..5e4ac05f53980700b05f83b1d9fd013c5b5e2e42 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_heartbeat.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT 150 #define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN 4 #define MAVLINK_MSG_150_LEN 4 +#define MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_KEY 0x5C +#define MAVLINK_MSG_150_KEY 0x5C typedef struct __mavlink_watchdog_heartbeat_t { - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_count; ///< Number of processes + uint16_t watchdog_id; ///< Watchdog ID + uint16_t process_count; ///< Number of processes } mavlink_watchdog_heartbeat_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack(uint8_t system_id, ui mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_count = process_count; // uint16_t:Number of processes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_pack_chan(uint8_t system_i mavlink_watchdog_heartbeat_t *p = (mavlink_watchdog_heartbeat_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_count = process_count; // uint16_t:Number of processes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_count = process_count; // uint16_t:Number of processes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, return mavlink_msg_watchdog_heartbeat_pack(system_id, component_id, msg, watchdog_heartbeat->watchdog_id, watchdog_heartbeat->process_count); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a watchdog_heartbeat message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_watchdog_heartbeat_encode(uint8_t system_id, * @param watchdog_id Watchdog ID * @param process_count Number of processes */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_count) { mavlink_header_t hdr; mavlink_watchdog_heartbeat_t payload; - uint16_t checksum; - mavlink_watchdog_heartbeat_t *p = &payload; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_count = process_count; // uint16_t:Number of processes + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN ) + payload.watchdog_id = watchdog_id; // uint16_t:Watchdog ID + payload.process_count = process_count; // uint16_t:Number of processes hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WATCHDOG_HEARTBEAT_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_watchdog_heartbeat_send(mavlink_channel_t chan, u 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x5C, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h index e9d7703f9e56e76dd5b5cc25d133609933540149..36b2480080f3298110f56f8b8dad1babc76257a6 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_info.h @@ -3,14 +3,16 @@ #define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO 151 #define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN 255 #define MAVLINK_MSG_151_LEN 255 +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_KEY 0x63 +#define MAVLINK_MSG_151_KEY 0x63 typedef struct __mavlink_watchdog_process_info_t { - int32_t timeout; ///< Timeout (seconds) - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - char name[100]; ///< Process name - char arguments[147]; ///< Process arguments + int32_t timeout; ///< Timeout (seconds) + uint16_t watchdog_id; ///< Watchdog ID + uint16_t process_id; ///< Process ID + char name[100]; ///< Process name + char arguments[147]; ///< Process arguments } mavlink_watchdog_process_info_t; #define MAVLINK_MSG_WATCHDOG_PROCESS_INFO_FIELD_NAME_LEN 100 @@ -34,11 +36,11 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack(uint8_t system_id, mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments - p->timeout = timeout; // int32_t:Timeout (seconds) + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); } @@ -61,11 +63,11 @@ static inline uint16_t mavlink_msg_watchdog_process_info_pack_chan(uint8_t syste mavlink_watchdog_process_info_t *p = (mavlink_watchdog_process_info_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments - p->timeout = timeout; // int32_t:Timeout (seconds) + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name + memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments + p->timeout = timeout; // int32_t:Timeout (seconds) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN); } @@ -83,6 +85,8 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i return mavlink_msg_watchdog_process_info_pack(system_id, component_id, msg, watchdog_process_info->watchdog_id, watchdog_process_info->process_id, watchdog_process_info->name, watchdog_process_info->arguments, watchdog_process_info->timeout); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a watchdog_process_info message * @param chan MAVLink channel to send the message @@ -93,21 +97,17 @@ static inline uint16_t mavlink_msg_watchdog_process_info_encode(uint8_t system_i * @param arguments Process arguments * @param timeout Timeout (seconds) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, const char* name, const char* arguments, int32_t timeout) { mavlink_header_t hdr; mavlink_watchdog_process_info_t payload; - uint16_t checksum; - mavlink_watchdog_process_info_t *p = &payload; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - memcpy(p->name, name, sizeof(p->name)); // char[100]:Process name - memcpy(p->arguments, arguments, sizeof(p->arguments)); // char[147]:Process arguments - p->timeout = timeout; // int32_t:Timeout (seconds) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN ) + payload.watchdog_id = watchdog_id; // uint16_t:Watchdog ID + payload.process_id = process_id; // uint16_t:Process ID + memcpy(payload.name, name, sizeof(payload.name)); // char[100]:Process name + memcpy(payload.arguments, arguments, sizeof(payload.arguments)); // char[147]:Process arguments + payload.timeout = timeout; // int32_t:Timeout (seconds) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_INFO_LEN; @@ -118,14 +118,12 @@ static inline void mavlink_msg_watchdog_process_info_send(mavlink_channel_t chan 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x63, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h index f6f3dd26d77402bd6819047873106f4d39654703..7765a92406810d84d05c18cc70cbed71ea3dced3 100644 --- a/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h +++ b/thirdParty/mavlink/include/pixhawk/mavlink_msg_watchdog_process_status.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS 152 #define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN 12 #define MAVLINK_MSG_152_LEN 12 +#define MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_KEY 0x4 +#define MAVLINK_MSG_152_KEY 0x4 typedef struct __mavlink_watchdog_process_status_t { - int32_t pid; ///< PID - uint16_t watchdog_id; ///< Watchdog ID - uint16_t process_id; ///< Process ID - uint16_t crashes; ///< Number of crashes - uint8_t state; ///< Is running / finished / suspended / crashed - uint8_t muted; ///< Is muted + int32_t pid; ///< PID + uint16_t watchdog_id; ///< Watchdog ID + uint16_t process_id; ///< Process ID + uint16_t crashes; ///< Number of crashes + uint8_t state; ///< Is running / finished / suspended / crashed + uint8_t muted; ///< Is muted } mavlink_watchdog_process_status_t; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack(uint8_t system_i mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - p->state = state; // uint8_t:Is running / finished / suspended / crashed - p->muted = muted; // uint8_t:Is muted - p->pid = pid; // int32_t:PID - p->crashes = crashes; // uint16_t:Number of crashes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_watchdog_process_status_pack_chan(uint8_t sys mavlink_watchdog_process_status_t *p = (mavlink_watchdog_process_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - p->state = state; // uint8_t:Is running / finished / suspended / crashed - p->muted = muted; // uint8_t:Is muted - p->pid = pid; // int32_t:PID - p->crashes = crashes; // uint16_t:Number of crashes + p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID + p->process_id = process_id; // uint16_t:Process ID + p->state = state; // uint8_t:Is running / finished / suspended / crashed + p->muted = muted; // uint8_t:Is muted + p->pid = pid; // int32_t:PID + p->crashes = crashes; // uint16_t:Number of crashes return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN); } @@ -86,6 +88,8 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system return mavlink_msg_watchdog_process_status_pack(system_id, component_id, msg, watchdog_process_status->watchdog_id, watchdog_process_status->process_id, watchdog_process_status->state, watchdog_process_status->muted, watchdog_process_status->pid, watchdog_process_status->crashes); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a watchdog_process_status message * @param chan MAVLink channel to send the message @@ -97,22 +101,18 @@ static inline uint16_t mavlink_msg_watchdog_process_status_encode(uint8_t system * @param pid PID * @param crashes Number of crashes */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t chan, uint16_t watchdog_id, uint16_t process_id, uint8_t state, uint8_t muted, int32_t pid, uint16_t crashes) { mavlink_header_t hdr; mavlink_watchdog_process_status_t payload; - uint16_t checksum; - mavlink_watchdog_process_status_t *p = &payload; - p->watchdog_id = watchdog_id; // uint16_t:Watchdog ID - p->process_id = process_id; // uint16_t:Process ID - p->state = state; // uint8_t:Is running / finished / suspended / crashed - p->muted = muted; // uint8_t:Is muted - p->pid = pid; // int32_t:PID - p->crashes = crashes; // uint16_t:Number of crashes + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN ) + payload.watchdog_id = watchdog_id; // uint16_t:Watchdog ID + payload.process_id = process_id; // uint16_t:Process ID + payload.state = state; // uint8_t:Is running / finished / suspended / crashed + payload.muted = muted; // uint8_t:Is muted + payload.pid = pid; // int32_t:PID + payload.crashes = crashes; // uint16_t:Number of crashes hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_WATCHDOG_PROCESS_STATUS_LEN; @@ -123,14 +123,12 @@ static inline void mavlink_msg_watchdog_process_status_send(mavlink_channel_t ch 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x4, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/pixhawk/pixhawk.h b/thirdParty/mavlink/include/pixhawk/pixhawk.h index a0bd474b19a7c78ebfcfbad22273a04a4a82b6c7..a94bde4f2834cd3653882b0812266e663d083c17 100644 --- a/thirdParty/mavlink/include/pixhawk/pixhawk.h +++ b/thirdParty/mavlink/include/pixhawk/pixhawk.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:49 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef PIXHAWK_H #define PIXHAWK_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_PIXHAWK @@ -71,7 +71,12 @@ enum DATA_TYPES // MESSAGE CRC KEYS #undef MAVLINK_MESSAGE_KEYS -#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 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, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 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, 144 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 251, 20, 22, 0, 0, 0, 0, 0, 0, 0, 0, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 99, 4, 169, 10, 0, 0, 0, 0, 0, 52, 163, 16, 0, 0, 0, 0, 0, 0, 0, 200, 135, 217, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 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, 255, 73, 239, 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, 138, 43, 141, 211, 144 } + +// MESSAGE LENGTHS + +#undef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 0, 2, 2, 0, 0, 0, 0, 0, 0, 0, 20, 2, 24, 22, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 20, 20, 20, 8, 0, 0, 0, 0, 0, 0, 0, 0, 21, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 11, 52, 1, 92, 0, 0, 0, 0, 0, 0, 0, 32, 32, 20, 0, 0, 0, 0, 0, 0, 20, 18, 0, 0, 0, 0, 0, 0, 0, 0, 26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 16, 12, 0, 0, 0, 0, 0, 0, 0, 4, 255, 12, 6, 18, 0, 0, 0, 0, 0, 106, 43, 55, 0, 0, 0, 0, 0, 0, 0, 8, 255, 53, 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, 36, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/slugs/mavlink.h b/thirdParty/mavlink/include/slugs/mavlink.h index 04bffe2e3147b56e2e04dfcc61d43ce45740025c..cbf9b6ae3b3734af4aa17937b564da63f3064092 100644 --- a/thirdParty/mavlink/include/slugs/mavlink.h +++ b/thirdParty/mavlink/include/slugs/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h index b345be106dfdaaebd76b1780efd65bff6f8646a5..b1858d990a62147b7d4d50ede52c4f95c4dc34b7 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_air_data.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_AIR_DATA 171 #define MAVLINK_MSG_ID_AIR_DATA_LEN 10 #define MAVLINK_MSG_171_LEN 10 +#define MAVLINK_MSG_ID_AIR_DATA_KEY 0x90 +#define MAVLINK_MSG_171_KEY 0x90 typedef struct __mavlink_air_data_t { - float dynamicPressure; ///< Dynamic pressure (Pa) - float staticPressure; ///< Static pressure (Pa) - uint16_t temperature; ///< Board temperature + float dynamicPressure; ///< Dynamic pressure (Pa) + float staticPressure; ///< Static pressure (Pa) + uint16_t temperature; ///< Board temperature } mavlink_air_data_t; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_air_data_pack(uint8_t system_id, uint8_t comp mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) - p->staticPressure = staticPressure; // float:Static pressure (Pa) - p->temperature = temperature; // uint16_t:Board temperature + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIR_DATA_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_air_data_pack_chan(uint8_t system_id, uint8_t mavlink_air_data_t *p = (mavlink_air_data_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_AIR_DATA; - p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) - p->staticPressure = staticPressure; // float:Static pressure (Pa) - p->temperature = temperature; // uint16_t:Board temperature + p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + p->staticPressure = staticPressure; // float:Static pressure (Pa) + p->temperature = temperature; // uint16_t:Board temperature return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIR_DATA_LEN); } @@ -71,6 +73,8 @@ static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t co return mavlink_msg_air_data_pack(system_id, component_id, msg, air_data->dynamicPressure, air_data->staticPressure, air_data->temperature); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a air_data message * @param chan MAVLink channel to send the message @@ -79,19 +83,15 @@ static inline uint16_t mavlink_msg_air_data_encode(uint8_t system_id, uint8_t co * @param staticPressure Static pressure (Pa) * @param temperature Board temperature */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynamicPressure, float staticPressure, uint16_t temperature) { mavlink_header_t hdr; mavlink_air_data_t payload; - uint16_t checksum; - mavlink_air_data_t *p = &payload; - p->dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) - p->staticPressure = staticPressure; // float:Static pressure (Pa) - p->temperature = temperature; // uint16_t:Board temperature + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_AIR_DATA_LEN ) + payload.dynamicPressure = dynamicPressure; // float:Dynamic pressure (Pa) + payload.staticPressure = staticPressure; // float:Static pressure (Pa) + payload.temperature = temperature; // uint16_t:Board temperature hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_AIR_DATA_LEN; @@ -102,14 +102,12 @@ static inline void mavlink_msg_air_data_send(mavlink_channel_t chan, float dynam 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x90, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h index f30e1d8b6216a358abe85d47bee4510f9f43ed50..42efc6407ba307191181a876daf2401eb99afd3e 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_cpu_load.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_CPU_LOAD 170 #define MAVLINK_MSG_ID_CPU_LOAD_LEN 4 #define MAVLINK_MSG_170_LEN 4 +#define MAVLINK_MSG_ID_CPU_LOAD_KEY 0xCA +#define MAVLINK_MSG_170_KEY 0xCA typedef struct __mavlink_cpu_load_t { - uint16_t batVolt; ///< Battery Voltage in millivolts - uint8_t sensLoad; ///< Sensor DSC Load - uint8_t ctrlLoad; ///< Control DSC Load + uint16_t batVolt; ///< Battery Voltage in millivolts + uint8_t sensLoad; ///< Sensor DSC Load + uint8_t ctrlLoad; ///< Control DSC Load } mavlink_cpu_load_t; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_cpu_load_pack(uint8_t system_id, uint8_t comp mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load - p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load - p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CPU_LOAD_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_cpu_load_pack_chan(uint8_t system_id, uint8_t mavlink_cpu_load_t *p = (mavlink_cpu_load_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CPU_LOAD; - p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load - p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load - p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts + p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load + p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CPU_LOAD_LEN); } @@ -71,6 +73,8 @@ static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t co return mavlink_msg_cpu_load_pack(system_id, component_id, msg, cpu_load->sensLoad, cpu_load->ctrlLoad, cpu_load->batVolt); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a cpu_load message * @param chan MAVLink channel to send the message @@ -79,19 +83,15 @@ static inline uint16_t mavlink_msg_cpu_load_encode(uint8_t system_id, uint8_t co * @param ctrlLoad Control DSC Load * @param batVolt Battery Voltage in millivolts */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sensLoad, uint8_t ctrlLoad, uint16_t batVolt) { mavlink_header_t hdr; mavlink_cpu_load_t payload; - uint16_t checksum; - mavlink_cpu_load_t *p = &payload; - p->sensLoad = sensLoad; // uint8_t:Sensor DSC Load - p->ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load - p->batVolt = batVolt; // uint16_t:Battery Voltage in millivolts + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CPU_LOAD_LEN ) + payload.sensLoad = sensLoad; // uint8_t:Sensor DSC Load + payload.ctrlLoad = ctrlLoad; // uint8_t:Control DSC Load + payload.batVolt = batVolt; // uint16_t:Battery Voltage in millivolts hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_CPU_LOAD_LEN; @@ -102,14 +102,12 @@ static inline void mavlink_msg_cpu_load_send(mavlink_channel_t chan, uint8_t sen 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xCA, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h index 81b5fa59a29a427330222c1d1ad0490f3029e8e0..604412c4c8c5b0045f9a4c6d895677fa527a39fd 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_ctrl_srfc_pt.h @@ -3,11 +3,13 @@ #define MAVLINK_MSG_ID_CTRL_SRFC_PT 181 #define MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN 3 #define MAVLINK_MSG_181_LEN 3 +#define MAVLINK_MSG_ID_CTRL_SRFC_PT_KEY 0x35 +#define MAVLINK_MSG_181_KEY 0x35 typedef struct __mavlink_ctrl_srfc_pt_t { - uint16_t bitfieldPt; ///< Bitfield containing the PT configuration - uint8_t target; ///< The system setting the commands + uint16_t bitfieldPt; ///< Bitfield containing the PT configuration + uint8_t target; ///< The system setting the commands } mavlink_ctrl_srfc_pt_t; @@ -26,8 +28,8 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack(uint8_t system_id, uint8_t mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - p->target = target; // uint8_t:The system setting the commands - p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); } @@ -47,8 +49,8 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_pack_chan(uint8_t system_id, uin mavlink_ctrl_srfc_pt_t *p = (mavlink_ctrl_srfc_pt_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_CTRL_SRFC_PT; - p->target = target; // uint8_t:The system setting the commands - p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + p->target = target; // uint8_t:The system setting the commands + p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN); } @@ -66,6 +68,8 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_ return mavlink_msg_ctrl_srfc_pt_pack(system_id, component_id, msg, ctrl_srfc_pt->target, ctrl_srfc_pt->bitfieldPt); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a ctrl_srfc_pt message * @param chan MAVLink channel to send the message @@ -73,18 +77,14 @@ static inline uint16_t mavlink_msg_ctrl_srfc_pt_encode(uint8_t system_id, uint8_ * @param target The system setting the commands * @param bitfieldPt Bitfield containing the PT configuration */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t target, uint16_t bitfieldPt) { mavlink_header_t hdr; mavlink_ctrl_srfc_pt_t payload; - uint16_t checksum; - mavlink_ctrl_srfc_pt_t *p = &payload; - p->target = target; // uint8_t:The system setting the commands - p->bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN ) + payload.target = target; // uint8_t:The system setting the commands + payload.bitfieldPt = bitfieldPt; // uint16_t:Bitfield containing the PT configuration hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_CTRL_SRFC_PT_LEN; @@ -95,14 +95,12 @@ static inline void mavlink_msg_ctrl_srfc_pt_send(mavlink_channel_t chan, uint8_t 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x35, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h index 14da8b9a99688e7bb02e628c7b1a3cd32528dab2..2f3a881c49523a4d2d860fd7782644f972530e9b 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_data_log.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_DATA_LOG 177 #define MAVLINK_MSG_ID_DATA_LOG_LEN 24 #define MAVLINK_MSG_177_LEN 24 +#define MAVLINK_MSG_ID_DATA_LOG_KEY 0xB9 +#define MAVLINK_MSG_177_KEY 0xB9 typedef struct __mavlink_data_log_t { - float fl_1; ///< Log value 1 - float fl_2; ///< Log value 2 - float fl_3; ///< Log value 3 - float fl_4; ///< Log value 4 - float fl_5; ///< Log value 5 - float fl_6; ///< Log value 6 + float fl_1; ///< Log value 1 + float fl_2; ///< Log value 2 + float fl_3; ///< Log value 3 + float fl_4; ///< Log value 4 + float fl_5; ///< Log value 5 + float fl_6; ///< Log value 6 } mavlink_data_log_t; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_data_log_pack(uint8_t system_id, uint8_t comp mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - p->fl_1 = fl_1; // float:Log value 1 - p->fl_2 = fl_2; // float:Log value 2 - p->fl_3 = fl_3; // float:Log value 3 - p->fl_4 = fl_4; // float:Log value 4 - p->fl_5 = fl_5; // float:Log value 5 - p->fl_6 = fl_6; // float:Log value 6 + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DATA_LOG_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_data_log_pack_chan(uint8_t system_id, uint8_t mavlink_data_log_t *p = (mavlink_data_log_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DATA_LOG; - p->fl_1 = fl_1; // float:Log value 1 - p->fl_2 = fl_2; // float:Log value 2 - p->fl_3 = fl_3; // float:Log value 3 - p->fl_4 = fl_4; // float:Log value 4 - p->fl_5 = fl_5; // float:Log value 5 - p->fl_6 = fl_6; // float:Log value 6 + p->fl_1 = fl_1; // float:Log value 1 + p->fl_2 = fl_2; // float:Log value 2 + p->fl_3 = fl_3; // float:Log value 3 + p->fl_4 = fl_4; // float:Log value 4 + p->fl_5 = fl_5; // float:Log value 5 + p->fl_6 = fl_6; // float:Log value 6 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DATA_LOG_LEN); } @@ -86,6 +88,8 @@ static inline uint16_t mavlink_msg_data_log_encode(uint8_t system_id, uint8_t co return mavlink_msg_data_log_pack(system_id, component_id, msg, data_log->fl_1, data_log->fl_2, data_log->fl_3, data_log->fl_4, data_log->fl_5, data_log->fl_6); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a data_log message * @param chan MAVLink channel to send the message @@ -97,22 +101,18 @@ static inline uint16_t mavlink_msg_data_log_encode(uint8_t system_id, uint8_t co * @param fl_5 Log value 5 * @param fl_6 Log value 6 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, float fl_2, float fl_3, float fl_4, float fl_5, float fl_6) { mavlink_header_t hdr; mavlink_data_log_t payload; - uint16_t checksum; - mavlink_data_log_t *p = &payload; - p->fl_1 = fl_1; // float:Log value 1 - p->fl_2 = fl_2; // float:Log value 2 - p->fl_3 = fl_3; // float:Log value 3 - p->fl_4 = fl_4; // float:Log value 4 - p->fl_5 = fl_5; // float:Log value 5 - p->fl_6 = fl_6; // float:Log value 6 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DATA_LOG_LEN ) + payload.fl_1 = fl_1; // float:Log value 1 + payload.fl_2 = fl_2; // float:Log value 2 + payload.fl_3 = fl_3; // float:Log value 3 + payload.fl_4 = fl_4; // float:Log value 4 + payload.fl_5 = fl_5; // float:Log value 5 + payload.fl_6 = fl_6; // float:Log value 6 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_DATA_LOG_LEN; @@ -123,14 +123,12 @@ static inline void mavlink_msg_data_log_send(mavlink_channel_t chan, float fl_1, 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xB9, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h index e9aa6279b9c35c0885ae28617081441e35dd7581..40a1ef617efe402c18a39a901017e15a75f55a2b 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_diagnostic.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_DIAGNOSTIC 173 #define MAVLINK_MSG_ID_DIAGNOSTIC_LEN 18 #define MAVLINK_MSG_173_LEN 18 +#define MAVLINK_MSG_ID_DIAGNOSTIC_KEY 0xFE +#define MAVLINK_MSG_173_KEY 0xFE typedef struct __mavlink_diagnostic_t { - float diagFl1; ///< Diagnostic float 1 - float diagFl2; ///< Diagnostic float 2 - float diagFl3; ///< Diagnostic float 3 - int16_t diagSh1; ///< Diagnostic short 1 - int16_t diagSh2; ///< Diagnostic short 2 - int16_t diagSh3; ///< Diagnostic short 3 + float diagFl1; ///< Diagnostic float 1 + float diagFl2; ///< Diagnostic float 2 + float diagFl3; ///< Diagnostic float 3 + int16_t diagSh1; ///< Diagnostic short 1 + int16_t diagSh2; ///< Diagnostic short 2 + int16_t diagSh3; ///< Diagnostic short 3 } mavlink_diagnostic_t; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_diagnostic_pack(uint8_t system_id, uint8_t co mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - p->diagFl1 = diagFl1; // float:Diagnostic float 1 - p->diagFl2 = diagFl2; // float:Diagnostic float 2 - p->diagFl3 = diagFl3; // float:Diagnostic float 3 - p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 - p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 - p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_diagnostic_pack_chan(uint8_t system_id, uint8 mavlink_diagnostic_t *p = (mavlink_diagnostic_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_DIAGNOSTIC; - p->diagFl1 = diagFl1; // float:Diagnostic float 1 - p->diagFl2 = diagFl2; // float:Diagnostic float 2 - p->diagFl3 = diagFl3; // float:Diagnostic float 3 - p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 - p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 - p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 + p->diagFl1 = diagFl1; // float:Diagnostic float 1 + p->diagFl2 = diagFl2; // float:Diagnostic float 2 + p->diagFl3 = diagFl3; // float:Diagnostic float 3 + p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DIAGNOSTIC_LEN); } @@ -86,6 +88,8 @@ static inline uint16_t mavlink_msg_diagnostic_encode(uint8_t system_id, uint8_t return mavlink_msg_diagnostic_pack(system_id, component_id, msg, diagnostic->diagFl1, diagnostic->diagFl2, diagnostic->diagFl3, diagnostic->diagSh1, diagnostic->diagSh2, diagnostic->diagSh3); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a diagnostic message * @param chan MAVLink channel to send the message @@ -97,22 +101,18 @@ static inline uint16_t mavlink_msg_diagnostic_encode(uint8_t system_id, uint8_t * @param diagSh2 Diagnostic short 2 * @param diagSh3 Diagnostic short 3 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float diagFl1, float diagFl2, float diagFl3, int16_t diagSh1, int16_t diagSh2, int16_t diagSh3) { mavlink_header_t hdr; mavlink_diagnostic_t payload; - uint16_t checksum; - mavlink_diagnostic_t *p = &payload; - p->diagFl1 = diagFl1; // float:Diagnostic float 1 - p->diagFl2 = diagFl2; // float:Diagnostic float 2 - p->diagFl3 = diagFl3; // float:Diagnostic float 3 - p->diagSh1 = diagSh1; // int16_t:Diagnostic short 1 - p->diagSh2 = diagSh2; // int16_t:Diagnostic short 2 - p->diagSh3 = diagSh3; // int16_t:Diagnostic short 3 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_DIAGNOSTIC_LEN ) + payload.diagFl1 = diagFl1; // float:Diagnostic float 1 + payload.diagFl2 = diagFl2; // float:Diagnostic float 2 + payload.diagFl3 = diagFl3; // float:Diagnostic float 3 + payload.diagSh1 = diagSh1; // int16_t:Diagnostic short 1 + payload.diagSh2 = diagSh2; // int16_t:Diagnostic short 2 + payload.diagSh3 = diagSh3; // int16_t:Diagnostic short 3 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_DIAGNOSTIC_LEN; @@ -123,14 +123,12 @@ static inline void mavlink_msg_diagnostic_send(mavlink_channel_t chan, float dia 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xFE, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h index bab695cfa408dd0392d8c8287d056ea90a5f8478..ba616ad3c69705773fc7760a7cdbd7f4fc5641ea 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_gps_date_time.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_GPS_DATE_TIME 179 #define MAVLINK_MSG_ID_GPS_DATE_TIME_LEN 7 #define MAVLINK_MSG_179_LEN 7 +#define MAVLINK_MSG_ID_GPS_DATE_TIME_KEY 0xE +#define MAVLINK_MSG_179_KEY 0xE typedef struct __mavlink_gps_date_time_t { - uint8_t year; ///< Year reported by Gps - uint8_t month; ///< Month reported by Gps - uint8_t day; ///< Day reported by Gps - uint8_t hour; ///< Hour reported by Gps - uint8_t min; ///< Min reported by Gps - uint8_t sec; ///< Sec reported by Gps - uint8_t visSat; ///< Visible sattelites reported by Gps + uint8_t year; ///< Year reported by Gps + uint8_t month; ///< Month reported by Gps + uint8_t day; ///< Day reported by Gps + uint8_t hour; ///< Hour reported by Gps + uint8_t min; ///< Min reported by Gps + uint8_t sec; ///< Sec reported by Gps + uint8_t visSat; ///< Visible sattelites reported by Gps } mavlink_gps_date_time_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_gps_date_time_pack(uint8_t system_id, uint8_t mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - p->year = year; // uint8_t:Year reported by Gps - p->month = month; // uint8_t:Month reported by Gps - p->day = day; // uint8_t:Day reported by Gps - p->hour = hour; // uint8_t:Hour reported by Gps - p->min = min; // uint8_t:Min reported by Gps - p->sec = sec; // uint8_t:Sec reported by Gps - p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_gps_date_time_pack_chan(uint8_t system_id, ui mavlink_gps_date_time_t *p = (mavlink_gps_date_time_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_GPS_DATE_TIME; - p->year = year; // uint8_t:Year reported by Gps - p->month = month; // uint8_t:Month reported by Gps - p->day = day; // uint8_t:Day reported by Gps - p->hour = hour; // uint8_t:Hour reported by Gps - p->min = min; // uint8_t:Min reported by Gps - p->sec = sec; // uint8_t:Sec reported by Gps - p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps + p->year = year; // uint8_t:Year reported by Gps + p->month = month; // uint8_t:Month reported by Gps + p->day = day; // uint8_t:Day reported by Gps + p->hour = hour; // uint8_t:Hour reported by Gps + p->min = min; // uint8_t:Min reported by Gps + p->sec = sec; // uint8_t:Sec reported by Gps + p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_gps_date_time_encode(uint8_t system_id, uint8 return mavlink_msg_gps_date_time_pack(system_id, component_id, msg, gps_date_time->year, gps_date_time->month, gps_date_time->day, gps_date_time->hour, gps_date_time->min, gps_date_time->sec, gps_date_time->visSat); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a gps_date_time message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_gps_date_time_encode(uint8_t system_id, uint8 * @param sec Sec reported by Gps * @param visSat Visible sattelites reported by Gps */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_t year, uint8_t month, uint8_t day, uint8_t hour, uint8_t min, uint8_t sec, uint8_t visSat) { mavlink_header_t hdr; mavlink_gps_date_time_t payload; - uint16_t checksum; - mavlink_gps_date_time_t *p = &payload; - p->year = year; // uint8_t:Year reported by Gps - p->month = month; // uint8_t:Month reported by Gps - p->day = day; // uint8_t:Day reported by Gps - p->hour = hour; // uint8_t:Hour reported by Gps - p->min = min; // uint8_t:Min reported by Gps - p->sec = sec; // uint8_t:Sec reported by Gps - p->visSat = visSat; // uint8_t:Visible sattelites reported by Gps + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_GPS_DATE_TIME_LEN ) + payload.year = year; // uint8_t:Year reported by Gps + payload.month = month; // uint8_t:Month reported by Gps + payload.day = day; // uint8_t:Day reported by Gps + payload.hour = hour; // uint8_t:Hour reported by Gps + payload.min = min; // uint8_t:Min reported by Gps + payload.sec = sec; // uint8_t:Sec reported by Gps + payload.visSat = visSat; // uint8_t:Visible sattelites reported by Gps hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_GPS_DATE_TIME_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_gps_date_time_send(mavlink_channel_t chan, uint8_ 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xE, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h index 0436ae9498cc578ab304b9784a88fa1d57d9d8eb..15c1474428fd298e6158537ffe3471f9dc67bd2c 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_mid_lvl_cmds.h @@ -3,13 +3,15 @@ #define MAVLINK_MSG_ID_MID_LVL_CMDS 180 #define MAVLINK_MSG_ID_MID_LVL_CMDS_LEN 13 #define MAVLINK_MSG_180_LEN 13 +#define MAVLINK_MSG_ID_MID_LVL_CMDS_KEY 0x88 +#define MAVLINK_MSG_180_KEY 0x88 typedef struct __mavlink_mid_lvl_cmds_t { - float hCommand; ///< Commanded Airspeed - float uCommand; ///< Log value 2 - float rCommand; ///< Log value 3 - uint8_t target; ///< The system setting the commands + float hCommand; ///< Commanded Airspeed + float uCommand; ///< Log value 2 + float rCommand; ///< Log value 3 + uint8_t target; ///< The system setting the commands } mavlink_mid_lvl_cmds_t; @@ -30,10 +32,10 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack(uint8_t system_id, uint8_t mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - p->target = target; // uint8_t:The system setting the commands - p->hCommand = hCommand; // float:Commanded Airspeed - p->uCommand = uCommand; // float:Log value 2 - p->rCommand = rCommand; // float:Log value 3 + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); } @@ -55,10 +57,10 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_pack_chan(uint8_t system_id, uin mavlink_mid_lvl_cmds_t *p = (mavlink_mid_lvl_cmds_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_MID_LVL_CMDS; - p->target = target; // uint8_t:The system setting the commands - p->hCommand = hCommand; // float:Commanded Airspeed - p->uCommand = uCommand; // float:Log value 2 - p->rCommand = rCommand; // float:Log value 3 + p->target = target; // uint8_t:The system setting the commands + p->hCommand = hCommand; // float:Commanded Airspeed + p->uCommand = uCommand; // float:Log value 2 + p->rCommand = rCommand; // float:Log value 3 return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN); } @@ -76,6 +78,8 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_encode(uint8_t system_id, uint8_ return mavlink_msg_mid_lvl_cmds_pack(system_id, component_id, msg, mid_lvl_cmds->target, mid_lvl_cmds->hCommand, mid_lvl_cmds->uCommand, mid_lvl_cmds->rCommand); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a mid_lvl_cmds message * @param chan MAVLink channel to send the message @@ -85,20 +89,16 @@ static inline uint16_t mavlink_msg_mid_lvl_cmds_encode(uint8_t system_id, uint8_ * @param uCommand Log value 2 * @param rCommand Log value 3 */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t target, float hCommand, float uCommand, float rCommand) { mavlink_header_t hdr; mavlink_mid_lvl_cmds_t payload; - uint16_t checksum; - mavlink_mid_lvl_cmds_t *p = &payload; - p->target = target; // uint8_t:The system setting the commands - p->hCommand = hCommand; // float:Commanded Airspeed - p->uCommand = uCommand; // float:Log value 2 - p->rCommand = rCommand; // float:Log value 3 + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_MID_LVL_CMDS_LEN ) + payload.target = target; // uint8_t:The system setting the commands + payload.hCommand = hCommand; // float:Commanded Airspeed + payload.uCommand = uCommand; // float:Log value 2 + payload.rCommand = rCommand; // float:Log value 3 hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_MID_LVL_CMDS_LEN; @@ -109,14 +109,12 @@ static inline void mavlink_msg_mid_lvl_cmds_send(mavlink_channel_t chan, uint8_t 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x88, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h index 5601a2a344f36e5ed988bcb386522eae204cede0..25d9fb339fce8d43c5f39648cb838abdd2c8742c 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_sensor_bias.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_SENSOR_BIAS 172 #define MAVLINK_MSG_ID_SENSOR_BIAS_LEN 24 #define MAVLINK_MSG_172_LEN 24 +#define MAVLINK_MSG_ID_SENSOR_BIAS_KEY 0x6A +#define MAVLINK_MSG_172_KEY 0x6A typedef struct __mavlink_sensor_bias_t { - float axBias; ///< Accelerometer X bias (m/s) - float ayBias; ///< Accelerometer Y bias (m/s) - float azBias; ///< Accelerometer Z bias (m/s) - float gxBias; ///< Gyro X bias (rad/s) - float gyBias; ///< Gyro Y bias (rad/s) - float gzBias; ///< Gyro Z bias (rad/s) + float axBias; ///< Accelerometer X bias (m/s) + float ayBias; ///< Accelerometer Y bias (m/s) + float azBias; ///< Accelerometer Z bias (m/s) + float gxBias; ///< Gyro X bias (rad/s) + float gyBias; ///< Gyro Y bias (rad/s) + float gzBias; ///< Gyro Z bias (rad/s) } mavlink_sensor_bias_t; @@ -34,12 +36,12 @@ static inline uint16_t mavlink_msg_sensor_bias_pack(uint8_t system_id, uint8_t c mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - p->axBias = axBias; // float:Accelerometer X bias (m/s) - p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) - p->azBias = azBias; // float:Accelerometer Z bias (m/s) - p->gxBias = gxBias; // float:Gyro X bias (rad/s) - p->gyBias = gyBias; // float:Gyro Y bias (rad/s) - p->gzBias = gzBias; // float:Gyro Z bias (rad/s) + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); } @@ -63,12 +65,12 @@ static inline uint16_t mavlink_msg_sensor_bias_pack_chan(uint8_t system_id, uint mavlink_sensor_bias_t *p = (mavlink_sensor_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SENSOR_BIAS; - p->axBias = axBias; // float:Accelerometer X bias (m/s) - p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) - p->azBias = azBias; // float:Accelerometer Z bias (m/s) - p->gxBias = gxBias; // float:Gyro X bias (rad/s) - p->gyBias = gyBias; // float:Gyro Y bias (rad/s) - p->gzBias = gzBias; // float:Gyro Z bias (rad/s) + p->axBias = axBias; // float:Accelerometer X bias (m/s) + p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) + p->azBias = azBias; // float:Accelerometer Z bias (m/s) + p->gxBias = gxBias; // float:Gyro X bias (rad/s) + p->gyBias = gyBias; // float:Gyro Y bias (rad/s) + p->gzBias = gzBias; // float:Gyro Z bias (rad/s) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SENSOR_BIAS_LEN); } @@ -86,6 +88,8 @@ static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t return mavlink_msg_sensor_bias_pack(system_id, component_id, msg, sensor_bias->axBias, sensor_bias->ayBias, sensor_bias->azBias, sensor_bias->gxBias, sensor_bias->gyBias, sensor_bias->gzBias); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a sensor_bias message * @param chan MAVLink channel to send the message @@ -97,22 +101,18 @@ static inline uint16_t mavlink_msg_sensor_bias_encode(uint8_t system_id, uint8_t * @param gyBias Gyro Y bias (rad/s) * @param gzBias Gyro Z bias (rad/s) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float axBias, float ayBias, float azBias, float gxBias, float gyBias, float gzBias) { mavlink_header_t hdr; mavlink_sensor_bias_t payload; - uint16_t checksum; - mavlink_sensor_bias_t *p = &payload; - p->axBias = axBias; // float:Accelerometer X bias (m/s) - p->ayBias = ayBias; // float:Accelerometer Y bias (m/s) - p->azBias = azBias; // float:Accelerometer Z bias (m/s) - p->gxBias = gxBias; // float:Gyro X bias (rad/s) - p->gyBias = gyBias; // float:Gyro Y bias (rad/s) - p->gzBias = gzBias; // float:Gyro Z bias (rad/s) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SENSOR_BIAS_LEN ) + payload.axBias = axBias; // float:Accelerometer X bias (m/s) + payload.ayBias = ayBias; // float:Accelerometer Y bias (m/s) + payload.azBias = azBias; // float:Accelerometer Z bias (m/s) + payload.gxBias = gxBias; // float:Gyro X bias (rad/s) + payload.gyBias = gyBias; // float:Gyro Y bias (rad/s) + payload.gzBias = gzBias; // float:Gyro Z bias (rad/s) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SENSOR_BIAS_LEN; @@ -123,14 +123,12 @@ static inline void mavlink_msg_sensor_bias_send(mavlink_channel_t chan, float ax 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x6A, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h index fe3399e2ecb835fb581681b901167ef52f8f8858..d073928c2a7c948d70ab3cc2d7e4533389e080c6 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_action.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_SLUGS_ACTION 183 #define MAVLINK_MSG_ID_SLUGS_ACTION_LEN 4 #define MAVLINK_MSG_183_LEN 4 +#define MAVLINK_MSG_ID_SLUGS_ACTION_KEY 0xD4 +#define MAVLINK_MSG_183_KEY 0xD4 typedef struct __mavlink_slugs_action_t { - uint16_t actionVal; ///< Value associated with the action - uint8_t target; ///< The system reporting the action - uint8_t actionId; ///< Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + uint16_t actionVal; ///< Value associated with the action + uint8_t target; ///< The system reporting the action + uint8_t actionId; ///< Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names } mavlink_slugs_action_t; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_slugs_action_pack(uint8_t system_id, uint8_t mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - p->target = target; // uint8_t:The system reporting the action - p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - p->actionVal = actionVal; // uint16_t:Value associated with the action + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_slugs_action_pack_chan(uint8_t system_id, uin mavlink_slugs_action_t *p = (mavlink_slugs_action_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_ACTION; - p->target = target; // uint8_t:The system reporting the action - p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - p->actionVal = actionVal; // uint16_t:Value associated with the action + p->target = target; // uint8_t:The system reporting the action + p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + p->actionVal = actionVal; // uint16_t:Value associated with the action return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SLUGS_ACTION_LEN); } @@ -71,6 +73,8 @@ static inline uint16_t mavlink_msg_slugs_action_encode(uint8_t system_id, uint8_ return mavlink_msg_slugs_action_pack(system_id, component_id, msg, slugs_action->target, slugs_action->actionId, slugs_action->actionVal); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a slugs_action message * @param chan MAVLink channel to send the message @@ -79,19 +83,15 @@ static inline uint16_t mavlink_msg_slugs_action_encode(uint8_t system_id, uint8_ * @param actionId Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names * @param actionVal Value associated with the action */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t target, uint8_t actionId, uint16_t actionVal) { mavlink_header_t hdr; mavlink_slugs_action_t payload; - uint16_t checksum; - mavlink_slugs_action_t *p = &payload; - p->target = target; // uint8_t:The system reporting the action - p->actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names - p->actionVal = actionVal; // uint16_t:Value associated with the action + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SLUGS_ACTION_LEN ) + payload.target = target; // uint8_t:The system reporting the action + payload.actionId = actionId; // uint8_t:Action ID. See apDefinitions.h in the SLUGS /clib directory for the ID names + payload.actionVal = actionVal; // uint16_t:Value associated with the action hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SLUGS_ACTION_LEN; @@ -102,14 +102,12 @@ static inline void mavlink_msg_slugs_action_send(mavlink_channel_t chan, uint8_t 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xD4, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h index dc69df79e88854f90519d88b6b00397c67236428..f3e337cf17ff860485ac91b1304e01a0e84db83e 100644 --- a/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h +++ b/thirdParty/mavlink/include/slugs/mavlink_msg_slugs_navigation.h @@ -3,18 +3,20 @@ #define MAVLINK_MSG_ID_SLUGS_NAVIGATION 176 #define MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN 30 #define MAVLINK_MSG_176_LEN 30 +#define MAVLINK_MSG_ID_SLUGS_NAVIGATION_KEY 0xFF +#define MAVLINK_MSG_176_KEY 0xFF typedef struct __mavlink_slugs_navigation_t { - float u_m; ///< Measured Airspeed prior to the Nav Filter - float phi_c; ///< Commanded Roll - float theta_c; ///< Commanded Pitch - float psiDot_c; ///< Commanded Turn rate - float ay_body; ///< Y component of the body acceleration - float totalDist; ///< Total Distance to Run on this leg of Navigation - float dist2Go; ///< Remaining distance to Run on this leg of Navigation - uint8_t fromWP; ///< Origin WP - uint8_t toWP; ///< Destination WP + float u_m; ///< Measured Airspeed prior to the Nav Filter + float phi_c; ///< Commanded Roll + float theta_c; ///< Commanded Pitch + float psiDot_c; ///< Commanded Turn rate + float ay_body; ///< Y component of the body acceleration + float totalDist; ///< Total Distance to Run on this leg of Navigation + float dist2Go; ///< Remaining distance to Run on this leg of Navigation + uint8_t fromWP; ///< Origin WP + uint8_t toWP; ///< Destination WP } mavlink_slugs_navigation_t; @@ -40,15 +42,15 @@ static inline uint16_t mavlink_msg_slugs_navigation_pack(uint8_t system_id, uint mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter - p->phi_c = phi_c; // float:Commanded Roll - p->theta_c = theta_c; // float:Commanded Pitch - p->psiDot_c = psiDot_c; // float:Commanded Turn rate - p->ay_body = ay_body; // float:Y component of the body acceleration - p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation - p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation - p->fromWP = fromWP; // uint8_t:Origin WP - p->toWP = toWP; // uint8_t:Destination WP + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN); } @@ -75,15 +77,15 @@ static inline uint16_t mavlink_msg_slugs_navigation_pack_chan(uint8_t system_id, mavlink_slugs_navigation_t *p = (mavlink_slugs_navigation_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_SLUGS_NAVIGATION; - p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter - p->phi_c = phi_c; // float:Commanded Roll - p->theta_c = theta_c; // float:Commanded Pitch - p->psiDot_c = psiDot_c; // float:Commanded Turn rate - p->ay_body = ay_body; // float:Y component of the body acceleration - p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation - p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation - p->fromWP = fromWP; // uint8_t:Origin WP - p->toWP = toWP; // uint8_t:Destination WP + p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + p->phi_c = phi_c; // float:Commanded Roll + p->theta_c = theta_c; // float:Commanded Pitch + p->psiDot_c = psiDot_c; // float:Commanded Turn rate + p->ay_body = ay_body; // float:Y component of the body acceleration + p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + p->fromWP = fromWP; // uint8_t:Origin WP + p->toWP = toWP; // uint8_t:Destination WP return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN); } @@ -101,6 +103,8 @@ static inline uint16_t mavlink_msg_slugs_navigation_encode(uint8_t system_id, ui return mavlink_msg_slugs_navigation_pack(system_id, component_id, msg, slugs_navigation->u_m, slugs_navigation->phi_c, slugs_navigation->theta_c, slugs_navigation->psiDot_c, slugs_navigation->ay_body, slugs_navigation->totalDist, slugs_navigation->dist2Go, slugs_navigation->fromWP, slugs_navigation->toWP); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a slugs_navigation message * @param chan MAVLink channel to send the message @@ -115,25 +119,21 @@ static inline uint16_t mavlink_msg_slugs_navigation_encode(uint8_t system_id, ui * @param fromWP Origin WP * @param toWP Destination WP */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, float u_m, float phi_c, float theta_c, float psiDot_c, float ay_body, float totalDist, float dist2Go, uint8_t fromWP, uint8_t toWP) { mavlink_header_t hdr; mavlink_slugs_navigation_t payload; - uint16_t checksum; - mavlink_slugs_navigation_t *p = &payload; - p->u_m = u_m; // float:Measured Airspeed prior to the Nav Filter - p->phi_c = phi_c; // float:Commanded Roll - p->theta_c = theta_c; // float:Commanded Pitch - p->psiDot_c = psiDot_c; // float:Commanded Turn rate - p->ay_body = ay_body; // float:Y component of the body acceleration - p->totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation - p->dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation - p->fromWP = fromWP; // uint8_t:Origin WP - p->toWP = toWP; // uint8_t:Destination WP + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN ) + payload.u_m = u_m; // float:Measured Airspeed prior to the Nav Filter + payload.phi_c = phi_c; // float:Commanded Roll + payload.theta_c = theta_c; // float:Commanded Pitch + payload.psiDot_c = psiDot_c; // float:Commanded Turn rate + payload.ay_body = ay_body; // float:Y component of the body acceleration + payload.totalDist = totalDist; // float:Total Distance to Run on this leg of Navigation + payload.dist2Go = dist2Go; // float:Remaining distance to Run on this leg of Navigation + payload.fromWP = fromWP; // uint8_t:Origin WP + payload.toWP = toWP; // uint8_t:Destination WP hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_SLUGS_NAVIGATION_LEN; @@ -144,14 +144,12 @@ static inline void mavlink_msg_slugs_navigation_send(mavlink_channel_t chan, flo 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xFF, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/slugs/slugs.h b/thirdParty/mavlink/include/slugs/slugs.h index 990857c7294df2417646ca34324fbc874843999d..eae6c7e30280d11f0d4517967dcb537a410a295a 100644 --- a/thirdParty/mavlink/include/slugs/slugs.h +++ b/thirdParty/mavlink/include/slugs/slugs.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef SLUGS_H #define SLUGS_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_SLUGS @@ -48,7 +48,12 @@ extern "C" { // 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, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 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 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 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, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 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, 255, 73, 239, 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, 138, 43, 141, 211, 144 } + +// MESSAGE LENGTHS + +#undef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 0, 2, 2, 0, 0, 0, 0, 0, 0, 0, 20, 2, 24, 22, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 20, 20, 20, 8, 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, 4, 10, 24, 18, 0, 0, 30, 24, 0, 7, 13, 3, 0, 4, 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, 36, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/include/ualberta/mavlink.h b/thirdParty/mavlink/include/ualberta/mavlink.h index 0bb9f2f90603b286a1d2f17a2c67aee0d313c0ae..591a13326144856021817060bf6954e5c6a5cf89 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink.h +++ b/thirdParty/mavlink/include/ualberta/mavlink.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://pixhawk.ethz.ch/software/mavlink - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef MAVLINK_H #define MAVLINK_H diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h index d1b777ee46823ec2a3e1a459d33d9376d534c18e..6971ea408342d6d1905c2e8dd7fc03e9edba3fb8 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_nav_filter_bias.h @@ -3,16 +3,18 @@ #define MAVLINK_MSG_ID_NAV_FILTER_BIAS 220 #define MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN 32 #define MAVLINK_MSG_220_LEN 32 +#define MAVLINK_MSG_ID_NAV_FILTER_BIAS_KEY 0xFF +#define MAVLINK_MSG_220_KEY 0xFF typedef struct __mavlink_nav_filter_bias_t { - uint64_t usec; ///< Timestamp (microseconds) - float accel_0; ///< b_f[0] - float accel_1; ///< b_f[1] - float accel_2; ///< b_f[2] - float gyro_0; ///< b_f[0] - float gyro_1; ///< b_f[1] - float gyro_2; ///< b_f[2] + uint64_t usec; ///< Timestamp (microseconds) + float accel_0; ///< b_f[0] + float accel_1; ///< b_f[1] + float accel_2; ///< b_f[2] + float gyro_0; ///< b_f[0] + float gyro_1; ///< b_f[1] + float gyro_2; ///< b_f[2] } mavlink_nav_filter_bias_t; @@ -36,13 +38,13 @@ static inline uint16_t mavlink_msg_nav_filter_bias_pack(uint8_t system_id, uint8 mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - p->usec = usec; // uint64_t:Timestamp (microseconds) - p->accel_0 = accel_0; // float:b_f[0] - p->accel_1 = accel_1; // float:b_f[1] - p->accel_2 = accel_2; // float:b_f[2] - p->gyro_0 = gyro_0; // float:b_f[0] - p->gyro_1 = gyro_1; // float:b_f[1] - p->gyro_2 = gyro_2; // float:b_f[2] + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); } @@ -67,13 +69,13 @@ static inline uint16_t mavlink_msg_nav_filter_bias_pack_chan(uint8_t system_id, mavlink_nav_filter_bias_t *p = (mavlink_nav_filter_bias_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_NAV_FILTER_BIAS; - p->usec = usec; // uint64_t:Timestamp (microseconds) - p->accel_0 = accel_0; // float:b_f[0] - p->accel_1 = accel_1; // float:b_f[1] - p->accel_2 = accel_2; // float:b_f[2] - p->gyro_0 = gyro_0; // float:b_f[0] - p->gyro_1 = gyro_1; // float:b_f[1] - p->gyro_2 = gyro_2; // float:b_f[2] + p->usec = usec; // uint64_t:Timestamp (microseconds) + p->accel_0 = accel_0; // float:b_f[0] + p->accel_1 = accel_1; // float:b_f[1] + p->accel_2 = accel_2; // float:b_f[2] + p->gyro_0 = gyro_0; // float:b_f[0] + p->gyro_1 = gyro_1; // float:b_f[1] + p->gyro_2 = gyro_2; // float:b_f[2] return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN); } @@ -91,6 +93,8 @@ static inline uint16_t mavlink_msg_nav_filter_bias_encode(uint8_t system_id, uin return mavlink_msg_nav_filter_bias_pack(system_id, component_id, msg, nav_filter_bias->usec, nav_filter_bias->accel_0, nav_filter_bias->accel_1, nav_filter_bias->accel_2, nav_filter_bias->gyro_0, nav_filter_bias->gyro_1, nav_filter_bias->gyro_2); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a nav_filter_bias message * @param chan MAVLink channel to send the message @@ -103,23 +107,19 @@ static inline uint16_t mavlink_msg_nav_filter_bias_encode(uint8_t system_id, uin * @param gyro_1 b_f[1] * @param gyro_2 b_f[2] */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint64_t usec, float accel_0, float accel_1, float accel_2, float gyro_0, float gyro_1, float gyro_2) { mavlink_header_t hdr; mavlink_nav_filter_bias_t payload; - uint16_t checksum; - mavlink_nav_filter_bias_t *p = &payload; - p->usec = usec; // uint64_t:Timestamp (microseconds) - p->accel_0 = accel_0; // float:b_f[0] - p->accel_1 = accel_1; // float:b_f[1] - p->accel_2 = accel_2; // float:b_f[2] - p->gyro_0 = gyro_0; // float:b_f[0] - p->gyro_1 = gyro_1; // float:b_f[1] - p->gyro_2 = gyro_2; // float:b_f[2] + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN ) + payload.usec = usec; // uint64_t:Timestamp (microseconds) + payload.accel_0 = accel_0; // float:b_f[0] + payload.accel_1 = accel_1; // float:b_f[1] + payload.accel_2 = accel_2; // float:b_f[2] + payload.gyro_0 = gyro_0; // float:b_f[0] + payload.gyro_1 = gyro_1; // float:b_f[1] + payload.gyro_2 = gyro_2; // float:b_f[2] hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_NAV_FILTER_BIAS_LEN; @@ -130,14 +130,12 @@ static inline void mavlink_msg_nav_filter_bias_send(mavlink_channel_t chan, uint 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xFF, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h index 22b139afc655125e254c789a02652e3aa4fa8bb6..107c5f8609d9ea57cfc2cc1e4ac426d40703663a 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_radio_calibration.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_RADIO_CALIBRATION 221 #define MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN 42 #define MAVLINK_MSG_221_LEN 42 +#define MAVLINK_MSG_ID_RADIO_CALIBRATION_KEY 0x49 +#define MAVLINK_MSG_221_KEY 0x49 typedef struct __mavlink_radio_calibration_t { - uint16_t aileron[3]; ///< Aileron setpoints: left, center, right - uint16_t elevator[3]; ///< Elevator setpoints: nose down, center, nose up - uint16_t rudder[3]; ///< Rudder setpoints: nose left, center, nose right - uint16_t gyro[2]; ///< Tail gyro mode/gain setpoints: heading hold, rate mode - uint16_t pitch[5]; ///< Pitch curve setpoints (every 25%) - uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%) + uint16_t aileron[3]; ///< Aileron setpoints: left, center, right + uint16_t elevator[3]; ///< Elevator setpoints: nose down, center, nose up + uint16_t rudder[3]; ///< Rudder setpoints: nose left, center, nose right + uint16_t gyro[2]; ///< Tail gyro mode/gain setpoints: heading hold, rate mode + uint16_t pitch[5]; ///< Pitch curve setpoints (every 25%) + uint16_t throttle[5]; ///< Throttle curve setpoints (every 25%) } mavlink_radio_calibration_t; #define MAVLINK_MSG_RADIO_CALIBRATION_FIELD_AILERON_LEN 3 @@ -40,12 +42,12 @@ static inline uint16_t mavlink_msg_radio_calibration_pack(uint8_t system_id, uin mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right - memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up - memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right - memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode - memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) - memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); } @@ -69,12 +71,12 @@ static inline uint16_t mavlink_msg_radio_calibration_pack_chan(uint8_t system_id mavlink_radio_calibration_t *p = (mavlink_radio_calibration_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_RADIO_CALIBRATION; - memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right - memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up - memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right - memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode - memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) - memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) + memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN); } @@ -92,6 +94,8 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u return mavlink_msg_radio_calibration_pack(system_id, component_id, msg, radio_calibration->aileron, radio_calibration->elevator, radio_calibration->rudder, radio_calibration->gyro, radio_calibration->pitch, radio_calibration->throttle); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a radio_calibration message * @param chan MAVLink channel to send the message @@ -103,22 +107,18 @@ static inline uint16_t mavlink_msg_radio_calibration_encode(uint8_t system_id, u * @param pitch Pitch curve setpoints (every 25%) * @param throttle Throttle curve setpoints (every 25%) */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, const uint16_t* aileron, const uint16_t* elevator, const uint16_t* rudder, const uint16_t* gyro, const uint16_t* pitch, const uint16_t* throttle) { mavlink_header_t hdr; mavlink_radio_calibration_t payload; - uint16_t checksum; - mavlink_radio_calibration_t *p = &payload; - memcpy(p->aileron, aileron, sizeof(p->aileron)); // uint16_t[3]:Aileron setpoints: left, center, right - memcpy(p->elevator, elevator, sizeof(p->elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up - memcpy(p->rudder, rudder, sizeof(p->rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right - memcpy(p->gyro, gyro, sizeof(p->gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode - memcpy(p->pitch, pitch, sizeof(p->pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) - memcpy(p->throttle, throttle, sizeof(p->throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN ) + memcpy(payload.aileron, aileron, sizeof(payload.aileron)); // uint16_t[3]:Aileron setpoints: left, center, right + memcpy(payload.elevator, elevator, sizeof(payload.elevator)); // uint16_t[3]:Elevator setpoints: nose down, center, nose up + memcpy(payload.rudder, rudder, sizeof(payload.rudder)); // uint16_t[3]:Rudder setpoints: nose left, center, nose right + memcpy(payload.gyro, gyro, sizeof(payload.gyro)); // uint16_t[2]:Tail gyro mode/gain setpoints: heading hold, rate mode + memcpy(payload.pitch, pitch, sizeof(payload.pitch)); // uint16_t[5]:Pitch curve setpoints (every 25%) + memcpy(payload.throttle, throttle, sizeof(payload.throttle)); // uint16_t[5]:Throttle curve setpoints (every 25%) hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_RADIO_CALIBRATION_LEN; @@ -129,14 +129,12 @@ static inline void mavlink_msg_radio_calibration_send(mavlink_channel_t chan, co 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0x49, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h index 16e2ff6853ca2559b1d0c99e013f38df910347e3..a42653e9821d6d14287fe48b23d43523961967e3 100644 --- a/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h +++ b/thirdParty/mavlink/include/ualberta/mavlink_msg_ualberta_sys_status.h @@ -3,12 +3,14 @@ #define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS 222 #define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN 3 #define MAVLINK_MSG_222_LEN 3 +#define MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_KEY 0xEF +#define MAVLINK_MSG_222_KEY 0xEF typedef struct __mavlink_ualberta_sys_status_t { - uint8_t mode; ///< System mode, see UALBERTA_AUTOPILOT_MODE ENUM - uint8_t nav_mode; ///< Navigation mode, see UALBERTA_NAV_MODE ENUM - uint8_t pilot; ///< Pilot mode, see UALBERTA_PILOT_MODE + uint8_t mode; ///< System mode, see UALBERTA_AUTOPILOT_MODE ENUM + uint8_t nav_mode; ///< Navigation mode, see UALBERTA_NAV_MODE ENUM + uint8_t pilot; ///< Pilot mode, see UALBERTA_PILOT_MODE } mavlink_ualberta_sys_status_t; @@ -28,9 +30,9 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_pack(uint8_t system_id, u mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM - p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); } @@ -51,9 +53,9 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_pack_chan(uint8_t system_ mavlink_ualberta_sys_status_t *p = (mavlink_ualberta_sys_status_t *)&msg->payload[0]; msg->msgid = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS; - p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM - p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE + p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN); } @@ -71,6 +73,8 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, return mavlink_msg_ualberta_sys_status_pack(system_id, component_id, msg, ualberta_sys_status->mode, ualberta_sys_status->nav_mode, ualberta_sys_status->pilot); } + +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Send a ualberta_sys_status message * @param chan MAVLink channel to send the message @@ -79,19 +83,15 @@ static inline uint16_t mavlink_msg_ualberta_sys_status_encode(uint8_t system_id, * @param nav_mode Navigation mode, see UALBERTA_NAV_MODE ENUM * @param pilot Pilot mode, see UALBERTA_PILOT_MODE */ - - -#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, uint8_t mode, uint8_t nav_mode, uint8_t pilot) { mavlink_header_t hdr; mavlink_ualberta_sys_status_t payload; - uint16_t checksum; - mavlink_ualberta_sys_status_t *p = &payload; - p->mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM - p->nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM - p->pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE + MAVLINK_BUFFER_CHECK_START( chan, MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN ) + payload.mode = mode; // uint8_t:System mode, see UALBERTA_AUTOPILOT_MODE ENUM + payload.nav_mode = nav_mode; // uint8_t:Navigation mode, see UALBERTA_NAV_MODE ENUM + payload.pilot = pilot; // uint8_t:Pilot mode, see UALBERTA_PILOT_MODE hdr.STX = MAVLINK_STX; hdr.len = MAVLINK_MSG_ID_UALBERTA_SYS_STATUS_LEN; @@ -102,14 +102,12 @@ static inline void mavlink_msg_ualberta_sys_status_send(mavlink_channel_t chan, 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); + crc_init(&hdr.ck); + crc_calculate_mem((uint8_t *)&hdr.len, &hdr.ck, MAVLINK_CORE_HEADER_LEN); + crc_calculate_mem((uint8_t *)&payload, &hdr.ck, hdr.len ); + crc_accumulate( 0xEF, &hdr.ck); /// include key in X25 checksum + mavlink_send_mem(chan, (uint8_t *)&hdr.ck, MAVLINK_NUM_CHECKSUM_BYTES); + MAVLINK_BUFFER_CHECK_END } #endif diff --git a/thirdParty/mavlink/include/ualberta/ualberta.h b/thirdParty/mavlink/include/ualberta/ualberta.h index c0c03d82799e60c4cd307de501f3a1ec6038107f..90cf2cd08fb429203ec2832969fd1ebfe8fd68bc 100644 --- a/thirdParty/mavlink/include/ualberta/ualberta.h +++ b/thirdParty/mavlink/include/ualberta/ualberta.h @@ -1,7 +1,7 @@ /** @file * @brief MAVLink comm protocol. * @see http://qgroundcontrol.org/mavlink/ - * Generated on Tuesday, August 9 2011, 16:16 UTC + * Generated on Saturday, August 13 2011, 07:19 UTC */ #ifndef UALBERTA_H #define UALBERTA_H @@ -11,7 +11,7 @@ extern "C" { #endif -#include "../protocol.h" +#include "../mavlink_protocol.h" #define MAVLINK_ENABLED_UALBERTA @@ -71,7 +71,12 @@ enum UALBERTA_PILOT_MODE // 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, 127, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 36, 8, 238, 165, 0, 0, 0, 0, 0, 0, 0, 218, 218, 235, 0, 0, 0, 0, 0, 0, 225, 114, 0, 0, 0, 0, 0, 0, 0, 0, 221, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 171, 122, 0, 0, 0, 0, 0, 0, 0, 92, 114, 4, 169, 10, 0, 0, 0, 0, 0, 72, 62, 83, 0, 0, 0, 0, 0, 0, 0, 202, 144, 106, 254, 0, 0, 255, 185, 0, 14, 136, 53, 0, 212, 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, 255, 73, 239, 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 } +#define MAVLINK_MESSAGE_KEYS { 71, 249, 232, 226, 76, 126, 117, 186, 0, 144, 0, 249, 133, 0, 0, 0, 0, 0, 0, 0, 33, 34, 163, 45, 0, 166, 28, 99, 28, 21, 243, 240, 91, 21, 111, 43, 192, 234, 22, 197, 192, 192, 166, 34, 233, 34, 166, 158, 142, 60, 10, 75, 249, 247, 234, 161, 116, 56, 245, 0, 0, 0, 62, 75, 185, 18, 42, 38, 0, 127, 200, 0, 0, 212, 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, 255, 73, 239, 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, 138, 43, 141, 211, 144 } + +// MESSAGE LENGTHS + +#undef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS { 3, 4, 8, 14, 8, 28, 3, 32, 0, 2, 0, 2, 2, 0, 0, 0, 0, 0, 0, 0, 20, 2, 24, 22, 0, 30, 26, 101, 26, 16, 32, 32, 38, 32, 11, 17, 17, 16, 18, 36, 4, 4, 2, 2, 4, 2, 2, 3, 14, 12, 18, 16, 8, 27, 25, 18, 18, 20, 20, 0, 0, 0, 26, 16, 36, 5, 6, 56, 0, 21, 18, 0, 0, 20, 20, 20, 8, 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, 32, 42, 3, 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, 36, 30, 14, 14, 51 } #ifdef __cplusplus } diff --git a/thirdParty/mavlink/missionlib/mavlink_parameters.c b/thirdParty/mavlink/missionlib/mavlink_parameters.c new file mode 100644 index 0000000000000000000000000000000000000000..420b296f86a0456d6a55d2fae2e6903e6751350e --- /dev/null +++ b/thirdParty/mavlink/missionlib/mavlink_parameters.c @@ -0,0 +1,146 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +#include "testing/mavlink_missionlib_data.h" +#include "mavlink_parameters.h" +#include "math.h" /* isinf / isnan checks */ + +extern mavlink_system_t mavlink_system; +extern mavlink_pm_storage pm; + +extern void mavlink_missionlib_send_message(mavlink_message_t* msg); +extern void mavlink_missionlib_send_gcs_string(const char* string); + +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg) +{ + switch (msg->msgid) + { + case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: + { + // Start sending parameters + pm.next_param = 0; + mavlink_missionlib_send_gcs_string("PM SENDING LIST"); + } + break; + case MAVLINK_MSG_ID_PARAM_SET: + { + mavlink_param_set_t set; + mavlink_msg_param_set_decode(msg, &set); + + // Check if this message is for this system + if (set.target_system == mavlink_system.sysid && set.target_component == mavlink_system.compid) + { + char* key = set.param_id; + + for (uint16_t i = 0; i < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; i++) + { + bool match = true; + for (uint16_t j = 0; j < MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN; j++) + { + // Compare + if (pm.param_names[i][j] != key[j]) + { + match = false; + } + + // End matching if null termination is reached + if (pm.param_names[i][j] == '\0') + { + break; + } + } + + // Check if matched + if (match) + { + // Only write and emit changes if there is actually a difference + // AND only write if new value is NOT "not-a-number" + // AND is NOT infinity + if (pm.param_values[i] != set.param_value + && !isnan(set.param_value) + && !isinf(set.param_value)) + { + pm.param_values[i] = set.param_value; + // Report back new value +#ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS + mavlink_message_t tx_msg; + mavlink_msg_param_value_pack_chan(mavlink_system.sysid, + mavlink_system.compid, + MAVLINK_COMM_0, + &tx_msg, + pm.param_names[i], + pm.param_values[i], + pm.size, + i); + mavlink_missionlib_send_message(&tx_msg); +#else + mavlink_msg_param_value_send(MAVLINK_COMM_0, + pm.param_names[i], + pm.param_values[i], + pm.size, + i); +#endif + + mavlink_missionlib_send_gcs_string("PM received param"); + } // End valid value check + } // End match check + } // End for loop + } // End system ID check + + } // End case + break; + + } // End switch + +} + + /** + * @brief Send low-priority messages at a maximum rate of xx Hertz + * + * This function sends messages at a lower rate to not exceed the wireless + * bandwidth. It sends one message each time it is called until the buffer is empty. + * Call this function with xx Hertz to increase/decrease the bandwidth. + */ + void mavlink_pm_queued_send(void) + { + //send parameters one by one + if (pm.next_param < pm.size) + { + //for (int i.. all active comm links) +#ifndef MAVLINK_USE_CONVENIENCE_FUNCTIONS + mavlink_message_t tx_msg; + mavlink_msg_param_value_pack_chan(mavlink_system.sysid, + mavlink_system.compid, + MAVLINK_COMM_0, + &tx_msg, + pm.param_names[pm.next_param], + pm.param_values[pm.next_param], + pm.size, + pm.next_param); + mavlink_missionlib_send_message(&tx_msg); +#else + mavlink_msg_param_value_send(MAVLINK_COMM_0, + pm.param_names[pm.next_param], + pm.param_values[pm.next_param], + pm.size, + pm.next_param); +#endif + pm.next_param++; + } + } \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/mavlink_parameters.h b/thirdParty/mavlink/missionlib/mavlink_parameters.h new file mode 100644 index 0000000000000000000000000000000000000000..82e7032772a55657686e655ab10c3e5c5bb29d77 --- /dev/null +++ b/thirdParty/mavlink/missionlib/mavlink_parameters.h @@ -0,0 +1,42 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + +// Disable auto-data structures +#ifndef MAVLINK_NO_DATA +#define MAVLINK_NO_DATA +#endif + +#include "mavlink.h" +#include + +/* PARAMETER MANAGER - MISSION LIB */ + +//#ifndef MAVLINK_PM_MAX_PARAM_COUNT +//#error You have not defined the ENUM with your parameters with MAVLINK_PM_MAX_PARAM_COUNT at the end. +//#endif + +#ifndef MAVLINK_PM_TEXT_FEEDBACK +#define MAVLINK_PM_TEXT_FEEDBACK 1 ///< Report back status information as text +#endif + +void mavlink_pm_message_handler(const mavlink_channel_t chan, const mavlink_message_t* msg); +void mavlink_pm_queued_send(void); \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/testing/main.c b/thirdParty/mavlink/missionlib/testing/main.c index 60b653a7982396c3e21ecf2c204e19e86c7b2307..f90000b1ba5234175327069a442e65f09cfe8b7b 100644 --- a/thirdParty/mavlink/missionlib/testing/main.c +++ b/thirdParty/mavlink/missionlib/testing/main.c @@ -52,41 +52,57 @@ #include #endif -#include -<<<<<<< HEAD -#include -======= ->>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f +/* FIRST: MAVLink setup */ +//#define MAVLINK_CONFIGURED +//#define MAVLINK_NO_DATA +#define MAVLINK_WPM_VERBOSE -#include <../mavlink_types.h> +/* 0: Include MAVLink types */ +#include "mavlink_types.h" +/* 1: Define mavlink system storage */ mavlink_system_t mavlink_system; -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ -#include +/* 2: Include actual protocol, REQUIRES mavlink_system */ +#include "mavlink.h" +/* 3: Include MAVLink data structures */ +#include "mavlink_data.h" -<<<<<<< HEAD -======= -/* Provide the interface functions for the waypoint manager */ +/* 3: Define waypoint helper functions */ +void mavlink_wpm_send_message(mavlink_message_t* msg); +void mavlink_wpm_send_gcs_string(const char* string); +uint64_t mavlink_wpm_get_system_timestamp(); +void mavlink_missionlib_send_message(mavlink_message_t* msg); +void mavlink_missionlib_send_gcs_string(const char* string); +uint64_t mavlink_missionlib_get_system_timestamp(); -/* - * @brief Sends a MAVLink message over UDP +/* 4: Include waypoint protocol */ +#include "waypoints.h" +mavlink_wpm_storage wpm; + + +#include "mavlink_missionlib_data.h" +#include "mavlink_parameters.h" + +mavlink_pm_storage pm; + +/** + * @brief reset all parameters to default + * @warning DO NOT USE THIS IN FLIGHT! */ -void mavlink_wpm_send_message(mavlink_message_t* msg) +void mavlink_pm_reset_params(mavlink_pm_storage* pm) { - uint16_t len = mavlink_msg_to_send_buffer(buf, msg); - uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); - - printf("SENT %d bytes", bytes_sent); + pm->size = MAVLINK_PM_MAX_PARAM_COUNT; + // 1) MAVLINK_PM_PARAM_SYSTEM_ID + pm->param_values[MAVLINK_PM_PARAM_SYSTEM_ID] = 12; + strcpy(pm->param_names[MAVLINK_PM_PARAM_SYSTEM_ID], "SYS_ID"); + // 2) MAVLINK_PM_PARAM_ATT_K_D + pm->param_values[MAVLINK_PM_PARAM_ATT_K_D] = 0.3f; + strcpy(pm->param_names[MAVLINK_PM_PARAM_ATT_K_D], "ATT_K_D"); } -#include - - ->>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f #define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) char help[] = "--help"; @@ -110,956 +126,60 @@ unsigned int temp = 0; uint64_t microsSinceEpoch(); -<<<<<<< HEAD -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES -{ - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES -{ - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 100 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text -#define MAVLINK_WPM_SYSTEM_ID 1 -#define MAVLINK_WPM_COMPONENT_ID 1 -#define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000 -#define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 -#define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40 - - -struct mavlink_wpm_storage { - mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif - uint16_t size; - uint16_t max_size; - uint16_t rcv_size; - enum MAVLINK_WPM_STATES current_state; - uint16_t current_wp_id; ///< Waypoint in current transmission - uint16_t current_active_wp_id; ///< Waypoint the system is currently heading towards - uint16_t current_count; - uint8_t current_partner_sysid; - uint8_t current_partner_compid; - uint64_t timestamp_lastaction; - uint64_t timestamp_last_send_setpoint; - uint64_t timestamp_firstinside_orbit; - uint64_t timestamp_lastoutside_orbit; - uint32_t timeout; - uint32_t delay_setpoint; - float accept_range_yaw; - float accept_range_distance; - bool yaw_reached; - bool pos_reached; - bool idle; -}; - -typedef struct mavlink_wpm_storage mavlink_wpm_storage; - -mavlink_wpm_storage wpm; - -bool debug = true; -bool verbose = true; - - -void mavlink_wpm_init(mavlink_wpm_storage* state) -{ - // Set all waypoints to zero - - // Set count to zero - state->size = 0; - state->max_size = MAVLINK_WPM_MAX_WP_COUNT; - state->current_state = MAVLINK_WPM_STATE_IDLE; - state->current_partner_sysid = 0; - state->current_partner_compid = 0; - state->timestamp_lastaction = 0; - state->timestamp_last_send_setpoint = 0; - state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT; - state->delay_setpoint = MAVLINK_WPM_SETPOINT_DELAY_DEFAULT; - state->idle = false; ///< indicates if the system is following the waypoints or is waiting - state->current_active_wp_id = -1; ///< id of current waypoint - state->yaw_reached = false; ///< boolean for yaw attitude reached - state->pos_reached = false; ///< boolean for position reached - state->timestamp_lastoutside_orbit = 0;///< timestamp when the MAV was last outside the orbit or had the wrong yaw value - state->timestamp_firstinside_orbit = 0;///< timestamp when the MAV was the first time after a waypoint change inside the orbit and had the correct yaw value - -} +/* Provide the interface functions for the waypoint manager */ /* * @brief Sends a MAVLink message over UDP */ -void mavlink_wpm_send_message(mavlink_message_t* msg) + +void mavlink_missionlib_send_message(mavlink_message_t* msg) { + uint8_t buf[MAVLINK_MAX_PACKET_LEN]; uint16_t len = mavlink_msg_to_send_buffer(buf, msg); uint16_t bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); - - printf("SENT %d bytes", bytes_sent); + + printf("SENT %d bytes\n", bytes_sent); } -void mavlink_wpm_send_gcs_string(const char* string) +void mavlink_missionlib_send_gcs_string(const char* string) { - printf("%s",string); + printf("%s\n",string); } -uint64_t mavlink_wpm_get_system_timestamp() +uint64_t mavlink_missionlib_get_system_timestamp() { struct timeval tv; gettimeofday(&tv, NULL); return ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; } -/* - * @brief Sends an waypoint ack message - */ -void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_waypoint_ack_t wpa; - - wpa.target_system = wpm.current_partner_sysid; - wpa.target_component = wpm.current_partner_compid; - wpa.type = type; - - mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa); - mavlink_wpm_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) - { - //printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); - mavlink_wpm_send_gcs_string("Sent waypoint ACK"); - } -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if(seq < wpm.size) - { - mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); - - mavlink_message_t msg; - mavlink_waypoint_current_t wpc; - - wpc.seq = cur->seq; - - mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); - mavlink_wpm_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Broadcasted new current waypoint\n"); //printf("Broadcasted new current waypoint %u\n", wpc.seq); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: index out of bounds\n"); - } -} -/* - * @brief Directs the MAV to fly to a position - * - * Sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_setpoint(uint16_t seq) -{ - if(seq < wpm.size) - { - mavlink_waypoint_t *cur = &(wpm.waypoints[seq]); - - mavlink_message_t msg; - mavlink_local_position_setpoint_set_t position_control_set_point; - - // Send new NED or ENU setpoint to onbaord autopilot - if (cur->frame == MAV_FRAME_LOCAL_NED || cur->frame == MAV_FRAME_LOCAL_ENU) - { - position_control_set_point.target_system = mavlink_system.sysid; - position_control_set_point.target_component = MAV_COMP_ID_IMU; - position_control_set_point.x = cur->x; - position_control_set_point.y = cur->y; - position_control_set_point.z = cur->z; - position_control_set_point.yaw = cur->param4; - - mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point); - mavlink_wpm_send_message(&msg); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("No new setpoint set because of invalid coordinate frame of waypoint");//if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); - } - - wpm.timestamp_last_send_setpoint = mavlink_wpm_get_system_timestamp(); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); //if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_waypoint_count_t wpc; - - wpc.target_system = wpm.current_partner_sysid; - wpc.target_component = wpm.current_partner_compid; - wpc.count = count; - - mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); - mavlink_wpm_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) -{ - if (seq < wpm.size) - { - mavlink_message_t msg; - mavlink_waypoint_t *wp = &(wpm.waypoints[seq]); - wp->target_system = wpm.current_partner_sysid; - wp->target_component = wpm.current_partner_compid; - mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); - mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t seq) +void mavlink_wpm_send_message(mavlink_message_t* msg) { - if (seq < wpm.max_size) - { - mavlink_message_t msg; - mavlink_waypoint_request_t wpr; - wpr.target_system = wpm.current_partner_sysid; - wpr.target_component = wpm.current_partner_compid; - wpr.seq = seq; - mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); - mavlink_wpm_send_message(&msg); - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("ERROR: Waypoint index exceeds list capacity\n"); - } + mavlink_missionlib_send_message(msg); } -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) +void mavlink_wpm_send_gcs_string(const char* string) { - mavlink_message_t msg; - mavlink_waypoint_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); - mavlink_wpm_send_message(&msg); - - if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); - - // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); + mavlink_missionlib_send_gcs_string(string); } -//float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -//{ -// if (seq < wpm.size) -// { -// mavlink_waypoint_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// // seq not the second last waypoint -// if ((uint16_t)(seq+1) < wpm.size) -// { -// mavlink_waypoint_t *next = waypoints->at(seq+1); -// const PxVector3 B(next->x, next->y, next->z); -// const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); -// if (r >= 0 && r <= 1) -// { -// const PxVector3 P(A + r*(B-A)); -// return (P-C).length(); -// } -// else if (r < 0.f) -// { -// return (C-A).length(); -// } -// else -// { -// return (C-B).length(); -// } -// } -// else -// { -// return (C-A).length(); -// } -// } -// else -// { -// if (verbose) printf("ERROR: index out of bounds\n"); -// } -// return -1.f; -//} - -float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) +uint64_t mavlink_wpm_get_system_timestamp() { -// if (seq < wpm.size) -// { -// mavlink_waypoint_t *cur = waypoints->at(seq); -// -// const PxVector3 A(cur->x, cur->y, cur->z); -// const PxVector3 C(x, y, z); -// -// return (C-A).length(); -// } -// else -// { -// if (verbose) printf("ERROR: index out of bounds\n"); -// } - return -1.f; + return mavlink_missionlib_get_system_timestamp(); } -static void mavlink_wpm_message_handler(const mavlink_message_t* msg) -{ - // Handle param messages - //paramClient->handleMAVLinkPacket(msg); - - //check for timed-out operations - struct timeval tv; - gettimeofday(&tv, NULL); - uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - if (now-wpm.timestamp_lastaction > wpm.timeout && wpm.current_state != MAVLINK_WPM_STATE_IDLE) - { - if (verbose) printf("Last operation (state=%u) timed out, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_state); - wpm.current_state = MAVLINK_WPM_STATE_IDLE; - wpm.current_count = 0; - wpm.current_partner_sysid = 0; - wpm.current_partner_compid = 0; - wpm.current_wp_id = -1; - - if(wpm.size == 0) - { - wpm.current_active_wp_id = -1; - } - } - - if(now-wpm.timestamp_last_send_setpoint > wpm.delay_setpoint && wpm.current_active_wp_id < wpm.size) - { - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - } - - switch(msg->msgid) - { - case MAVLINK_MSG_ID_ATTITUDE: - { - if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) - { - mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); - if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) - { - mavlink_attitude_t att; - mavlink_msg_attitude_decode(msg, &att); - float yaw_tolerance = wpm.accept_range_yaw; - //compare current yaw - if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) - { - if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) - wpm.yaw_reached = true; - } - else if(att.yaw - yaw_tolerance < 0.0f) - { - float lowerBound = 360.0f + att.yaw - yaw_tolerance; - if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) - wpm.yaw_reached = true; - } - else - { - float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; - if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) - wpm.yaw_reached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_LOCAL_POSITION: - { - if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) - { - mavlink_waypoint_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); - - if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) - { - mavlink_local_position_t pos; - mavlink_msg_local_position_decode(msg, &pos); - //if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); - - wpm.pos_reached = false; - - // compare current position (given in message) with current waypoint - float orbit = wp->param1; - - float dist; - if (wp->param2 == 0) - { - // FIXME segment distance - //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); - } - else - { - dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z); - } - - if (dist >= 0.f && dist <= orbit && wpm.yaw_reached) - { - wpm.pos_reached = true; - } - } - } - break; - } - -// case MAVLINK_MSG_ID_CMD: // special action from ground station -// { -// mavlink_cmd_t action; -// mavlink_msg_cmd_decode(msg, &action); -// if(action.target == mavlink_system.sysid) -// { -// if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; -// switch (action.action) -// { -// // case MAV_ACTION_LAUNCH: -// // if (verbose) std::cerr << "Launch received" << std::endl; -// // current_active_wp_id = 0; -// // if (wpm.size>0) -// // { -// // setActive(waypoints[current_active_wp_id]); -// // } -// // else -// // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; -// // break; -// -// // case MAV_ACTION_CONTINUE: -// // if (verbose) std::c -// // err << "Continue received" << std::endl; -// // idle = false; -// // setActive(waypoints[current_active_wp_id]); -// // break; -// -// // case MAV_ACTION_HALT: -// // if (verbose) std::cerr << "Halt received" << std::endl; -// // idle = true; -// // break; -// -// // default: -// // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; -// // break; -// } -// } -// break; -// } - - case MAVLINK_MSG_ID_WAYPOINT_ACK: - { - mavlink_waypoint_ack_t wpa; - mavlink_msg_waypoint_ack_decode(msg, &wpa); - - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid && wpa.target_component == mavlink_system.compid)) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) - { - if (wpm.current_wp_id == wpm.size-1) - { - if (verbose) printf("Received Ack after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); - wpm.current_state = MAVLINK_WPM_STATE_IDLE; - wpm.current_wp_id = 0; - } - } - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: - { - mavlink_waypoint_set_current_t wpc; - mavlink_msg_waypoint_set_current_decode(msg, &wpc); - - if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) - { - if (wpc.seq < wpm.size) - { - if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); - wpm.current_active_wp_id = wpc.seq; - uint32_t i; - for(i = 0; i < wpm.size; i++) - { - if (i == wpm.current_active_wp_id) - { - wpm.waypoints[i].current = true; - } - else - { - wpm.waypoints[i].current = false; - } - } - if (verbose) printf("New current waypoint %u\n", wpm.current_active_wp_id); - wpm.yaw_reached = false; - wpm.pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - wpm.timestamp_firstinside_orbit = 0; - } - else - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); - } - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); - } - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: - { - mavlink_waypoint_request_list_t wprl; - mavlink_msg_waypoint_request_list_decode(msg, &wprl); - if(wprl.target_system == mavlink_system.sysid && wprl.target_component == mavlink_system.compid) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) - { - if (wpm.size > 0) - { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; - wpm.current_wp_id = 0; - wpm.current_partner_sysid = msg->sysid; - wpm.current_partner_compid = msg->compid; - } - else - { - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); - } - wpm.current_count = wpm.size; - mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); - } - else - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); - } - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); - } - - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: - { - mavlink_waypoint_request_t wpr; - mavlink_msg_waypoint_request_decode(msg, &wpr); - if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) - { - wpm.timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) - { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - - wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; - wpm.current_wp_id = wpr.seq; - mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq); - } - else - { - if (verbose) - { - if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); break; } - else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) - { - if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - } - else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) - { - if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); - else if (wpr.seq >= wpm.size) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); - } - } - } - else - { - //we we're target but already communicating with someone else - if((wpr.target_system == mavlink_system.sysid && wpr.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - } - - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_COUNT: - { - mavlink_waypoint_count_t wpc; - mavlink_msg_waypoint_count_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid && wpc.target_component == mavlink_system.compid) - { - wpm.timestamp_lastaction = now; - - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0)) - { - if (wpc.count > 0) - { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - - wpm.current_state = MAVLINK_WPM_STATE_GETLIST; - wpm.current_wp_id = 0; - wpm.current_partner_sysid = msg->sysid; - wpm.current_partner_compid = msg->compid; - wpm.current_count = wpc.count; - - printf("clearing receive buffer and readying for receiving waypoints\n"); - wpm.rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints_receive_buffer->back(); -// waypoints_receive_buffer->pop_back(); -// } - - mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); - } - else if (wpc.count == 0) - { - printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); - wpm.rcv_size = 0; - //while(waypoints_receive_buffer->size() > 0) -// { -// delete waypoints->back(); -// waypoints->pop_back(); -// } - wpm.current_active_wp_id = -1; - wpm.yaw_reached = false; - wpm.pos_reached = false; - break; - - } - else - { - if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - } - } - else - { - if (verbose && !(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); - else if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); - } - } - else - { - if (verbose) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); - } - - } - break; - - case MAVLINK_MSG_ID_WAYPOINT: - { - mavlink_waypoint_t wp; - mavlink_msg_waypoint_decode(msg, &wp); - - if (verbose) printf("GOT WAYPOINT!"); - - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid)) - { - wpm.timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) - { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); - - wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; - mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); - memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); - - wpm.current_wp_id = wp.seq + 1; - - if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); - - if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) - { - if (verbose) printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); - - mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); - - if (wpm.current_active_wp_id > wpm.rcv_size-1) - { - wpm.current_active_wp_id = wpm.rcv_size-1; - } - - // switch the waypoints list - // FIXME CHECK!!! - for (int i = 0; i < wpm.current_count; ++i) - { - wpm.waypoints[i] = wpm.rcv_waypoints[i]; - } - wpm.size = wpm.current_count; - - //get the new current waypoint - uint32_t i; - for(i = 0; i < wpm.size; i++) - { - if (wpm.waypoints[i].current == 1) - { - wpm.current_active_wp_id = i; - //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); - wpm.yaw_reached = false; - wpm.pos_reached = false; - mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - wpm.timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == wpm.size) - { - wpm.current_active_wp_id = -1; - wpm.yaw_reached = false; - wpm.pos_reached = false; - wpm.timestamp_firstinside_orbit = 0; - } - - wpm.current_state = MAVLINK_WPM_STATE_IDLE; - } - else - { - mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); - } - } - else - { - if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) - { - //we're done receiving waypoints, answer with ack. - mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_WAYPOINT while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); - } - if (verbose) - { - if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; } - else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) - { - if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) - { - if (!(wp.seq == wpm.current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); - else if (!(wp.seq < wpm.current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - } - } - else - { - //we we're target but already communicating with someone else - if((wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); - } - else if(wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_system.compid) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: - { - mavlink_waypoint_clear_all_t wpca; - mavlink_msg_waypoint_clear_all_decode(msg, &wpca); - - if(wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state == MAVLINK_WPM_STATE_IDLE) - { - wpm.timestamp_lastaction = now; - - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - // Delete all waypoints - wpm.size = 0; - wpm.current_active_wp_id = -1; - wpm.yaw_reached = false; - wpm.pos_reached = false; - } - else if (wpca.target_system == mavlink_system.sysid && wpca.target_component == mavlink_system.compid && wpm.current_state != MAVLINK_WPM_STATE_IDLE) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); - } - break; - } - - default: - { - if (debug) printf("Waypoint: received message of unknown type"); - break; - } - } - - //check if the current waypoint was reached - if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle) - { - if (wpm.current_active_wp_id < wpm.size) - { - mavlink_waypoint_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]); - - if (wpm.timestamp_firstinside_orbit == 0) - { - // Announce that last waypoint was reached - if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); - mavlink_wpm_send_waypoint_reached(cur_wp->seq); - wpm.timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000) - { - if (cur_wp->autocontinue) - { - cur_wp->current = 0; - if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1) - { - //the last waypoint was reached, if auto continue is - //activated restart the waypoint list from the beginning - wpm.current_active_wp_id = 1; - } - else - { - if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size) - wpm.current_active_wp_id++; - } - - // Fly to next waypoint - wpm.timestamp_firstinside_orbit = 0; - mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); - mavlink_wpm_send_setpoint(wpm.current_active_wp_id); - wpm.waypoints[wpm.current_active_wp_id].current = true; - wpm.pos_reached = false; - wpm.yaw_reached = false; - if (verbose) printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); - } - } - } - } - else - { - wpm.timestamp_lastoutside_orbit = now; - } -} - - - - - - - -======= ->>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f int main(int argc, char* argv[]) { // Initialize MAVLink mavlink_wpm_init(&wpm); mavlink_system.sysid = 1; - mavlink_system.compid = MAV_COMP_ID_WAYPOINTPLANNER; + mavlink_system.compid = 20; + mavlink_pm_reset_params(&pm); @@ -1131,20 +251,6 @@ int main(int argc, char* argv[]) len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); -<<<<<<< HEAD -// /* Send Local Position */ -// mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), -// position[0], position[1], position[2], -// position[3], position[4], position[5]); -// len = mavlink_msg_to_send_buffer(buf, &msg); -// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); -// -// /* Send attitude */ -// mavlink_msg_attitude_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); -// len = mavlink_msg_to_send_buffer(buf, &msg); -// bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); -// -======= /* Send Local Position */ mavlink_msg_local_position_pack(mavlink_system.sysid, 200, &msg, microsSinceEpoch(), position[0], position[1], position[2], @@ -1157,7 +263,6 @@ int main(int argc, char* argv[]) len = mavlink_msg_to_send_buffer(buf, &msg); bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); ->>>>>>> 26a845613229fbb0cb8bee589c12f477fd30a42f memset(buf, 0, BUFFER_LENGTH); recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); @@ -1181,12 +286,17 @@ int main(int argc, char* argv[]) mavlink_wpm_message_handler(&msg); // Handle packet with parameter component + mavlink_pm_message_handler(MAVLINK_COMM_0, &msg); } } printf("\n"); } memset(buf, 0, BUFFER_LENGTH); - sleep(1); // Sleep one second + usleep(50000); // Sleep one second + + + // Send one parameter + mavlink_pm_queued_send(); } } diff --git a/thirdParty/mavlink/missionlib/testing/mavlink_missionlib_data.h b/thirdParty/mavlink/missionlib/testing/mavlink_missionlib_data.h new file mode 100644 index 0000000000000000000000000000000000000000..a040941e2ccbafcd87b7170cc2534e0b0199d29f --- /dev/null +++ b/thirdParty/mavlink/missionlib/testing/mavlink_missionlib_data.h @@ -0,0 +1,54 @@ +/******************************************************************************* + + Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + + ****************************************************************************/ + +/* This assumes you have the mavlink headers on your include path + or in the same folder as this source file */ + +// Disable auto-data structures +#ifndef MAVLINK_NO_DATA +#define MAVLINK_NO_DATA +#endif + +#include "mavlink.h" +#include + +/* MISSION LIB DATA STORAGE */ + +enum MAVLINK_PM_PARAMETERS +{ + MAVLINK_PM_PARAM_SYSTEM_ID, + MAVLINK_PM_PARAM_ATT_K_D, + MAVLINK_PM_MAX_PARAM_COUNT // VERY IMPORTANT! KEEP THIS AS LAST ENTRY +}; + + +/* DO NOT EDIT THIS FILE BELOW THIS POINT! */ + + +//extern void mavlink_pm_queued_send(); +struct mavlink_pm_storage { + char param_names[MAVLINK_PM_MAX_PARAM_COUNT][MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN]; ///< Parameter names + float param_values[MAVLINK_PM_MAX_PARAM_COUNT]; ///< Parameter values + uint16_t next_param; + uint16_t size; +}; + +typedef struct mavlink_pm_storage mavlink_pm_storage; + +void mavlink_pm_reset_params(mavlink_pm_storage* pm); \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/testing/udp.c b/thirdParty/mavlink/missionlib/testing/udp.c deleted file mode 100644 index dc096be51bd2b38c58240eb833703e90af31c54f..0000000000000000000000000000000000000000 --- a/thirdParty/mavlink/missionlib/testing/udp.c +++ /dev/null @@ -1,1073 +0,0 @@ -/******************************************************************************* - - Copyright (C) 2011 Lorenz Meier lm ( a t ) inf.ethz.ch - and Bryan Godbolt godbolt ( a t ) ualberta.ca - - adapted from example written by Bryan Godbolt godbolt ( a t ) ualberta.ca - - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - - ****************************************************************************/ -/* - This program sends some data to qgroundcontrol using the mavlink protocol. The sent packets - cause qgroundcontrol to respond with heartbeats. Any settings or custom commands sent from - qgroundcontrol are printed by this program along with the heartbeats. - - - I compiled this program sucessfully on Ubuntu 10.04 with the following command - - gcc -I ../../pixhawk/mavlink/include -o udp-server udp.c - - the rt library is needed for the clock_gettime on linux - */ -/* These headers are for QNX, but should all be standard on unix/linux */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if (defined __QNX__) | (defined __QNXNTO__) -/* QNX specific headers */ -#include -#else -/* Linux / MacOS POSIX timer headers */ -#include -#include -#include -#endif - -/* This assumes you have the mavlink headers on your include path - or in the same folder as this source file */ -#include - - -#define BUFFER_LENGTH 2041 // minimum buffer size that can be used with qnx (I don't know why) - -uint64_t microsSinceEpoch(); - - -// FIXME XXX - TO BE MOVED TO XML -enum MAVLINK_WPM_STATES -{ - MAVLINK_WPM_STATE_IDLE = 0, - MAVLINK_WPM_STATE_SENDLIST, - MAVLINK_WPM_STATE_SENDLIST_SENDWPS, - MAVLINK_WPM_STATE_GETLIST, - MAVLINK_WPM_STATE_GETLIST_GETWPS, - MAVLINK_WPM_STATE_GETLIST_GOTALL, - MAVLINK_WPM_STATE_ENUM_END -}; - -enum MAVLINK_WPM_CODES -{ - MAVLINK_WPM_CODE_OK = 0, - MAVLINK_WPM_CODE_ERR_WAYPOINT_ACTION_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_FRAME_NOT_SUPPORTED, - MAVLINK_WPM_CODE_ERR_WAYPOINT_OUT_OF_BOUNDS, - MAVLINK_WPM_CODE_ERR_WAYPOINT_MAX_NUMBER_EXCEEDED, - MAVLINK_WPM_CODE_ENUM_END -}; - - -/* WAYPOINT MANAGER - MISSION LIB */ - -#define MAVLINK_WPM_MAX_WP_COUNT 100 -#define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates -#define MAVLINK_WPM_TEXT_FEEDBACK ///< Report back status information as text -#define MAVLINK_WPM_SYSTEM_ID 1 -#define MAVLINK_WPM_COMPONENT_ID 1 - -struct _mavlink_wpm_storage { - mavlink_waypoint_t waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Currently active waypoints -#ifdef MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE - mavlink_waypoint_t rcv_waypoints[MAVLINK_WPM_MAX_WP_COUNT]; ///< Receive buffer for next waypoints -#endif - uint16_t count; - MAVLINK_WPM_STATES current_state; -} mavlink_wpm_storage; - - -void mavlink_wpm_init(mavlink_wpm_storage* state) -{ - // Set all waypoints to zero - - // Set count to zero - state->count = 0; - state->current_state = MAVLINK_WPM_STATE_IDLE; -} - - -PX_WAYPOINTPLANNER_STATES current_state = PX_WPP_IDLE; -uint16_t protocol_current_wp_id = 0; -uint16_t protocol_current_count = 0; -uint8_t protocol_current_partner_systemid = 0; -uint8_t protocol_current_partner_compid = 0; -uint64_t protocol_timestamp_lastaction = 0; - -uint64_t timestamp_last_send_setpoint = 0; - - -/* - * @brief Sends an waypoint ack message - */ -void mavlink_wpm_send_waypoint_ack(uint8_t target_systemid, uint8_t target_compid, uint8_t type) -{ - mavlink_message_t msg; - mavlink_waypoint_ack_t wpa; - - wpa.target_system = target_systemid; - wpa.target_component = target_compid; - wpa.type = type; - - mavlink_msg_waypoint_ack_encode(systemid, compid, &msg, &wpa); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (verbose) printf("Sent waypoint ack (%u) to ID %u\n", wpa.type, wpa.target_system); -} - -/* - * @brief Broadcasts the new target waypoint and directs the MAV to fly there - * - * This function broadcasts its new active waypoint sequence number and - * sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_waypoint_current(uint16_t seq) -{ - if(seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - mavlink_message_t msg; - mavlink_waypoint_current_t wpc; - - wpc.seq = cur->seq; - - mavlink_msg_waypoint_current_encode(systemid, compid, &msg, &wpc); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - - if (verbose) printf("Broadcasted new current waypoint %u\n", wpc.seq); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -/* - * @brief Directs the MAV to fly to a position - * - * Sends a message to the controller, advising it to fly to the coordinates - * of the waypoint with a given orientation - * - * @param seq The waypoint sequence number the MAV should fly to. - */ -void mavlink_wpm_send_setpoint(uint16_t seq) -{ - if(seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - mavlink_message_t msg; - mavlink_local_position_setpoint_set_t PControlSetPoint; - - // send new set point to local IMU - if (cur->frame == 1) - { - PControlSetPoint.target_system = systemid; - PControlSetPoint.target_component = MAV_COMP_ID_IMU; - PControlSetPoint.x = cur->x; - PControlSetPoint.y = cur->y; - PControlSetPoint.z = cur->z; - PControlSetPoint.yaw = cur->param4; - - mavlink_msg_local_position_setpoint_set_encode(systemid, compid, &msg, &PControlSetPoint); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (verbose) printf("No new set point sent to IMU because the new waypoint %u had no local coordinates\n", cur->seq); - } - - struct timeval tv; - gettimeofday(&tv, NULL); - uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - timestamp_last_send_setpoint = now; - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_count(uint8_t target_systemid, uint8_t target_compid, uint16_t count) -{ - mavlink_message_t msg; - mavlink_waypoint_count_t wpc; - - wpc.target_system = target_systemid; - wpc.target_component = target_compid; - wpc.count = count; - - mavlink_msg_waypoint_count_encode(systemid, compid, &msg, &wpc); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - if (verbose) printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -void mavlink_wpm_send_waypoint(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) -{ - if (seq < waypoints->size()) - { - mavlink_message_t msg; - mavlink_waypoint_t *wp = waypoints->at(seq); - wp->target_system = target_systemid; - wp->target_component = target_compid; - mavlink_msg_waypoint_encode(systemid, compid, &msg, wp); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - if (verbose) printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -void mavlink_wpm_send_waypoint_request(uint8_t target_systemid, uint8_t target_compid, uint16_t seq) -{ - if (seq < waypoints->size()) - { - mavlink_message_t msg; - mavlink_waypoint_request_t wpr; - wpr.target_system = target_systemid; - wpr.target_component = target_compid; - wpr.seq = seq; - mavlink_msg_waypoint_request_encode(systemid, compid, &msg, &wpr); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - if (verbose) printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } -} - -/* - * @brief emits a message that a waypoint reached - * - * This function broadcasts a message that a waypoint is reached. - * - * @param seq The waypoint sequence number the MAV has reached. - */ -void mavlink_wpm_send_waypoint_reached(uint16_t seq) -{ - mavlink_message_t msg; - mavlink_waypoint_reached_t wp_reached; - - wp_reached.seq = seq; - - mavlink_msg_waypoint_reached_encode(systemid, compid, &msg, &wp_reached); - mavlink_message_t_publish(lcm, "MAVLINK", &msg); - - if (verbose) printf("Sent waypoint %u reached message\n", wp_reached.seq); - - usleep(paramClient->getParamValue("PROTOCOLDELAY")); -} - -float mavlink_wpm_distance_to_segment(uint16_t seq, float x, float y, float z) -{ - if (seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, cur->z); - const PxVector3 C(x, y, z); - - // seq not the second last waypoint - if ((uint16_t)(seq+1) < waypoints->size()) - { - mavlink_waypoint_t *next = waypoints->at(seq+1); - const PxVector3 B(next->x, next->y, next->z); - const float r = (B-A).dot(C-A) / (B-A).lengthSquared(); - if (r >= 0 && r <= 1) - { - const PxVector3 P(A + r*(B-A)); - return (P-C).length(); - } - else if (r < 0.f) - { - return (C-A).length(); - } - else - { - return (C-B).length(); - } - } - else - { - return (C-A).length(); - } - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } - return -1.f; -} - -float mavlink_wpm_distance_to_point(uint16_t seq, float x, float y, float z) -{ - if (seq < waypoints->size()) - { - mavlink_waypoint_t *cur = waypoints->at(seq); - - const PxVector3 A(cur->x, cur->y, cur->z); - const PxVector3 C(x, y, z); - - return (C-A).length(); - } - else - { - if (verbose) printf("ERROR: index out of bounds\n"); - } - return -1.f; -} - - -static void mavlink_wpm_mavlink_handler(const lcm_recv_buf_t *rbuf, const char * channel, const mavlink_message_t* msg, void * user) -{ - // Handle param messages - paramClient->handleMAVLinkPacket(msg); - - //check for timed-out operations - struct timeval tv; - gettimeofday(&tv, NULL); - uint64_t now = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - if (now-protocol_timestamp_lastaction > paramClient->getParamValue("PROTOCOLTIMEOUT") && current_state != PX_WPP_IDLE) - { - if (verbose) printf("Last operation (state=%u) timed out, changing state to PX_WPP_IDLE\n", current_state); - current_state = PX_WPP_IDLE; - protocol_current_count = 0; - protocol_current_partner_systemid = 0; - protocol_current_partner_compid = 0; - protocol_current_wp_id = -1; - - if(waypoints->size() == 0) - { - current_active_wp_id = -1; - } - } - - if(now-timestamp_last_send_setpoint > paramClient->getParamValue("SETPOINTDELAY") && current_active_wp_id < waypoints->size()) - { - send_setpoint(current_active_wp_id); - } - - switch(msg->msgid) - { - case MAVLINK_MSG_ID_ATTITUDE: - { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) - { - mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); - if(wp->frame == 1) - { - mavlink_attitude_t att; - mavlink_msg_attitude_decode(msg, &att); - float yaw_tolerance = paramClient->getParamValue("YAWTOLERANCE"); - //compare current yaw - if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) - { - if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) - yawReached = true; - } - else if(att.yaw - yaw_tolerance < 0.0f) - { - float lowerBound = 360.0f + att.yaw - yaw_tolerance; - if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) - yawReached = true; - } - else - { - float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; - if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) - yawReached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_LOCAL_POSITION: - { - if(msg->sysid == systemid && current_active_wp_id < waypoints->size()) - { - mavlink_waypoint_t *wp = waypoints->at(current_active_wp_id); - - if(wp->frame == 1) - { - mavlink_local_position_t pos; - mavlink_msg_local_position_decode(msg, &pos); - if (debug) printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); - - posReached = false; - - // compare current position (given in message) with current waypoint - float orbit = wp->param1; - - float dist; - if (wp->param2 == 0) - { - dist = distanceToSegment(current_active_wp_id, pos.x, pos.y, pos.z); - } - else - { - dist = distanceToPoint(current_active_wp_id, pos.x, pos.y, pos.z); - } - - if (dist >= 0.f && dist <= orbit && yawReached) - { - posReached = true; - } - } - } - break; - } - - case MAVLINK_MSG_ID_CMD: // special action from ground station - { - mavlink_cmd_t action; - mavlink_msg_cmd_decode(msg, &action); - if(action.target == systemid) - { - if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; - switch (action.action) - { - // case MAV_ACTION_LAUNCH: - // if (verbose) std::cerr << "Launch received" << std::endl; - // current_active_wp_id = 0; - // if (waypoints->size()>0) - // { - // setActive(waypoints[current_active_wp_id]); - // } - // else - // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; - // break; - - // case MAV_ACTION_CONTINUE: - // if (verbose) std::c - // err << "Continue received" << std::endl; - // idle = false; - // setActive(waypoints[current_active_wp_id]); - // break; - - // case MAV_ACTION_HALT: - // if (verbose) std::cerr << "Halt received" << std::endl; - // idle = true; - // break; - - // default: - // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; - // break; - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_ACK: - { - mavlink_waypoint_ack_t wpa; - mavlink_msg_waypoint_ack_decode(msg, &wpa); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wpa.target_system == systemid && wpa.target_component == compid)) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS) - { - if (protocol_current_wp_id == waypoints->size()-1) - { - if (verbose) printf("Received Ack after having sent last waypoint, going to state PX_WPP_IDLE\n"); - current_state = PX_WPP_IDLE; - protocol_current_wp_id = 0; - } - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: - { - mavlink_waypoint_set_current_t wpc; - mavlink_msg_waypoint_set_current_decode(msg, &wpc); - - if(wpc.target_system == systemid && wpc.target_component == compid) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE) - { - if (wpc.seq < waypoints->size()) - { - if (verbose) printf("Received MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT\n"); - current_active_wp_id = wpc.seq; - uint32_t i; - for(i = 0; i < waypoints->size(); i++) - { - if (i == current_active_wp_id) - { - waypoints->at(i)->current = true; - } - else - { - waypoints->at(i)->current = false; - } - } - if (verbose) printf("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - } - else - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_SET_CURRENT: Index out of bounds\n"); - } - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST: - { - mavlink_waypoint_request_list_t wprl; - mavlink_msg_waypoint_request_list_decode(msg, &wprl); - if(wprl.target_system == systemid && wprl.target_component == compid) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || current_state == PX_WPP_SENDLIST) - { - if (waypoints->size() > 0) - { - if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to PX_WPP_SENDLIST\n", msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state PX_WPP_SENDLIST\n", msg->sysid); - current_state = PX_WPP_SENDLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - } - else - { - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); - } - protocol_current_count = waypoints->size(); - send_waypoint_count(msg->sysid,msg->compid, protocol_current_count); - } - else - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST because i'm doing something else already (state=%i).\n", current_state); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_REQUEST: - { - mavlink_waypoint_request_t wpr; - mavlink_msg_waypoint_request_decode(msg, &wpr); - if(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid && wpr.target_system == systemid && wpr.target_component == compid) - { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) - if ((current_state == PX_WPP_SENDLIST && wpr.seq == 0) || (current_state == PX_WPP_SENDLIST_SENDWPS && (wpr.seq == protocol_current_wp_id || wpr.seq == protocol_current_wp_id + 1) && wpr.seq < waypoints->size())) - { - if (verbose && current_state == PX_WPP_SENDLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u changing state to PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id + 1) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - if (verbose && current_state == PX_WPP_SENDLIST_SENDWPS && wpr.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST of waypoint %u (again) from %u staying in state PX_WPP_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); - - current_state = PX_WPP_SENDLIST_SENDWPS; - protocol_current_wp_id = wpr.seq; - send_waypoint(protocol_current_partner_systemid, protocol_current_partner_compid, wpr.seq); - } - else - { - if (verbose) - { - if (!(current_state == PX_WPP_SENDLIST || current_state == PX_WPP_SENDLIST_SENDWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because i'm doing something else already (state=%i).\n", current_state); break; } - else if (current_state == PX_WPP_SENDLIST) - { - if (wpr.seq != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); - } - else if (current_state == PX_WPP_SENDLIST_SENDWPS) - { - if (wpr.seq != protocol_current_wp_id && wpr.seq != protocol_current_wp_id + 1) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, protocol_current_wp_id, protocol_current_wp_id+1); - else if (wpr.seq >= waypoints->size()) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); - } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST - FIXME: missed error description\n"); - } - } - } - else - { - //we we're target but already communicating with someone else - if((wpr.target_system == systemid && wpr.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid)) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, protocol_current_partner_systemid); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_COUNT: - { - mavlink_waypoint_count_t wpc; - mavlink_msg_waypoint_count_decode(msg, &wpc); - if(wpc.target_system == systemid && wpc.target_component == compid) - { - protocol_timestamp_lastaction = now; - - if (current_state == PX_WPP_IDLE || (current_state == PX_WPP_GETLIST && protocol_current_wp_id == 0)) - { - if (wpc.count > 0) - { - if (verbose && current_state == PX_WPP_IDLE) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) from %u changing state to PX_WPP_GETLIST\n", wpc.count, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT_COUNT (%u) again from %u\n", wpc.count, msg->sysid); - - current_state = PX_WPP_GETLIST; - protocol_current_wp_id = 0; - protocol_current_partner_systemid = msg->sysid; - protocol_current_partner_compid = msg->compid; - protocol_current_count = wpc.count; - - printf("clearing receive buffer and readying for receiving waypoints\n"); - while(waypoints_receive_buffer->size() > 0) - { - delete waypoints_receive_buffer->back(); - waypoints_receive_buffer->pop_back(); - } - - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } - else if (wpc.count == 0) - { - printf("got waypoint count of 0, clearing waypoint list and staying in state PX_WPP_IDLE\n"); - while(waypoints_receive_buffer->size() > 0) - { - delete waypoints->back(); - waypoints->pop_back(); - } - current_active_wp_id = -1; - yawReached = false; - posReached = false; - break; - - } - else - { - if (verbose) printf("Ignoring MAVLINK_MSG_ID_WAYPOINT_COUNT from %u with count of %u\n", msg->sysid, wpc.count); - } - } - else - { - if (verbose && !(current_state == PX_WPP_IDLE || current_state == PX_WPP_GETLIST)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm doing something else already (state=%i).\n", current_state); - else if (verbose && current_state == PX_WPP_GETLIST && protocol_current_wp_id != 0) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT because i'm already receiving waypoint %u.\n", protocol_current_wp_id); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT_COUNT - FIXME: missed error description\n"); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT: - { - mavlink_waypoint_t wp; - mavlink_msg_waypoint_decode(msg, &wp); - - if((msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && (wp.target_system == systemid && wp.target_component == compid)) - { - protocol_timestamp_lastaction = now; - - //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids - if ((current_state == PX_WPP_GETLIST && wp.seq == 0) || (current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id && wp.seq < protocol_current_count)) - { - if (verbose && current_state == PX_WPP_GETLIST) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to PX_WPP_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); - if (verbose && current_state == PX_WPP_GETLIST_GETWPS && wp.seq-1 == protocol_current_wp_id) printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); - - current_state = PX_WPP_GETLIST_GETWPS; - protocol_current_wp_id = wp.seq + 1; - mavlink_waypoint_t* newwp = new mavlink_waypoint_t; - memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); - waypoints_receive_buffer->push_back(newwp); - - if (verbose) printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); - - if(protocol_current_wp_id == protocol_current_count && current_state == PX_WPP_GETLIST_GETWPS) - { - if (verbose) printf("Got all %u waypoints, changing state to PX_WPP_IDLE\n", protocol_current_count); - - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - - if (current_active_wp_id > waypoints_receive_buffer->size()-1) - { - current_active_wp_id = waypoints_receive_buffer->size() - 1; - } - - // switch the waypoints list - std::vector* waypoints_temp = waypoints; - waypoints = waypoints_receive_buffer; - waypoints_receive_buffer = waypoints_temp; - - //get the new current waypoint - uint32_t i; - for(i = 0; i < waypoints->size(); i++) - { - if (waypoints->at(i)->current == 1) - { - current_active_wp_id = i; - //if (verbose) printf("New current waypoint %u\n", current_active_wp_id); - yawReached = false; - posReached = false; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - timestamp_firstinside_orbit = 0; - break; - } - } - - if (i == waypoints->size()) - { - current_active_wp_id = -1; - yawReached = false; - posReached = false; - timestamp_firstinside_orbit = 0; - } - - current_state = PX_WPP_IDLE; - } - else - { - send_waypoint_request(protocol_current_partner_systemid, protocol_current_partner_compid, protocol_current_wp_id); - } - } - else - { - if (current_state == PX_WPP_IDLE) - { - //we're done receiving waypoints, answer with ack. - send_waypoint_ack(protocol_current_partner_systemid, protocol_current_partner_compid, 0); - printf("Received MAVLINK_MSG_ID_WAYPOINT while state=PX_WPP_IDLE, answered with WAYPOINT_ACK.\n"); - } - if (verbose) - { - if (!(current_state == PX_WPP_GETLIST || current_state == PX_WPP_GETLIST_GETWPS)) { printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u because i'm doing something else already (state=%i).\n", wp.seq, current_state); break; } - else if (current_state == PX_WPP_GETLIST) - { - if(!(wp.seq == 0)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the first waypoint ID (%u) was not 0.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - else if (current_state == PX_WPP_GETLIST_GETWPS) - { - if (!(wp.seq == protocol_current_wp_id)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was not the expected %u.\n", wp.seq, protocol_current_wp_id); - else if (!(wp.seq < protocol_current_count)) printf("Ignored MAVLINK_MSG_ID_WAYPOINT because the waypoint ID (%u) was out of bounds.\n", wp.seq); - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - else printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u - FIXME: missed error description\n", wp.seq); - } - } - } - else - { - //we we're target but already communicating with someone else - if((wp.target_system == systemid && wp.target_component == compid) && !(msg->sysid == protocol_current_partner_systemid && msg->compid == protocol_current_partner_compid) && current_state != PX_WPP_IDLE) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, protocol_current_partner_systemid); - } - else if(wp.target_system == systemid && wp.target_component == compid) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); - } - } - break; - } - - case MAVLINK_MSG_ID_WAYPOINT_CLEAR_ALL: - { - mavlink_waypoint_clear_all_t wpca; - mavlink_msg_waypoint_clear_all_decode(msg, &wpca); - - if(wpca.target_system == systemid && wpca.target_component == compid && current_state == PX_WPP_IDLE) - { - protocol_timestamp_lastaction = now; - - if (verbose) printf("Got MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); - while(waypoints->size() > 0) - { - delete waypoints->back(); - waypoints->pop_back(); - } - current_active_wp_id = -1; - yawReached = false; - posReached = false; - } - else if (wpca.target_system == systemid && wpca.target_component == compid && current_state != PX_WPP_IDLE) - { - if (verbose) printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, current_state); - } - break; - } - - default: - { - if (debug) std::cerr << "Waypoint: received message of unknown type" << std::endl; - break; - } - } - - //check if the current waypoint was reached - if ((posReached && /*yawReached &&*/ !idle)) - { - if (current_active_wp_id < waypoints->size()) - { - mavlink_waypoint_t *cur_wp = waypoints->at(current_active_wp_id); - - if (timestamp_firstinside_orbit == 0) - { - // Announce that last waypoint was reached - if (verbose) printf("*** Reached waypoint %u ***\n", cur_wp->seq); - send_waypoint_reached(cur_wp->seq); - timestamp_firstinside_orbit = now; - } - - // check if the MAV was long enough inside the waypoint orbit - //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) - if(now-timestamp_firstinside_orbit >= cur_wp->param2*1000) - { - if (cur_wp->autocontinue) - { - cur_wp->current = 0; - if (current_active_wp_id == waypoints->size() - 1 && waypoints->size() > 1) - { - //the last waypoint was reached, if auto continue is - //activated restart the waypoint list from the beginning - current_active_wp_id = 1; - } - else - { - if ((uint16_t)(current_active_wp_id + 1) < waypoints->size()) - current_active_wp_id++; - } - - // Fly to next waypoint - timestamp_firstinside_orbit = 0; - send_waypoint_current(current_active_wp_id); - send_setpoint(current_active_wp_id); - waypoints->at(current_active_wp_id)->current = true; - posReached = false; - yawReached = false; - if (verbose) printf("Set new waypoint (%u)\n", current_active_wp_id); - } - } - } - } - else - { - timestamp_lastoutside_orbit = now; - } -} - - - - - - - - -int main(int argc, char* argv[]) -{ - - char help[] = "--help"; - - - char target_ip[100]; - - float position[6] = {}; - int sock = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP); - struct sockaddr_in gcAddr; - struct sockaddr_in locAddr; - //struct sockaddr_in fromAddr; - uint8_t buf[BUFFER_LENGTH]; - ssize_t recsize; - socklen_t fromlen; - int bytes_sent; - mavlink_message_t msg; - uint16_t len; - int i = 0; - //int success = 0; - unsigned int temp = 0; - - // Check if --help flag was used - if ((argc == 2) && (strcmp(argv[1], help) == 0)) - { - printf("\n"); - printf("\tUsage:\n\n"); - printf("\t"); - printf("%s", argv[0]); - printf(" \n"); - printf("\tDefault for localhost: udp-server 127.0.0.1\n\n"); - exit(EXIT_FAILURE); - } - - - // Change the target ip if parameter was given - strcpy(target_ip, "127.0.0.1"); - if (argc == 2) - { - strcpy(target_ip, argv[1]); - } - - - memset(&locAddr, 0, sizeof(locAddr)); - locAddr.sin_family = AF_INET; - locAddr.sin_addr.s_addr = INADDR_ANY; - locAddr.sin_port = htons(14551); - - /* Bind the socket to port 14551 - necessary to receive packets from qgroundcontrol */ - if (-1 == bind(sock,(struct sockaddr *)&locAddr, sizeof(struct sockaddr))) - { - perror("error bind failed"); - close(sock); - exit(EXIT_FAILURE); - } - - /* Attempt to make it non blocking */ - if (fcntl(sock, F_SETFL, O_NONBLOCK | FASYNC) < 0) - { - fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); - close(sock); - exit(EXIT_FAILURE); - } - - - memset(&gcAddr, 0, sizeof(gcAddr)); - gcAddr.sin_family = AF_INET; - gcAddr.sin_addr.s_addr = inet_addr(target_ip); - gcAddr.sin_port = htons(14550); - - - printf("MAVLINK MISSION LIBRARY EXAMPLE PROCESS INITIALIZATION DONE, RUNNING..\n"); - - - for (;;) - { - - /*Send Heartbeat */ - mavlink_msg_heartbeat_pack(1, 200, &msg, MAV_TYPE_HELICOPTER, MAV_CLASS_GENERIC); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - /* Send Status */ - mavlink_msg_sys_status_pack(1, 200, &msg, MAV_MODE_GUIDED, MAV_NAV_HOLD, MAV_STATE_ACTIVE, 500, 7500, 0, 0); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof (struct sockaddr_in)); - - /* Send Local Position */ - mavlink_msg_local_position_pack(1, 200, &msg, microsSinceEpoch(), - position[0], position[1], position[2], - position[3], position[4], position[5]); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - /* Send attitude */ - mavlink_msg_attitude_pack(1, 200, &msg, microsSinceEpoch(), 1.2, 1.7, 3.14, 0.01, 0.02, 0.03); - len = mavlink_msg_to_send_buffer(buf, &msg); - bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in)); - - - memset(buf, 0, BUFFER_LENGTH); - recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen); - if (recsize > 0) - { - // Something received - print out all bytes and parse packet - mavlink_message_t msg; - mavlink_status_t status; - - printf("Bytes Received: %d\nDatagram: ", (int)recsize); - for (i = 0; i < recsize; ++i) - { - temp = buf[i]; - printf("%02x ", (unsigned char)temp); - if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status)) - { - // Packet received - printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); - } - } - printf("\n"); - } - memset(buf, 0, BUFFER_LENGTH); - sleep(1); // Sleep one second - } -} - - -/* QNX timer version */ -#if (defined __QNX__) | (defined __QNXNTO__) -uint64_t microsSinceEpoch() -{ - - struct timespec time; - - uint64_t micros = 0; - - clock_gettime(CLOCK_REALTIME, &time); - micros = (uint64_t)time.tv_sec * 100000 + time.tv_nsec/1000; - - return micros; -} -#else -uint64_t microsSinceEpoch() -{ - - struct timeval tv; - - uint64_t micros = 0; - - gettimeofday(&tv, NULL); - micros = ((uint64_t)tv.tv_sec) * 1000000 + tv.tv_usec; - - return micros; -} -#endif \ No newline at end of file diff --git a/thirdParty/mavlink/missionlib/waypoints.c b/thirdParty/mavlink/missionlib/waypoints.c index d0e7621f758db0741b191abda23d765d5cfd1f50..552fb12f51a83dcca83662aec9ee15af304fc4b2 100644 --- a/thirdParty/mavlink/missionlib/waypoints.c +++ b/thirdParty/mavlink/missionlib/waypoints.c @@ -17,7 +17,7 @@ ****************************************************************************/ -#include +#include "waypoints.h" #include bool debug = true; @@ -33,6 +33,8 @@ extern uint64_t mavlink_wpm_get_system_timestamp(); #define MAVLINK_WPM_NO_PRINTF +uint8_t mavlink_wpm_comp_id = MAV_COMP_ID_WAYPOINTPLANNER; + void mavlink_wpm_init(mavlink_wpm_storage* state) { // Set all waypoints to zero @@ -68,7 +70,7 @@ void mavlink_wpm_send_waypoint_ack(uint8_t sysid, uint8_t compid, uint8_t type) wpa.target_component = wpm.current_partner_compid; wpa.type = type; - mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpa); + mavlink_msg_waypoint_ack_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpa); mavlink_wpm_send_message(&msg); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); @@ -104,7 +106,7 @@ void mavlink_wpm_send_waypoint_current(uint16_t seq) wpc.seq = cur->seq; - mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_msg_waypoint_current_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); mavlink_wpm_send_message(&msg); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); @@ -144,7 +146,7 @@ void mavlink_wpm_send_setpoint(uint16_t seq) position_control_set_point.z = cur->z; position_control_set_point.yaw = cur->param4; - mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &position_control_set_point); + mavlink_msg_local_position_setpoint_set_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &position_control_set_point); mavlink_wpm_send_message(&msg); // FIXME TIMING usleep(paramClient->getParamValue("PROTOCOLDELAY")); @@ -171,7 +173,7 @@ void mavlink_wpm_send_waypoint_count(uint8_t sysid, uint8_t compid, uint16_t cou wpc.target_component = wpm.current_partner_compid; wpc.count = count; - mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpc); + mavlink_msg_waypoint_count_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpc); mavlink_wpm_send_message(&msg); if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint count"); //// if (verbose) // printf("Sent waypoint count (%u) to ID %u\n", wpc.count, wpc.target_system); @@ -187,7 +189,7 @@ void mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t seq) mavlink_waypoint_t *wp = &(wpm.waypoints[seq]); wp->target_system = wpm.current_partner_sysid; wp->target_component = wpm.current_partner_compid; - mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_system.compid, &msg, wp); + mavlink_msg_waypoint_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, wp); mavlink_wpm_send_message(&msg); if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint"); //// if (verbose) // printf("Sent waypoint %u to ID %u\n", wp->seq, wp->target_system); @@ -208,7 +210,7 @@ void mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, uint16_t s wpr.target_system = wpm.current_partner_sysid; wpr.target_component = wpm.current_partner_compid; wpr.seq = seq; - mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wpr); + mavlink_msg_waypoint_request_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wpr); mavlink_wpm_send_message(&msg); if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint request"); //// if (verbose) // printf("Sent waypoint request %u to ID %u\n", wpr.seq, wpr.target_system); @@ -234,7 +236,7 @@ void mavlink_wpm_send_waypoint_reached(uint16_t seq) wp_reached.seq = seq; - mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_system.compid, &msg, &wp_reached); + mavlink_msg_waypoint_reached_encode(mavlink_system.sysid, mavlink_wpm_comp_id, &msg, &wp_reached); mavlink_wpm_send_message(&msg); if (MAVLINK_WPM_TEXT_FEEDBACK) mavlink_wpm_send_gcs_string("Sent waypoint reached message"); //// if (verbose) // printf("Sent waypoint %u reached message\n", wp_reached.seq); @@ -450,7 +452,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_ack_t wpa; mavlink_msg_waypoint_ack_decode(msg, &wpa); - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_system.compid*/)) + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { wpm.timestamp_lastaction = now; @@ -484,7 +486,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_set_current_t wpc; mavlink_msg_waypoint_set_current_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_system.compid*/) + if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; @@ -550,7 +552,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_request_list_t wprl; mavlink_msg_waypoint_request_list_decode(msg, &wprl); - if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_system.compid*/) + if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; @@ -558,8 +560,8 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) { if (wpm.size > 0) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); + //if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); +// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; wpm.current_wp_id = 0; wpm.current_partner_sysid = msg->sysid; @@ -589,7 +591,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_request_t wpr; mavlink_msg_waypoint_request_decode(msg, &wpr); - if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) + if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; @@ -682,7 +684,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) else { //we we're target but already communicating with someone else - if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) + if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_wpm_send_gcs_string("REJ. WP CMD: Busy"); @@ -707,7 +709,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) { mavlink_waypoint_count_t wpc; mavlink_msg_waypoint_count_decode(msg, &wpc); - if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_system.compid*/) + if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; @@ -825,29 +827,29 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_t wp; mavlink_msg_waypoint_decode(msg, &wp); - // if (verbose) // printf("GOT WAYPOINT!"); + mavlink_wpm_send_gcs_string("GOT WP"); - if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/)) + if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) { wpm.timestamp_lastaction = now; //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) { - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); - if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); - +// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); +// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u from %u\n", wp.seq, msg->sysid); +// if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_WAYPOINT %u (again) from %u\n", wp.seq, msg->sysid); +// wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; mavlink_waypoint_t* newwp = &(wpm.rcv_waypoints[wp.seq]); memcpy(newwp, &wp, sizeof(mavlink_waypoint_t)); - - wpm.current_wp_id = wp.seq + 1; + wpm.current_wp_id = wp.seq + 1; // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { + mavlink_wpm_send_gcs_string("GOT ALL WPS"); // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); @@ -948,11 +950,11 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) else { //we we're target but already communicating with someone else - if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_system.compid*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); } - else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_system.compid*/) + else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); } @@ -965,7 +967,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) mavlink_waypoint_clear_all_t wpca; mavlink_msg_waypoint_clear_all_decode(msg, &wpca); - if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE) + if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE) { wpm.timestamp_lastaction = now; @@ -976,7 +978,7 @@ void mavlink_wpm_message_handler(const mavlink_message_t* msg) wpm.yaw_reached = false; wpm.pos_reached = false; } - else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_system.compid */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE) + else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_WAYPOINT_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); } diff --git a/thirdParty/mavlink/missionlib/waypoints.h b/thirdParty/mavlink/missionlib/waypoints.h index 2c3416a821289dcce894cb151bd4e20b584f091e..a0f2aa6d1764cee660b618b7bc36e32c5b92ec1f 100644 --- a/thirdParty/mavlink/missionlib/waypoints.h +++ b/thirdParty/mavlink/missionlib/waypoints.h @@ -19,6 +19,12 @@ /* This assumes you have the mavlink headers on your include path or in the same folder as this source file */ + +// Disable auto-data structures +#ifndef MAVLINK_NO_DATA +#define MAVLINK_NO_DATA +#endif + #include #include @@ -47,11 +53,9 @@ enum MAVLINK_WPM_CODES /* WAYPOINT MANAGER - MISSION LIB */ -#define MAVLINK_WPM_MAX_WP_COUNT 30 +#define MAVLINK_WPM_MAX_WP_COUNT 15 #define MAVLINK_WPM_CONFIG_IN_FLIGHT_UPDATE ///< Enable double buffer and in-flight updates #define MAVLINK_WPM_TEXT_FEEDBACK 1 ///< Report back status information as text -#define MAVLINK_WPM_SYSTEM_ID 1 -#define MAVLINK_WPM_COMPONENT_ID 1 #define MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT 2000000 #define MAVLINK_WPM_SETPOINT_DELAY_DEFAULT 1000000 #define MAVLINK_WPM_PROTOCOL_DELAY_DEFAULT 40