From 36403e9025dc0b679427d2a8f0869a547ae372a0 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 6 Aug 2018 11:25:05 -0400
Subject: [PATCH] Mavlink expand telemetry_status and split radio_status
---
msg/CMakeLists.txt | 3 +-
msg/radio_status.msg | 22 +++
msg/telemetry_status.msg | 28 ++-
src/modules/logger/logger.cpp | 2 +
src/modules/mavlink/mavlink_main.cpp | 228 ++++++++++-------------
src/modules/mavlink/mavlink_main.h | 55 +++---
src/modules/mavlink/mavlink_receiver.cpp | 89 +++------
src/modules/mavlink/mavlink_receiver.h | 2 +-
8 files changed, 184 insertions(+), 245 deletions(-)
create mode 100644 msg/radio_status.msg
diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt
index 4adc266dd3..b047f00f2e 100644
--- a/msg/CMakeLists.txt
+++ b/msg/CMakeLists.txt
@@ -86,6 +86,7 @@ set(msg_files
pwm_input.msg
qshell_req.msg
qshell_retval.msg
+ radio_status.msg
rate_ctrl_status.msg
rc_channels.msg
rc_parameter_map.msg
@@ -120,6 +121,7 @@ set(msg_files
vehicle_attitude_setpoint.msg
vehicle_command.msg
vehicle_command_ack.msg
+ vehicle_constraints.msg
vehicle_control_mode.msg
vehicle_global_position.msg
vehicle_gps_position.msg
@@ -134,7 +136,6 @@ set(msg_files
vehicle_trajectory_waypoint.msg
vtol_vehicle_status.msg
wind_estimate.msg
- vehicle_constraints.msg
)
if(NOT EXTERNAL_MODULES_LOCATION STREQUAL "")
diff --git a/msg/radio_status.msg b/msg/radio_status.msg
new file mode 100644
index 0000000000..29ee47af74
--- /dev/null
+++ b/msg/radio_status.msg
@@ -0,0 +1,22 @@
+
+uint64 timestamp # time since system start (microseconds)
+
+uint8 RADIO_TYPE_GENERIC = 0
+uint8 RADIO_TYPE_3DR_RADIO = 1
+uint8 RADIO_TYPE_UBIQUITY_BULLET = 2
+uint8 RADIO_TYPE_WIRE = 3
+uint8 RADIO_TYPE_USB = 4
+uint8 RADIO_TYPE_IRIDIUM = 5
+
+uint8 type # type of the radio hardware
+
+uint8 rssi # local signal strength
+uint8 remote_rssi # remote signal strength
+
+uint8 txbuf # how full the tx buffer is as a percentage
+uint8 noise # background noise level
+
+uint8 remote_noise # remote background noise level
+uint16 rxerrors # receive errors
+
+uint16 fixed # count of error corrected packets
diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg
index 4424fb9a74..2bc34ce4d8 100644
--- a/msg/telemetry_status.msg
+++ b/msg/telemetry_status.msg
@@ -8,14 +8,26 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4
uint8 TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM = 5
uint64 heartbeat_time # Time of last received heartbeat from remote system
-uint64 telem_time # Time of last received telemetry status packet, 0 for none
+
uint8 type # type of the radio hardware
-uint8 rssi # local signal strength
-uint8 remote_rssi # remote signal strength
-uint16 rxerrors # receive errors
-uint16 fixed # count of error corrected packets
-uint8 noise # background noise level
-uint8 remote_noise # remote background noise level
-uint8 txbuf # how full the tx buffer is as a percentage
+
uint8 system_id # system id of the remote system
uint8 component_id # component id of the remote system
+
+uint8 mode
+
+bool flow_control
+bool forwarding
+bool mavlink_v2
+bool ftp
+
+uint8 streams
+
+float32 data_rate
+
+float32 rate_multiplier
+
+float32 rate_rx
+
+float32 rate_tx
+float32 rate_txerr
diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp
index 00f8d880bf..780c601e24 100644
--- a/src/modules/logger/logger.cpp
+++ b/src/modules/logger/logger.cpp
@@ -624,11 +624,13 @@ void Logger::add_default_topics()
add_topic("mission_result");
add_topic("optical_flow", 50);
add_topic("position_setpoint_triplet", 200);
+ add_topic("radio_status");
add_topic("rate_ctrl_status", 30);
add_topic("sensor_combined", 100);
add_topic("sensor_preflight", 200);
add_topic("system_power", 500);
add_topic("tecs_status", 200);
+ add_topic("telemetry_status");
add_topic("vehicle_attitude", 30);
add_topic("vehicle_attitude_setpoint", 100);
add_topic("vehicle_command");
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index a82a58fbcd..b004f549aa 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -72,6 +72,7 @@
#include <lib/ecl/geo/geo.h>
#include <dataman/dataman.h>
#include <version/version.h>
+#include <mathlib/mathlib.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_command_ack.h>
@@ -186,7 +187,6 @@ mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
static void usage();
bool Mavlink::_boot_complete = false;
-bool Mavlink::_config_link_on = false;
Mavlink::Mavlink() :
_device_name("/dev/ttyS1"),
@@ -222,9 +222,7 @@ Mavlink::Mavlink() :
_uart_fd(-1),
_baudrate(57600),
_datarate(1000),
- _datarate_events(500),
_rate_mult(1.0f),
- _last_hw_rate_timestamp(0),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
@@ -240,9 +238,6 @@ Mavlink::Mavlink() :
_bytes_txerr(0),
_bytes_rx(0),
_bytes_timestamp(0),
- _rate_tx(0.0f),
- _rate_txerr(0.0f),
- _rate_rx(0.0f),
#ifdef __PX4_POSIX
_myaddr {},
_src_addr{},
@@ -258,8 +253,6 @@ Mavlink::Mavlink() :
_protocol(SERIAL),
_network_port(14556),
_remote_port(DEFAULT_REMOTE_PORT_UDP),
- _rstatus {},
- _ping_stats{},
_message_buffer {},
_message_buffer_mutex {},
_send_mutex {},
@@ -275,8 +268,7 @@ Mavlink::Mavlink() :
_system_type(0),
/* performance counters */
- _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
- _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
+ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el"))
{
_instance_id = Mavlink::instance_count();
@@ -322,13 +314,12 @@ Mavlink::Mavlink() :
break;
}
- _rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
+ _tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
}
Mavlink::~Mavlink()
{
perf_free(_loop_perf);
- perf_free(_txerr_perf);
if (_task_running) {
/* task wakes up every 10ms or so at the longest */
@@ -366,12 +357,6 @@ Mavlink::set_proto_version(unsigned version)
}
}
-void
-Mavlink::count_txerr()
-{
- perf_count(_txerr_perf);
-}
-
int
Mavlink::instance_count()
{
@@ -814,7 +799,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_
_is_usb_uart = true;
/* USB has no baudrate, but use a magic number for 'fast' */
_baudrate = 2000000;
- _rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB;
+ _tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB;
}
#if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN)
@@ -919,8 +904,9 @@ Mavlink::get_free_tx_buf()
* and if the last try was not the last successful write
*/
if (_last_write_try_time != 0 &&
- hrt_elapsed_time(&_last_write_success_time) > 500 * 1000UL &&
+ hrt_elapsed_time(&_last_write_success_time) > 500_ms &&
_last_write_success_time != _last_write_try_time) {
+
enable_flow_control(FLOW_CONTROL_OFF);
}
}
@@ -952,16 +938,13 @@ Mavlink::send_packet()
if (get_protocol() == UDP) {
-
ret = sendto(_socket_fd, _network_buf, _network_buf_len, 0,
(struct sockaddr *)&_src_addr, sizeof(_src_addr));
- struct telemetry_status_s &tstatus = get_rx_status();
-
/* resend message via broadcast if no valid connection exists */
if ((_mode != MAVLINK_MODE_ONBOARD) && broadcast_enabled() &&
(!get_client_source_initialized()
- || (hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000))) {
+ || (hrt_elapsed_time(&_tstatus.heartbeat_time) > 3_s))) {
if (!_broadcast_address_found) {
find_broadcast_address();
@@ -1017,7 +1000,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
if (buf_free < packet_len) {
/* not enough space in buffer to send */
- count_txerr();
count_txerrbytes(packet_len);
return;
}
@@ -1044,7 +1026,6 @@ Mavlink::send_bytes(const uint8_t *buf, unsigned packet_len)
#endif
if (ret != (size_t) packet_len) {
- count_txerr();
count_txerrbytes(packet_len);
} else {
@@ -1247,10 +1228,6 @@ Mavlink::init_udp()
void
Mavlink::handle_message(const mavlink_message_t *msg)
{
- if (!accepting_commands()) {
- return;
- }
-
/*
* NOTE: this is called from the receiver thread
*/
@@ -1378,27 +1355,20 @@ MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int
return sub_new;
}
-int
-Mavlink::interval_from_rate(float rate)
-{
- if (rate > 0.000001f) {
- return (1000000.0f / rate);
-
- } else if (rate < 0.0f) {
- return -1;
-
- } else {
- return 0;
- }
-}
-
int
Mavlink::configure_stream(const char *stream_name, const float rate)
{
PX4_DEBUG("configure_stream(%s, %.3f)", stream_name, (double)rate);
/* calculate interval in us, -1 means unlimited stream, 0 means disabled */
- int interval = interval_from_rate(rate);
+ int interval = 0;
+
+ if (rate > 0.000001f) {
+ interval = (1000000.0f / rate);
+
+ } else if (rate < 0.0f) {
+ interval = -1;
+ }
/* search if stream exists */
MavlinkStream *stream;
@@ -1440,38 +1410,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate)
return PX4_ERROR;
}
-void
-Mavlink::adjust_stream_rates(const float multiplier)
-{
- /* do not allow to push us to zero */
- if (multiplier < MAVLINK_MIN_MULTIPLIER) {
- return;
- }
-
- /* search if stream exists */
- MavlinkStream *stream;
- LL_FOREACH(_streams, stream) {
- /* set new interval */
- int interval = stream->get_interval();
-
- if (interval > 0) {
- interval /= multiplier;
-
- /* limit min / max interval */
- if (interval < MAVLINK_MIN_INTERVAL) {
- interval = MAVLINK_MIN_INTERVAL;
- }
-
- if (interval > MAVLINK_MAX_INTERVAL) {
- interval = MAVLINK_MAX_INTERVAL;
- }
-
- /* set new interval */
- stream->set_interval(interval);
- }
- }
-}
-
void
Mavlink::configure_stream_threadsafe(const char *stream_name, const float rate)
{
@@ -1631,12 +1569,6 @@ Mavlink::pass_message(const mavlink_message_t *msg)
}
}
-float
-Mavlink::get_rate_mult()
-{
- return _rate_mult;
-}
-
MavlinkShell *
Mavlink::get_shell()
{
@@ -1700,57 +1632,52 @@ Mavlink::update_rate_mult()
bandwidth_mult = fminf(1.0f, bandwidth_mult);
}
- /* check if we have radio feedback */
- struct telemetry_status_s &tstatus = get_rx_status();
+ float hardware_mult = 1.0f;
- bool radio_critical = false;
- bool radio_found = false;
+ /* scale down if we have a TX err rate suggesting link congestion */
+ if (_tstatus.rate_txerr > 0.0f && !_radio_status_critical) {
+ hardware_mult = (_tstatus.rate_tx) / (_tstatus.rate_tx + _tstatus.rate_txerr);
- /* 2nd pass: Now check hardware limits */
- if (tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
+ } else if (_radio_status_available) {
+ hardware_mult *= _radio_status_mult;
+ }
- radio_found = true;
+ /* pick the minimum from bandwidth mult and hardware mult as limit */
+ _rate_mult = fminf(bandwidth_mult, hardware_mult);
- if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
- radio_critical = true;
- }
- }
+ /* ensure the rate multiplier never drops below 5% so that something is always sent */
+ _rate_mult = math::constrain(_rate_mult, 0.05f, 1.0f);
+}
- float hardware_mult = _rate_mult;
+void
+Mavlink::update_radio_status(const radio_status_s &radio_status)
+{
+ _rstatus = radio_status;
- /* scale down if we have a TX err rate suggesting link congestion */
- if (_rate_txerr > 0.0f && !radio_critical) {
- hardware_mult = (_rate_tx) / (_rate_tx + _rate_txerr);
+ /* check hardware limits */
+ if (radio_status.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
- } else if (radio_found && tstatus.telem_time != _last_hw_rate_timestamp) {
+ _radio_status_available = true;
+ _radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE);
- if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
+ if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 20% */
- hardware_mult *= 0.80f;
+ _radio_status_mult *= 0.80f;
- } else if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
+ } else if (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 2.5% */
- hardware_mult *= 0.975f;
+ _radio_status_mult *= 0.975f;
- } else if (tstatus.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
+ } else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
/* this indicates spare bandwidth, increase by 2.5% */
- hardware_mult *= 1.025f;
- /* limit to a max multiplier of 1 */
- hardware_mult = fminf(1.0f, hardware_mult);
+ _radio_status_mult *= 1.025f;
}
- } else if (!radio_found) {
- /* no limitation, set hardware to 1 */
- hardware_mult = 1.0f;
+ } else {
+ _radio_status_available = false;
+ _radio_status_critical = false;
+ _radio_status_mult = 1.0f;
}
-
- _last_hw_rate_timestamp = tstatus.telem_time;
-
- /* pick the minimum from bandwidth mult and hardware mult as limit */
- _rate_mult = fminf(bandwidth_mult, hardware_mult);
-
- /* ensure the rate multiplier never drops below 5% so that something is always sent */
- _rate_mult = fmaxf(0.05f, _rate_mult);
}
int
@@ -2079,7 +2006,7 @@ Mavlink::task_main(int argc, char *argv[])
} else if (strcmp(myoptarg, "iridium") == 0) {
_mode = MAVLINK_MODE_IRIDIUM;
- _rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
+ _tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
} else if (strcmp(myoptarg, "minimal") == 0) {
_mode = MAVLINK_MODE_MINIMAL;
@@ -2517,10 +2444,12 @@ Mavlink::task_main(int argc, char *argv[])
/* update TX/RX rates*/
if (t > _bytes_timestamp + 1000000) {
if (_bytes_timestamp != 0) {
- float dt = (t - _bytes_timestamp) / 1000.0f;
- _rate_tx = _bytes_tx / dt;
- _rate_txerr = _bytes_txerr / dt;
- _rate_rx = _bytes_rx / dt;
+ const float dt = (t - _bytes_timestamp) / 1000.0f;
+
+ _tstatus.rate_tx = _bytes_tx / dt;
+ _tstatus.rate_txerr = _bytes_txerr / dt;
+ _tstatus.rate_rx = _bytes_rx / dt;
+
_bytes_tx = 0;
_bytes_txerr = 0;
_bytes_rx = 0;
@@ -2529,6 +2458,11 @@ Mavlink::task_main(int argc, char *argv[])
_bytes_timestamp = t;
}
+ // publish status at 1 Hz, or sooner if HEARTBEAT has updated
+ if ((hrt_elapsed_time(&_tstatus.timestamp) >= 1_s) || (_tstatus.timestamp < _tstatus.heartbeat_time)) {
+ publish_telemetry_status();
+ }
+
perf_end(_loop_perf);
/* confirm task running only once fully initialized */
@@ -2590,10 +2524,37 @@ Mavlink::task_main(int argc, char *argv[])
return OK;
}
+void Mavlink::publish_telemetry_status()
+{
+ // many fields are populated in place
+
+ _tstatus.mode = _mode;
+ _tstatus.data_rate = _datarate;
+ _tstatus.rate_multiplier = _rate_mult;
+ _tstatus.flow_control = get_flow_control_enabled();
+ _tstatus.ftp = ftp_enabled();
+ _tstatus.forwarding = get_forwarding_on();
+ _tstatus.mavlink_v2 = (_protocol_version == 2);
+
+ int num_streams = 0;
+
+ MavlinkStream *stream;
+ LL_FOREACH(_streams, stream) {
+ // count
+ num_streams++;
+ }
+
+ _tstatus.streams = num_streams;
+
+ _tstatus.timestamp = hrt_absolute_time();
+ int instance;
+ orb_publish_auto(ORB_ID(telemetry_status), &_telem_status_pub, &_tstatus, &instance, ORB_PRIO_DEFAULT);
+}
+
void Mavlink::check_radio_config()
{
/* radio config check */
- if (_uart_fd >= 0 && _radio_id != 0 && _rstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
+ if (_uart_fd >= 0 && _radio_id != 0 && _tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
/* request to configure radio and radio is present */
FILE *fs = fdopen(_uart_fd, "w");
@@ -2727,17 +2688,17 @@ void
Mavlink::display_status()
{
- if (_rstatus.heartbeat_time > 0) {
- printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_rstatus.heartbeat_time));
+ if (_tstatus.heartbeat_time > 0) {
+ printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_tstatus.heartbeat_time));
}
printf("\tmavlink chan: #%u\n", _channel);
- if (_rstatus.timestamp > 0) {
+ if (_tstatus.timestamp > 0) {
printf("\ttype:\t\t");
- switch (_rstatus.type) {
+ switch (_tstatus.type) {
case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
printf("3DR RADIO\n");
printf("\trssi:\t\t%d\n", _rstatus.rssi);
@@ -2759,24 +2720,23 @@ Mavlink::display_status()
}
} else {
- printf("\tno telem status.\n");
+ printf("\tno radio status.\n");
}
printf("\tflow control: %s\n", _flow_control_mode ? "ON" : "OFF");
printf("\trates:\n");
- printf("\t tx: %.3f kB/s\n", (double)_rate_tx);
- printf("\t txerr: %.3f kB/s\n", (double)_rate_txerr);
+ printf("\t tx: %.3f kB/s\n", (double)_tstatus.rate_tx);
+ printf("\t txerr: %.3f kB/s\n", (double)_tstatus.rate_txerr);
printf("\t tx rate mult: %.3f\n", (double)_rate_mult);
printf("\t tx rate max: %i B/s\n", _datarate);
- printf("\t rx: %.3f kB/s\n", (double)_rate_rx);
+ printf("\t rx: %.3f kB/s\n", (double)_tstatus.rate_rx);
if (_mavlink_ulog) {
printf("\tULog rate: %.1f%% of max %.1f%%\n", (double)_mavlink_ulog->current_data_rate() * 100.,
(double)_mavlink_ulog->maximum_data_rate() * 100.);
}
- printf("\taccepting commands: %s, FTP enabled: %s, TX enabled: %s\n",
- accepting_commands() ? "YES" : "NO",
+ printf("\tFTP enabled: %s, TX enabled: %s\n",
_ftp_on ? "YES" : "NO",
_transmitting_enabled ? "YES" : "NO");
printf("\tmode: %s\n", mavlink_mode_str(_mode));
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 09a54fdfae..37d9129de6 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -66,6 +66,7 @@
#include <uORB/uORB.h>
#include <uORB/topics/mission.h>
#include <uORB/topics/mission_result.h>
+#include <uORB/topics/radio_status.h>
#include <uORB/topics/telemetry_status.h>
#include "mavlink_bridge_header.h"
@@ -81,6 +82,8 @@ enum Protocol {
TCP,
};
+using namespace time_literals;
+
#define HASH_PARAM "_HASH_CHECK"
class Mavlink
@@ -233,13 +236,9 @@ public:
bool get_forwarding_on() { return _forwarding_on; }
- bool get_config_link_on() { return _config_link_on; }
-
- void set_config_link_on(bool on) { _config_link_on = on; }
+ bool is_connected() { return ((_tstatus.heartbeat_time > 0) && (hrt_absolute_time() - _tstatus.heartbeat_time < 3_s)); }
- bool is_connected() { return ((_rstatus.heartbeat_time > 0) && (hrt_absolute_time() - _rstatus.heartbeat_time < 3000000)); }
-
- bool broadcast_enabled() { return _broadcast_mode > BROADCAST_MODE_OFF; }
+ bool broadcast_enabled() { return _broadcast_mode == BROADCAST_MODE_ON; }
/**
* Set the boot complete flag on all instances
@@ -387,7 +386,7 @@ public:
MavlinkStream *get_streams() const { return _streams; }
- float get_rate_mult();
+ float get_rate_mult() const { return _rate_mult; }
float get_baudrate() { return _baudrate; }
@@ -403,11 +402,6 @@ public:
void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }
- /**
- * Count a transmission error
- */
- void count_txerr();
-
/**
* Count transmitted bytes
*/
@@ -426,7 +420,9 @@ public:
/**
* Get the receive status of this MAVLink link
*/
- struct telemetry_status_s &get_rx_status() { return _rstatus; }
+ telemetry_status_s &get_telemetry_status() { return _tstatus; }
+
+ void update_radio_status(const radio_status_s &radio_status);
ringbuffer::RingBuffer *get_logbuffer() { return &_logbuffer; }
@@ -459,8 +455,6 @@ public:
bool is_usb_uart() { return _is_usb_uart; }
- bool accepting_commands() { return true; /* non-trivial side effects ((!_config_link_on) || (_mode == MAVLINK_MODE_CONFIG));*/ }
-
int get_data_rate() { return _datarate; }
void set_data_rate(int rate) { if (rate > 0) { _datarate = rate; } }
@@ -514,7 +508,9 @@ private:
bool _transmitting_enabled_commanded;
bool _first_heartbeat_sent{false};
- orb_advert_t _mavlink_log_pub;
+ orb_advert_t _mavlink_log_pub{nullptr};
+ orb_advert_t _telem_status_pub{nullptr};
+
bool _task_running;
static bool _boot_complete;
static constexpr int MAVLINK_MAX_INSTANCES = 4;
@@ -558,9 +554,11 @@ private:
int _baudrate;
int _datarate; ///< data rate for normal streams (attitude, position, etc.)
- int _datarate_events; ///< data rate for params, waypoints, text messages
float _rate_mult;
- hrt_abstime _last_hw_rate_timestamp;
+
+ bool _radio_status_available{false};
+ bool _radio_status_critical{false};
+ float _radio_status_mult{1.0f};
/**
* If the queue index is not at 0, the queue sending
@@ -586,9 +584,6 @@ private:
unsigned _bytes_txerr;
unsigned _bytes_rx;
uint64_t _bytes_timestamp;
- float _rate_tx;
- float _rate_txerr;
- float _rate_rx;
#ifdef __PX4_POSIX
struct sockaddr_in _myaddr;
@@ -609,9 +604,10 @@ private:
unsigned short _network_port;
unsigned short _remote_port;
- struct telemetry_status_s _rstatus; ///< receive status
+ radio_status_s _rstatus{};
+ telemetry_status_s _tstatus{};
- struct ping_statistics_s _ping_stats; ///< ping statistics
+ ping_statistics_s _ping_stats{};
struct mavlink_message_buffer {
int write_ptr;
@@ -638,17 +634,13 @@ private:
param_t _param_broadcast;
unsigned _system_type;
- static bool _config_link_on;
perf_counter_t _loop_perf; /**< loop performance counter */
- perf_counter_t _txerr_perf; /**< TX error counter */
void mavlink_update_system();
int mavlink_open_uart(int baudrate, const char *uart_name, bool force_flow_control);
- static int interval_from_rate(float rate);
-
static constexpr unsigned RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE = 25;
static constexpr unsigned RADIO_BUFFER_LOW_PERCENTAGE = 35;
static constexpr unsigned RADIO_BUFFER_HALF_PERCENTAGE = 50;
@@ -669,13 +661,6 @@ private:
*/
int configure_streams_to_default(const char *configure_single_stream = nullptr);
- /**
- * Adjust the stream rates based on the current rate
- *
- * @param multiplier if greater than 1, the transmission rate will increase, if smaller than one decrease
- */
- void adjust_stream_rates(const float multiplier);
-
int message_buffer_init(int size);
void message_buffer_destroy();
@@ -690,6 +675,8 @@ private:
void pass_message(const mavlink_message_t *msg);
+ void publish_telemetry_status();
+
/**
* Check the configuration of a connected radio
*
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index dc87d49545..f04b2464e3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -89,6 +89,7 @@
#include <commander/px4_custom_mode.h>
+#include <uORB/topics/radio_status.h>
#include <uORB/topics/vehicle_command_ack.h>
#include "mavlink_bridge_header.h"
@@ -132,7 +133,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
_att_pos_mocap_pub(nullptr),
_vision_position_pub(nullptr),
_vision_attitude_pub(nullptr),
- _telemetry_status_pub(nullptr),
+ _radio_status_pub(nullptr),
_ping_pub(nullptr),
_rc_pub(nullptr),
_manual_pub(nullptr),
@@ -193,25 +194,13 @@ void MavlinkReceiver::acknowledge(uint8_t sysid, uint8_t compid, uint16_t comman
void
MavlinkReceiver::handle_message(mavlink_message_t *msg)
{
- if (!_mavlink->get_config_link_on()) {
- if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_CONFIG) {
- _mavlink->set_config_link_on(true);
- }
- }
-
switch (msg->msgid) {
case MAVLINK_MSG_ID_COMMAND_LONG:
- if (_mavlink->accepting_commands()) {
- handle_message_command_long(msg);
- }
-
+ handle_message_command_long(msg);
break;
case MAVLINK_MSG_ID_COMMAND_INT:
- if (_mavlink->accepting_commands()) {
- handle_message_command_int(msg);
- }
-
+ handle_message_command_int(msg);
break;
case MAVLINK_MSG_ID_COMMAND_ACK:
@@ -227,10 +216,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
break;
case MAVLINK_MSG_ID_SET_MODE:
- if (_mavlink->accepting_commands()) {
- handle_message_set_mode(msg);
- }
-
+ handle_message_set_mode(msg);
break;
case MAVLINK_MSG_ID_ATT_POS_MOCAP:
@@ -1426,29 +1412,21 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
mavlink_radio_status_t rstatus;
mavlink_msg_radio_status_decode(msg, &rstatus);
- struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
-
- tstatus.timestamp = hrt_absolute_time();
- tstatus.telem_time = tstatus.timestamp;
- /* tstatus.heartbeat_time is set by system heartbeats */
- tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
- tstatus.rssi = rstatus.rssi;
- tstatus.remote_rssi = rstatus.remrssi;
- tstatus.txbuf = rstatus.txbuf;
- tstatus.noise = rstatus.noise;
- tstatus.remote_noise = rstatus.remnoise;
- tstatus.rxerrors = rstatus.rxerrors;
- tstatus.fixed = rstatus.fixed;
- tstatus.system_id = msg->sysid;
- tstatus.component_id = msg->compid;
-
- if (_telemetry_status_pub == nullptr) {
- int multi_instance;
- _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
+ radio_status_s status = {};
+ status.timestamp = hrt_absolute_time();
+ status.type = radio_status_s::RADIO_TYPE_3DR_RADIO;
+ status.rssi = rstatus.rssi;
+ status.remote_rssi = rstatus.remrssi;
+ status.txbuf = rstatus.txbuf;
+ status.noise = rstatus.noise;
+ status.remote_noise = rstatus.remnoise;
+ status.rxerrors = rstatus.rxerrors;
+ status.fixed = rstatus.fixed;
- } else {
- orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
- }
+ _mavlink->update_radio_status(status);
+
+ int multi_instance;
+ orb_publish_auto(ORB_ID(radio_status), &_radio_status_pub, &status, &multi_instance, ORB_PRIO_HIGH);
}
}
@@ -1895,21 +1873,14 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
/* ignore own heartbeats, accept only heartbeats from GCS */
if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
- struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
+ telemetry_status_s &tstatus = _mavlink->get_telemetry_status();
/* set heartbeat time and topic time and publish -
* the telem status also gets updated on telemetry events
*/
- tstatus.timestamp = hrt_absolute_time();
tstatus.heartbeat_time = tstatus.timestamp;
-
- if (_telemetry_status_pub == nullptr) {
- int multi_instance;
- _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
-
- } else {
- orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
- }
+ tstatus.system_id = msg->sysid;
+ tstatus.component_id = msg->compid;
}
}
}
@@ -2538,22 +2509,6 @@ MavlinkReceiver::receive_thread(void *arg)
// poll timeout in ms. Also defines the max update frequency of the mission & param manager, etc.
const int timeout = 10;
- // publish the telemetry status once for the iridium telemetry
- if (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_IRIDIUM) {
- struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
-
- tstatus.timestamp = hrt_absolute_time();
- tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM;
-
- if (_telemetry_status_pub == nullptr) {
- int multi_instance;
- _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
-
- } else {
- orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
- }
- }
-
#ifdef __PX4_POSIX
/* 1500 is the Wifi MTU, so we make sure to fit a full packet */
uint8_t buf[1600 * 5];
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index 407322c39a..ee638c3ab1 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -229,7 +229,7 @@ private:
orb_advert_t _att_pos_mocap_pub;
orb_advert_t _vision_position_pub;
orb_advert_t _vision_attitude_pub;
- orb_advert_t _telemetry_status_pub;
+ orb_advert_t _radio_status_pub;
orb_advert_t _ping_pub;
orb_advert_t _rc_pub;
orb_advert_t _manual_pub;
--
GitLab