Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

GCS_MAVLink: Add support for High Latency MAVLink protocol

Browse Source
This commit is contained in:
Stephen Dade 2022-01-26 13:51:54 +11:00 committed by Peter Barker
parent da4602b5d2
commit 1344a0f41a
4 changed files with 95 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/GCS_MAVLink/GCS.cpp
View File

@ -119,6 +119,17 @@ void GCS::send_named_float(const char *name, float value) const
(const char *)&packet);
}
#if HAL_HIGH_LATENCY2_ENABLED
void GCS::enable_high_latency_connections(bool enabled)
{
for (uint8_t i=0; i<num_gcs(); i++) {
GCS_MAVLINK &c = *chan(i);
c.high_latency_link_enabled = enabled && c.is_high_latency_link;
}
gcs().send_text(MAV_SEVERITY_NOTICE, "High Latency %s", enabled ? "enabled" : "disabled");
}
#endif // HAL_HIGH_LATENCY2_ENABLED
/*
install an alternative protocol handler. This allows another
protocol to take over the link if MAVLink goes idle. It is used to

20
libraries/GCS_MAVLink/GCS.h
View File

@ -351,6 +351,11 @@ public:
// return true if channel is private
bool is_private(void) const { return is_private(chan); }
#if HAL_HIGH_LATENCY2_ENABLED
// return true if the link should be sending. Will return false if is a high latency link AND is not active
bool should_send() { return is_high_latency_link ? high_latency_link_enabled : true; }
#endif
/*
send a MAVLink message to all components with this vehicle's system id
This is a no-op if no routes to components have been learned
@ -599,8 +604,14 @@ protected:
virtual uint8_t high_latency_wind_speed() const { return 0; }
virtual uint8_t high_latency_wind_direction() const { return 0; }
int8_t high_latency_air_temperature() const;
#endif // HAL_HIGH_LATENCY2_ENABLED
MAV_RESULT handle_control_high_latency(const mavlink_command_long_t &packet);
// true if this is a high latency link
bool is_high_latency_link;
bool high_latency_link_enabled;
#endif // HAL_HIGH_LATENCY2_ENABLED
static constexpr const float magic_force_arm_value = 2989.0f;
static constexpr const float magic_force_disarm_value = 21196.0f;
@ -682,9 +693,10 @@ private:
const ap_message id;
uint16_t interval_ms;
uint16_t last_sent_ms; // from AP_HAL::millis16()
} deferred_message[2] = {
} deferred_message[3] = {
{ MSG_HEARTBEAT, },
{ MSG_NEXT_PARAM, },
{ MSG_HIGH_LATENCY2, },
};
// returns index of id in deferred_message[] or -1 if not present
int8_t get_deferred_message_index(const ap_message id) const;
@ -1101,6 +1113,10 @@ public:
uint8_t get_channel_from_port_number(uint8_t port_num);
#if HAL_HIGH_LATENCY2_ENABLED
void enable_high_latency_connections(bool enabled);
#endif // HAL_HIGH_LATENCY2_ENABLED
protected:
virtual uint8_t sysid_this_mav() const = 0;

65
libraries/GCS_MAVLink/GCS_Common.cpp
View File

@ -164,14 +164,20 @@ bool GCS_MAVLINK::init(uint8_t instance)
return false;
}
if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2) {
if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 ||
mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) {
// load signing key
load_signing_key();
} else if (status) {
// user has asked to only send MAVLink1
status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1;
}
#if HAL_HIGH_LATENCY2_ENABLED
if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) {
is_high_latency_link = true;
}
#endif
return true;
}
@ -938,8 +944,9 @@ bool GCS_MAVLINK::should_send_message_in_delay_callback(const ap_message id) con
// microseconds to return!
switch (id) {
case MSG_HEARTBEAT:
case MSG_NEXT_PARAM:
case MSG_HEARTBEAT:
case MSG_HIGH_LATENCY2:
case MSG_AUTOPILOT_VERSION:
return true;
default:
@ -1405,7 +1412,7 @@ bool GCS_MAVLINK::set_ap_message_interval(enum ap_message id, uint16_t interval_
// mavlink work!)
void GCS_MAVLINK::send_message(enum ap_message id)
{
if (id == MSG_HEARTBEAT) {
if (id == MSG_HEARTBEAT || id == MSG_HIGH_LATENCY2) {
save_signing_timestamp(false);
// update the mask of all streaming channels
if (is_streaming()) {
@ -1428,7 +1435,8 @@ void GCS_MAVLINK::packetReceived(const mavlink_status_t &status,
}
if (!(status.flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) &&
(status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) &&
AP::serialmanager().get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLink2) {
(AP::serialmanager().get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLink2 ||
AP::serialmanager().get_mavlink_protocol(chan) == AP_SerialManager::SerialProtocol_MAVLinkHL)) {
// if we receive any MAVLink2 packets on a connection
// currently sending MAVLink1 then switch to sending
// MAVLink2
@ -4354,6 +4362,12 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
result = handle_preflight_reboot(packet);
break;
#if HAL_HIGH_LATENCY2_ENABLED
case MAV_CMD_CONTROL_HIGH_LATENCY:
result = handle_control_high_latency(packet);
break;
#endif // HAL_HIGH_LATENCY2_ENABLED
case MAV_CMD_DO_START_MAG_CAL:
case MAV_CMD_DO_ACCEPT_MAG_CAL:
case MAV_CMD_DO_CANCEL_MAG_CAL: {
@ -5695,7 +5709,15 @@ void GCS_MAVLINK::initialise_message_intervals_from_streamrates()
for (uint8_t i=0; all_stream_entries[i].ap_message_ids != nullptr; i++) {
initialise_message_intervals_for_stream(all_stream_entries[i].stream_id);
}
#if HAL_HIGH_LATENCY2_ENABLED
if (!is_high_latency_link) {
set_mavlink_message_id_interval(MAVLINK_MSG_ID_HEARTBEAT, 1000);
} else {
set_mavlink_message_id_interval(MAVLINK_MSG_ID_HIGH_LATENCY2, 5000);
}
#else
set_mavlink_message_id_interval(MAVLINK_MSG_ID_HEARTBEAT, 1000);
#endif
}
bool GCS_MAVLINK::get_default_interval_for_ap_message(const ap_message id, uint16_t &interval) const
@ -5706,6 +5728,12 @@ bool GCS_MAVLINK::get_default_interval_for_ap_message(const ap_message id, uint1
return true;
}
if (id == MSG_HIGH_LATENCY2) {
// handle HL2 requests as a special case because HL2 is not "streamed"
interval = 5000;
return true;
}
#if HAL_MAVLINK_INTERVALS_FROM_FILES_ENABLED
// a user can specify default rates in files, which are read close
// to vehicle startup
@ -5941,7 +5969,7 @@ uint64_t GCS_MAVLINK::capabilities() const
MAV_PROTOCOL_CAPABILITY_COMPASS_CALIBRATION;
AP_SerialManager::SerialProtocol mavlink_protocol = AP::serialmanager().get_mavlink_protocol(chan);
if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2) {
if (mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLink2 || mavlink_protocol == AP_SerialManager::SerialProtocol_MAVLinkHL) {
ret |= MAV_PROTOCOL_CAPABILITY_MAVLINK2;
}
@ -6103,4 +6131,29 @@ int8_t GCS_MAVLINK::high_latency_air_temperature() const
return INT8_MIN;
}
/*
handle a MAV_CMD_CONTROL_HIGH_LATENCY command
Enable or disable any marked (via SERIALn_PROTOCOL) high latency connections
*/
MAV_RESULT GCS_MAVLINK::handle_control_high_latency(const mavlink_command_long_t &packet)
{
// high latency mode is enabled if param1=1 or disabled if param1=0
if (is_equal(packet.param1, 0.0f)) {
gcs().enable_high_latency_connections(false);
} else if (is_equal(packet.param1, 1.0f)) {
gcs().enable_high_latency_connections(true);
} else {
return MAV_RESULT_FAILED;
}
// send to all other mavlink components with same sysid
mavlink_command_long_t hl_msg{};
hl_msg.command = MAV_CMD_CONTROL_HIGH_LATENCY;
hl_msg.param1 = packet.param1;
GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&hl_msg, sizeof(hl_msg));
return MAV_RESULT_ACCEPTED;
}
#endif // HAL_HIGH_LATENCY2_ENABLED

7
libraries/GCS_MAVLink/GCS_MAVLink.cpp
View File

@ -94,6 +94,13 @@ void comm_send_buffer(mavlink_channel_t chan, const uint8_t *buf, uint8_t len)
if (!valid_channel(chan) || mavlink_comm_port[chan] == nullptr || chan_discard[chan]) {
return;
}
#if HAL_HIGH_LATENCY2_ENABLED
// if it's a disabled high latency channel, don't send
GCS_MAVLINK *link = gcs().chan(chan);
if (!link->should_send()) {
return;
}
#endif
if (gcs_alternative_active[chan]) {
// an alternative protocol is active
return;