зеркало из https://github.com/microsoft/AirSim.git
update mavlinkcom to support mavlink2 protocol.
This commit is contained in:
Chris Lovett
Родитель
d8e0093009
Коммит
13b95600af
|
|
@ -1410,8 +1410,9 @@ int console(std::stringstream& script) {
|
|||
auto str = std::string(Command::kCommandLogPrefix) + line;
|
||||
MavLinkStatustext st;
|
||||
strncpy(st.text, str.c_str(), 50);
|
||||
MavLinkMessage m;
|
||||
st.encode(m, 0);
|
||||
MavLinkMessage m;
|
||||
st.encode(m);
|
||||
droneConnection->prepareForSending(m);
|
||||
std::lock_guard<std::mutex> lock(logLock);
|
||||
inLogFile->write(m);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -121,6 +121,10 @@ namespace mavlinkcom {
|
|||
//are bridged and you don't want certain connection to read ceratin messages.
|
||||
void ignoreMessage(uint8_t message_id);
|
||||
|
||||
// Compute crc checksums, and pack according to mavlink1 or mavlink2 (depending on what target node supports) and do optional
|
||||
// message signing according to the target node we are communicating with, and return the message length.
|
||||
int prepareForSending(MavLinkMessage& msg);
|
||||
|
||||
protected:
|
||||
void startListening(const std::string& nodeName, std::shared_ptr<Port> connectedPort);
|
||||
|
||||
|
|
|
|||
|
|
@ -16,6 +16,8 @@ namespace mavlinkcom
|
|||
{
|
||||
class MavLinkConnection;
|
||||
|
||||
#define PayloadSize ((255 + 2 + 7) / 8)
|
||||
|
||||
// This is the raw undecoded message, use the msgid field to figure out which strongly typed
|
||||
// MavLinkMessageBase subclass can be used to decode this message. For example, a heartbeat:
|
||||
// MavLinkMessage msg;
|
||||
|
|
@ -26,17 +28,18 @@ namespace mavlinkcom
|
|||
// }
|
||||
class MavLinkMessage {
|
||||
public:
|
||||
// These are the raw packet message fields.
|
||||
// See MavLinkMessage subclasses for the nice unpacked strongly typed fields.
|
||||
uint16_t checksum; ///< sent at end of packet
|
||||
uint8_t magic; ///< protocol magic marker
|
||||
uint8_t len; ///< Length of payload
|
||||
uint8_t seq; ///< Sequence of packet
|
||||
uint8_t sysid; ///< ID of message sender system/aircraft
|
||||
uint8_t compid; ///< ID of the message sender component
|
||||
uint8_t msgid; ///< ID of message in payload
|
||||
uint64_t payload64[(255 + 2 + 7) / 8];
|
||||
|
||||
uint16_t checksum; ///< sent at end of packet
|
||||
uint8_t magic; ///< protocol magic marker
|
||||
uint8_t len; ///< Length of payload
|
||||
uint8_t incompat_flags; ///< flags that must be understood
|
||||
uint8_t compat_flags; ///< flags that can be ignored if not understood
|
||||
uint8_t seq; ///< Sequence of packet
|
||||
uint8_t sysid; ///< ID of message sender system/aircraft
|
||||
uint8_t compid; ///< ID of the message sender component
|
||||
uint32_t msgid : 24; ///< ID of message in payload
|
||||
uint64_t payload64[PayloadSize];
|
||||
uint8_t ck[2]; ///< incoming checksum bytes
|
||||
uint8_t signature[13];
|
||||
};
|
||||
|
||||
// This is the base class for all the strongly typed messages define in MavLinkMessages.hpp
|
||||
|
|
@ -50,9 +53,8 @@ namespace mavlinkcom
|
|||
|
||||
// unpack the given message
|
||||
void decode(const MavLinkMessage& msg);
|
||||
|
||||
// encode this message into given message buffer
|
||||
void encode(MavLinkMessage& msg, int seq) const;
|
||||
// pack this message into given message buffer
|
||||
void encode(MavLinkMessage& msg) const;
|
||||
|
||||
// find what type of message this is and decode it on the heap (call delete when you are done with it).
|
||||
static MavLinkMessageBase* lookup(const MavLinkMessage& msg);
|
||||
|
|
@ -100,7 +102,7 @@ namespace mavlinkcom
|
|||
std::string int16_t_array_tostring(int len, const int16_t* field);
|
||||
std::string uint16_t_array_tostring(int len, const uint16_t* field);
|
||||
std::string float_array_tostring(int len, const float* field);
|
||||
std::string float_tostring(float value);
|
||||
std::string float_tostring(float value);
|
||||
};
|
||||
|
||||
// Base class for all strongly typed MavLinkCommand classes defined in MavLinkMessages.hpp
|
||||
|
|
@ -120,6 +122,7 @@ namespace mavlinkcom
|
|||
float param7 = 0;
|
||||
|
||||
friend class mavlinkcom_impl::MavLinkNodeImpl;
|
||||
friend class mavlinkcom_impl::MavLinkConnectionImpl;
|
||||
};
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -1,10 +1,11 @@
|
|||
#ifdef __cplusplus
|
||||
#pragma once
|
||||
|
||||
#if defined(MAVLINK_USE_CXX_NAMESPACE)
|
||||
namespace mavlink {
|
||||
#elif defined(__cplusplus)
|
||||
extern "C" {
|
||||
#endif
|
||||
|
||||
#ifndef _CHECKSUM_H_
|
||||
#define _CHECKSUM_H_
|
||||
|
||||
// Visual Studio versions before 2010 don't have stdint.h, so we just error out.
|
||||
#if (defined _MSC_VER) && (_MSC_VER < 1600)
|
||||
#error "The C-MAVLink implementation requires Visual Studio 2010 or greater"
|
||||
|
|
@ -89,8 +90,6 @@ static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer
|
|||
}
|
||||
}
|
||||
|
||||
#endif /* _CHECKSUM_H_ */
|
||||
|
||||
#ifdef __cplusplus
|
||||
#if defined(MAVLINK_USE_CXX_NAMESPACE) || defined(__cplusplus)
|
||||
}
|
||||
#endif
|
||||
|
|
|
|||
Различия файлов скрыты, потому что одна или несколько строк слишком длинны
|
|
@ -9,7 +9,7 @@
|
|||
#define MAVLINK_PRIMARY_XML_IDX 1
|
||||
|
||||
#ifndef MAVLINK_STX
|
||||
#define MAVLINK_STX 254
|
||||
#define MAVLINK_STX 253
|
||||
#endif
|
||||
|
||||
#ifndef MAVLINK_ENDIAN
|
||||
|
|
@ -25,7 +25,7 @@
|
|||
#endif
|
||||
|
||||
#ifndef MAVLINK_COMMAND_24BIT
|
||||
#define MAVLINK_COMMAND_24BIT 0
|
||||
#define MAVLINK_COMMAND_24BIT 1
|
||||
#endif
|
||||
|
||||
#include "version.h"
|
||||
|
|
|
|||
Разница между файлами не показана из-за своего большого размера
Загрузить разницу
|
|
@ -7,8 +7,8 @@
|
|||
#ifndef MAVLINK_VERSION_H
|
||||
#define MAVLINK_VERSION_H
|
||||
|
||||
#define MAVLINK_BUILD_DATE "Sun Oct 23 2016"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "1.0"
|
||||
#define MAVLINK_BUILD_DATE "Fri Apr 28 2017"
|
||||
#define MAVLINK_WIRE_PROTOCOL_VERSION "2.0"
|
||||
#define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255
|
||||
|
||||
#endif // MAVLINK_VERSION_H
|
||||
|
|
|
|||
|
|
@ -1,5 +1,4 @@
|
|||
#ifndef _MAVLINK_CONVERSIONS_H_
|
||||
#define _MAVLINK_CONVERSIONS_H_
|
||||
#pragma once
|
||||
|
||||
/* enable math defines on Windows */
|
||||
#ifdef _MSC_VER
|
||||
|
|
@ -208,4 +207,3 @@ MAVLINK_HELPER void mavlink_euler_to_dcm(float roll, float pitch, float yaw, flo
|
|||
dcm[2][2] = cosPhi * cosThe;
|
||||
}
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,15 +1,21 @@
|
|||
#ifndef _MAVLINK_HELPERS_H_
|
||||
#define _MAVLINK_HELPERS_H_
|
||||
#pragma once
|
||||
|
||||
#include "string.h"
|
||||
#include "checksum.h"
|
||||
#include "mavlink_types.h"
|
||||
#include "mavlink_conversions.h"
|
||||
#include <stdio.h>
|
||||
|
||||
#ifndef MAVLINK_HELPER
|
||||
#define MAVLINK_HELPER
|
||||
#endif
|
||||
|
||||
#include "mavlink_sha256.h"
|
||||
|
||||
#ifdef MAVLINK_USE_CXX_NAMESPACE
|
||||
namespace mavlink {
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Internal function to give access to the channel status for each channel
|
||||
*/
|
||||
|
|
@ -41,7 +47,7 @@ MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan)
|
|||
#endif
|
||||
return &m_mavlink_buffer[chan];
|
||||
}
|
||||
#endif
|
||||
#endif // MAVLINK_GET_CHANNEL_BUFFER
|
||||
|
||||
/**
|
||||
* @brief Reset the status of a channel.
|
||||
|
|
@ -52,6 +58,135 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
|
|||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief create a signature block for a packet
|
||||
*/
|
||||
MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing,
|
||||
uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN],
|
||||
const uint8_t *header, uint8_t header_len,
|
||||
const uint8_t *packet, uint8_t packet_len,
|
||||
const uint8_t crc[2])
|
||||
{
|
||||
mavlink_sha256_ctx ctx;
|
||||
union {
|
||||
uint64_t t64;
|
||||
uint8_t t8[8];
|
||||
} tstamp;
|
||||
if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) {
|
||||
return 0;
|
||||
}
|
||||
signature[0] = signing->link_id;
|
||||
tstamp.t64 = signing->timestamp;
|
||||
memcpy(&signature[1], tstamp.t8, 6);
|
||||
signing->timestamp++;
|
||||
|
||||
mavlink_sha256_init(&ctx);
|
||||
mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
|
||||
mavlink_sha256_update(&ctx, header, header_len);
|
||||
mavlink_sha256_update(&ctx, packet, packet_len);
|
||||
mavlink_sha256_update(&ctx, crc, 2);
|
||||
mavlink_sha256_update(&ctx, signature, 7);
|
||||
mavlink_sha256_final_48(&ctx, &signature[7]);
|
||||
|
||||
return MAVLINK_SIGNATURE_BLOCK_LEN;
|
||||
}
|
||||
|
||||
/**
|
||||
* return new packet length for trimming payload of any trailing zero
|
||||
* bytes. Used in MAVLink2 to give simple support for variable length
|
||||
* arrays.
|
||||
*/
|
||||
MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length)
|
||||
{
|
||||
while (length > 1 && payload[length-1] == 0) {
|
||||
length--;
|
||||
}
|
||||
return length;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief check a signature block for a packet
|
||||
*/
|
||||
MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing,
|
||||
mavlink_signing_streams_t *signing_streams,
|
||||
const mavlink_message_t *msg)
|
||||
{
|
||||
if (signing == NULL) {
|
||||
return true;
|
||||
}
|
||||
const uint8_t *p = (const uint8_t *)&msg->magic;
|
||||
const uint8_t *psig = msg->signature;
|
||||
const uint8_t *incoming_signature = psig+7;
|
||||
mavlink_sha256_ctx ctx;
|
||||
uint8_t signature[6];
|
||||
uint16_t i;
|
||||
|
||||
mavlink_sha256_init(&ctx);
|
||||
mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key));
|
||||
mavlink_sha256_update(&ctx, p, MAVLINK_CORE_HEADER_LEN+1+msg->len);
|
||||
mavlink_sha256_update(&ctx, msg->ck, 2);
|
||||
mavlink_sha256_update(&ctx, psig, 1+6);
|
||||
mavlink_sha256_final_48(&ctx, signature);
|
||||
if (memcmp(signature, incoming_signature, 6) != 0) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// now check timestamp
|
||||
union tstamp {
|
||||
uint64_t t64;
|
||||
uint8_t t8[8];
|
||||
} tstamp;
|
||||
uint8_t link_id = psig[0];
|
||||
tstamp.t64 = 0;
|
||||
memcpy(tstamp.t8, psig+1, 6);
|
||||
|
||||
if (signing_streams == NULL) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// find stream
|
||||
for (i=0; i<signing_streams->num_signing_streams; i++) {
|
||||
if (msg->sysid == signing_streams->stream[i].sysid &&
|
||||
msg->compid == signing_streams->stream[i].compid &&
|
||||
link_id == signing_streams->stream[i].link_id) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (i == signing_streams->num_signing_streams) {
|
||||
if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) {
|
||||
// over max number of streams
|
||||
return false;
|
||||
}
|
||||
// new stream. Only accept if timestamp is not more than 1 minute old
|
||||
if (tstamp.t64 + 6000*1000UL < signing->timestamp) {
|
||||
return false;
|
||||
}
|
||||
// add new stream
|
||||
signing_streams->stream[i].sysid = msg->sysid;
|
||||
signing_streams->stream[i].compid = msg->compid;
|
||||
signing_streams->stream[i].link_id = link_id;
|
||||
signing_streams->num_signing_streams++;
|
||||
} else {
|
||||
union tstamp last_tstamp;
|
||||
last_tstamp.t64 = 0;
|
||||
memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6);
|
||||
if (tstamp.t64 <= last_tstamp.t64) {
|
||||
// repeating old timestamp
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// remember last timestamp
|
||||
memcpy(signing_streams->stream[i].timestamp_bytes, psig+1, 6);
|
||||
|
||||
// our next timestamp must be at least this timestamp
|
||||
if (tstamp.t64 > signing->timestamp) {
|
||||
signing->timestamp = tstamp.t64;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* @brief Finalize a MAVLink message with channel assignment
|
||||
*
|
||||
|
|
@ -64,52 +199,87 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan)
|
|||
* @param system_id Id of the sending (this) system, 1-127
|
||||
* @param length Message length
|
||||
*/
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
||||
uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
||||
#else
|
||||
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
||||
uint8_t chan, uint8_t length)
|
||||
#endif
|
||||
MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
||||
mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
||||
{
|
||||
// This is only used for the v2 protocol and we silence it here
|
||||
(void)min_length;
|
||||
// This code part is the same for all messages;
|
||||
msg->magic = MAVLINK_STX;
|
||||
msg->len = length;
|
||||
bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
|
||||
bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
|
||||
uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
|
||||
uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1;
|
||||
uint8_t buf[MAVLINK_CORE_HEADER_LEN+1];
|
||||
if (mavlink1) {
|
||||
msg->magic = MAVLINK_STX_MAVLINK1;
|
||||
header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1;
|
||||
} else {
|
||||
msg->magic = MAVLINK_STX;
|
||||
}
|
||||
msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length);
|
||||
msg->sysid = system_id;
|
||||
msg->compid = component_id;
|
||||
// One sequence number per channel
|
||||
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->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN);
|
||||
msg->incompat_flags = 0;
|
||||
if (signing) {
|
||||
msg->incompat_flags |= MAVLINK_IFLAG_SIGNED;
|
||||
}
|
||||
msg->compat_flags = 0;
|
||||
msg->seq = status->current_tx_seq;
|
||||
status->current_tx_seq = status->current_tx_seq + 1;
|
||||
|
||||
// form the header as a byte array for the crc
|
||||
buf[0] = msg->magic;
|
||||
buf[1] = msg->len;
|
||||
if (mavlink1) {
|
||||
buf[2] = msg->seq;
|
||||
buf[3] = msg->sysid;
|
||||
buf[4] = msg->compid;
|
||||
buf[5] = msg->msgid & 0xFF;
|
||||
} else {
|
||||
buf[2] = msg->incompat_flags;
|
||||
buf[3] = msg->compat_flags;
|
||||
buf[4] = msg->seq;
|
||||
buf[5] = msg->sysid;
|
||||
buf[6] = msg->compid;
|
||||
buf[7] = msg->msgid & 0xFF;
|
||||
buf[8] = (msg->msgid >> 8) & 0xFF;
|
||||
buf[9] = (msg->msgid >> 16) & 0xFF;
|
||||
}
|
||||
|
||||
msg->checksum = crc_calculate(&buf[1], header_len-1);
|
||||
crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len);
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
crc_accumulate(crc_extra, &msg->checksum);
|
||||
#endif
|
||||
mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF);
|
||||
mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8);
|
||||
|
||||
return length + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
if (signing) {
|
||||
mavlink_sign_packet(status->signing,
|
||||
msg->signature,
|
||||
(const uint8_t *)buf, header_len,
|
||||
(const uint8_t *)_MAV_PAYLOAD(msg), msg->len,
|
||||
(const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len);
|
||||
}
|
||||
|
||||
return msg->len + header_len + 2 + signature_len;
|
||||
}
|
||||
|
||||
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
||||
uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
||||
{
|
||||
mavlink_status_t *status = mavlink_get_channel_status(chan);
|
||||
return mavlink_finalize_message_buffer(msg, system_id, component_id, status, min_length, length, crc_extra);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel
|
||||
*/
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
||||
uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
||||
{
|
||||
return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra);
|
||||
}
|
||||
#else
|
||||
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
||||
uint8_t length)
|
||||
|
||||
static inline void _mav_parse_error(mavlink_status_t *status)
|
||||
{
|
||||
return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length);
|
||||
status->parse_error++;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
|
||||
|
|
@ -117,42 +287,78 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf,
|
|||
/**
|
||||
* @brief Finalize a MAVLink message with channel assignment and send
|
||||
*/
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
|
||||
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid,
|
||||
const char *packet,
|
||||
uint8_t min_length, uint8_t length, uint8_t crc_extra)
|
||||
#else
|
||||
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length)
|
||||
#endif
|
||||
{
|
||||
uint16_t checksum;
|
||||
uint8_t buf[MAVLINK_NUM_HEADER_BYTES];
|
||||
uint8_t ck[2];
|
||||
mavlink_status_t *status = mavlink_get_channel_status(chan);
|
||||
buf[0] = MAVLINK_STX;
|
||||
buf[1] = length;
|
||||
buf[2] = status->current_tx_seq;
|
||||
buf[3] = mavlink_system.sysid;
|
||||
buf[4] = mavlink_system.compid;
|
||||
buf[5] = msgid;
|
||||
uint8_t header_len = MAVLINK_CORE_HEADER_LEN;
|
||||
uint8_t signature_len = 0;
|
||||
uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];
|
||||
bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0;
|
||||
bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
|
||||
|
||||
if (mavlink1) {
|
||||
length = min_length;
|
||||
if (msgid > 255) {
|
||||
// can't send 16 bit messages
|
||||
_mav_parse_error(status);
|
||||
return;
|
||||
}
|
||||
header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
|
||||
buf[0] = MAVLINK_STX_MAVLINK1;
|
||||
buf[1] = length;
|
||||
buf[2] = status->current_tx_seq;
|
||||
buf[3] = mavlink_system.sysid;
|
||||
buf[4] = mavlink_system.compid;
|
||||
buf[5] = msgid & 0xFF;
|
||||
} else {
|
||||
uint8_t incompat_flags = 0;
|
||||
if (signing) {
|
||||
incompat_flags |= MAVLINK_IFLAG_SIGNED;
|
||||
}
|
||||
length = _mav_trim_payload(packet, length);
|
||||
buf[0] = MAVLINK_STX;
|
||||
buf[1] = length;
|
||||
buf[2] = incompat_flags;
|
||||
buf[3] = 0; // compat_flags
|
||||
buf[4] = status->current_tx_seq;
|
||||
buf[5] = mavlink_system.sysid;
|
||||
buf[6] = mavlink_system.compid;
|
||||
buf[7] = msgid & 0xFF;
|
||||
buf[8] = (msgid >> 8) & 0xFF;
|
||||
buf[9] = (msgid >> 16) & 0xFF;
|
||||
}
|
||||
status->current_tx_seq++;
|
||||
checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN);
|
||||
checksum = crc_calculate((const uint8_t*)&buf[1], header_len);
|
||||
crc_accumulate_buffer(&checksum, packet, length);
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
crc_accumulate(crc_extra, &checksum);
|
||||
#endif
|
||||
ck[0] = (uint8_t)(checksum & 0xFF);
|
||||
ck[1] = (uint8_t)(checksum >> 8);
|
||||
|
||||
MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
|
||||
_mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES);
|
||||
if (signing) {
|
||||
// possibly add a signature
|
||||
signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1,
|
||||
(const uint8_t *)packet, length, ck);
|
||||
}
|
||||
|
||||
MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
|
||||
_mavlink_send_uart(chan, (const char *)buf, header_len+1);
|
||||
_mavlink_send_uart(chan, packet, length);
|
||||
_mavlink_send_uart(chan, (const char *)ck, 2);
|
||||
MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length);
|
||||
if (signature_len != 0) {
|
||||
_mavlink_send_uart(chan, (const char *)signature, signature_len);
|
||||
}
|
||||
MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief re-send a message over a uart channel
|
||||
* this is more stack efficient than re-marshalling the message
|
||||
* If the message is signed then the original signature is also sent
|
||||
*/
|
||||
MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg)
|
||||
{
|
||||
|
|
@ -162,27 +368,92 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m
|
|||
ck[1] = (uint8_t)(msg->checksum >> 8);
|
||||
// XXX use the right sequence here
|
||||
|
||||
MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
|
||||
_mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES);
|
||||
uint8_t header_len;
|
||||
uint8_t signature_len;
|
||||
|
||||
if (msg->magic == MAVLINK_STX_MAVLINK1) {
|
||||
header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1;
|
||||
signature_len = 0;
|
||||
MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
|
||||
// we can't send the structure directly as it has extra mavlink2 elements in it
|
||||
uint8_t buf[MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1];
|
||||
buf[0] = msg->magic;
|
||||
buf[1] = msg->len;
|
||||
buf[2] = msg->seq;
|
||||
buf[3] = msg->sysid;
|
||||
buf[4] = msg->compid;
|
||||
buf[5] = msg->msgid & 0xFF;
|
||||
_mavlink_send_uart(chan, (const char*)buf, header_len);
|
||||
} else {
|
||||
header_len = MAVLINK_CORE_HEADER_LEN + 1;
|
||||
signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
|
||||
MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
|
||||
uint8_t buf[MAVLINK_CORE_HEADER_LEN + 1];
|
||||
buf[0] = msg->magic;
|
||||
buf[1] = msg->len;
|
||||
buf[2] = msg->incompat_flags;
|
||||
buf[3] = msg->compat_flags;
|
||||
buf[4] = msg->seq;
|
||||
buf[5] = msg->sysid;
|
||||
buf[6] = msg->compid;
|
||||
buf[7] = msg->msgid & 0xFF;
|
||||
buf[8] = (msg->msgid >> 8) & 0xFF;
|
||||
buf[9] = (msg->msgid >> 16) & 0xFF;
|
||||
_mavlink_send_uart(chan, (const char *)buf, header_len);
|
||||
}
|
||||
_mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len);
|
||||
_mavlink_send_uart(chan, (const char *)ck, 2);
|
||||
MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len);
|
||||
if (signature_len != 0) {
|
||||
_mavlink_send_uart(chan, (const char *)msg->signature, MAVLINK_SIGNATURE_BLOCK_LEN);
|
||||
}
|
||||
MAVLINK_END_UART_SEND(chan, header_len + msg->len + 2 + signature_len);
|
||||
}
|
||||
#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
|
||||
/**
|
||||
* @brief Pack a message to send it over a serial byte stream
|
||||
*/
|
||||
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg)
|
||||
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg)
|
||||
{
|
||||
memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
|
||||
|
||||
uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len);
|
||||
|
||||
uint8_t signature_len, header_len;
|
||||
uint8_t *ck;
|
||||
uint8_t length = msg->len;
|
||||
|
||||
if (msg->magic == MAVLINK_STX_MAVLINK1) {
|
||||
signature_len = 0;
|
||||
header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN;
|
||||
buf[0] = msg->magic;
|
||||
buf[1] = length;
|
||||
buf[2] = msg->seq;
|
||||
buf[3] = msg->sysid;
|
||||
buf[4] = msg->compid;
|
||||
buf[5] = msg->msgid & 0xFF;
|
||||
memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len);
|
||||
ck = buf + header_len + 1 + (uint16_t)msg->len;
|
||||
} else {
|
||||
length = _mav_trim_payload(_MAV_PAYLOAD(msg), length);
|
||||
header_len = MAVLINK_CORE_HEADER_LEN;
|
||||
buf[0] = msg->magic;
|
||||
buf[1] = length;
|
||||
buf[2] = msg->incompat_flags;
|
||||
buf[3] = msg->compat_flags;
|
||||
buf[4] = msg->seq;
|
||||
buf[5] = msg->sysid;
|
||||
buf[6] = msg->compid;
|
||||
buf[7] = msg->msgid & 0xFF;
|
||||
buf[8] = (msg->msgid >> 8) & 0xFF;
|
||||
buf[9] = (msg->msgid >> 16) & 0xFF;
|
||||
memcpy(&buf[10], _MAV_PAYLOAD(msg), length);
|
||||
ck = buf + header_len + 1 + (uint16_t)length;
|
||||
signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
|
||||
}
|
||||
ck[0] = (uint8_t)(msg->checksum & 0xFF);
|
||||
ck[1] = (uint8_t)(msg->checksum >> 8);
|
||||
if (signature_len > 0) {
|
||||
memcpy(&ck[2], msg->signature, signature_len);
|
||||
}
|
||||
|
||||
return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len;
|
||||
return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len;
|
||||
}
|
||||
|
||||
union __mavlink_bitfield {
|
||||
|
|
@ -205,8 +476,60 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c)
|
|||
crc_accumulate(c, &msg->checksum);
|
||||
}
|
||||
|
||||
/*
|
||||
return the crc_entry value for a msgid
|
||||
*/
|
||||
#ifndef MAVLINK_GET_MSG_ENTRY
|
||||
MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid)
|
||||
{
|
||||
static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS;
|
||||
/*
|
||||
use a bisection search to find the right entry. A perfect hash may be better
|
||||
Note that this assumes the table is sorted by msgid
|
||||
*/
|
||||
uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]);
|
||||
while (low < high) {
|
||||
uint32_t mid = (low+1+high)/2;
|
||||
if (msgid < mavlink_message_crcs[mid].msgid) {
|
||||
high = mid-1;
|
||||
continue;
|
||||
}
|
||||
if (msgid > mavlink_message_crcs[mid].msgid) {
|
||||
low = mid;
|
||||
continue;
|
||||
}
|
||||
low = mid;
|
||||
break;
|
||||
}
|
||||
if (mavlink_message_crcs[low].msgid != msgid) {
|
||||
// msgid is not in the table
|
||||
return NULL;
|
||||
}
|
||||
return &mavlink_message_crcs[low];
|
||||
}
|
||||
#endif // MAVLINK_GET_MSG_ENTRY
|
||||
|
||||
/*
|
||||
return the crc_extra value for a message
|
||||
*/
|
||||
MAVLINK_HELPER uint8_t mavlink_get_crc_extra(const mavlink_message_t *msg)
|
||||
{
|
||||
const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
|
||||
return e?e->crc_extra:0;
|
||||
}
|
||||
|
||||
/*
|
||||
return the expected message length
|
||||
*/
|
||||
#define MAVLINK_HAVE_EXPECTED_MESSAGE_LENGTH
|
||||
MAVLINK_HELPER uint8_t mavlink_expected_message_length(const mavlink_message_t *msg)
|
||||
{
|
||||
const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid);
|
||||
return e?e->msg_len:0;
|
||||
}
|
||||
|
||||
/**
|
||||
* This is a variant of mavlink_frame_char() but with caller supplied
|
||||
* This is a varient of mavlink_frame_char() but with caller supplied
|
||||
* parsing buffers. It is useful when you want to create a MAVLink
|
||||
* parser in a library that doesn't use any global variables
|
||||
*
|
||||
|
|
@ -245,17 +568,6 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
|||
mavlink_message_t* r_message,
|
||||
mavlink_status_t* r_mavlink_status)
|
||||
{
|
||||
/*
|
||||
default message crc function. You can override this per-system to
|
||||
put this data in a different memory segment
|
||||
*/
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
#ifndef MAVLINK_MESSAGE_CRC
|
||||
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
|
||||
#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid]
|
||||
#endif
|
||||
#endif
|
||||
|
||||
/* Enable this option to check the length of each message.
|
||||
This allows invalid messages to be caught much sooner. Use if the transmission
|
||||
medium is prone to missing (or extra) characters (e.g. a radio that fades in
|
||||
|
|
@ -282,6 +594,14 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
|||
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
|
||||
rxmsg->len = 0;
|
||||
rxmsg->magic = c;
|
||||
status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1;
|
||||
mavlink_start_checksum(rxmsg);
|
||||
} else if (c == MAVLINK_STX_MAVLINK1)
|
||||
{
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_STX;
|
||||
rxmsg->len = 0;
|
||||
rxmsg->magic = c;
|
||||
status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1;
|
||||
mavlink_start_checksum(rxmsg);
|
||||
}
|
||||
break;
|
||||
|
|
@ -296,7 +616,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
|||
)
|
||||
{
|
||||
status->buffer_overrun++;
|
||||
status->parse_error++;
|
||||
_mav_parse_error(status);
|
||||
status->msg_received = 0;
|
||||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
}
|
||||
|
|
@ -306,16 +626,41 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
|||
rxmsg->len = c;
|
||||
status->packet_idx = 0;
|
||||
mavlink_update_checksum(rxmsg, c);
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
|
||||
if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
|
||||
rxmsg->incompat_flags = 0;
|
||||
rxmsg->compat_flags = 0;
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
|
||||
} else {
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH;
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_LENGTH:
|
||||
rxmsg->incompat_flags = c;
|
||||
if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) {
|
||||
// message includes an incompatible feature flag
|
||||
_mav_parse_error(status);
|
||||
status->msg_received = 0;
|
||||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
mavlink_update_checksum(rxmsg, c);
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS;
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS:
|
||||
rxmsg->compat_flags = c;
|
||||
mavlink_update_checksum(rxmsg, c);
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS;
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS:
|
||||
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);
|
||||
|
|
@ -325,31 +670,48 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
|||
case MAVLINK_PARSE_STATE_GOT_SYSID:
|
||||
rxmsg->compid = c;
|
||||
mavlink_update_checksum(rxmsg, c);
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID;
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_COMPID:
|
||||
#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
|
||||
if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c))
|
||||
{
|
||||
status->parse_error++;
|
||||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
rxmsg->msgid = c;
|
||||
mavlink_update_checksum(rxmsg, c);
|
||||
if (rxmsg->len == 0)
|
||||
{
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD;
|
||||
}
|
||||
else
|
||||
{
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID;
|
||||
}
|
||||
if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) {
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
|
||||
#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
|
||||
if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid))
|
||||
{
|
||||
_mav_parse_error(status);
|
||||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
} else {
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1;
|
||||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID:
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID1:
|
||||
rxmsg->msgid |= c<<8;
|
||||
mavlink_update_checksum(rxmsg, c);
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2;
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID2:
|
||||
rxmsg->msgid |= c<<16;
|
||||
mavlink_update_checksum(rxmsg, c);
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3;
|
||||
#ifdef MAVLINK_CHECK_MESSAGE_LENGTH
|
||||
if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(rxmsg->msgid))
|
||||
{
|
||||
_mav_parse_error(status);
|
||||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_MSGID3:
|
||||
_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c;
|
||||
mavlink_update_checksum(rxmsg, c);
|
||||
if (status->packet_idx == rxmsg->len)
|
||||
|
|
@ -358,17 +720,23 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
|||
}
|
||||
break;
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_PAYLOAD:
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid));
|
||||
#endif
|
||||
case MAVLINK_PARSE_STATE_GOT_PAYLOAD: {
|
||||
const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid);
|
||||
uint8_t crc_extra = e?e->crc_extra:0;
|
||||
mavlink_update_checksum(rxmsg, crc_extra);
|
||||
if (c != (rxmsg->checksum & 0xFF)) {
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1;
|
||||
} else {
|
||||
status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1;
|
||||
}
|
||||
_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c;
|
||||
rxmsg->ck[0] = c;
|
||||
|
||||
// zero-fill the packet to cope with short incoming packets
|
||||
if (e && status->packet_idx < e->msg_len) {
|
||||
memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->msg_len - status->packet_idx);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case MAVLINK_PARSE_STATE_GOT_CRC1:
|
||||
case MAVLINK_PARSE_STATE_GOT_BAD_CRC1:
|
||||
|
|
@ -378,15 +746,57 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
|||
} else {
|
||||
// Successfully got message
|
||||
status->msg_received = MAVLINK_FRAMING_OK;
|
||||
}
|
||||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c;
|
||||
memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
|
||||
}
|
||||
rxmsg->ck[1] = c;
|
||||
|
||||
if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) {
|
||||
status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT;
|
||||
status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN;
|
||||
|
||||
// If the CRC is already wrong, don't overwrite msg_received,
|
||||
// otherwise we can end up with garbage flagged as valid.
|
||||
if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {
|
||||
status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
|
||||
}
|
||||
} else {
|
||||
if (status->signing &&
|
||||
(status->signing->accept_unsigned_callback == NULL ||
|
||||
!status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
|
||||
|
||||
// If the CRC is already wrong, don't overwrite msg_received.
|
||||
if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) {
|
||||
status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
|
||||
}
|
||||
}
|
||||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
|
||||
}
|
||||
break;
|
||||
case MAVLINK_PARSE_STATE_SIGNATURE_WAIT:
|
||||
rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c;
|
||||
status->signature_wait--;
|
||||
if (status->signature_wait == 0) {
|
||||
// we have the whole signature, check it is OK
|
||||
bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg);
|
||||
if (!sig_ok &&
|
||||
(status->signing->accept_unsigned_callback &&
|
||||
status->signing->accept_unsigned_callback(status, rxmsg->msgid))) {
|
||||
// accepted via application level override
|
||||
sig_ok = true;
|
||||
}
|
||||
if (sig_ok) {
|
||||
status->msg_received = MAVLINK_FRAMING_OK;
|
||||
} else {
|
||||
status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE;
|
||||
}
|
||||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
memcpy(r_message, rxmsg, sizeof(mavlink_message_t));
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
bufferIndex++;
|
||||
// If a message has been successfully decoded, check index
|
||||
// If a message has been sucessfully decoded, check index
|
||||
if (status->msg_received == MAVLINK_FRAMING_OK)
|
||||
{
|
||||
//while(status->current_seq != rxmsg->seq)
|
||||
|
|
@ -407,6 +817,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
|||
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;
|
||||
r_mavlink_status->flags = status->flags;
|
||||
status->parse_error = 0;
|
||||
|
||||
if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) {
|
||||
|
|
@ -417,7 +828,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg,
|
|||
mavlink_msg_to_send_buffer() won't overwrite the
|
||||
checksum
|
||||
*/
|
||||
r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8);
|
||||
r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8);
|
||||
}
|
||||
|
||||
return status->msg_received;
|
||||
|
|
@ -473,6 +884,33 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa
|
|||
r_mavlink_status);
|
||||
}
|
||||
|
||||
/**
|
||||
* Set the protocol version
|
||||
*/
|
||||
MAVLINK_HELPER void mavlink_set_proto_version(uint8_t chan, unsigned int version)
|
||||
{
|
||||
mavlink_status_t *status = mavlink_get_channel_status(chan);
|
||||
if (version > 1) {
|
||||
status->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1);
|
||||
} else {
|
||||
status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Get the protocol version
|
||||
*
|
||||
* @return 1 for v1, 2 for v2
|
||||
*/
|
||||
MAVLINK_HELPER unsigned int mavlink_get_proto_version(uint8_t chan)
|
||||
{
|
||||
mavlink_status_t *status = mavlink_get_channel_status(chan);
|
||||
if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) > 0) {
|
||||
return 1;
|
||||
} else {
|
||||
return 2;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* This is a convenience function which handles the complete MAVLink parsing.
|
||||
|
|
@ -517,11 +955,12 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa
|
|||
MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status)
|
||||
{
|
||||
uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status);
|
||||
if (msg_received == MAVLINK_FRAMING_BAD_CRC) {
|
||||
if (msg_received == MAVLINK_FRAMING_BAD_CRC ||
|
||||
msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) {
|
||||
// we got a bad CRC. Treat as a parse failure
|
||||
mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan);
|
||||
mavlink_status_t* status = mavlink_get_channel_status(chan);
|
||||
status->parse_error++;
|
||||
_mav_parse_error(status);
|
||||
status->msg_received = MAVLINK_FRAMING_INCOMPLETE;
|
||||
status->parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
if (c == MAVLINK_STX)
|
||||
|
|
@ -675,4 +1114,6 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf,
|
|||
}
|
||||
#endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
|
||||
#endif /* _MAVLINK_HELPERS_H_ */
|
||||
#ifdef MAVLINK_USE_CXX_NAMESPACE
|
||||
} // namespace mavlink
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,13 +1,17 @@
|
|||
#ifndef MAVLINK_TYPES_H_
|
||||
#define MAVLINK_TYPES_H_
|
||||
#pragma once
|
||||
|
||||
// Visual Studio versions before 2010 don't have stdint.h, so we just error out.
|
||||
#if (defined _MSC_VER) && (_MSC_VER < 1600)
|
||||
#error "The C-MAVLink implementation requires Visual Studio 2010 or greater"
|
||||
#endif
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#ifdef MAVLINK_USE_CXX_NAMESPACE
|
||||
namespace mavlink {
|
||||
#endif
|
||||
|
||||
// Macro to define packed structures
|
||||
#ifdef __GNUC__
|
||||
#define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed))
|
||||
|
|
@ -20,26 +24,15 @@
|
|||
#define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length
|
||||
#endif
|
||||
|
||||
#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte)
|
||||
#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum
|
||||
#define MAVLINK_CORE_HEADER_LEN 9 ///< Length of core header (of the comm. layer)
|
||||
#define MAVLINK_CORE_HEADER_MAVLINK1_LEN 5 ///< Length of MAVLink1 core header (of the comm. layer)
|
||||
#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and stx
|
||||
#define MAVLINK_NUM_CHECKSUM_BYTES 2
|
||||
#define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES)
|
||||
|
||||
#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length
|
||||
|
||||
#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255
|
||||
#define MAVLINK_EXTENDED_HEADER_LEN 14
|
||||
|
||||
#if (defined _MSC_VER) || ((defined __APPLE__) && (defined __MACH__)) || (defined __linux__)
|
||||
/* full fledged 32bit++ OS */
|
||||
#define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507
|
||||
#else
|
||||
/* small microcontrollers */
|
||||
#define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048
|
||||
#endif
|
||||
|
||||
#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES)
|
||||
#define MAVLINK_SIGNATURE_BLOCK_LEN 13
|
||||
|
||||
#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN) ///< Maximum packet length
|
||||
|
||||
/**
|
||||
* Old-style 4 byte param union
|
||||
|
|
@ -113,23 +106,20 @@ typedef struct __mavlink_system {
|
|||
|
||||
MAVPACKED(
|
||||
typedef struct __mavlink_message {
|
||||
uint16_t checksum; ///< sent at end of packet
|
||||
uint8_t magic; ///< protocol magic marker
|
||||
uint8_t len; ///< Length of payload
|
||||
uint8_t seq; ///< Sequence of packet
|
||||
uint8_t sysid; ///< ID of message sender system/aircraft
|
||||
uint8_t compid; ///< ID of the message sender component
|
||||
uint8_t msgid; ///< ID of message in payload
|
||||
uint16_t checksum; ///< sent at end of packet
|
||||
uint8_t magic; ///< protocol magic marker
|
||||
uint8_t len; ///< Length of payload
|
||||
uint8_t incompat_flags; ///< flags that must be understood
|
||||
uint8_t compat_flags; ///< flags that can be ignored if not understood
|
||||
uint8_t seq; ///< Sequence of packet
|
||||
uint8_t sysid; ///< ID of message sender system/aircraft
|
||||
uint8_t compid; ///< ID of the message sender component
|
||||
uint32_t msgid:24; ///< ID of message in payload
|
||||
uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8];
|
||||
uint8_t ck[2]; ///< incoming checksum bytes
|
||||
uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN];
|
||||
}) mavlink_message_t;
|
||||
|
||||
MAVPACKED(
|
||||
typedef struct __mavlink_extended_message {
|
||||
mavlink_message_t base_msg;
|
||||
int32_t extended_payload_len; ///< Length of extended payload if any
|
||||
uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN];
|
||||
}) mavlink_extended_message_t;
|
||||
|
||||
typedef enum {
|
||||
MAVLINK_TYPE_CHAR = 0,
|
||||
MAVLINK_TYPE_UINT8_T = 1,
|
||||
|
|
@ -147,7 +137,7 @@ typedef enum {
|
|||
#define MAVLINK_MAX_FIELDS 64
|
||||
|
||||
typedef struct __mavlink_field_info {
|
||||
const char *name; // name of this field
|
||||
const char *name; // name of this field
|
||||
const char *print_format; // printing format hint, or NULL
|
||||
mavlink_message_type_t type; // type of this field
|
||||
unsigned int array_length; // if non-zero, field is an array
|
||||
|
|
@ -158,6 +148,7 @@ typedef struct __mavlink_field_info {
|
|||
// note that in this structure the order of fields is the order
|
||||
// in the XML file, not necessary the wire order
|
||||
typedef struct __mavlink_message_info {
|
||||
uint32_t msgid; // message ID
|
||||
const char *name; // name of the message
|
||||
unsigned num_fields; // how many fields in this message
|
||||
mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information
|
||||
|
|
@ -194,22 +185,35 @@ typedef enum {
|
|||
MAVLINK_PARSE_STATE_UNINIT=0,
|
||||
MAVLINK_PARSE_STATE_IDLE,
|
||||
MAVLINK_PARSE_STATE_GOT_STX,
|
||||
MAVLINK_PARSE_STATE_GOT_SEQ,
|
||||
MAVLINK_PARSE_STATE_GOT_LENGTH,
|
||||
MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS,
|
||||
MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS,
|
||||
MAVLINK_PARSE_STATE_GOT_SEQ,
|
||||
MAVLINK_PARSE_STATE_GOT_SYSID,
|
||||
MAVLINK_PARSE_STATE_GOT_COMPID,
|
||||
MAVLINK_PARSE_STATE_GOT_MSGID,
|
||||
MAVLINK_PARSE_STATE_GOT_MSGID1,
|
||||
MAVLINK_PARSE_STATE_GOT_MSGID2,
|
||||
MAVLINK_PARSE_STATE_GOT_MSGID3,
|
||||
MAVLINK_PARSE_STATE_GOT_PAYLOAD,
|
||||
MAVLINK_PARSE_STATE_GOT_CRC1,
|
||||
MAVLINK_PARSE_STATE_GOT_BAD_CRC1
|
||||
MAVLINK_PARSE_STATE_GOT_BAD_CRC1,
|
||||
MAVLINK_PARSE_STATE_SIGNATURE_WAIT
|
||||
} mavlink_parse_state_t; ///< The state machine for the comm parser
|
||||
|
||||
typedef enum {
|
||||
MAVLINK_FRAMING_INCOMPLETE=0,
|
||||
MAVLINK_FRAMING_OK=1,
|
||||
MAVLINK_FRAMING_BAD_CRC=2
|
||||
MAVLINK_FRAMING_BAD_CRC=2,
|
||||
MAVLINK_FRAMING_BAD_SIGNATURE=3
|
||||
} mavlink_framing_t;
|
||||
|
||||
#define MAVLINK_STATUS_FLAG_IN_MAVLINK1 1 // last incoming packet was MAVLink1
|
||||
#define MAVLINK_STATUS_FLAG_OUT_MAVLINK1 2 // generate MAVLink1 by default
|
||||
#define MAVLINK_STATUS_FLAG_IN_SIGNED 4 // last incoming packet was signed and validated
|
||||
#define MAVLINK_STATUS_FLAG_IN_BADSIG 8 // last incoming packet had a bad signature
|
||||
|
||||
#define MAVLINK_STX_MAVLINK1 0xFE // marker for old protocol
|
||||
|
||||
typedef struct __mavlink_status {
|
||||
uint8_t msg_received; ///< Number of received messages
|
||||
uint8_t buffer_overrun; ///< Number of buffer overruns
|
||||
|
|
@ -220,9 +224,75 @@ typedef struct __mavlink_status {
|
|||
uint8_t current_tx_seq; ///< Sequence number of last packet sent
|
||||
uint16_t packet_rx_success_count; ///< Received packets
|
||||
uint16_t packet_rx_drop_count; ///< Number of packet drops
|
||||
uint8_t flags; ///< MAVLINK_STATUS_FLAG_*
|
||||
uint8_t signature_wait; ///< number of signature bytes left to receive
|
||||
struct __mavlink_signing *signing; ///< optional signing state
|
||||
struct __mavlink_signing_streams *signing_streams; ///< global record of stream timestamps
|
||||
} mavlink_status_t;
|
||||
|
||||
/*
|
||||
a callback function to allow for accepting unsigned packets
|
||||
*/
|
||||
typedef bool (*mavlink_accept_unsigned_t)(const mavlink_status_t *status, uint32_t msgid);
|
||||
|
||||
/*
|
||||
flags controlling signing
|
||||
*/
|
||||
#define MAVLINK_SIGNING_FLAG_SIGN_OUTGOING 1
|
||||
|
||||
/*
|
||||
state of MAVLink signing for this channel
|
||||
*/
|
||||
typedef struct __mavlink_signing {
|
||||
uint8_t flags; ///< MAVLINK_SIGNING_FLAG_*
|
||||
uint8_t link_id;
|
||||
uint64_t timestamp;
|
||||
uint8_t secret_key[32];
|
||||
mavlink_accept_unsigned_t accept_unsigned_callback;
|
||||
} mavlink_signing_t;
|
||||
|
||||
/*
|
||||
timestamp state of each logical signing stream. This needs to be the same structure for all
|
||||
connections in order to be secure
|
||||
*/
|
||||
#ifndef MAVLINK_MAX_SIGNING_STREAMS
|
||||
#define MAVLINK_MAX_SIGNING_STREAMS 16
|
||||
#endif
|
||||
typedef struct __mavlink_signing_streams {
|
||||
uint16_t num_signing_streams;
|
||||
struct __mavlink_signing_stream {
|
||||
uint8_t link_id;
|
||||
uint8_t sysid;
|
||||
uint8_t compid;
|
||||
uint8_t timestamp_bytes[6];
|
||||
} stream[MAVLINK_MAX_SIGNING_STREAMS];
|
||||
} mavlink_signing_streams_t;
|
||||
|
||||
|
||||
#define MAVLINK_BIG_ENDIAN 0
|
||||
#define MAVLINK_LITTLE_ENDIAN 1
|
||||
|
||||
#endif /* MAVLINK_TYPES_H_ */
|
||||
#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM 1
|
||||
#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT 2
|
||||
|
||||
/*
|
||||
entry in table of information about each message type
|
||||
*/
|
||||
typedef struct __mavlink_msg_entry {
|
||||
uint32_t msgid;
|
||||
uint8_t crc_extra;
|
||||
uint8_t msg_len;
|
||||
uint8_t flags; // MAV_MSG_ENTRY_FLAG_*
|
||||
uint8_t target_system_ofs; // payload offset to target_system, or 0
|
||||
uint8_t target_component_ofs; // payload offset to target_component, or 0
|
||||
} mavlink_msg_entry_t;
|
||||
|
||||
/*
|
||||
incompat_flags bits
|
||||
*/
|
||||
#define MAVLINK_IFLAG_SIGNED 0x01
|
||||
#define MAVLINK_IFLAG_MASK 0x01 // mask of all understood bits
|
||||
|
||||
#ifdef MAVLINK_USE_CXX_NAMESPACE
|
||||
} // namespace mavlink
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,5 +1,4 @@
|
|||
#ifndef _MAVLINK_PROTOCOL_H_
|
||||
#define _MAVLINK_PROTOCOL_H_
|
||||
#pragma once
|
||||
|
||||
#include "string.h"
|
||||
#include "mavlink_types.h"
|
||||
|
|
@ -44,24 +43,14 @@
|
|||
MAVLINK_HELPER mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
|
||||
#endif
|
||||
MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan);
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
||||
uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra);
|
||||
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
||||
uint8_t min_length, uint8_t length, uint8_t crc_extra);
|
||||
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet,
|
||||
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid, const char *packet,
|
||||
uint8_t min_length, uint8_t length, uint8_t crc_extra);
|
||||
#endif
|
||||
#else
|
||||
MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
||||
uint8_t chan, uint8_t length);
|
||||
MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id,
|
||||
uint8_t length);
|
||||
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length);
|
||||
#endif
|
||||
#endif // MAVLINK_CRC_EXTRA
|
||||
MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg);
|
||||
MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg);
|
||||
MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c);
|
||||
|
|
@ -74,6 +63,7 @@
|
|||
MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status);
|
||||
MAVLINK_HELPER uint8_t put_bitfield_n_by_index(int32_t b, uint8_t bits, uint8_t packet_index, uint8_t bit_index,
|
||||
uint8_t* r_bit_index, uint8_t* buffer);
|
||||
MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid);
|
||||
#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS
|
||||
MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len);
|
||||
MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg);
|
||||
|
|
@ -86,12 +76,17 @@
|
|||
|
||||
#endif // MAVLINK_SEPARATE_HELPERS
|
||||
|
||||
|
||||
/**
|
||||
* @brief Get the required buffer size for this message
|
||||
*/
|
||||
static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg)
|
||||
{
|
||||
return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
if (msg->magic == MAVLINK_STX_MAVLINK1) {
|
||||
return msg->len + MAVLINK_CORE_HEADER_MAVLINK1_LEN+1 + 2;
|
||||
}
|
||||
uint16_t signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0;
|
||||
return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES + signature_len;
|
||||
}
|
||||
|
||||
#if MAVLINK_NEED_BYTE_SWAP
|
||||
|
|
@ -336,4 +331,4 @@ _MAV_RETURN_ARRAY(int64_t, i64)
|
|||
_MAV_RETURN_ARRAY(float, f)
|
||||
_MAV_RETURN_ARRAY(double, d)
|
||||
|
||||
#endif // _MAVLINK_PROTOCOL_H_
|
||||
|
||||
|
|
|
|||
|
|
@ -12,11 +12,6 @@ MavLinkConnection::MavLinkConnection()
|
|||
pImpl.reset(new MavLinkConnectionImpl());
|
||||
}
|
||||
|
||||
std::vector<SerialPortInfo> MavLinkConnection::findSerialPorts(int vid, int pid)
|
||||
{
|
||||
return MavLinkConnectionImpl::findSerialPorts(vid, pid);
|
||||
}
|
||||
|
||||
std::shared_ptr<MavLinkConnection> MavLinkConnection::connectSerial(const std::string& nodeName, std::string portName, int baudrate, const std::string initString)
|
||||
{
|
||||
return MavLinkConnectionImpl::connectSerial(nodeName, portName, baudrate, initString);
|
||||
|
|
@ -68,7 +63,10 @@ void MavLinkConnection::ignoreMessage(uint8_t message_id)
|
|||
{
|
||||
pImpl->ignoreMessage(message_id);
|
||||
}
|
||||
|
||||
int MavLinkConnection::prepareForSending(MavLinkMessage& msg)
|
||||
{
|
||||
return pImpl->prepareForSending(msg);
|
||||
}
|
||||
|
||||
std::string MavLinkConnection::getName()
|
||||
{
|
||||
|
|
|
|||
|
|
@ -110,6 +110,7 @@ void MavLinkFileLog::write(const mavlinkcom::MavLinkMessage& msg, uint64_t times
|
|||
timestamp = getTimeStamp();
|
||||
}
|
||||
// for compatibility with QGroundControl we have to save the time field in big endian.
|
||||
// todo: mavlink2 support?
|
||||
timestamp = FlipEndianness(timestamp);
|
||||
fwrite(×tamp, sizeof(uint64_t), 1, ptr_);
|
||||
fwrite(&msg.magic, 1, 1, ptr_);
|
||||
|
|
@ -117,7 +118,8 @@ void MavLinkFileLog::write(const mavlinkcom::MavLinkMessage& msg, uint64_t times
|
|||
fwrite(&msg.seq, 1, 1, ptr_);
|
||||
fwrite(&msg.sysid, 1, 1, ptr_);
|
||||
fwrite(&msg.compid, 1, 1, ptr_);
|
||||
fwrite(&msg.msgid, 1, 1, ptr_);
|
||||
uint8_t msgid = msg.msgid & 0xff; // truncate to mavlink1 msgid
|
||||
fwrite(&msgid, 1, 1, ptr_);
|
||||
fwrite(&msg.payload64, 1, msg.len, ptr_);
|
||||
fwrite(&msg.checksum, sizeof(uint16_t), 1, ptr_);
|
||||
}
|
||||
|
|
@ -160,7 +162,9 @@ bool MavLinkFileLog::read(mavlinkcom::MavLinkMessage& msg, uint64_t& timestamp)
|
|||
if (s == 0) {
|
||||
return false;
|
||||
}
|
||||
s = fread(&msg.msgid, 1, 1, ptr_);
|
||||
uint8_t msgid = 0;
|
||||
s = fread(&msgid, 1, 1, ptr_);
|
||||
msg.msgid = msgid;
|
||||
if (s < 1) {
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -15,52 +15,25 @@ STRICT_MODE_OFF
|
|||
#include "../mavlink/mavlink_helpers.h"
|
||||
STRICT_MODE_ON
|
||||
|
||||
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
|
||||
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
|
||||
|
||||
|
||||
using namespace mavlinkcom;
|
||||
void MavLinkMessageBase::decode(const MavLinkMessage& msg)
|
||||
{
|
||||
// unpack the message...
|
||||
this->msgid = msg.msgid;
|
||||
unpack(reinterpret_cast<const char*>(msg.payload64));
|
||||
|
||||
}
|
||||
void MavLinkMessageBase::encode(MavLinkMessage& msg, int seqno) const {
|
||||
mavlink_message_t& m = reinterpret_cast<mavlink_message_t&>(msg);
|
||||
m.magic = MAVLINK_STX;
|
||||
m.msgid = this->msgid;
|
||||
m.sysid = this->sysid;
|
||||
m.compid = this->compid;
|
||||
m.seq = seqno;
|
||||
|
||||
// pack the payload buffer.
|
||||
int len = this->pack(reinterpret_cast<char*>(m.payload64));
|
||||
void MavLinkMessageBase::encode(MavLinkMessage& msg) const {
|
||||
|
||||
// calculate checksum
|
||||
uint8_t crc_extra = mavlink_message_crcs[m.msgid];
|
||||
int msglen = mavlink_message_lengths[m.msgid];
|
||||
if (m.msgid == MavLinkTelemetry::kMessageId) {
|
||||
msglen = 28; // mavlink doesn't know about our custom telemetry message.
|
||||
}
|
||||
if (len != msglen) {
|
||||
throw std::runtime_error(Utils::stringf("Message length %d doesn't match expected length%d\n", len, msglen));
|
||||
}
|
||||
m.len = msglen;
|
||||
m.checksum = crc_calculate((reinterpret_cast<const uint8_t*>(&m)) + 3, MAVLINK_CORE_HEADER_LEN);
|
||||
crc_accumulate_buffer(&m.checksum, reinterpret_cast<const char*>(&m.payload64[0]), msglen);
|
||||
#if MAVLINK_CRC_EXTRA
|
||||
crc_accumulate(crc_extra, &m.checksum);
|
||||
#endif
|
||||
|
||||
// these macros use old style cast.
|
||||
STRICT_MODE_OFF
|
||||
mavlink_ck_a(&m) = (uint8_t)(m.checksum & 0xFF);
|
||||
mavlink_ck_b(&m) = (uint8_t)(m.checksum >> 8);
|
||||
STRICT_MODE_ON
|
||||
msg.msgid = this->msgid;
|
||||
msg.sysid = this->sysid;
|
||||
msg.compid = this->compid;
|
||||
// pack the payload buffer.
|
||||
int len = this->pack(reinterpret_cast<char*>(msg.payload64));
|
||||
msg.len = len;
|
||||
}
|
||||
|
||||
|
||||
void MavLinkMessageBase::pack_uint8_t(char* buffer, const uint8_t* field, int offset) const
|
||||
{
|
||||
buffer[offset] = *reinterpret_cast<const char*>(field);
|
||||
|
|
|
|||
|
|
@ -8,13 +8,6 @@
|
|||
#include "../serial_com/SerialPort.hpp"
|
||||
#include "../serial_com/UdpClientPort.hpp"
|
||||
#include "../serial_com/TcpClientPort.hpp"
|
||||
#define MAVLINK_PACKED
|
||||
|
||||
STRICT_MODE_OFF
|
||||
#include "../mavlink/common/mavlink.h"
|
||||
#include "../mavlink/mavlink_helpers.h"
|
||||
#include "../mavlink/mavlink_types.h"
|
||||
STRICT_MODE_ON
|
||||
|
||||
using namespace mavlink_utils;
|
||||
using namespace mavlinkcom_impl;
|
||||
|
|
@ -29,6 +22,10 @@ MavLinkConnectionImpl::MavLinkConnectionImpl()
|
|||
telemetry_.messagesSent = 0;
|
||||
telemetry_.renderTime = 0;
|
||||
closed = true;
|
||||
::memset(&mavlink_intermediate_status_, 0, sizeof(mavlink_status_t));
|
||||
::memset(&mavlink_status_, 0, sizeof(mavlink_status_t));
|
||||
// todo: if we support signing then initialize
|
||||
// mavlink_intermediate_status_.signing callbacks
|
||||
}
|
||||
std::string MavLinkConnectionImpl::getName() {
|
||||
return name;
|
||||
|
|
@ -168,38 +165,146 @@ void MavLinkConnectionImpl::ignoreMessage(uint8_t message_id)
|
|||
ignored_messageids.insert(message_id);
|
||||
}
|
||||
|
||||
void MavLinkConnectionImpl::sendMessage(const MavLinkMessage& msg)
|
||||
void MavLinkConnectionImpl::sendMessage(const MavLinkMessage& m)
|
||||
{
|
||||
if (ignored_messageids.find(msg.msgid) != ignored_messageids.end())
|
||||
if (ignored_messageids.find(m.msgid) != ignored_messageids.end())
|
||||
return;
|
||||
|
||||
if (!closed) {
|
||||
if (closed) {
|
||||
return;
|
||||
}
|
||||
|
||||
{
|
||||
MavLinkMessage msg;
|
||||
::memcpy(&msg, &m, sizeof(MavLinkMessage));
|
||||
prepareForSending(msg);
|
||||
|
||||
if (sendLog_ != nullptr)
|
||||
{
|
||||
sendLog_->write(msg);
|
||||
}
|
||||
{
|
||||
const mavlink_message_t& m = reinterpret_cast<const mavlink_message_t&>(msg);
|
||||
std::lock_guard<std::mutex> guard(buffer_mutex);
|
||||
unsigned len = mavlink_msg_to_send_buffer(message_buf, &m);
|
||||
try {
|
||||
port->write(message_buf, len);
|
||||
}
|
||||
catch (std::exception& e) {
|
||||
throw std::runtime_error(Utils::stringf("MavLinkConnectionImpl: Error sending message on connection '%s', details: %s", name.c_str(), e.what()));
|
||||
}
|
||||
|
||||
mavlink_message_t message;
|
||||
message.compid = msg.compid;
|
||||
message.sysid = msg.sysid;
|
||||
message.len = msg.len;
|
||||
message.checksum = msg.checksum;
|
||||
message.magic = msg.magic;
|
||||
message.incompat_flags = msg.incompat_flags;
|
||||
message.compat_flags = msg.compat_flags;
|
||||
message.seq = msg.seq;
|
||||
message.msgid = msg.msgid;
|
||||
::memcpy(message.signature, msg.signature, 13);
|
||||
::memcpy(message.payload64, msg.payload64, PayloadSize * sizeof(uint64_t));
|
||||
|
||||
std::lock_guard<std::mutex> guard(buffer_mutex);
|
||||
unsigned len = mavlink_msg_to_send_buffer(message_buf, &message);
|
||||
|
||||
try {
|
||||
port->write(message_buf, len);
|
||||
}
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(telemetry_mutex_);
|
||||
telemetry_.messagesSent++;
|
||||
catch (std::exception& e) {
|
||||
throw std::runtime_error(Utils::stringf("MavLinkConnectionImpl: Error sending message on connection '%s', details: %s", name.c_str(), e.what()));
|
||||
}
|
||||
}
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(telemetry_mutex_);
|
||||
telemetry_.messagesSent++;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
int MavLinkConnectionImpl::prepareForSending(MavLinkMessage& msg)
|
||||
{
|
||||
// as per https://github.com/mavlink/mavlink/blob/master/doc/MAVLink2.md
|
||||
int seqno = getNextSequence();
|
||||
|
||||
bool mavlink1 = !supports_mavlink2_;
|
||||
bool signing = !mavlink1 && mavlink_status_.signing && (mavlink_status_.signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING);
|
||||
uint8_t signature_len = signing ? MAVLINK_SIGNATURE_BLOCK_LEN : 0;
|
||||
|
||||
uint8_t header_len = MAVLINK_CORE_HEADER_LEN + 1;
|
||||
uint8_t buf[MAVLINK_CORE_HEADER_LEN + 1];
|
||||
if (mavlink1) {
|
||||
msg.magic = MAVLINK_STX_MAVLINK1;
|
||||
header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1;
|
||||
}
|
||||
else {
|
||||
msg.magic = MAVLINK_STX;
|
||||
}
|
||||
|
||||
msg.seq = seqno;
|
||||
msg.incompat_flags = 0;
|
||||
if (signing_) {
|
||||
msg.incompat_flags |= MAVLINK_IFLAG_SIGNED;
|
||||
}
|
||||
msg.compat_flags = 0;
|
||||
|
||||
// pack the payload buffer.
|
||||
char* payload = reinterpret_cast<char*>(&msg.payload64[0]);
|
||||
int len = msg.len;
|
||||
|
||||
// calculate checksum
|
||||
const mavlink_msg_entry_t* entry = mavlink_get_msg_entry(msg.msgid);
|
||||
uint8_t crc_extra = 0;
|
||||
int msglen = 0;
|
||||
if (entry != nullptr) {
|
||||
crc_extra = entry->crc_extra;
|
||||
msglen = entry->msg_len;
|
||||
}
|
||||
if (msg.msgid == MavLinkTelemetry::kMessageId) {
|
||||
msglen = 28; // mavlink doesn't know about our custom telemetry message.
|
||||
}
|
||||
if (len != msglen) {
|
||||
throw std::runtime_error(Utils::stringf("Message length %d doesn't match expected length%d\n", len, msglen));
|
||||
}
|
||||
msg.len = mavlink1 ? msglen : _mav_trim_payload(payload, msglen);
|
||||
|
||||
// form the header as a byte array for the crc
|
||||
buf[0] = msg.magic;
|
||||
buf[1] = msg.len;
|
||||
if (mavlink1) {
|
||||
buf[2] = msg.seq;
|
||||
buf[3] = msg.sysid;
|
||||
buf[4] = msg.compid;
|
||||
buf[5] = msg.msgid & 0xFF;
|
||||
}
|
||||
else {
|
||||
buf[2] = msg.incompat_flags;
|
||||
buf[3] = msg.compat_flags;
|
||||
buf[4] = msg.seq;
|
||||
buf[5] = msg.sysid;
|
||||
buf[6] = msg.compid;
|
||||
buf[7] = msg.msgid & 0xFF;
|
||||
buf[8] = (msg.msgid >> 8) & 0xFF;
|
||||
buf[9] = (msg.msgid >> 16) & 0xFF;
|
||||
}
|
||||
|
||||
msg.checksum = crc_calculate(&buf[1], header_len - 1);
|
||||
crc_accumulate_buffer(&msg.checksum, payload, msg.len);
|
||||
crc_accumulate(crc_extra, &msg.checksum);
|
||||
|
||||
// these macros use old style cast.
|
||||
STRICT_MODE_OFF
|
||||
mavlink_ck_a(&msg) = (uint8_t)(msg.checksum & 0xFF);
|
||||
mavlink_ck_b(&msg) = (uint8_t)(msg.checksum >> 8);
|
||||
STRICT_MODE_ON
|
||||
|
||||
if (signing_) {
|
||||
mavlink_sign_packet(mavlink_status_.signing,
|
||||
reinterpret_cast<uint8_t *>(msg.signature),
|
||||
reinterpret_cast<const uint8_t *>(message_buf), header_len,
|
||||
reinterpret_cast<const uint8_t *>(payload), msg.len,
|
||||
reinterpret_cast<const uint8_t *>(payload) + msg.len);
|
||||
}
|
||||
|
||||
return msg.len + header_len + 2 + signature_len;
|
||||
}
|
||||
|
||||
void MavLinkConnectionImpl::sendMessage(const MavLinkMessageBase& msg)
|
||||
{
|
||||
MavLinkMessage m;
|
||||
msg.encode(m, getNextSequence());
|
||||
msg.encode(m);
|
||||
sendMessage(m);
|
||||
}
|
||||
|
||||
|
|
@ -251,12 +356,10 @@ void MavLinkConnectionImpl::readPackets()
|
|||
CurrentThread::setMaximumPriority();
|
||||
std::shared_ptr<Port> safePort = this->port;
|
||||
mavlink_message_t msg;
|
||||
mavlink_status_t status;
|
||||
mavlink_status_t statusBuffer; // intermediate state
|
||||
mavlink_message_t msgBuffer; // intermediate state.
|
||||
const int MAXBUFFER = 512;
|
||||
uint8_t* buffer = new uint8_t[MAXBUFFER];
|
||||
statusBuffer.parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
mavlink_intermediate_status_.parse_state = MAVLINK_PARSE_STATE_IDLE;
|
||||
int channel = 0;
|
||||
int hr = 0;
|
||||
while (hr == 0 && con_ != nullptr && !closed)
|
||||
|
|
@ -277,7 +380,7 @@ void MavLinkConnectionImpl::readPackets()
|
|||
}
|
||||
for (int i = 0; i < count; i++)
|
||||
{
|
||||
uint8_t frame_state = mavlink_frame_char_buffer(&msgBuffer, &statusBuffer, buffer[i], &msg, &status);
|
||||
uint8_t frame_state = mavlink_frame_char_buffer(&msgBuffer, &mavlink_intermediate_status_, buffer[i], &msg, &mavlink_status_);
|
||||
|
||||
if (frame_state == MAVLINK_FRAMING_INCOMPLETE) {
|
||||
continue;
|
||||
|
|
@ -296,6 +399,14 @@ void MavLinkConnectionImpl::readPackets()
|
|||
other_component_id = msg.compid;
|
||||
}
|
||||
|
||||
if (mavlink_intermediate_status_.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1)
|
||||
{
|
||||
// then this is a mavlink 1 message
|
||||
} else {
|
||||
// then this mavlink sender supports mavlink 2
|
||||
supports_mavlink2_ = true;
|
||||
}
|
||||
|
||||
if (con_ != nullptr && !closed)
|
||||
{
|
||||
{
|
||||
|
|
@ -305,7 +416,18 @@ void MavLinkConnectionImpl::readPackets()
|
|||
// queue event for publishing.
|
||||
{
|
||||
std::lock_guard<std::mutex> guard(msg_queue_mutex_);
|
||||
MavLinkMessage& message = reinterpret_cast<MavLinkMessage&>(msg);
|
||||
MavLinkMessage message;
|
||||
message.compid = msg.compid;
|
||||
message.sysid = msg.sysid;
|
||||
message.len = msg.len;
|
||||
message.checksum = msg.checksum;
|
||||
message.magic = msg.magic;
|
||||
message.incompat_flags = msg.incompat_flags;
|
||||
message.compat_flags = msg.compat_flags;
|
||||
message.seq = msg.seq;
|
||||
message.msgid = msg.msgid;
|
||||
::memcpy(message.signature, msg.signature, 13);
|
||||
::memcpy(message.payload64, msg.payload64, PayloadSize * sizeof(uint64_t));
|
||||
msg_queue_.push(message);
|
||||
}
|
||||
if (waiting_for_msg_) {
|
||||
|
|
|
|||
|
|
@ -14,6 +14,14 @@
|
|||
#include "MavLinkMessageBase.hpp"
|
||||
#include "Semaphore.hpp"
|
||||
#include "../serial_com/TcpClientPort.hpp"
|
||||
#include "StrictMode.hpp"
|
||||
#define MAVLINK_PACKED
|
||||
|
||||
STRICT_MODE_OFF
|
||||
#include "../mavlink/common/mavlink.h"
|
||||
#include "../mavlink/mavlink_helpers.h"
|
||||
#include "../mavlink/mavlink_types.h"
|
||||
STRICT_MODE_ON
|
||||
|
||||
using namespace mavlinkcom;
|
||||
|
||||
|
|
@ -24,7 +32,6 @@ namespace mavlinkcom_impl {
|
|||
{
|
||||
public:
|
||||
MavLinkConnectionImpl();
|
||||
static std::vector<SerialPortInfo> findSerialPorts(int vid, int pid);
|
||||
static std::shared_ptr<MavLinkConnection> connectSerial(const std::string& nodeName, std::string portName, int baudrate = 115200, const std::string initString = "");
|
||||
static std::shared_ptr<MavLinkConnection> connectLocalUdp(const std::string& nodeName, std::string localAddr, int localPort);
|
||||
static std::shared_ptr<MavLinkConnection> connectRemoteUdp(const std::string& nodeName, std::string localAddr, std::string remoteAddr, int remotePort);
|
||||
|
|
@ -47,7 +54,7 @@ namespace mavlinkcom_impl {
|
|||
void join(std::shared_ptr<MavLinkConnection> remote, bool subscribeToLeft = true, bool subscribeToRight = true);
|
||||
void getTelemetry(MavLinkTelemetry& result);
|
||||
void ignoreMessage(uint8_t message_id);
|
||||
|
||||
int prepareForSending(MavLinkMessage& msg);
|
||||
private:
|
||||
static std::shared_ptr<MavLinkConnection> createConnection(const std::string& nodeName, std::shared_ptr<Port> port);
|
||||
void joinLeftSubscriber(std::shared_ptr<MavLinkConnection> remote, std::shared_ptr<MavLinkConnection>con, const MavLinkMessage& msg);
|
||||
|
|
@ -75,7 +82,7 @@ namespace mavlinkcom_impl {
|
|||
std::vector<MessageHandlerEntry> snapshot;
|
||||
bool snapshot_stale;
|
||||
std::mutex listener_mutex;
|
||||
uint8_t message_buf[300];
|
||||
uint8_t message_buf[300]; // must be bigger than sizeof(mavlink_message_t), which is currently 292.
|
||||
std::mutex buffer_mutex;
|
||||
bool closed;
|
||||
std::thread publish_thread_;
|
||||
|
|
@ -83,7 +90,11 @@ namespace mavlinkcom_impl {
|
|||
std::mutex msg_queue_mutex_;
|
||||
mavlink_utils::Semaphore msg_available_;
|
||||
bool waiting_for_msg_ = false;
|
||||
std::mutex telemetry_mutex_;
|
||||
bool supports_mavlink2_ = false;
|
||||
bool signing_ = false;
|
||||
mavlink_status_t mavlink_intermediate_status_;
|
||||
mavlink_status_t mavlink_status_;
|
||||
std::mutex telemetry_mutex_;
|
||||
MavLinkTelemetry telemetry_;
|
||||
std::unordered_set<uint8_t> ignored_messageids;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,12 +1,12 @@
|
|||
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||
// Licensed under the MIT License.
|
||||
|
||||
#include "../MavLinkConnectionImpl.hpp"
|
||||
#include "MavLinkConnection.hpp"
|
||||
|
||||
using namespace mavlinkcom;
|
||||
using namespace mavlinkcom_impl;
|
||||
|
||||
std::vector<SerialPortInfo> MavLinkConnectionImpl::findSerialPorts(int vid, int pid)
|
||||
std::vector<SerialPortInfo> MavLinkConnection::findSerialPorts(int vid, int pid)
|
||||
{
|
||||
vid; // avoid warning: unused parameter
|
||||
pid; // avoid warning: unused parameter
|
||||
|
|
|
|||
|
|
@ -1,12 +1,13 @@
|
|||
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||||
// Licensed under the MIT License.
|
||||
|
||||
#include "../MavLinkConnectionImpl.hpp"
|
||||
#include "MavLinkConnection.hpp"
|
||||
#include <Windows.h>
|
||||
#include <ObjIdl.h>
|
||||
#include <SetupAPI.h>
|
||||
#include <Cfgmgr32.h>
|
||||
#include <propkey.h>
|
||||
#include <string>
|
||||
|
||||
using namespace mavlinkcom;
|
||||
using namespace mavlinkcom_impl;
|
||||
|
|
@ -58,7 +59,7 @@ void parseDisplayName(std::wstring displayName, SerialPortInfo* info)
|
|||
}
|
||||
|
||||
|
||||
std::vector<SerialPortInfo> MavLinkConnectionImpl::findSerialPorts(int vid, int pid)
|
||||
std::vector<SerialPortInfo> MavLinkConnection::findSerialPorts(int vid, int pid)
|
||||
{
|
||||
bool debug = false;
|
||||
std::vector<SerialPortInfo> result;
|
||||
|
|
|
|||
11
docs/faq.md
11
docs/faq.md
|
|
@ -63,4 +63,13 @@ Unreal 4.15 added the ability for Foliage LOD dithering to be disabled on a case
|
|||
|
||||
|
||||
### Can I use an xbox controller to fly?
|
||||
Yes, see [xbox controller](xbox_controller.md) for details.
|
||||
Yes, see [xbox controller](xbox_controller.md) for details.
|
||||
|
||||
### When I tell the drone to do something it always lands
|
||||
|
||||
For example, you use DroneShell `moveToPosition -z -20 -x 50 -y 0` which it does, but when it gets to the target location the
|
||||
drone starts to land. This is the default behavior of PX4 when offboard mode completes. To set the drone to hover instead
|
||||
set this PX4 parameter:
|
||||
````
|
||||
param set COM_OBL_ACT 1
|
||||
````
|
||||
|
|
@ -49,6 +49,12 @@ param set LPE_LAT 47.641468
|
|||
param set LPE_LON -122.140165
|
||||
````
|
||||
|
||||
You might also want to set this one so that the drone automatically hovers after each offboard control command
|
||||
(the default setting is to land)
|
||||
````
|
||||
param set COM_OBL_ACT 1
|
||||
````
|
||||
|
||||
Now close Unreal app, restart `./build_posix_sitl_default/src/firmware/posix/px4` and re-start the unreal app.
|
||||
|
||||
## Check the Home Position
|
||||
|
|
|
|||
|
|
@ -101,8 +101,8 @@ This is a handy mode that lets you edit the game content in the Unreal Editor an
|
|||
Make sure your ~/Documents/AirSim/settings.json looks like this
|
||||
````
|
||||
{
|
||||
"LocalHostIp": "127.0.0.1",
|
||||
"Pixhawk": {
|
||||
"LocalHostIp": "127.0.0.1",
|
||||
"LogViewerHostIp": "127.0.0.1",
|
||||
"LogViewerPort": 14388,
|
||||
"OffboardCompID": 1,
|
||||
|
|
|
|||
Загрузка…
Ссылка в новой задаче