fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

MAVLink 2.0: Take a first stab at integration, enable heartbeat packets

Browse Source
This commit is contained in:
Lorenz Meier 2016-05-13 14:53:47 +02:00
parent c67907ffb2
commit 6eccfe3d14
5 changed files with 44 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/mavlink/mavlink_bridge_header.h
View File

@ -44,12 +44,7 @@
__BEGIN_DECLS
/*
* We are NOT using convenience functions,
* but instead send messages with a custom function.
* So we do NOT do this:
* #define MAVLINK_USE_CONVENIENCE_FUNCTIONS
*/
#define MAVLINK_USE_CONVENIENCE_FUNCTIONS
/* use efficient approach, see mavlink_helpers.h */
#define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes
@ -57,7 +52,7 @@ __BEGIN_DECLS
#define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer
#define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status
#include <v1.0/mavlink_types.h>
#include <v2.0/mavlink_types.h>
#include <unistd.h>
@ -84,7 +79,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng
extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
#include <v1.0/common/mavlink.h>
#include <v2.0/common/mavlink.h>
__END_DECLS

2
src/modules/mavlink/mavlink_log_handler.h
View File

@ -40,7 +40,7 @@
#include <queue.h>
#include <time.h>
#include <stdio.h>
#include <v1.0/mavlink_types.h>
#include <v2.0/mavlink_types.h>
#include "mavlink_stream.h"
class Mavlink;

47
src/modules/mavlink/mavlink_main.cpp
View File

@ -106,8 +106,7 @@ static const int ERROR = -1;
static Mavlink *_mavlink_instances = nullptr;
static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS;
static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS;
static const mavlink_crc_entry_t mavlink_message_meta[] = MAVLINK_MESSAGE_CRCS;
/**
* mavlink app start / stop handling function
@ -118,6 +117,17 @@ extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
extern mavlink_system_t mavlink_system;
void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length)
{
Mavlink* m = Mavlink::get_instance((unsigned)chan);
if (m->get_free_tx_buf() < length) {
return;
}
m->send_bytes(ch, length);
}
static void usage(void);
bool Mavlink::_boot_complete = false;
@ -816,15 +826,7 @@ Mavlink::get_free_tx_buf()
void
Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
uint8_t payload_len = mavlink_message_lengths[msgid];
uint8_t payload_len = mavlink_message_meta[msgid].msg_len;
unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
_last_write_try_time = hrt_absolute_time();
@ -845,6 +847,8 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
}
}
mavlink_get_channel_status(_channel)->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
uint8_t buf[MAVLINK_MAX_PACKET_LEN];
/* header */
@ -863,11 +867,25 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
uint16_t checksum;
crc_init(&checksum);
crc_accumulate_buffer(&checksum, (const char *) &buf[1], MAVLINK_CORE_HEADER_LEN + payload_len);
crc_accumulate(mavlink_message_crcs[msgid], &checksum);
crc_accumulate(mavlink_message_meta[msgid].crc_extra, &checksum);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len] = (uint8_t)(checksum & 0xFF);
buf[MAVLINK_NUM_HEADER_BYTES + payload_len + 1] = (uint8_t)(checksum >> 8);
send_bytes(&buf[0], packet_len);
}
void
Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
{
/* If the wait until transmit flag is on, only transmit after we've received messages.
Otherwise, transmit all the time. */
if (!should_transmit()) {
return;
}
pthread_mutex_lock(&_send_mutex);
size_t ret = -1;
/* send message to UART */
@ -881,11 +899,10 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
struct telemetry_status_s &tstatus = get_rx_status();
/* resend heartbeat via broadcast */
/* resend message via broadcast if no valid connection exists */
if ((_mode != MAVLINK_MODE_ONBOARD) &&
(!get_client_source_initialized()
|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))
&& (msgid == MAVLINK_MSG_ID_HEARTBEAT)) {
|| (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
if (!_broadcast_address_found) {
find_broadcast_address();

2
src/modules/mavlink/mavlink_main.h
View File

@ -226,6 +226,8 @@ public:
void send_message(const uint8_t msgid, const void *msg, uint8_t component_ID = 0);
void send_bytes(const uint8_t *buf, unsigned packet_len);
/**
* Resend message as is, don't change sequence number and CRC.
*/

15
src/modules/mavlink/mavlink_messages.cpp
View File

@ -333,16 +333,13 @@ protected:
memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet));
}
mavlink_heartbeat_t msg;
uint8_t base_mode = 0;
uint32_t custom_mode = 0;
uint8_t system_status = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &system_status, &base_mode, &custom_mode);
msg.base_mode = 0;
msg.custom_mode = 0;
get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
msg.type = _mavlink->get_system_type();
msg.autopilot = MAV_AUTOPILOT_PX4;
msg.mavlink_version = 3;
_mavlink->send_message(MAVLINK_MSG_ID_HEARTBEAT, &msg);
mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
base_mode, custom_mode, system_status);
}
};