diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 3f6f3248c8..878a472332 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 f4ee194485..82232256a0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1530,7 +1530,6 @@ Mavlink::update_rate_mult() // check for RADIO_STATUS timeout and reset if (hrt_elapsed_time(&_rstatus.timestamp) > 5_s) { PX4_ERR("instance %d: RADIO_STATUS timeout", _instance_id); - set_telemetry_status_type(telemetry_status_s::LINK_TYPE_GENERIC); _radio_status_available = false; _radio_status_critical = false; @@ -1551,23 +1550,25 @@ void Mavlink::update_radio_status(const radio_status_s &radio_status) { _rstatus = radio_status; - set_telemetry_status_type(telemetry_status_s::LINK_TYPE_3DR_RADIO); - - /* 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) { - /* this indicates link congestion, reduce rate by 20% */ - _radio_status_mult *= 0.80f; + if (_use_software_mav_throttling) { + + /* check hardware limits */ + _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_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 if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) { + /* this indicates spare bandwidth, increase by 2.5% */ + _radio_status_mult *= 1.025f; + } } } @@ -1847,7 +1848,7 @@ Mavlink::task_main(int argc, char *argv[]) int temp_int_arg; #endif - while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fwxz", &myoptind, &myoptarg)) != EOF) { + while ((ch = px4_getopt(argc, argv, "b:r:d:n:u:o:m:t:c:fswxz", &myoptind, &myoptarg)) != EOF) { switch (ch) { case 'b': if (px4_get_parameter_value(myoptarg, _baudrate) != 0) { @@ -2028,6 +2029,10 @@ Mavlink::task_main(int argc, char *argv[]) _forwarding_on = true; break; + case 's': + _use_software_mav_throttling = true; + break; + case 'w': _wait_to_transmit = true; break; @@ -2253,7 +2258,7 @@ Mavlink::task_main(int argc, char *argv[]) #endif // CONFIG_NET } - check_radio_config(); + configure_sik_radio(); if (status_sub->update(&status_time, &status)) { /* switch HIL mode if required */ @@ -2603,11 +2608,10 @@ void Mavlink::publish_telemetry_status() _telem_status_pub.publish(_tstatus); } -void Mavlink::check_radio_config() +void Mavlink::configure_sik_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_sik_radio_id.get() != 0) { /* request to configure radio and radio is present */ FILE *fs = fdopen(_uart_fd, "w"); @@ -2617,9 +2621,9 @@ void Mavlink::check_radio_config() fprintf(fs, "+++\n"); px4_usleep(1200000); - if (_param_mav_radio_id.get() > 0) { + if (_param_sik_radio_id.get() > 0) { /* set channel */ - fprintf(fs, "ATS3=%u\n", _param_mav_radio_id.get()); + fprintf(fs, "ATS3=%u\n", _param_sik_radio_id.get()); px4_usleep(200000); } else { @@ -2650,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_sik_radio_id.set(0); + _param_sik_radio_id.commit_no_notification(); } } @@ -2750,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); @@ -2760,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 4fad5312a8..96c1833821 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -583,6 +583,7 @@ private: bool _forwarding_on{false}; bool _ftp_on{false}; + bool _use_software_mav_throttling{false}; int _uart_fd{-1}; @@ -662,7 +663,7 @@ private: (ParamInt) _param_mav_sys_id, (ParamInt) _param_mav_comp_id, (ParamInt) _param_mav_proto_ver, - (ParamInt) _param_mav_radio_id, + (ParamInt) _param_sik_radio_id, (ParamInt) _param_mav_type, (ParamBool) _param_mav_usehilgps, (ParamBool) _param_mav_fwdextsp, @@ -725,12 +726,12 @@ private: void check_requested_subscriptions(); /** - * Check the configuration of a connected radio + * Reconfigure a SiK radio if requested by MAV_SIK_RADIO_ID * * This convenience function allows to re-configure a connected - * radio without removing it from the main system harness. + * SiK radio without removing it from the main system harness. */ - void check_radio_config(); + void configure_sik_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 2968712e33..66104d106d 100644 --- a/src/modules/mavlink/mavlink_params.c +++ b/src/modules/mavlink/mavlink_params.c @@ -59,18 +59,18 @@ PARAM_DEFINE_INT32(MAV_COMP_ID, 1); PARAM_DEFINE_INT32(MAV_PROTO_VER, 0); /** - * MAVLink Radio ID + * MAVLink SiK 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 + * SiK 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. + * factory defaults. Only applies if this mavlink instance is going through a SiK radio * * @group MAVLink * @min -1 * @max 240 */ -PARAM_DEFINE_INT32(MAV_RADIO_ID, 0); +PARAM_DEFINE_INT32(MAV_SIK_RADIO_ID, 0); /** * MAVLink airframe type diff --git a/src/modules/mavlink/module.yaml b/src/modules/mavlink/module.yaml index c0eca8dd82..7936df5a58 100644 --- a/src/modules/mavlink/module.yaml +++ b/src/modules/mavlink/module.yaml @@ -8,6 +8,10 @@ serial_config: then set MAV_ARGS "${MAV_ARGS} -f" fi + if param compare MAV_${i}_RADIO_CTL 1 + then + set MAV_ARGS "${MAV_ARGS} -s" + fi mavlink start -d ${SERIAL_DEV} ${MAV_ARGS} -x port_config_param: name: MAV_${i}_CONFIG @@ -78,3 +82,16 @@ parameters: num_instances: *max_num_config_instances default: [true, false, false] + MAV_${i}_RADIO_CTL: + description: + short: Enable software throttling of mavlink on instance ${i} + long: | + If enabled, MAVLink messages will be throttled according to + `txbuf` field reported by radio_status. + + Requires a radio to send the mavlink message RADIO_STATUS. + + type: boolean + reboot_required: true + num_instances: *max_num_config_instances + default: [true, true, true]