diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 3f6f3248c84f..878a47233244 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -1,9 +1,8 @@ uint8 LINK_TYPE_GENERIC = 0 -uint8 LINK_TYPE_3DR_RADIO = 1 -uint8 LINK_TYPE_UBIQUITY_BULLET = 2 -uint8 LINK_TYPE_WIRE = 3 -uint8 LINK_TYPE_USB = 4 -uint8 LINK_TYPE_IRIDIUM = 5 +uint8 LINK_TYPE_UBIQUITY_BULLET = 1 +uint8 LINK_TYPE_WIRE = 2 +uint8 LINK_TYPE_USB = 3 +uint8 LINK_TYPE_IRIDIUM = 4 # MAV_COMPONENT (fill in as needed) uint8 COMPONENT_ID_ALL = 0 diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index af294ab50792..2aac69ce6455 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1550,11 +1550,11 @@ void Mavlink::update_radio_status(const radio_status_s &radio_status) { _rstatus = radio_status; + _radio_status_available = true; if (_use_software_mav_throttling) { /* check hardware limits */ - _radio_status_available = true; _radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE); if (radio_status.txbuf < RADIO_BUFFER_CRITICAL_LOW_PERCENTAGE) { @@ -2258,7 +2258,7 @@ Mavlink::task_main(int argc, char *argv[]) #endif // CONFIG_NET } - check_radio_config(); + configure_3dr_radio(); if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ @@ -2608,11 +2608,10 @@ void Mavlink::publish_telemetry_status() _telem_status_pub.publish(_tstatus); } -void Mavlink::check_radio_config() +void Mavlink::configure_3dr_radio() { /* radio config check */ - if (_uart_fd >= 0 && _param_mav_radio_id.get() != 0 - && _tstatus.type == telemetry_status_s::LINK_TYPE_3DR_RADIO) { + if (_uart_fd >= 0 && _param_3dr_radio_id.get() != 0) { /* request to configure radio and radio is present */ FILE *fs = fdopen(_uart_fd, "w"); @@ -2655,8 +2654,8 @@ void Mavlink::check_radio_config() } /* reset param and save */ - _param_mav_radio_id.set(0); - _param_mav_radio_id.commit_no_notification(); + _param_3dr_radio_id.set(0); + _param_3dr_radio_id.commit_no_notification(); } } @@ -2755,9 +2754,8 @@ Mavlink::display_status() printf("\ttype:\t\t"); - switch (_tstatus.type) { - case telemetry_status_s::LINK_TYPE_3DR_RADIO: - printf("3DR RADIO\n"); + if (_radio_status_available) { + printf("RADIO Link\n"); 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); @@ -2765,15 +2763,12 @@ Mavlink::display_status() 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.fix); - break; - case telemetry_status_s::LINK_TYPE_USB: + } else if (_is_usb_uart) { printf("USB CDC\n"); - break; - default: + } else { printf("GENERIC LINK OR RADIO\n"); - break; } } else { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 22314aff1c7d..33e2a74d91aa 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -663,7 +663,7 @@ class Mavlink : public ModuleParams (ParamInt) _param_mav_sys_id, (ParamInt) _param_mav_comp_id, (ParamInt) _param_mav_proto_ver, - (ParamInt) _param_mav_radio_id, + (ParamInt) _param_3dr_radio_id, (ParamInt) _param_mav_type, (ParamBool) _param_mav_usehilgps, (ParamBool) _param_mav_fwdextsp, @@ -726,12 +726,12 @@ class Mavlink : public ModuleParams void check_requested_subscriptions(); /** - * Check the configuration of a connected radio + * Reconfigure a 3DR radio if requested by MAV_3DR_RADIO_ID * * This convenience function allows to re-configure a connected - * radio without removing it from the main system harness. + * 3DR radio without removing it from the main system harness. */ - void check_radio_config(); + void configure_3dr_radio(); /** * Update rate mult so total bitrate will be equal to _datarate. diff --git a/src/modules/mavlink/mavlink_params.c b/src/modules/mavlink/mavlink_params.c index 2968712e33c5..ee1b74938622 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -59,18 +59,20 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 1); PARAM_DEFINE_INT32(MAV_PROTO_VER, 0); /** - * MAVLink Radio ID + * MAVLink 3DR Radio ID * * When non-zero the MAVLink app will attempt to configure the - * radio to this ID and re-set the parameter to 0. If the value + * 3DR radio to this ID and re-set the parameter to 0. If the value * is negative it will reset the complete radio config to * factory defaults. * + * @note Only applies if this mavlink instance is going through a 3DR radio + * * @group MAVLink * @min -1 * @max 240 */ -PARAM_DEFINE_INT32(MAV_RADIO_ID, 0); +PARAM_DEFINE_INT32(MAV_3DR_RADIO_ID, 0); /** * MAVLink airframe type