dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Mavlink expand telemetry_status and split radio_status

This commit is contained in:
Daniel Agar 2018-08-06 11:25:05 -04:00 committed by Lorenz Meier
parent f4f112424f
commit 36403e9025
8 changed files with 190 additions and 251 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
msg/CMakeLists.txt
View File

@ -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 "")

22
msg/radio_status.msg Normal file
View File

@ -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

28
msg/telemetry_status.msg
View File

@ -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

2
src/modules/logger/logger.cpp
View File

@ -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");

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

@ -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();
bool radio_critical = false;
bool radio_found = false;
/* 2nd pass: Now check hardware limits */
if (tstatus.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
radio_found = true;
if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
radio_critical = true;
}
}
float hardware_mult = _rate_mult;
float hardware_mult = 1.0f;
/* 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);
if (_tstatus.rate_txerr > 0.0f && !_radio_status_critical) {
hardware_mult = (_tstatus.rate_tx) / (_tstatus.rate_tx + _tstatus.rate_txerr);
} else if (radio_found && tstatus.telem_time != _last_hw_rate_timestamp) {
if (tstatus.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 20% */
hardware_mult *= 0.80f;
} else if (tstatus.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 2.5% */
hardware_mult *= 0.975f;
} else if (tstatus.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);
}
} else if (!radio_found) {
/* no limitation, set hardware to 1 */
hardware_mult = 1.0f;
} else if (_radio_status_available) {
hardware_mult *= _radio_status_mult;
}
_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);
_rate_mult = math::constrain(_rate_mult, 0.05f, 1.0f);
}
void
Mavlink::update_radio_status(const radio_status_s &radio_status)
{
_rstatus = radio_status;
/* check hardware limits */
if (radio_status.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO) {
_radio_status_available = true;
_radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE);
if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 20% */
_radio_status_mult *= 0.80f;
} else if (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 2.5% */
_radio_status_mult *= 0.975f;
} else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
/* this indicates spare bandwidth, increase by 2.5% */
_radio_status_mult *= 1.025f;
}
} else {
_radio_status_available = false;
_radio_status_critical = false;
_radio_status_mult = 1.0f;
}
}
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));

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

@ -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; }
bool is_connected() { return ((_tstatus.heartbeat_time > 0) && (hrt_absolute_time() - _tstatus.heartbeat_time < 3_s)); }
void set_config_link_on(bool on) { _config_link_on = on; }
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
*

87
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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();
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;
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;
_mavlink->update_radio_status(status);
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);
}
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];

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

@ -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;