mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-02 05:04:08 +08:00
mavlink: rate multiplier and scheduling optimizations
- only update rate multipler if streams, radio status, or tx error rate has changed - by default schedule main loop at 3x fastest stream - cleanup and optimize mavlink stream update
This commit is contained in:
Daniel Agar
parent
8b9a856cf7
commit
8d95c70003
@ -138,7 +138,9 @@ void SendProtocol::update(const hrt_abstime &now)
|
||||
|
||||
while (_latest_sequence != buffer_sequence) {
|
||||
// only send if enough tx buffer space available
|
||||
if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_EVENT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
|
||||
if ((_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_EVENT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)
|
||||
|| _mavlink.over_data_rate()) {
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
@ -218,7 +220,9 @@ void SendProtocol::on_gcs_connected()
|
||||
void SendProtocol::send_current_sequence(const hrt_abstime &now)
|
||||
{
|
||||
// only send if enough tx buffer space available
|
||||
if (_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) {
|
||||
if ((_mavlink.get_free_tx_buf() < MAVLINK_MSG_ID_CURRENT_EVENT_SEQUENCE_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES)
|
||||
|| _mavlink.over_data_rate()) {
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@ -1064,7 +1064,8 @@ void MavlinkFTP::send()
|
||||
unsigned max_bytes_to_send = _mavlink->get_free_tx_buf();
|
||||
PX4_DEBUG("MavlinkFTP::send max_bytes_to_send(%u) get_free_tx_buf(%u)", max_bytes_to_send, _mavlink->get_free_tx_buf());
|
||||
|
||||
if (max_bytes_to_send < get_size()) {
|
||||
// Skip send if not enough room
|
||||
if ((max_bytes_to_send < get_size()) || _mavlink->over_data_rate()) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@ -731,25 +731,11 @@ void Mavlink::send_start(int length)
|
||||
{
|
||||
pthread_mutex_lock(&_send_mutex);
|
||||
_last_write_try_time = hrt_absolute_time();
|
||||
|
||||
// check if there is space in the buffer
|
||||
if (length > (int)get_free_tx_buf()) {
|
||||
// not enough space in buffer to send
|
||||
count_txerrbytes(length);
|
||||
|
||||
_tstatus.tx_buffer_overruns++;
|
||||
|
||||
// prevent writes
|
||||
_tx_buffer_low = true;
|
||||
|
||||
} else {
|
||||
_tx_buffer_low = false;
|
||||
}
|
||||
}
|
||||
|
||||
void Mavlink::send_finish()
|
||||
{
|
||||
if (_tx_buffer_low || (_buf_fill == 0)) {
|
||||
if (_buf_fill == 0) {
|
||||
pthread_mutex_unlock(&_send_mutex);
|
||||
return;
|
||||
}
|
||||
@ -806,6 +792,8 @@ void Mavlink::send_finish()
|
||||
count_txbytes(_buf_fill);
|
||||
_last_write_success_time = _last_write_try_time;
|
||||
|
||||
_over_data_rate.store(_bytes_tx > _datarate * hrt_elapsed_time(&_bytes_timestamp) * 1e-6f);
|
||||
|
||||
} else {
|
||||
count_txerrbytes(_buf_fill);
|
||||
}
|
||||
@ -817,14 +805,12 @@ void Mavlink::send_finish()
|
||||
|
||||
void Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
|
||||
{
|
||||
if (!_tx_buffer_low) {
|
||||
if (_buf_fill + packet_len < sizeof(_buf)) {
|
||||
memcpy(&_buf[_buf_fill], buf, packet_len);
|
||||
_buf_fill += packet_len;
|
||||
if (_buf_fill + packet_len < sizeof(_buf)) {
|
||||
memcpy(&_buf[_buf_fill], buf, packet_len);
|
||||
_buf_fill += packet_len;
|
||||
|
||||
} else {
|
||||
perf_count(_send_byte_error_perf);
|
||||
}
|
||||
} else {
|
||||
perf_count(_send_byte_error_perf);
|
||||
}
|
||||
}
|
||||
|
||||
@ -1155,10 +1141,12 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
|
||||
if (interval != 0) {
|
||||
/* set new interval */
|
||||
stream->set_interval(interval);
|
||||
_force_rate_mult_update = true;
|
||||
|
||||
} else {
|
||||
/* delete stream */
|
||||
_streams.deleteNode(stream);
|
||||
_force_rate_mult_update = true;
|
||||
return OK; // must finish with loop after node is deleted
|
||||
}
|
||||
|
||||
@ -1173,6 +1161,7 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
|
||||
if (stream != nullptr) {
|
||||
stream->set_interval(interval);
|
||||
_streams.add(stream);
|
||||
_force_rate_mult_update = true;
|
||||
|
||||
return OK;
|
||||
}
|
||||
@ -1364,23 +1353,39 @@ Mavlink::close_shell()
|
||||
void
|
||||
Mavlink::update_rate_mult()
|
||||
{
|
||||
int min_interval = INT32_MAX;
|
||||
|
||||
float const_rate = 0.0f;
|
||||
float rate = 0.0f;
|
||||
|
||||
/* scale down rates if their theoretical bandwidth is exceeding the link bandwidth */
|
||||
for (const auto &stream : _streams) {
|
||||
if (stream->const_rate()) {
|
||||
const_rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
|
||||
const int interval = stream->get_interval();
|
||||
|
||||
} else {
|
||||
rate += (stream->get_interval() > 0) ? stream->get_size_avg() * 1000000.0f / stream->get_interval() : 0;
|
||||
if (interval > 0) {
|
||||
if (stream->const_rate()) {
|
||||
const_rate += stream->get_size_avg() * 1e6f / interval;
|
||||
|
||||
} else {
|
||||
rate += stream->get_size_avg() * 1e6f / interval;
|
||||
}
|
||||
|
||||
if (interval < min_interval) {
|
||||
min_interval = interval;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// update main loop delay
|
||||
_main_loop_delay = math::constrain(min_interval / 3, MAVLINK_MIN_INTERVAL, MAVLINK_MAX_INTERVAL);
|
||||
|
||||
float mavlink_ulog_streaming_rate_inv = 1.0f;
|
||||
|
||||
if (_mavlink_ulog) {
|
||||
mavlink_ulog_streaming_rate_inv = 1.0f - _mavlink_ulog->current_data_rate();
|
||||
|
||||
// ensure update is at least twice the default logger rate
|
||||
_main_loop_delay = math::min(_main_loop_delay, 3500 / 2); // TODO: poll ulog_stream
|
||||
}
|
||||
|
||||
/* scale up and down as the link permits */
|
||||
@ -1416,17 +1421,21 @@ Mavlink::update_rate_mult()
|
||||
hardware_mult *= _radio_status_mult;
|
||||
}
|
||||
|
||||
// reset
|
||||
_radio_status_changed = false;
|
||||
|
||||
pthread_mutex_unlock(&_radio_status_mutex);
|
||||
|
||||
if (log_radio_timeout) {
|
||||
PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id);
|
||||
}
|
||||
|
||||
/* pick the minimum from bandwidth mult and hardware mult as limit */
|
||||
_rate_mult = fminf(bandwidth_mult, hardware_mult);
|
||||
// pick the minimum from bandwidth mult and hardware mult as limit
|
||||
// ensure the rate multiplier never drops below 5% so that something is always sent
|
||||
float rate_mult = math::constrain(fminf(bandwidth_mult, hardware_mult), 0.05f, 1.0f);
|
||||
|
||||
/* ensure the rate multiplier never drops below 5% so that something is always sent */
|
||||
_rate_mult = math::constrain(_rate_mult, 0.05f, 1.0f);
|
||||
_rate_mult = rate_mult;
|
||||
_rate_div = 1.f / rate_mult;
|
||||
}
|
||||
|
||||
void
|
||||
@ -1438,6 +1447,9 @@ Mavlink::update_radio_status(const radio_status_s &radio_status)
|
||||
|
||||
if (_use_software_mav_throttling) {
|
||||
|
||||
const bool radio_status_critical = _radio_status_critical;
|
||||
const float radio_status_mult = _radio_status_mult;
|
||||
|
||||
/* check hardware limits */
|
||||
_radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE);
|
||||
|
||||
@ -1456,6 +1468,10 @@ Mavlink::update_radio_status(const radio_status_s &radio_status)
|
||||
|
||||
/* Constrain radio status multiplier between 1% and 100% to allow recovery */
|
||||
_radio_status_mult = math::constrain(_radio_status_mult, 0.01f, 1.0f);
|
||||
|
||||
if ((radio_status_critical != _radio_status_critical) || (fabsf(radio_status_mult - _radio_status_mult) > 0.01f)) {
|
||||
_radio_status_changed = true;
|
||||
}
|
||||
}
|
||||
|
||||
pthread_mutex_unlock(&_radio_status_mutex);
|
||||
@ -1821,6 +1837,8 @@ Mavlink::configure_streams_to_default(const char *configure_single_stream)
|
||||
break;
|
||||
}
|
||||
|
||||
_force_rate_mult_update = true;
|
||||
|
||||
if (configure_single_stream && !stream_configured && strcmp(configure_single_stream, "HEARTBEAT") != 0) {
|
||||
// stream was not found, assume it is disabled by default
|
||||
return configure_stream(configure_single_stream, 0.0f);
|
||||
@ -2190,19 +2208,6 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
PX4_ERR("configure_streams_to_default() failed");
|
||||
}
|
||||
|
||||
/* set main loop delay depending on data rate to minimize CPU overhead */
|
||||
_main_loop_delay = (MAIN_LOOP_DELAY * 1000) / _datarate;
|
||||
|
||||
/* hard limit to 1000 Hz at max */
|
||||
if (_main_loop_delay < MAVLINK_MIN_INTERVAL) {
|
||||
_main_loop_delay = MAVLINK_MIN_INTERVAL;
|
||||
}
|
||||
|
||||
/* hard limit to 100 Hz at least */
|
||||
if (_main_loop_delay > MAVLINK_MAX_INTERVAL) {
|
||||
_main_loop_delay = MAVLINK_MAX_INTERVAL;
|
||||
}
|
||||
|
||||
/* open the UART device after setting the instance, as it might block */
|
||||
if (get_protocol() == Protocol::SERIAL) {
|
||||
_uart_fd = mavlink_open_uart(_baudrate, _device_name, _flow_control);
|
||||
@ -2234,6 +2239,7 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
uint16_t event_sequence_offset = 0; // offset to account for skipped events, not sent via MAVLink
|
||||
|
||||
_mavlink_start_time = hrt_absolute_time();
|
||||
_bytes_timestamp = _mavlink_start_time;
|
||||
|
||||
while (!should_exit()) {
|
||||
/* main loop */
|
||||
@ -2249,7 +2255,12 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
|
||||
const hrt_abstime t = hrt_absolute_time();
|
||||
|
||||
update_rate_mult();
|
||||
// update rate multiplier periodically, or immediately if radio status or intervals have changed
|
||||
if (_radio_status_changed || _force_rate_mult_update || (t >= _rate_multi_update_last + 1_s)) {
|
||||
update_rate_mult();
|
||||
_rate_multi_update_last = t;
|
||||
_force_rate_mult_update = false;
|
||||
}
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
@ -2436,19 +2447,6 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
/* update streams */
|
||||
for (const auto &stream : _streams) {
|
||||
stream->update(t);
|
||||
|
||||
if (!_first_heartbeat_sent) {
|
||||
if (_mode == MAVLINK_MODE_IRIDIUM) {
|
||||
if (stream->get_id() == MAVLINK_MSG_ID_HIGH_LATENCY2) {
|
||||
_first_heartbeat_sent = stream->first_message_sent();
|
||||
}
|
||||
|
||||
} else {
|
||||
if (stream->get_id() == MAVLINK_MSG_ID_HEARTBEAT) {
|
||||
_first_heartbeat_sent = stream->first_message_sent();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* check for ulog streaming messages */
|
||||
@ -2551,21 +2549,25 @@ Mavlink::task_main(int argc, char *argv[])
|
||||
}
|
||||
}
|
||||
|
||||
/* update TX/RX rates*/
|
||||
if (t > _bytes_timestamp + 1_s) {
|
||||
if (_bytes_timestamp != 0) {
|
||||
const float dt = (t - _bytes_timestamp) * 1e-6f;
|
||||
// update TX/RX rates
|
||||
if (t >= _bytes_timestamp + 1_s) {
|
||||
pthread_mutex_lock(&_send_mutex);
|
||||
|
||||
_tstatus.tx_rate_avg = _bytes_tx / dt;
|
||||
_tstatus.tx_error_rate_avg = _bytes_txerr / dt;
|
||||
_tstatus.rx_rate_avg = _bytes_rx / dt;
|
||||
const float dt_inv = 1e6f / (t - _bytes_timestamp);
|
||||
|
||||
_bytes_tx = 0;
|
||||
_bytes_txerr = 0;
|
||||
_bytes_rx = 0;
|
||||
}
|
||||
_tstatus.tx_rate_avg = _bytes_tx * dt_inv;
|
||||
_tstatus.tx_error_rate_avg = _bytes_txerr * dt_inv;
|
||||
_tstatus.rx_rate_avg = _bytes_rx * dt_inv;
|
||||
|
||||
_bytes_timestamp = t;
|
||||
_bytes_tx = 0;
|
||||
_bytes_txerr = 0;
|
||||
_bytes_rx = 0;
|
||||
|
||||
_bytes_timestamp = _last_write_try_time;
|
||||
|
||||
_force_rate_mult_update = true;
|
||||
|
||||
pthread_mutex_unlock(&_send_mutex);
|
||||
}
|
||||
|
||||
// publish status at 1 Hz, or sooner if HEARTBEAT has updated
|
||||
|
||||
@ -405,6 +405,7 @@ public:
|
||||
List<MavlinkStream *> &get_streams() { return _streams; }
|
||||
|
||||
float get_rate_mult() const { return _rate_mult; }
|
||||
float get_rate_div() const { return _rate_div; }
|
||||
|
||||
float get_baudrate() { return _baudrate; }
|
||||
|
||||
@ -415,6 +416,19 @@ public:
|
||||
bool get_wait_to_transmit() { return _wait_to_transmit; }
|
||||
bool should_transmit() { return (_transmitting_enabled && (!_wait_to_transmit || (_wait_to_transmit && _received_messages))); }
|
||||
|
||||
bool canTransmit(size_t size_bytes)
|
||||
{
|
||||
if (size_bytes <= get_free_tx_buf()) {
|
||||
if ((_bytes_tx + size_bytes) <= _datarate * hrt_elapsed_time(&_bytes_timestamp) * 1e-6f) {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool over_data_rate() { return _over_data_rate.load(); }
|
||||
|
||||
bool message_buffer_write(const void *ptr, int size);
|
||||
|
||||
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
|
||||
@ -519,7 +533,9 @@ public:
|
||||
|
||||
static hrt_abstime &get_first_start_time() { return _first_start_time; }
|
||||
|
||||
bool radio_status_critical() const { return _radio_status_critical; }
|
||||
bool radio_status_critical() const { return _radio_status_available && _radio_status_critical; }
|
||||
|
||||
void set_first_heartbeat_sent() { _first_heartbeat_sent = true; }
|
||||
|
||||
private:
|
||||
MavlinkReceiver _receiver;
|
||||
@ -548,9 +564,8 @@ private:
|
||||
|
||||
static bool _boot_complete;
|
||||
|
||||
static constexpr int MAVLINK_MIN_INTERVAL{1500};
|
||||
static constexpr int MAVLINK_MAX_INTERVAL{10000};
|
||||
static constexpr float MAVLINK_MIN_MULTIPLIER{0.0005f};
|
||||
static constexpr int MAVLINK_MIN_INTERVAL{1000}; // 1000 Hz
|
||||
static constexpr int MAVLINK_MAX_INTERVAL{20000}; // 50 Hz
|
||||
|
||||
mavlink_message_t _mavlink_buffer {};
|
||||
mavlink_status_t _mavlink_status {};
|
||||
@ -563,7 +578,7 @@ private:
|
||||
|
||||
px4::atomic_bool _should_check_events{false}; /**< Events subscription: only one MAVLink instance should check */
|
||||
|
||||
unsigned _main_loop_delay{1000}; /**< mainloop delay, depends on data rate */
|
||||
int _main_loop_delay{MAVLINK_MAX_INTERVAL}; /**< mainloop delay, depends on data rate */
|
||||
|
||||
List<MavlinkStream *> _streams;
|
||||
|
||||
@ -585,9 +600,11 @@ private:
|
||||
int _baudrate{57600};
|
||||
int _datarate{1000}; ///< data rate for normal streams (attitude, position, etc.)
|
||||
float _rate_mult{1.0f};
|
||||
float _rate_div{1.0f};
|
||||
|
||||
bool _radio_status_available{false};
|
||||
bool _radio_status_critical{false};
|
||||
bool _radio_status_changed{false};
|
||||
float _radio_status_mult{1.0f};
|
||||
|
||||
/**
|
||||
@ -616,6 +633,11 @@ private:
|
||||
unsigned _bytes_rx{0};
|
||||
hrt_abstime _bytes_timestamp{0};
|
||||
|
||||
px4::atomic_bool _over_data_rate{false};
|
||||
|
||||
hrt_abstime _rate_multi_update_last{0};
|
||||
bool _force_rate_mult_update{true};
|
||||
|
||||
#if defined(MAVLINK_UDP)
|
||||
BROADCAST_MODE _mav_broadcast {BROADCAST_MODE_OFF};
|
||||
|
||||
@ -635,8 +657,6 @@ private:
|
||||
uint8_t _buf[MAVLINK_MAX_PACKET_LEN] {};
|
||||
unsigned _buf_fill{0};
|
||||
|
||||
bool _tx_buffer_low{false};
|
||||
|
||||
const char *_interface_name{nullptr};
|
||||
|
||||
int _socket_fd{-1};
|
||||
|
||||
@ -466,12 +466,7 @@ MavlinkMissionManager::send_mission_item_reached(uint16_t seq)
|
||||
void
|
||||
MavlinkMissionManager::send()
|
||||
{
|
||||
// do not send anything over high latency communication
|
||||
if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
|
||||
return;
|
||||
}
|
||||
|
||||
mission_result_s mission_result{};
|
||||
mission_result_s mission_result;
|
||||
|
||||
if (_mission_result_sub.update(&mission_result)) {
|
||||
|
||||
@ -1744,11 +1739,6 @@ MavlinkMissionManager::format_mavlink_mission_item(const struct mission_item_s *
|
||||
|
||||
void MavlinkMissionManager::check_active_mission()
|
||||
{
|
||||
// do not send anything over high latency communication
|
||||
if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!(_my_dataman_id == _dataman_id)) {
|
||||
PX4_DEBUG("WPM: New mission detected (possibly over different Mavlink instance) Updating");
|
||||
|
||||
|
||||
@ -66,6 +66,9 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
||||
mavlink_param_request_list_t req_list;
|
||||
mavlink_msg_param_request_list_decode(msg, &req_list);
|
||||
|
||||
PX4_DEBUG("PARAM_REQUEST_LIST target_system: %d, target_component: %d", req_list.target_system,
|
||||
req_list.target_component);
|
||||
|
||||
if (req_list.target_system == mavlink_system.sysid &&
|
||||
(req_list.target_component == mavlink_system.compid || req_list.target_component == MAV_COMP_ID_ALL)) {
|
||||
if (_send_all_index < 0) {
|
||||
@ -96,6 +99,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
||||
mavlink_param_set_t set;
|
||||
mavlink_msg_param_set_decode(msg, &set);
|
||||
|
||||
PX4_DEBUG("PARAM_SET param_id:%s", set.param_id);
|
||||
|
||||
if (set.target_system == mavlink_system.sysid &&
|
||||
(set.target_component == mavlink_system.compid || set.target_component == MAV_COMP_ID_ALL)) {
|
||||
|
||||
@ -162,6 +167,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_REQUEST_READ: {
|
||||
PX4_DEBUG("PARAM_REQUEST_READ");
|
||||
/* request one parameter */
|
||||
mavlink_param_request_read_t req_read;
|
||||
mavlink_msg_param_request_read_decode(msg, &req_read);
|
||||
@ -183,6 +189,8 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
||||
strncpy(param_value.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
|
||||
param_value.param_type = MAV_PARAM_TYPE_UINT32;
|
||||
memcpy(¶m_value.param_value, &hash, sizeof(hash));
|
||||
|
||||
PX4_DEBUG("sending PARAM_REQUEST_READ param_id:'%s', param_index:%d", param_value.param_id, param_value.param_index);
|
||||
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), ¶m_value);
|
||||
|
||||
} else {
|
||||
@ -228,6 +236,7 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
||||
}
|
||||
|
||||
case MAVLINK_MSG_ID_PARAM_MAP_RC: {
|
||||
PX4_DEBUG("PARAM_MAP_RC");
|
||||
/* map a rc channel to a parameter */
|
||||
mavlink_param_map_rc_t map_rc;
|
||||
mavlink_msg_param_map_rc_decode(msg, &map_rc);
|
||||
@ -278,6 +287,10 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
|
||||
void
|
||||
MavlinkParametersManager::send()
|
||||
{
|
||||
if (_mavlink->over_data_rate() || _mavlink->radio_status_critical()) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (!_first_send) {
|
||||
// parameters QGC can't tolerate not finding (2020-11-11)
|
||||
param_find("BAT_CRIT_THR");
|
||||
@ -308,21 +321,32 @@ MavlinkParametersManager::send()
|
||||
_first_send = true;
|
||||
}
|
||||
|
||||
int max_num_to_send;
|
||||
int max_num_to_send = 3;
|
||||
|
||||
if (_mavlink->get_protocol() == Protocol::SERIAL && !_mavlink->is_usb_uart()) {
|
||||
max_num_to_send = 3;
|
||||
|
||||
} else {
|
||||
// speed up parameter loading via UDP or USB: try to send 20 at once
|
||||
max_num_to_send = 20;
|
||||
if ((_mavlink->get_protocol() != Protocol::SERIAL)
|
||||
|| _mavlink->is_usb_uart()
|
||||
|| ((_mavlink->get_protocol() == Protocol::SERIAL) && _mavlink->get_flow_control_enabled())
|
||||
) {
|
||||
// speed up parameter loading via UDP, USB, or serial with working flow control: try to send up to 30 at once (respecting max data rate)
|
||||
max_num_to_send = 30;
|
||||
}
|
||||
|
||||
int i = 0;
|
||||
for (int i = 0; i < max_num_to_send; i++) {
|
||||
// Send while burst is not exceeded, we still have buffer space and still something to send
|
||||
bool transmit = _mavlink->get_free_tx_buf() >= get_size() && !_mavlink->radio_status_critical()
|
||||
&& !_mavlink->over_data_rate();
|
||||
|
||||
// Send while burst is not exceeded, we still have buffer space and still something to send
|
||||
while ((i++ < max_num_to_send) && (_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical()
|
||||
&& send_params()) {}
|
||||
if (transmit) {
|
||||
if (!send_params()) {
|
||||
return;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("MavlinkParametersManager::send() can't transmit, free TX buf: %d, over data rate: %d",
|
||||
_mavlink->get_free_tx_buf(), _mavlink->over_data_rate());
|
||||
return;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool
|
||||
@ -382,6 +406,7 @@ MavlinkParametersManager::send_untransmitted()
|
||||
}
|
||||
}
|
||||
} while ((_mavlink->get_free_tx_buf() >= get_size()) && !_mavlink->radio_status_critical()
|
||||
&& !_mavlink->over_data_rate()
|
||||
&& (_param_update_index < (int) param_count()));
|
||||
|
||||
// Flag work as done once all params have been sent
|
||||
@ -397,7 +422,7 @@ bool
|
||||
MavlinkParametersManager::send_uavcan()
|
||||
{
|
||||
/* Send parameter values received from the UAVCAN topic */
|
||||
uavcan_parameter_value_s value{};
|
||||
uavcan_parameter_value_s value;
|
||||
|
||||
if (_uavcan_parameter_value_sub.update(&value)) {
|
||||
|
||||
@ -470,6 +495,8 @@ MavlinkParametersManager::send_one()
|
||||
strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
|
||||
msg.param_type = MAV_PARAM_TYPE_UINT32;
|
||||
memcpy(&msg.param_value, &hash, sizeof(hash));
|
||||
|
||||
PX4_DEBUG("sending PARAM_VALUE param_id:'%s', param_index:%d", msg.param_id, msg.param_index);
|
||||
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
/* after this we should start sending all params */
|
||||
@ -511,12 +538,7 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
|
||||
return 1;
|
||||
}
|
||||
|
||||
/* no free TX buf to send this param */
|
||||
if (_mavlink->get_free_tx_buf() < MAVLINK_MSG_ID_PARAM_VALUE_LEN) {
|
||||
return 1;
|
||||
}
|
||||
|
||||
mavlink_param_value_t msg;
|
||||
mavlink_param_value_t msg{};
|
||||
|
||||
/*
|
||||
* get param value, since MAVLink encodes float and int params in the same
|
||||
@ -579,6 +601,8 @@ MavlinkParametersManager::send_param(param_t param, int component_id)
|
||||
|
||||
/* default component ID */
|
||||
if (component_id < 0) {
|
||||
PX4_DEBUG("sending PARAM_VALUE param_id:'%s', param_index:%d, param_count:%d", msg.param_id, msg.param_index,
|
||||
msg.param_count);
|
||||
mavlink_msg_param_value_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
} else {
|
||||
|
||||
@ -3085,9 +3085,6 @@ MavlinkReceiver::run()
|
||||
px4_prctl(PR_SET_NAME, thread_name, px4_getpid());
|
||||
}
|
||||
|
||||
// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
|
||||
const int timeout = 10;
|
||||
|
||||
#if defined(__PX4_POSIX)
|
||||
/* 1500 is the Wifi MTU, so we make sure to fit a full packet */
|
||||
uint8_t buf[1600 * 5];
|
||||
@ -3133,7 +3130,10 @@ MavlinkReceiver::run()
|
||||
updateParams();
|
||||
}
|
||||
|
||||
int ret = poll(&fds[0], 1, timeout);
|
||||
// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
|
||||
const int timeout_ms = 10;
|
||||
|
||||
int ret = poll(&fds[0], 1, timeout_ms);
|
||||
|
||||
if (ret > 0) {
|
||||
if (_mavlink->get_protocol() == Protocol::SERIAL) {
|
||||
@ -3268,17 +3268,24 @@ MavlinkReceiver::run()
|
||||
|
||||
CheckHeartbeats(t);
|
||||
|
||||
if (t - last_send_update > timeout * 1000) {
|
||||
_mission_manager.check_active_mission();
|
||||
_mission_manager.send();
|
||||
if (t - last_send_update > timeout_ms * 1000) {
|
||||
|
||||
_parameters_manager.send();
|
||||
// do not send anything over high latency communication
|
||||
if (_mavlink->get_mode() != Mavlink::MAVLINK_MODE_IRIDIUM) {
|
||||
|
||||
_mission_manager.check_active_mission();
|
||||
_mission_manager.send();
|
||||
|
||||
_parameters_manager.send();
|
||||
|
||||
if (_mavlink->ftp_enabled()) {
|
||||
_mavlink_ftp.send();
|
||||
}
|
||||
|
||||
_mavlink_log_handler.send();
|
||||
|
||||
if (_mavlink->ftp_enabled()) {
|
||||
_mavlink_ftp.send();
|
||||
}
|
||||
|
||||
_mavlink_log_handler.send();
|
||||
last_send_update = t;
|
||||
}
|
||||
|
||||
|
||||
@ -47,7 +47,6 @@
|
||||
MavlinkStream::MavlinkStream(Mavlink *mavlink) :
|
||||
_mavlink(mavlink)
|
||||
{
|
||||
_last_sent = hrt_absolute_time();
|
||||
}
|
||||
|
||||
/**
|
||||
@ -58,71 +57,38 @@ MavlinkStream::update(const hrt_abstime &t)
|
||||
{
|
||||
update_data();
|
||||
|
||||
// If the message has never been sent before we want
|
||||
// to send it immediately and can return right away
|
||||
if (_last_sent == 0) {
|
||||
// this will give different messages on the same run a different
|
||||
// initial timestamp which will help spacing them out
|
||||
// on the link scheduling
|
||||
if (send()) {
|
||||
_last_sent = hrt_absolute_time();
|
||||
|
||||
if (!_first_message_sent) {
|
||||
_first_message_sent = true;
|
||||
}
|
||||
}
|
||||
int sz = get_size();
|
||||
|
||||
if ((sz <= 0) || (_interval == 0)) {
|
||||
// We don't need to send anything if the size or interval is 0.
|
||||
return 0;
|
||||
}
|
||||
|
||||
// One of the previous iterations sent the update
|
||||
// already before the deadline
|
||||
if (_last_sent > t) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
int64_t dt = t - _last_sent;
|
||||
int interval = _interval;
|
||||
|
||||
if (!const_rate()) {
|
||||
interval /= _mavlink->get_rate_mult();
|
||||
}
|
||||
|
||||
// We don't need to send anything if the inverval is 0. send() will be called manually.
|
||||
if (interval == 0) {
|
||||
return 0;
|
||||
}
|
||||
|
||||
const bool unlimited_rate = interval < 0;
|
||||
|
||||
// Send the message if it is due or
|
||||
// if it will overrun the next scheduled send interval
|
||||
// by 30% of the interval time. This helps to avoid
|
||||
// sending a scheduled message on average slower than
|
||||
// scheduled. Doing this at 50% would risk sending
|
||||
// the message too often as the loop runtime of the app
|
||||
// needs to be accounted for as well.
|
||||
// This method is not theoretically optimal but a suitable
|
||||
// stopgap as it hits its deadlines well (0.5 Hz, 50 Hz and 250 Hz)
|
||||
|
||||
if (unlimited_rate || (dt > (interval - (_mavlink->get_main_loop_delay() / 10) * 3))) {
|
||||
// interval expired, send message
|
||||
|
||||
// If the interval is non-zero and dt is smaller than 1.5 times the interval
|
||||
// do not use the actual time but increment at a fixed rate, so that processing delays do not
|
||||
// distort the average rate. The check of the maximum interval is done to ensure that after a
|
||||
// long time not sending anything, sending multiple messages in a short time is avoided.
|
||||
if (send()) {
|
||||
_last_sent = ((interval > 0) && ((int64_t)(1.5f * interval) > dt)) ? _last_sent + interval : t;
|
||||
|
||||
if (!_first_message_sent) {
|
||||
_first_message_sent = true;
|
||||
} else if ((_interval < 0) || (_last_sent == 0)) {
|
||||
// if unlimited rate or message has never been sent before
|
||||
// we want to send it immediately and can return right away
|
||||
if (_mavlink->canTransmit(sz)) {
|
||||
if (send()) {
|
||||
_last_sent = t;
|
||||
return 0;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
} else {
|
||||
return -1;
|
||||
_mavlink->count_txerrbytes(sz);
|
||||
}
|
||||
|
||||
} else {
|
||||
int interval = const_rate() ? _interval : (_interval * _mavlink->get_rate_div());
|
||||
|
||||
if (t >= _last_sent + interval) {
|
||||
if (_mavlink->canTransmit(sz)) {
|
||||
if (send()) {
|
||||
_last_sent = math::max(_last_sent + interval, t - interval);
|
||||
return 0;
|
||||
}
|
||||
|
||||
} else {
|
||||
_mavlink->count_txerrbytes(sz);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -114,13 +114,7 @@ public:
|
||||
/**
|
||||
* @return true if the first message of this stream has been sent
|
||||
*/
|
||||
bool first_message_sent() const { return _first_message_sent; }
|
||||
|
||||
/**
|
||||
* Reset the time of last sent to 0. Can be used if a message over this
|
||||
* stream needs to be sent immediately.
|
||||
*/
|
||||
void reset_last_sent() { _last_sent = 0; }
|
||||
bool first_message_sent() const { return _last_sent != 0; }
|
||||
|
||||
protected:
|
||||
Mavlink *const _mavlink;
|
||||
@ -138,7 +132,6 @@ protected:
|
||||
|
||||
private:
|
||||
hrt_abstime _last_sent{0};
|
||||
bool _first_message_sent{false};
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -61,14 +61,12 @@ private:
|
||||
|
||||
bool send() override
|
||||
{
|
||||
bool sent = false;
|
||||
|
||||
transponder_report_s pos;
|
||||
|
||||
while ((_mavlink->get_free_tx_buf() >= get_size()) && _transponder_report_sub.update(&pos)) {
|
||||
if (_transponder_report_sub.update(&pos)) {
|
||||
|
||||
if (!(pos.flags & transponder_report_s::PX4_ADSB_FLAGS_RETRANSLATE)) {
|
||||
continue;
|
||||
return false;
|
||||
}
|
||||
|
||||
mavlink_adsb_vehicle_t msg{};
|
||||
@ -100,10 +98,10 @@ private:
|
||||
if (pos.flags & transponder_report_s::PX4_ADSB_FLAGS_VALID_SQUAWK) { msg.flags |= ADSB_FLAGS_VALID_SQUAWK; }
|
||||
|
||||
mavlink_msg_adsb_vehicle_send_struct(_mavlink->get_channel(), &msg);
|
||||
sent = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
return sent;
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -63,7 +63,7 @@ private:
|
||||
{
|
||||
camera_capture_s capture;
|
||||
|
||||
if ((_mavlink->get_free_tx_buf() >= get_size()) && _capture_sub.update(&capture)) {
|
||||
if (_capture_sub.update(&capture)) {
|
||||
mavlink_camera_image_captured_t msg{};
|
||||
|
||||
msg.time_boot_ms = capture.timestamp / 1000;
|
||||
|
||||
@ -76,7 +76,7 @@ private:
|
||||
{
|
||||
camera_trigger_s camera_trigger;
|
||||
|
||||
if ((_mavlink->get_free_tx_buf() >= get_size()) && _camera_trigger_sub.update(&camera_trigger)) {
|
||||
if (_camera_trigger_sub.update(&camera_trigger)) {
|
||||
/* ensure that only active trigger events are sent and ignore camera capture feedback messages*/
|
||||
if (camera_trigger.timestamp > 0 && !camera_trigger.feedback) {
|
||||
mavlink_camera_trigger_t msg{};
|
||||
|
||||
@ -62,7 +62,7 @@ private:
|
||||
collision_report_s report;
|
||||
bool sent = false;
|
||||
|
||||
while ((_mavlink->get_free_tx_buf() >= get_size()) && _collision_sub.update(&report)) {
|
||||
if (_collision_sub.update(&report)) {
|
||||
mavlink_collision_t msg = {};
|
||||
|
||||
msg.src = report.src;
|
||||
|
||||
@ -60,38 +60,32 @@ private:
|
||||
bool send() override
|
||||
{
|
||||
bool sent = false;
|
||||
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
|
||||
vehicle_command_s cmd;
|
||||
|
||||
static constexpr size_t COMMAND_LONG_SIZE = MAVLINK_MSG_ID_COMMAND_LONG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
|
||||
if (_vehicle_command_sub.update(&cmd)) {
|
||||
if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
|
||||
PX4_ERR("COMMAND_LONG vehicle_command lost, generation %d -> %d", last_generation,
|
||||
_vehicle_command_sub.get_last_generation());
|
||||
}
|
||||
|
||||
while ((_mavlink->get_free_tx_buf() >= COMMAND_LONG_SIZE) && _vehicle_command_sub.updated()) {
|
||||
// mavlink mavlink commands are <= UINT16_MAX
|
||||
const bool px4_internal_cmd = (cmd.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START);
|
||||
|
||||
const unsigned last_generation = _vehicle_command_sub.get_last_generation();
|
||||
vehicle_command_s cmd;
|
||||
// internal commands
|
||||
const bool target_system_internal = (cmd.target_system == _mavlink->get_system_id())
|
||||
&& (cmd.target_component == _mavlink->get_component_id())
|
||||
&& (cmd.source_system == cmd.target_system)
|
||||
&& (cmd.source_component == cmd.target_component);
|
||||
|
||||
if (_vehicle_command_sub.update(&cmd)) {
|
||||
if (_vehicle_command_sub.get_last_generation() != last_generation + 1) {
|
||||
PX4_ERR("COMMAND_LONG vehicle_command lost, generation %d -> %d", last_generation,
|
||||
_vehicle_command_sub.get_last_generation());
|
||||
}
|
||||
if (!cmd.from_external && !px4_internal_cmd && !target_system_internal) {
|
||||
PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
|
||||
|
||||
// mavlink mavlink commands are <= UINT16_MAX
|
||||
const bool px4_internal_cmd = (cmd.command >= vehicle_command_s::VEHICLE_CMD_PX4_INTERNAL_START);
|
||||
MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
|
||||
sent = true;
|
||||
|
||||
// internal commands
|
||||
const bool target_system_internal = (cmd.target_system == _mavlink->get_system_id())
|
||||
&& (cmd.target_component == _mavlink->get_component_id())
|
||||
&& (cmd.source_system == cmd.target_system)
|
||||
&& (cmd.source_component == cmd.target_component);
|
||||
|
||||
if (!cmd.from_external && !px4_internal_cmd && !target_system_internal) {
|
||||
PX4_DEBUG("sending command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
|
||||
|
||||
MavlinkCommandSender::instance().handle_vehicle_command(cmd, _mavlink->get_channel());
|
||||
sent = true;
|
||||
|
||||
} else {
|
||||
PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
|
||||
}
|
||||
} else {
|
||||
PX4_DEBUG("not forwarding command %d to %d/%d", cmd.command, cmd.target_system, cmd.target_component);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -60,9 +60,8 @@ private:
|
||||
bool send() override
|
||||
{
|
||||
gps_inject_data_s gps_inject_data;
|
||||
bool sent = false;
|
||||
|
||||
while ((_mavlink->get_free_tx_buf() >= get_size()) && _gps_inject_data_sub.update(&gps_inject_data)) {
|
||||
if (_gps_inject_data_sub.update(&gps_inject_data)) {
|
||||
mavlink_gps_rtcm_data_t msg{};
|
||||
|
||||
msg.len = gps_inject_data.len;
|
||||
@ -71,10 +70,10 @@ private:
|
||||
|
||||
mavlink_msg_gps_rtcm_data_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
sent = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
return sent;
|
||||
return false;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -67,85 +67,83 @@ private:
|
||||
|
||||
bool send() override
|
||||
{
|
||||
if (_mavlink->get_free_tx_buf() >= get_size()) {
|
||||
// always send the heartbeat, independent of the update status of the topics
|
||||
vehicle_status_s vehicle_status{};
|
||||
_vehicle_status_sub.copy(&vehicle_status);
|
||||
// always send the heartbeat, independent of the update status of the topics
|
||||
vehicle_status_s vehicle_status{};
|
||||
_vehicle_status_sub.copy(&vehicle_status);
|
||||
|
||||
vehicle_status_flags_s vehicle_status_flags{};
|
||||
_vehicle_status_flags_sub.copy(&vehicle_status_flags);
|
||||
vehicle_status_flags_s vehicle_status_flags{};
|
||||
_vehicle_status_flags_sub.copy(&vehicle_status_flags);
|
||||
|
||||
vehicle_control_mode_s vehicle_control_mode{};
|
||||
_vehicle_control_mode_sub.copy(&vehicle_control_mode);
|
||||
vehicle_control_mode_s vehicle_control_mode{};
|
||||
_vehicle_control_mode_sub.copy(&vehicle_control_mode);
|
||||
|
||||
actuator_armed_s actuator_armed{};
|
||||
_acturator_armed_sub.copy(&actuator_armed);
|
||||
actuator_armed_s actuator_armed{};
|
||||
_acturator_armed_sub.copy(&actuator_armed);
|
||||
|
||||
// uint8_t base_mode (MAV_MODE_FLAG) - System mode bitmap.
|
||||
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
|
||||
// uint8_t base_mode (MAV_MODE_FLAG) - System mode bitmap.
|
||||
uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
|
||||
|
||||
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
}
|
||||
|
||||
if (vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) {
|
||||
base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
||||
}
|
||||
|
||||
if (vehicle_control_mode.flag_control_manual_enabled) {
|
||||
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
||||
}
|
||||
|
||||
if (vehicle_control_mode.flag_control_attitude_enabled) {
|
||||
base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
|
||||
}
|
||||
|
||||
if (vehicle_control_mode.flag_control_auto_enabled) {
|
||||
base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
|
||||
}
|
||||
|
||||
|
||||
// uint32_t custom_mode - A bitfield for use for autopilot-specific flags
|
||||
union px4_custom_mode custom_mode {get_px4_custom_mode(vehicle_status.nav_state)};
|
||||
|
||||
|
||||
// uint8_t system_status (MAV_STATE) - System status flag.
|
||||
uint8_t system_status = MAV_STATE_UNINIT;
|
||||
|
||||
switch (vehicle_status.arming_state) {
|
||||
case vehicle_status_s::ARMING_STATE_ARMED:
|
||||
system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::ARMING_STATE_STANDBY:
|
||||
system_status = MAV_STATE_STANDBY;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::ARMING_STATE_SHUTDOWN:
|
||||
system_status = MAV_STATE_POWEROFF;
|
||||
break;
|
||||
}
|
||||
|
||||
// system_status overrides
|
||||
if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown
|
||||
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
|
||||
system_status = MAV_STATE_FLIGHT_TERMINATION;
|
||||
|
||||
} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
|
||||
system_status = MAV_STATE_EMERGENCY;
|
||||
|
||||
} else if (vehicle_status_flags.condition_calibration_enabled) {
|
||||
system_status = MAV_STATE_CALIBRATING;
|
||||
}
|
||||
|
||||
|
||||
mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
|
||||
base_mode, custom_mode.data, system_status);
|
||||
|
||||
return true;
|
||||
if (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
}
|
||||
|
||||
return false;
|
||||
if (vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON) {
|
||||
base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
|
||||
}
|
||||
|
||||
if (vehicle_control_mode.flag_control_manual_enabled) {
|
||||
base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
|
||||
}
|
||||
|
||||
if (vehicle_control_mode.flag_control_attitude_enabled) {
|
||||
base_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED;
|
||||
}
|
||||
|
||||
if (vehicle_control_mode.flag_control_auto_enabled) {
|
||||
base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
|
||||
}
|
||||
|
||||
|
||||
// uint32_t custom_mode - A bitfield for use for autopilot-specific flags
|
||||
union px4_custom_mode custom_mode {get_px4_custom_mode(vehicle_status.nav_state)};
|
||||
|
||||
|
||||
// uint8_t system_status (MAV_STATE) - System status flag.
|
||||
uint8_t system_status = MAV_STATE_UNINIT;
|
||||
|
||||
switch (vehicle_status.arming_state) {
|
||||
case vehicle_status_s::ARMING_STATE_ARMED:
|
||||
system_status = vehicle_status.failsafe ? MAV_STATE_CRITICAL : MAV_STATE_ACTIVE;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::ARMING_STATE_STANDBY:
|
||||
system_status = MAV_STATE_STANDBY;
|
||||
break;
|
||||
|
||||
case vehicle_status_s::ARMING_STATE_SHUTDOWN:
|
||||
system_status = MAV_STATE_POWEROFF;
|
||||
break;
|
||||
}
|
||||
|
||||
// system_status overrides
|
||||
if (actuator_armed.force_failsafe || actuator_armed.lockdown || actuator_armed.manual_lockdown
|
||||
|| vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_TERMINATION) {
|
||||
system_status = MAV_STATE_FLIGHT_TERMINATION;
|
||||
|
||||
} else if (vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL) {
|
||||
system_status = MAV_STATE_EMERGENCY;
|
||||
|
||||
} else if (vehicle_status_flags.condition_calibration_enabled) {
|
||||
system_status = MAV_STATE_CALIBRATING;
|
||||
}
|
||||
|
||||
|
||||
mavlink_msg_heartbeat_send(_mavlink->get_channel(), _mavlink->get_system_type(), MAV_AUTOPILOT_PX4,
|
||||
base_mode, custom_mode.data, system_status);
|
||||
|
||||
_mavlink->set_first_heartbeat_sent();
|
||||
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -87,7 +87,6 @@ private:
|
||||
_throttle(SimpleAnalyzer::AVERAGE),
|
||||
_windspeed(SimpleAnalyzer::AVERAGE)
|
||||
{
|
||||
reset_last_sent();
|
||||
}
|
||||
|
||||
struct PerBatteryData {
|
||||
@ -203,6 +202,10 @@ private:
|
||||
mavlink_msg_high_latency2_send_struct(_mavlink->get_channel(), &msg);
|
||||
}
|
||||
|
||||
if (updated) {
|
||||
_mavlink->set_first_heartbeat_sent();
|
||||
}
|
||||
|
||||
return updated;
|
||||
|
||||
} else {
|
||||
|
||||
@ -55,29 +55,25 @@ private:
|
||||
|
||||
bool send() override
|
||||
{
|
||||
if (_mavlink->get_free_tx_buf() >= get_size()) {
|
||||
mavlink_link_node_status_t link_node_status{};
|
||||
mavlink_link_node_status_t link_node_status{};
|
||||
|
||||
const telemetry_status_s &tstatus = _mavlink->telemetry_status();
|
||||
link_node_status.tx_buf = 0; // % TODO
|
||||
link_node_status.rx_buf = 0; // % TODO
|
||||
link_node_status.tx_rate = tstatus.tx_rate_avg;
|
||||
link_node_status.rx_rate = tstatus.rx_rate_avg;
|
||||
link_node_status.rx_parse_err = tstatus.rx_parse_errors;
|
||||
link_node_status.tx_overflows = tstatus.tx_buffer_overruns;
|
||||
link_node_status.rx_overflows = tstatus.rx_buffer_overruns;
|
||||
link_node_status.messages_sent = tstatus.tx_message_count;
|
||||
link_node_status.messages_received = tstatus.rx_message_count;
|
||||
link_node_status.messages_lost = tstatus.rx_message_lost_count;
|
||||
const telemetry_status_s &tstatus = _mavlink->telemetry_status();
|
||||
link_node_status.tx_buf = 0; // % TODO
|
||||
link_node_status.rx_buf = 0; // % TODO
|
||||
link_node_status.tx_rate = tstatus.tx_rate_avg;
|
||||
link_node_status.rx_rate = tstatus.rx_rate_avg;
|
||||
link_node_status.rx_parse_err = tstatus.rx_parse_errors;
|
||||
link_node_status.tx_overflows = tstatus.tx_buffer_overruns;
|
||||
link_node_status.rx_overflows = tstatus.rx_buffer_overruns;
|
||||
link_node_status.messages_sent = tstatus.tx_message_count;
|
||||
link_node_status.messages_received = tstatus.rx_message_count;
|
||||
link_node_status.messages_lost = tstatus.rx_message_lost_count;
|
||||
|
||||
link_node_status.timestamp = hrt_absolute_time();
|
||||
link_node_status.timestamp = hrt_absolute_time();
|
||||
|
||||
mavlink_msg_link_node_status_send_struct(_mavlink->get_channel(), &link_node_status);
|
||||
mavlink_msg_link_node_status_send_struct(_mavlink->get_channel(), &link_node_status);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
@ -69,56 +69,53 @@ private:
|
||||
bool send() override
|
||||
{
|
||||
if (_mavlink->is_connected()) {
|
||||
while (_mavlink_log_sub.updated() && (_mavlink->get_free_tx_buf() >= get_size())) {
|
||||
const unsigned last_generation = _mavlink_log_sub.get_last_generation();
|
||||
|
||||
const unsigned last_generation = _mavlink_log_sub.get_last_generation();
|
||||
mavlink_log_s mavlink_log;
|
||||
|
||||
mavlink_log_s mavlink_log;
|
||||
if (_mavlink_log_sub.update(&mavlink_log)) {
|
||||
// don't send stale messages
|
||||
if (hrt_elapsed_time(&mavlink_log.timestamp) < 2_s) {
|
||||
|
||||
if (_mavlink_log_sub.update(&mavlink_log)) {
|
||||
// don't send stale messages
|
||||
if (hrt_elapsed_time(&mavlink_log.timestamp) < 2_s) {
|
||||
|
||||
if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) {
|
||||
perf_count(_missed_msg_count_perf);
|
||||
PX4_DEBUG("channel %d has missed %d mavlink log messages", _mavlink->get_channel(),
|
||||
perf_event_count(_missed_msg_count_perf));
|
||||
}
|
||||
|
||||
mavlink_statustext_t msg{};
|
||||
const char *text = mavlink_log.text;
|
||||
constexpr unsigned max_chunk_size = sizeof(msg.text);
|
||||
msg.severity = mavlink_log.severity;
|
||||
msg.chunk_seq = 0;
|
||||
msg.id = _id++;
|
||||
unsigned text_size;
|
||||
|
||||
while ((text_size = strlen(text)) > 0) {
|
||||
unsigned chunk_size = math::min(text_size, max_chunk_size);
|
||||
|
||||
if (chunk_size < max_chunk_size) {
|
||||
memcpy(&msg.text[0], &text[0], chunk_size);
|
||||
// pad with zeros
|
||||
memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size);
|
||||
|
||||
} else {
|
||||
memcpy(&msg.text[0], &text[0], chunk_size);
|
||||
}
|
||||
|
||||
mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
if (text_size <= max_chunk_size) {
|
||||
break;
|
||||
|
||||
} else {
|
||||
text += max_chunk_size;
|
||||
}
|
||||
|
||||
msg.chunk_seq += 1;
|
||||
}
|
||||
|
||||
return true;
|
||||
if (_mavlink_log_sub.get_last_generation() != (last_generation + 1)) {
|
||||
perf_count(_missed_msg_count_perf);
|
||||
PX4_DEBUG("channel %d has missed %d mavlink log messages", _mavlink->get_channel(),
|
||||
perf_event_count(_missed_msg_count_perf));
|
||||
}
|
||||
|
||||
mavlink_statustext_t msg{};
|
||||
const char *text = mavlink_log.text;
|
||||
constexpr unsigned max_chunk_size = sizeof(msg.text);
|
||||
msg.severity = mavlink_log.severity;
|
||||
msg.chunk_seq = 0;
|
||||
msg.id = _id++;
|
||||
unsigned text_size;
|
||||
|
||||
while ((text_size = strlen(text)) > 0) {
|
||||
unsigned chunk_size = math::min(text_size, max_chunk_size);
|
||||
|
||||
if (chunk_size < max_chunk_size) {
|
||||
memcpy(&msg.text[0], &text[0], chunk_size);
|
||||
// pad with zeros
|
||||
memset(&msg.text[0] + chunk_size, 0, max_chunk_size - chunk_size);
|
||||
|
||||
} else {
|
||||
memcpy(&msg.text[0], &text[0], chunk_size);
|
||||
}
|
||||
|
||||
mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
|
||||
|
||||
if (text_size <= max_chunk_size) {
|
||||
break;
|
||||
|
||||
} else {
|
||||
text += max_chunk_size;
|
||||
}
|
||||
|
||||
msg.chunk_seq += 1;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user