Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_OpenDroneID: check available tx space and fixed message rates 

ensure we don't lose messages due to lack of serial TX space, and
fixed update rate of LOCATION and SYSTEM_UPDATE msgs
This commit is contained in:
Andrew Tridgell 2022-08-14 18:55:02 +10:00
parent 0fc6c813a1
commit b52c280704
2 changed files with 75 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

74
libraries/AP_OpenDroneID/AP_OpenDroneID.cpp
View File

@ -126,7 +126,9 @@ bool AP_OpenDroneID::pre_arm_check(char* failmsg, uint8_t failmsg_len)
return false;
}
if (last_system_ms == 0 || now_ms - last_system_ms > max_age_ms) {
if (last_system_ms == 0 ||
(now_ms - last_system_ms > max_age_ms &&
(now_ms - last_system_update_ms > max_age_ms))) {
strncpy(failmsg, "SYSTEM not available", failmsg_len);
return false;
}
@ -144,7 +146,6 @@ void AP_OpenDroneID::update()
if (_enable == 0) {
return;
}
const uint32_t now = AP_HAL::millis();
const bool armed = hal.util->get_soft_armed();
if (armed && !_was_armed) {
@ -153,17 +154,25 @@ void AP_OpenDroneID::update()
}
_was_armed = armed;
if (now - _last_send_dynamic_messages_ms >= _mavlink_dynamic_period_ms) {
_last_send_dynamic_messages_ms = now;
send_dynamic_out();
}
send_static_out();
}
void AP_OpenDroneID::send_dynamic_out()
{
const uint32_t now = AP_HAL::millis();
if (now - _last_send_location_ms >= _mavlink_dynamic_period_ms &&
HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_LOCATION)) {
_last_send_location_ms = now;
send_location_message();
}
// operator location needs to be sent at the same rate as location for FAA compliance
if (now - _last_send_system_update_ms >= _mavlink_dynamic_period_ms &&
HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_SYSTEM_UPDATE)) {
_last_send_system_update_ms = now;
send_system_update_message();
}
}
void AP_OpenDroneID::send_static_out()
@ -171,40 +180,60 @@ void AP_OpenDroneID::send_static_out()
const uint32_t now_ms = AP_HAL::millis();
// we need to notify user if we lost the transmitter
if (now_ms - last_arm_status_ms > 5000 && now_ms - last_lost_tx_ms > 5000) {
if (now_ms - last_arm_status_ms > 5000) {
if (now_ms - last_lost_tx_ms > 5000) {
last_lost_tx_ms = now_ms;
GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "ODID: lost transmitter");
}
} else if (last_lost_tx_ms != 0) {
// we're OK again
last_lost_tx_ms = 0;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ODID: transmitter OK");
}
const uint32_t msg_spacing_ms = _mavlink_dynamic_period_ms / 4;
const uint32_t msg_spacing_ms = _mavlink_static_period_ms / 4;
if (now_ms - last_msg_send_ms >= msg_spacing_ms) {
// allow update of channel during setup, this makes it easy to debug with a GCS
_chan = mavlink_channel_t(gcs().get_channel_from_port_number(_mav_port));
last_msg_send_ms = now_ms;
bool sent_ok = false;
switch (next_msg_to_send) {
case NEXT_MSG_BASIC_ID:
if (HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_BASIC_ID)) {
send_basic_id_message();
sent_ok = true;
}
break;
case NEXT_MSG_SYSTEM:
if (HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_SYSTEM)) {
send_system_message();
sent_ok = true;
}
break;
case NEXT_MSG_SELF_ID:
if (HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_SELF_ID)) {
send_self_id_message();
sent_ok = true;
}
break;
case NEXT_MSG_OPERATOR_ID:
if (HAVE_PAYLOAD_SPACE(_chan, OPEN_DRONE_ID_OPERATOR_ID)) {
send_operator_id_message();
sent_ok = true;
}
break;
case NEXT_MSG_ENUM_END:
break;
}
if (sent_ok) {
last_msg_send_ms = now_ms;
next_msg_to_send = next_msg((uint8_t(next_msg_to_send) + 1) % uint8_t(NEXT_MSG_ENUM_END));
}
}
}
// The send_location_message
// all open_drone_id send functions use data stored in the open drone id class.
//This location send function is an exception. It uses live location data from the ArduPilot system.
// This location send function is an exception. It uses live location data from the ArduPilot system.
void AP_OpenDroneID::send_location_message()
{
auto &ahrs = AP::ahrs();
@ -362,12 +391,28 @@ void AP_OpenDroneID::send_system_message()
void AP_OpenDroneID::send_self_id_message()
{
need_send_self_id |= dronecan_send_all;
// note that packet is filled in by the GCS
if (_chan != MAV_CHAN_INVALID) {
mavlink_msg_open_drone_id_self_id_send_struct(_chan, &pkt_self_id);
}
}
void AP_OpenDroneID::send_system_update_message()
{
need_send_system |= dronecan_send_all;
// note that packet is filled in by the GCS
if (_chan != MAV_CHAN_INVALID) {
const auto pkt_system_update = mavlink_open_drone_id_system_update_t {
operator_latitude : pkt_system.operator_latitude,
operator_longitude : pkt_system.operator_longitude,
operator_altitude_geo : pkt_system.operator_altitude_geo,
timestamp : pkt_system.timestamp,
target_system : pkt_system.target_system,
target_component : pkt_system.target_component,
};
mavlink_msg_open_drone_id_system_update_send_struct(_chan, &pkt_system_update);
}
}
void AP_OpenDroneID::send_operator_id_message()
{
need_send_operator_id |= dronecan_send_all;
@ -602,10 +647,11 @@ void AP_OpenDroneID::handle_msg(mavlink_channel_t chan, const mavlink_message_t
pkt_system.operator_longitude = pkt_system_update.operator_longitude;
pkt_system.operator_altitude_geo = pkt_system_update.operator_altitude_geo;
pkt_system.timestamp = pkt_system_update.timestamp;
last_system_update_ms = AP_HAL::millis();
if (last_system_ms != 0) {
// we can only mark this as updated if we have the other
// we can only mark system as updated if we have the other
// information already
last_system_ms = AP_HAL::millis();
last_system_ms = last_system_update_ms;
}
break;
}

7
libraries/AP_OpenDroneID/AP_OpenDroneID.h
View File

@ -118,7 +118,8 @@ private:
mavlink_channel_t _chan; // MAVLink channel that communicates with the Remote ID Transceiver
const mavlink_channel_t MAV_CHAN_INVALID = mavlink_channel_t(255U);
uint32_t _last_send_dynamic_messages_ms;
uint32_t _last_send_location_ms;
uint32_t _last_send_system_update_ms;
uint32_t _last_send_static_messages_ms;
const uint32_t _mavlink_dynamic_period_ms = 1000; //how often are mavlink dynamic messages sent in ms. E.g. 1000 = 1 Hz
const uint32_t _mavlink_static_period_ms = 5000; //how often are mavlink static messages sent in ms
@ -138,6 +139,9 @@ private:
// last time we got a SYSTEM message
uint32_t last_system_ms;
// last time we got a SYSTEM_UPDATE message
uint32_t last_system_update_ms;
// arm status from the transmitter
mavlink_open_drone_id_arm_status_t arm_status;
uint32_t last_arm_status_ms;
@ -150,6 +154,7 @@ private:
void send_static_out();
void send_basic_id_message();
void send_system_message();
void send_system_update_message();
void send_self_id_message();
void send_operator_id_message();
void send_location_message();