mavlink_diag.cpp
Go to the documentation of this file.
1 
6 /*
7  * Copyright 2014,2015 Vladimir Ermakov.
8  *
9  * This file is part of the mavros package and subject to the license terms
10  * in the top-level LICENSE file of the mavros repository.
11  * https://github.com/mavlink/mavros/tree/master/LICENSE.md
12  */
13 
14 #include <mavros/mavlink_diag.h>
15 
16 using namespace mavros;
17 
18 MavlinkDiag::MavlinkDiag(std::string name) :
19  diagnostic_updater::DiagnosticTask(name),
20  last_drop_count(0),
21  is_connected(false)
22 { };
23 
25 {
26  if (auto link = weak_link.lock()) {
27  auto mav_status = link->get_status();
28  auto iostat = link->get_iostat();
29 
30  stat.addf("Received packets:", "%u", mav_status.packet_rx_success_count);
31  stat.addf("Dropped packets:", "%u", mav_status.packet_rx_drop_count);
32  stat.addf("Buffer overruns:", "%u", mav_status.buffer_overrun);
33  stat.addf("Parse errors:", "%u", mav_status.parse_error);
34  stat.addf("Rx sequence number:", "%u", mav_status.current_rx_seq);
35  stat.addf("Tx sequence number:", "%u", mav_status.current_tx_seq);
36 
37  stat.addf("Rx total bytes:", "%u", iostat.rx_total_bytes);
38  stat.addf("Tx total bytes:", "%u", iostat.tx_total_bytes);
39  stat.addf("Rx speed:", "%f", iostat.rx_speed);
40  stat.addf("Tx speed:", "%f", iostat.tx_speed);
41 
42  if (mav_status.packet_rx_drop_count > last_drop_count)
43  stat.summaryf(1, "%d packeges dropped since last report",
44  mav_status.packet_rx_drop_count - last_drop_count);
45  else if (is_connected)
46  stat.summary(0, "connected");
47  else
48  // link operational, but not connected
49  stat.summary(1, "not connected");
50 
51  last_drop_count = mav_status.packet_rx_drop_count;
52  } else {
53  stat.summary(2, "not connected");
54  }
55 }
56 
void summary(unsigned char lvl, const std::string s)
mavconn::MAVConnInterface::WeakPtr weak_link
Definition: mavlink_diag.h:39
void addf(const std::string &key, const char *format,...)
void summaryf(unsigned char lvl, const char *format,...)
std::atomic< bool > is_connected
Definition: mavlink_diag.h:41
unsigned int last_drop_count
Definition: mavlink_diag.h:40
MavlinkDiag(std::string name)
void run(diagnostic_updater::DiagnosticStatusWrapper &stat)


mavros
Author(s): Vladimir Ermakov
autogenerated on Mon Jul 8 2019 03:20:11