|
|
@ -154,6 +154,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length |
|
|
|
instance = Mavlink::get_instance(6); |
|
|
|
instance = Mavlink::get_instance(6); |
|
|
|
break; |
|
|
|
break; |
|
|
|
#endif |
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
|
|
default: |
|
|
|
default: |
|
|
|
return; |
|
|
|
return; |
|
|
|
} |
|
|
|
} |
|
|
@ -178,8 +179,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length |
|
|
|
if (last_write_try_times[(unsigned)channel] != 0 && |
|
|
|
if (last_write_try_times[(unsigned)channel] != 0 && |
|
|
|
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && |
|
|
|
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && |
|
|
|
last_write_success_times[(unsigned)channel] != |
|
|
|
last_write_success_times[(unsigned)channel] != |
|
|
|
last_write_try_times[(unsigned)channel]) |
|
|
|
last_write_try_times[(unsigned)channel]) { |
|
|
|
{ |
|
|
|
|
|
|
|
warnx("DISABLING HARDWARE FLOW CONTROL"); |
|
|
|
warnx("DISABLING HARDWARE FLOW CONTROL"); |
|
|
|
instance->enable_flow_control(false); |
|
|
|
instance->enable_flow_control(false); |
|
|
|
} |
|
|
|
} |
|
|
@ -202,8 +202,10 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
ssize_t ret = write(uart, ch, desired); |
|
|
|
ssize_t ret = write(uart, ch, desired); |
|
|
|
|
|
|
|
|
|
|
|
if (ret != desired) { |
|
|
|
if (ret != desired) { |
|
|
|
instance->count_txerr(); |
|
|
|
instance->count_txerr(); |
|
|
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
} else { |
|
|
|
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; |
|
|
|
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; |
|
|
|
} |
|
|
|
} |
|
|
@ -637,7 +639,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * |
|
|
|
case 921600: speed = B921600; break; |
|
|
|
case 921600: speed = B921600; break; |
|
|
|
|
|
|
|
|
|
|
|
default: |
|
|
|
default: |
|
|
|
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud); |
|
|
|
warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", |
|
|
|
|
|
|
|
baud); |
|
|
|
return -EINVAL; |
|
|
|
return -EINVAL; |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
@ -867,6 +870,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav |
|
|
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { |
|
|
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { |
|
|
|
mavlink_param_request_list_t req; |
|
|
|
mavlink_param_request_list_t req; |
|
|
|
mavlink_msg_param_request_list_decode(msg, &req); |
|
|
|
mavlink_msg_param_request_list_decode(msg, &req); |
|
|
|
|
|
|
|
|
|
|
|
if (req.target_system == mavlink_system.sysid && |
|
|
|
if (req.target_system == mavlink_system.sysid && |
|
|
|
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { |
|
|
|
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { |
|
|
|
/* Start sending parameters */ |
|
|
|
/* Start sending parameters */ |
|
|
@ -883,7 +887,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav |
|
|
|
mavlink_param_set_t mavlink_param_set; |
|
|
|
mavlink_param_set_t mavlink_param_set; |
|
|
|
mavlink_msg_param_set_decode(msg, &mavlink_param_set); |
|
|
|
mavlink_msg_param_set_decode(msg, &mavlink_param_set); |
|
|
|
|
|
|
|
|
|
|
|
if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { |
|
|
|
if (mavlink_param_set.target_system == mavlink_system.sysid |
|
|
|
|
|
|
|
&& ((mavlink_param_set.target_component == mavlink_system.compid) |
|
|
|
|
|
|
|
|| (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) { |
|
|
|
/* local name buffer to enforce null-terminated string */ |
|
|
|
/* local name buffer to enforce null-terminated string */ |
|
|
|
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; |
|
|
|
char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1]; |
|
|
|
strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); |
|
|
|
strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN); |
|
|
@ -910,7 +916,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav |
|
|
|
mavlink_param_request_read_t mavlink_param_request_read; |
|
|
|
mavlink_param_request_read_t mavlink_param_request_read; |
|
|
|
mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); |
|
|
|
mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read); |
|
|
|
|
|
|
|
|
|
|
|
if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { |
|
|
|
if (mavlink_param_request_read.target_system == mavlink_system.sysid |
|
|
|
|
|
|
|
&& ((mavlink_param_request_read.target_component == mavlink_system.compid) |
|
|
|
|
|
|
|
|| (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) { |
|
|
|
/* when no index is given, loop through string ids and compare them */ |
|
|
|
/* when no index is given, loop through string ids and compare them */ |
|
|
|
if (mavlink_param_request_read.param_index == -1) { |
|
|
|
if (mavlink_param_request_read.param_index == -1) { |
|
|
|
/* local name buffer to enforce null-terminated string */ |
|
|
|
/* local name buffer to enforce null-terminated string */ |
|
|
@ -976,9 +984,11 @@ Mavlink::send_statustext(unsigned severity, const char *string) |
|
|
|
case MAVLINK_IOC_SEND_TEXT_INFO: |
|
|
|
case MAVLINK_IOC_SEND_TEXT_INFO: |
|
|
|
statustext.severity = MAV_SEVERITY_INFO; |
|
|
|
statustext.severity = MAV_SEVERITY_INFO; |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
case MAVLINK_IOC_SEND_TEXT_CRITICAL: |
|
|
|
case MAVLINK_IOC_SEND_TEXT_CRITICAL: |
|
|
|
statustext.severity = MAV_SEVERITY_CRITICAL; |
|
|
|
statustext.severity = MAV_SEVERITY_CRITICAL; |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
case MAVLINK_IOC_SEND_TEXT_EMERGENCY: |
|
|
|
case MAVLINK_IOC_SEND_TEXT_EMERGENCY: |
|
|
|
statustext.severity = MAV_SEVERITY_EMERGENCY; |
|
|
|
statustext.severity = MAV_SEVERITY_EMERGENCY; |
|
|
|
break; |
|
|
|
break; |
|
|
@ -1101,9 +1111,11 @@ Mavlink::message_buffer_init(int size) |
|
|
|
_message_buffer.data = (char *)malloc(_message_buffer.size); |
|
|
|
_message_buffer.data = (char *)malloc(_message_buffer.size); |
|
|
|
|
|
|
|
|
|
|
|
int ret; |
|
|
|
int ret; |
|
|
|
|
|
|
|
|
|
|
|
if (_message_buffer.data == 0) { |
|
|
|
if (_message_buffer.data == 0) { |
|
|
|
ret = ERROR; |
|
|
|
ret = ERROR; |
|
|
|
_message_buffer.size = 0; |
|
|
|
_message_buffer.size = 0; |
|
|
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
} else { |
|
|
|
ret = OK; |
|
|
|
ret = OK; |
|
|
|
} |
|
|
|
} |
|
|
@ -1475,7 +1487,8 @@ Mavlink::task_main(int argc, char *argv[]) |
|
|
|
if (_subscribe_to_stream != nullptr) { |
|
|
|
if (_subscribe_to_stream != nullptr) { |
|
|
|
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { |
|
|
|
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { |
|
|
|
if (_subscribe_to_stream_rate > 0.0f) { |
|
|
|
if (_subscribe_to_stream_rate > 0.0f) { |
|
|
|
warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate); |
|
|
|
warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, |
|
|
|
|
|
|
|
(double)_subscribe_to_stream_rate); |
|
|
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
} else { |
|
|
|
warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); |
|
|
|
warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); |
|
|
@ -1528,6 +1541,7 @@ Mavlink::task_main(int argc, char *argv[]) |
|
|
|
|
|
|
|
|
|
|
|
// Pull a single message from the buffer
|
|
|
|
// Pull a single message from the buffer
|
|
|
|
size_t read_count = available; |
|
|
|
size_t read_count = available; |
|
|
|
|
|
|
|
|
|
|
|
if (read_count > sizeof(mavlink_message_t)) { |
|
|
|
if (read_count > sizeof(mavlink_message_t)) { |
|
|
|
read_count = sizeof(mavlink_message_t); |
|
|
|
read_count = sizeof(mavlink_message_t); |
|
|
|
} |
|
|
|
} |
|
|
@ -1606,6 +1620,7 @@ Mavlink::task_main(int argc, char *argv[]) |
|
|
|
message_buffer_destroy(); |
|
|
|
message_buffer_destroy(); |
|
|
|
pthread_mutex_destroy(&_message_buffer_mutex); |
|
|
|
pthread_mutex_destroy(&_message_buffer_mutex); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
/* destroy log buffer */ |
|
|
|
/* destroy log buffer */ |
|
|
|
mavlink_logbuffer_destroy(&_logbuffer); |
|
|
|
mavlink_logbuffer_destroy(&_logbuffer); |
|
|
|
|
|
|
|
|
|
|
@ -1627,6 +1642,7 @@ int Mavlink::start_helper(int argc, char *argv[]) |
|
|
|
/* out of memory */ |
|
|
|
/* out of memory */ |
|
|
|
res = -ENOMEM; |
|
|
|
res = -ENOMEM; |
|
|
|
warnx("OUT OF MEM"); |
|
|
|
warnx("OUT OF MEM"); |
|
|
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
} else { |
|
|
|
/* this will actually only return once MAVLink exits */ |
|
|
|
/* this will actually only return once MAVLink exits */ |
|
|
|
res = instance->task_main(argc, argv); |
|
|
|
res = instance->task_main(argc, argv); |
|
|
@ -1699,6 +1715,7 @@ Mavlink::display_status() |
|
|
|
case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: |
|
|
|
case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: |
|
|
|
printf("3DR RADIO\n"); |
|
|
|
printf("3DR RADIO\n"); |
|
|
|
break; |
|
|
|
break; |
|
|
|
|
|
|
|
|
|
|
|
default: |
|
|
|
default: |
|
|
|
printf("UNKNOWN RADIO\n"); |
|
|
|
printf("UNKNOWN RADIO\n"); |
|
|
|
break; |
|
|
|
break; |
|
|
@ -1711,6 +1728,7 @@ Mavlink::display_status() |
|
|
|
printf("\tremote noise:\t%u\n", _rstatus.remote_noise); |
|
|
|
printf("\tremote noise:\t%u\n", _rstatus.remote_noise); |
|
|
|
printf("\trx errors:\t%u\n", _rstatus.rxerrors); |
|
|
|
printf("\trx errors:\t%u\n", _rstatus.rxerrors); |
|
|
|
printf("\tfixed:\t\t%u\n", _rstatus.fixed); |
|
|
|
printf("\tfixed:\t\t%u\n", _rstatus.fixed); |
|
|
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
} else { |
|
|
|
printf("\tno telem status.\n"); |
|
|
|
printf("\tno telem status.\n"); |
|
|
|
} |
|
|
|
} |
|
|
|