diff --git a/msg/radio_status.msg b/msg/radio_status.msg index 29ee47af74..3372abd10f 100644 --- a/msg/radio_status.msg +++ b/msg/radio_status.msg @@ -1,15 +1,6 @@ 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 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 730871ecd2..24681c2a81 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -302,12 +302,9 @@ Mavlink::Mavlink() : break; } - _tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_GENERIC; - // initialise parameter cache mavlink_update_parameters(); - // save the current system- and component ID because we don't allow them to change during operation int sys_id = _param_system_id.get(); @@ -756,9 +753,11 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, bool force_flow_ } else { _is_usb_uart = true; + /* USB has no baudrate, but use a magic number for 'fast' */ _baudrate = 2000000; - _tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB; + + set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB); } #if defined(__PX4_LINUX) || defined(__PX4_DARWIN) || defined(__PX4_CYGWIN) @@ -1612,30 +1611,23 @@ void Mavlink::update_radio_status(const radio_status_s &radio_status) { _rstatus = radio_status; + set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO); /* 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); - _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; - 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_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; + } else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) { + /* this indicates spare bandwidth, increase by 2.5% */ + _radio_status_mult *= 1.025f; } } @@ -1968,7 +1960,7 @@ Mavlink::task_main(int argc, char *argv[]) } else if (strcmp(myoptarg, "iridium") == 0) { _mode = MAVLINK_MODE_IRIDIUM; - _tstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM; + set_telemetry_status_type(telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_IRIDIUM); } else if (strcmp(myoptarg, "minimal") == 0) { _mode = MAVLINK_MODE_MINIMAL; @@ -2646,7 +2638,6 @@ Mavlink::start(int argc, char *argv[]) void Mavlink::display_status() { - if (_tstatus.heartbeat_time > 0) { printf("\tGCS heartbeat:\t%llu us ago\n", (unsigned long long)hrt_elapsed_time(&_tstatus.heartbeat_time)); } @@ -2660,13 +2651,13 @@ Mavlink::display_status() 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); - printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi); - printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf); - printf("\tnoise:\t\t%d\n", _rstatus.noise); - printf("\tremote noise:\t%u\n", _rstatus.remote_noise); - printf("\trx errors:\t%u\n", _rstatus.rxerrors); - printf("\tfixed:\t\t%u\n", _rstatus.fixed); + printf("\t rssi:\t\t%d\n", _rstatus.rssi); + printf("\t remote rssi:\t%u\n", _rstatus.remote_rssi); + printf("\t txbuf:\t%u\n", _rstatus.txbuf); + printf("\t noise:\t%d\n", _rstatus.noise); + printf("\t remote noise:\t%u\n", _rstatus.remote_noise); + printf("\t rx errors:\t%u\n", _rstatus.rxerrors); + printf("\t fixed:\t%u\n", _rstatus.fixed); break; case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB: diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 050b758388..4a4e1e0535 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -237,7 +237,7 @@ public: bool get_forwarding_on() { return _forwarding_on; } - bool is_connected() { return ((_tstatus.heartbeat_time > 0) && (hrt_absolute_time() - _tstatus.heartbeat_time < 3_s)); } + bool is_connected() { return (hrt_elapsed_time(&_tstatus.heartbeat_time) < 3_s); } bool broadcast_enabled() { return _param_broadcast_mode.get() == BROADCAST_MODE_ON; } @@ -423,6 +423,8 @@ public: */ telemetry_status_s &get_telemetry_status() { return _tstatus; } + void set_telemetry_status_type(uint8_t type) { _tstatus.type = type; } + void update_radio_status(const radio_status_s &radio_status); ringbuffer::RingBuffer *get_logbuffer() { return &_logbuffer; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 48d8cee891..2759d49f2b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1414,7 +1414,6 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) 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;