diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index d7cf45351c..5f4236b3e5 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -192,13 +192,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length if (buf_free < desired) { /* we don't want to send anything just in half, so return */ + instance->count_txerr(); return; } } ssize_t ret = write(uart, ch, desired); if (ret != desired) { - warnx("TX FAIL"); + instance->count_txerr(); } else { last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel]; } @@ -237,7 +238,8 @@ Mavlink::Mavlink() : _flow_control_enabled(true), _message_buffer({}), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mavlink")) + _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), + _txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe")) { _wpm = &_wpm_s; mission.count = 0; @@ -290,6 +292,7 @@ Mavlink::Mavlink() : Mavlink::~Mavlink() { perf_free(_loop_perf); + perf_free(_txerr_perf); if (_task_running) { /* task wakes up every 10ms or so at the longest */ @@ -314,6 +317,12 @@ Mavlink::~Mavlink() LL_DELETE(_mavlink_instances, this); } +void +Mavlink::count_txerr() +{ + perf_count(_txerr_perf); +} + void Mavlink::set_mode(enum MAVLINK_MODE mode) { @@ -2169,11 +2178,20 @@ int Mavlink::start_helper(int argc, char *argv[]) /* create the instance in task context */ Mavlink *instance = new Mavlink(); - /* this will actually only return once MAVLink exits */ - int res = instance->task_main(argc, argv); + int res; - /* delete instance on main thread end */ - delete instance; + if (!instance) { + + /* out of memory */ + res = -ENOMEM; + warnx("OUT OF MEM"); + } else { + /* this will actually only return once MAVLink exits */ + res = instance->task_main(argc, argv); + + /* delete instance on main thread end */ + delete instance; + } return res; } diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 40edc4b851..674efe01e7 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -213,6 +213,11 @@ public: bool get_wait_to_transmit() { return _wait_to_transmit; } bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } + /** + * Count a transmision error + */ + void count_txerr(); + protected: Mavlink *next; @@ -282,6 +287,7 @@ private: pthread_mutex_t _message_buffer_mutex; perf_counter_t _loop_perf; /**< loop performance counter */ + perf_counter_t _txerr_perf; /**< TX error counter */ /** * Send one parameter.