|
|
|
@ -81,7 +81,7 @@
@@ -81,7 +81,7 @@
|
|
|
|
|
#include "mavlink_commands.h" |
|
|
|
|
|
|
|
|
|
#ifndef MAVLINK_CRC_EXTRA |
|
|
|
|
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems |
|
|
|
|
#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
/* oddly, ERROR is not defined for c++ */ |
|
|
|
@ -154,6 +154,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
@@ -154,6 +154,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
|
|
|
|
|
instance = Mavlink::get_instance(6); |
|
|
|
|
break; |
|
|
|
|
#endif |
|
|
|
|
|
|
|
|
|
default: |
|
|
|
|
return; |
|
|
|
|
} |
|
|
|
@ -178,8 +179,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
@@ -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 && |
|
|
|
|
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL && |
|
|
|
|
last_write_success_times[(unsigned)channel] != |
|
|
|
|
last_write_try_times[(unsigned)channel]) |
|
|
|
|
{ |
|
|
|
|
last_write_try_times[(unsigned)channel]) { |
|
|
|
|
warnx("DISABLING HARDWARE FLOW CONTROL"); |
|
|
|
|
instance->enable_flow_control(false); |
|
|
|
|
} |
|
|
|
@ -202,8 +202,10 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
@@ -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); |
|
|
|
|
|
|
|
|
|
if (ret != desired) { |
|
|
|
|
instance->count_txerr(); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; |
|
|
|
|
} |
|
|
|
@ -232,9 +234,9 @@ Mavlink::Mavlink() :
@@ -232,9 +234,9 @@ Mavlink::Mavlink() :
|
|
|
|
|
_mission_result_sub(-1), |
|
|
|
|
_mode(MAVLINK_MODE_NORMAL), |
|
|
|
|
_channel(MAVLINK_COMM_0), |
|
|
|
|
_logbuffer{}, |
|
|
|
|
_logbuffer {}, |
|
|
|
|
_total_counter(0), |
|
|
|
|
_receive_thread{}, |
|
|
|
|
_receive_thread {}, |
|
|
|
|
_verbose(false), |
|
|
|
|
_forwarding_on(false), |
|
|
|
|
_passing_on(false), |
|
|
|
@ -247,16 +249,16 @@ Mavlink::Mavlink() :
@@ -247,16 +249,16 @@ Mavlink::Mavlink() :
|
|
|
|
|
_subscribe_to_stream(nullptr), |
|
|
|
|
_subscribe_to_stream_rate(0.0f), |
|
|
|
|
_flow_control_enabled(true), |
|
|
|
|
_rstatus{}, |
|
|
|
|
_message_buffer{}, |
|
|
|
|
_message_buffer_mutex{}, |
|
|
|
|
_rstatus {}, |
|
|
|
|
_message_buffer {}, |
|
|
|
|
_message_buffer_mutex {}, |
|
|
|
|
_param_initialized(false), |
|
|
|
|
_param_system_id(0), |
|
|
|
|
_param_component_id(0), |
|
|
|
|
_param_system_type(0), |
|
|
|
|
_param_use_hil_gps(0), |
|
|
|
|
|
|
|
|
|
/* performance counters */ |
|
|
|
|
/* performance counters */ |
|
|
|
|
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), |
|
|
|
|
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) |
|
|
|
|
{ |
|
|
|
@ -637,7 +639,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
@@ -637,7 +639,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
|
|
|
|
|
case 921600: speed = B921600; break; |
|
|
|
|
|
|
|
|
|
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; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -867,6 +870,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
@@ -867,6 +870,7 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
|
|
|
|
|
case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { |
|
|
|
|
mavlink_param_request_list_t req; |
|
|
|
|
mavlink_msg_param_request_list_decode(msg, &req); |
|
|
|
|
|
|
|
|
|
if (req.target_system == mavlink_system.sysid && |
|
|
|
|
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) { |
|
|
|
|
/* Start sending parameters */ |
|
|
|
@ -883,7 +887,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
@@ -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_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 */ |
|
|
|
|
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); |
|
|
|
@ -910,7 +916,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
@@ -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_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 */ |
|
|
|
|
if (mavlink_param_request_read.param_index == -1) { |
|
|
|
|
/* local name buffer to enforce null-terminated string */ |
|
|
|
@ -976,9 +984,11 @@ Mavlink::send_statustext(unsigned severity, const char *string)
@@ -976,9 +984,11 @@ Mavlink::send_statustext(unsigned severity, const char *string)
|
|
|
|
|
case MAVLINK_IOC_SEND_TEXT_INFO: |
|
|
|
|
statustext.severity = MAV_SEVERITY_INFO; |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAVLINK_IOC_SEND_TEXT_CRITICAL: |
|
|
|
|
statustext.severity = MAV_SEVERITY_CRITICAL; |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
case MAVLINK_IOC_SEND_TEXT_EMERGENCY: |
|
|
|
|
statustext.severity = MAV_SEVERITY_EMERGENCY; |
|
|
|
|
break; |
|
|
|
@ -1098,12 +1108,14 @@ Mavlink::message_buffer_init(int size)
@@ -1098,12 +1108,14 @@ Mavlink::message_buffer_init(int size)
|
|
|
|
|
_message_buffer.size = size; |
|
|
|
|
_message_buffer.write_ptr = 0; |
|
|
|
|
_message_buffer.read_ptr = 0; |
|
|
|
|
_message_buffer.data = (char*)malloc(_message_buffer.size); |
|
|
|
|
_message_buffer.data = (char *)malloc(_message_buffer.size); |
|
|
|
|
|
|
|
|
|
int ret; |
|
|
|
|
|
|
|
|
|
if (_message_buffer.data == 0) { |
|
|
|
|
ret = ERROR; |
|
|
|
|
_message_buffer.size = 0; |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
ret = OK; |
|
|
|
|
} |
|
|
|
@ -1475,7 +1487,8 @@ Mavlink::task_main(int argc, char *argv[])
@@ -1475,7 +1487,8 @@ Mavlink::task_main(int argc, char *argv[])
|
|
|
|
|
if (_subscribe_to_stream != nullptr) { |
|
|
|
|
if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) { |
|
|
|
|
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 { |
|
|
|
|
warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name); |
|
|
|
@ -1517,17 +1530,18 @@ Mavlink::task_main(int argc, char *argv[])
@@ -1517,17 +1530,18 @@ Mavlink::task_main(int argc, char *argv[])
|
|
|
|
|
uint8_t *write_ptr; |
|
|
|
|
|
|
|
|
|
pthread_mutex_lock(&_message_buffer_mutex); |
|
|
|
|
int available = message_buffer_get_ptr((void**)&read_ptr, &is_part); |
|
|
|
|
int available = message_buffer_get_ptr((void **)&read_ptr, &is_part); |
|
|
|
|
pthread_mutex_unlock(&_message_buffer_mutex); |
|
|
|
|
|
|
|
|
|
if (available > 0) { |
|
|
|
|
// Reconstruct message from buffer
|
|
|
|
|
|
|
|
|
|
mavlink_message_t msg; |
|
|
|
|
write_ptr = (uint8_t*)&msg; |
|
|
|
|
write_ptr = (uint8_t *)&msg; |
|
|
|
|
|
|
|
|
|
// Pull a single message from the buffer
|
|
|
|
|
size_t read_count = available; |
|
|
|
|
|
|
|
|
|
if (read_count > sizeof(mavlink_message_t)) { |
|
|
|
|
read_count = sizeof(mavlink_message_t); |
|
|
|
|
} |
|
|
|
@ -1544,7 +1558,7 @@ Mavlink::task_main(int argc, char *argv[])
@@ -1544,7 +1558,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|
|
|
|
/* write second part of buffer if there is some */ |
|
|
|
|
if (is_part && read_count < sizeof(mavlink_message_t)) { |
|
|
|
|
write_ptr += read_count; |
|
|
|
|
available = message_buffer_get_ptr((void**)&read_ptr, &is_part); |
|
|
|
|
available = message_buffer_get_ptr((void **)&read_ptr, &is_part); |
|
|
|
|
read_count = sizeof(mavlink_message_t) - read_count; |
|
|
|
|
memcpy(write_ptr, read_ptr, read_count); |
|
|
|
|
message_buffer_mark_read(available); |
|
|
|
@ -1606,6 +1620,7 @@ Mavlink::task_main(int argc, char *argv[])
@@ -1606,6 +1620,7 @@ Mavlink::task_main(int argc, char *argv[])
|
|
|
|
|
message_buffer_destroy(); |
|
|
|
|
pthread_mutex_destroy(&_message_buffer_mutex); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
/* destroy log buffer */ |
|
|
|
|
mavlink_logbuffer_destroy(&_logbuffer); |
|
|
|
|
|
|
|
|
@ -1627,6 +1642,7 @@ int Mavlink::start_helper(int argc, char *argv[])
@@ -1627,6 +1642,7 @@ int Mavlink::start_helper(int argc, char *argv[])
|
|
|
|
|
/* out of memory */ |
|
|
|
|
res = -ENOMEM; |
|
|
|
|
warnx("OUT OF MEM"); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
/* this will actually only return once MAVLink exits */ |
|
|
|
|
res = instance->task_main(argc, argv); |
|
|
|
@ -1699,6 +1715,7 @@ Mavlink::display_status()
@@ -1699,6 +1715,7 @@ Mavlink::display_status()
|
|
|
|
|
case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO: |
|
|
|
|
printf("3DR RADIO\n"); |
|
|
|
|
break; |
|
|
|
|
|
|
|
|
|
default: |
|
|
|
|
printf("UNKNOWN RADIO\n"); |
|
|
|
|
break; |
|
|
|
@ -1711,6 +1728,7 @@ Mavlink::display_status()
@@ -1711,6 +1728,7 @@ Mavlink::display_status()
|
|
|
|
|
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); |
|
|
|
|
|
|
|
|
|
} else { |
|
|
|
|
printf("\tno telem status.\n"); |
|
|
|
|
} |
|
|
|
|