Capture TX issues in performance counter instead of spamming console in mavlink app

This commit is contained in:
Lorenz Meier 2014-06-22 18:15:40 +02:00
parent f318ee2194
commit 72afa2ca2b
2 changed files with 30 additions and 6 deletions

View File

@ -192,13 +192,14 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
if (buf_free < desired) { if (buf_free < desired) {
/* we don't want to send anything just in half, so return */ /* we don't want to send anything just in half, so return */
instance->count_txerr();
return; return;
} }
} }
ssize_t ret = write(uart, ch, desired); ssize_t ret = write(uart, ch, desired);
if (ret != desired) { if (ret != desired) {
warnx("TX FAIL"); 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];
} }
@ -237,7 +238,8 @@ Mavlink::Mavlink() :
_flow_control_enabled(true), _flow_control_enabled(true),
_message_buffer({}), _message_buffer({}),
/* performance counters */ /* 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; _wpm = &_wpm_s;
mission.count = 0; mission.count = 0;
@ -290,6 +292,7 @@ Mavlink::Mavlink() :
Mavlink::~Mavlink() Mavlink::~Mavlink()
{ {
perf_free(_loop_perf); perf_free(_loop_perf);
perf_free(_txerr_perf);
if (_task_running) { if (_task_running) {
/* task wakes up every 10ms or so at the longest */ /* task wakes up every 10ms or so at the longest */
@ -314,6 +317,12 @@ Mavlink::~Mavlink()
LL_DELETE(_mavlink_instances, this); LL_DELETE(_mavlink_instances, this);
} }
void
Mavlink::count_txerr()
{
perf_count(_txerr_perf);
}
void void
Mavlink::set_mode(enum MAVLINK_MODE mode) 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 */ /* create the instance in task context */
Mavlink *instance = new Mavlink(); Mavlink *instance = new Mavlink();
/* this will actually only return once MAVLink exits */ int res;
int res = instance->task_main(argc, argv);
/* delete instance on main thread end */ if (!instance) {
delete 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; return res;
} }

View File

@ -213,6 +213,11 @@ public:
bool get_wait_to_transmit() { return _wait_to_transmit; } bool get_wait_to_transmit() { return _wait_to_transmit; }
bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }
/**
* Count a transmision error
*/
void count_txerr();
protected: protected:
Mavlink *next; Mavlink *next;
@ -282,6 +287,7 @@ private:
pthread_mutex_t _message_buffer_mutex; pthread_mutex_t _message_buffer_mutex;
perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _txerr_perf; /**< TX error counter */
/** /**
* Send one parameter. * Send one parameter.