Browse Source

Fix code style, no actual code edits

sbg
Lorenz Meier 11 years ago
parent
commit
6e3ebd3a36
  1. 30
      src/modules/mavlink/mavlink_main.cpp

30
src/modules/mavlink/mavlink_main.cpp

@ -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");
} }

Loading…
Cancel
Save