yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Fix code style, no actual code edits

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-19 16:00:37 +02:00
parent 5ace06f3b8
commit 6e3ebd3a36
1 changed files with 92 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -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,7 +154,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
instance = Mavlink::get_instance(6);
break;
#endif
default:
default:
return;
}
@@ -169,17 +170,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
int buf_free = 0;
if (instance->get_flow_control_enabled()
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
/* Disable hardware flow control:
* if no successful write since a defined time
* and if the last try was not the last successful write
*/
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])
{
hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
last_write_success_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
}
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,33 +234,33 @@ Mavlink::Mavlink() :
_mission_result_sub(-1),
_mode(MAVLINK_MODE_NORMAL),
_channel(MAVLINK_COMM_0),
_logbuffer{},
_total_counter(0),
_receive_thread{},
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_ftp_on(false),
_uart_fd(-1),
_baudrate(57600),
_datarate(10000),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_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),
_logbuffer {},
_total_counter(0),
_receive_thread {},
_verbose(false),
_forwarding_on(false),
_passing_on(false),
_ftp_on(false),
_uart_fd(-1),
_baudrate(57600),
_datarate(10000),
_mavlink_param_queue_index(0),
mavlink_link_termination_allowed(false),
_subscribe_to_stream(nullptr),
_subscribe_to_stream_rate(0.0f),
_flow_control_enabled(true),
_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 */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
/* performance counters */
_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
{
fops.ioctl = (int (*)(file *, int, long unsigned int))&mavlink_dev_ioctl;
@@ -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,8 +870,9 @@ 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)) {
(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
/* Start sending parameters */
mavlink_pm_start_queued_send();
send_statustext_info("[pm] sending list");
@@ -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
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 */
@@ -973,15 +981,17 @@ Mavlink::send_statustext(unsigned severity, const char *string)
/* Map severity */
switch (severity) {
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;
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;
}
mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
@@ -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[])
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);
@@ -1514,45 +1527,46 @@ Mavlink::task_main(int argc, char *argv[])
bool is_part;
uint8_t *read_ptr;
uint8_t *write_ptr;
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
// 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);
}
memcpy(write_ptr, read_ptr, read_count);
// We hold the mutex until after we complete the second part of the buffer. If we don't
// we may end up breaking the empty slot overflow detection semantics when we mark the
// possibly partial read below.
pthread_mutex_lock(&_message_buffer_mutex);
// 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);
}
memcpy(write_ptr, read_ptr, read_count);
// We hold the mutex until after we complete the second part of the buffer. If we don't
// we may end up breaking the empty slot overflow detection semantics when we mark the
// possibly partial read below.
pthread_mutex_lock(&_message_buffer_mutex);
message_buffer_mark_read(read_count);
/* 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);
read_count = sizeof(mavlink_message_t) - read_count;
memcpy(write_ptr, read_ptr, read_count);
write_ptr += read_count;
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);
}
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, &msg);
pthread_mutex_unlock(&_message_buffer_mutex);
_mavlink_resend_uart(_channel, &msg);
}
}
@@ -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[])
/* out of memory */
res = -ENOMEM;
warnx("OUT OF MEM");
} else {
/* this will actually only return once MAVLink exits */
res = instance->task_main(argc, argv);
@@ -1696,10 +1712,11 @@ Mavlink::display_status()
printf("\ttype:\t\t");
switch (_rstatus.type) {
case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
printf("3DR RADIO\n");
break;
default:
default:
printf("UNKNOWN RADIO\n");
break;
}
@@ -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");
}