fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Mavlink: Add MAV_x_RADIO_CTL parameter to disable RADIO_STATUS flow control

Browse Source 
* Add param for software flow ctl on 3DR radios
* Dont reset telemetry type on radio timeout
* Treat 3DR radio as generic link type
* Rename 3DR to SiK radio
This commit is contained in:
Nico van Duijn 2020-02-03 15:49:47 +01:00 committed by GitHub
parent 9612e40464
commit 7778cbd463
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
5 changed files with 61 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
msg/telemetry_status.msg
View File

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

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

@ -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) {
} else if (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE) {
/* this indicates link congestion, reduce rate by 2.5% */
_radio_status_mult *= 0.975f;
/* check hardware limits */
_radio_status_critical = (radio_status.txbuf < RADIO_BUFFER_LOW_PERCENTAGE);
} else if (radio_status.txbuf > RADIO_BUFFER_HALF_PERCENTAGE) {
/* this indicates spare bandwidth, increase by 2.5% */
_radio_status_mult *= 1.025f;
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;
}
}
}
@ -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 {

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

@ -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<px4::params::MAV_SYS_ID>) _param_mav_sys_id,
(ParamInt<px4::params::MAV_COMP_ID>) _param_mav_comp_id,
(ParamInt<px4::params::MAV_PROTO_VER>) _param_mav_proto_ver,
(ParamInt<px4::params::MAV_RADIO_ID>) _param_mav_radio_id,
(ParamInt<px4::params::MAV_SIK_RADIO_ID>) _param_sik_radio_id,
(ParamInt<px4::params::MAV_TYPE>) _param_mav_type,
(ParamBool<px4::params::MAV_USEHILGPS>) _param_mav_usehilgps,
(ParamBool<px4::params::MAV_FWDEXTSP>) _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.

8
src/modules/mavlink/mavlink_params.c
View File

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

17
src/modules/mavlink/module.yaml
View File

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