Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

sys_status.cpp: fix enabling of mem_diag and hwst_diag #1712

Merged
merged 2 commits into from
Feb 21, 2022
Merged

sys_status.cpp: fix enabling of mem_diag and hwst_diag #1712

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
021c164
sys_status.cpp: fix enabling of mem_diag and hwst_diag
amilcarlucas Feb 17, 2022
6f1604e
sys_status.cpp: Timeout on MEMINFO and HWSTATUS mavlink messages and …
karthikdesai Feb 18, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
65 changes: 43 additions & 22 deletions mavros/src/plugins/sys_status.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -351,33 +351,46 @@ class MemInfo : public diagnostic_updater::DiagnosticTask
MemInfo(const std::string &name) :
diagnostic_updater::DiagnosticTask(name),
freemem(-1),
brkval(0)
brkval(0),
last_rcd(0)
{ }

void set(uint16_t f, uint16_t b) {
freemem = f;
brkval = b;
last_rcd = ros::Time::now().toNSec();
}

void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
// access atomic variables just once
ssize_t freemem_ = freemem;
uint16_t brkval_ = brkval;

if (freemem < 0)
stat.summary(2, "No data");
else if (freemem < 200)
stat.summary(1, "Low mem");
else
stat.summary(0, "Normal");

ros::Time last_rcd_;
last_rcd_.fromNSec(last_rcd.load());
constexpr int timeout = 10.0; // seconds

// summarize the results
if (last_rcd_.isZero()) {
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not initialised");
} else if (ros::Time::now().toSec() - last_rcd_.toSec() > timeout) {
stat.summary(diagnostic_msgs::DiagnosticStatus::STALE, "Not received for more than " + std::to_string(timeout) + "s");
} else {
if (freemem < 0)
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No data");
else if (freemem < 200)
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Low mem");
else
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Normal");
}
stat.addf("Free memory (B)", "%zd", freemem_);
stat.addf("Heap top", "0x%04X", brkval_);
}

private:
std::atomic<ssize_t> freemem;
std::atomic<uint16_t> brkval;
std::atomic<uint64_t> last_rcd;
};


Expand All @@ -391,30 +404,37 @@ class HwStatus : public diagnostic_updater::DiagnosticTask
diagnostic_updater::DiagnosticTask(name),
vcc(-1.0),
i2cerr(0),
i2cerr_last(0)
i2cerr_last(0),
last_rcd(0)
{ }

void set(uint16_t v, uint8_t e) {
std::lock_guard<std::mutex> lock(mutex);
vcc = v * 0.001f;
i2cerr = e;
last_rcd = ros::Time::now();
}

void run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
std::lock_guard<std::mutex> lock(mutex);

if (vcc < 0)
stat.summary(2, "No data");
else if (vcc < 4.5)
stat.summary(1, "Low voltage");
else if (i2cerr != i2cerr_last) {
i2cerr_last = i2cerr;
stat.summary(1, "New I2C error");
constexpr int timeout = 10.0; // seconds
if (last_rcd.isZero()) {
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Not initialised");
} else if (ros::Time::now().toSec() - last_rcd.toSec() > timeout) {
stat.summary(diagnostic_msgs::DiagnosticStatus::STALE, "Not received for more than " + std::to_string(timeout) + "s");
} else {
if (vcc < 0)
stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "No data");
else if (vcc < 4.5)
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "Low voltage");
else if (i2cerr != i2cerr_last) {
i2cerr_last = i2cerr;
stat.summary(diagnostic_msgs::DiagnosticStatus::WARN, "New I2C error");
}
else
stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Normal");
}
else
stat.summary(0, "Normal");

stat.addf("Core voltage", "%f", vcc);
stat.addf("I2C errors", "%zu", i2cerr);
}
Expand All @@ -424,6 +444,7 @@ class HwStatus : public diagnostic_updater::DiagnosticTask
float vcc;
size_t i2cerr;
size_t i2cerr_last;
ros::Time last_rcd;
};


Expand Down Expand Up @@ -1090,7 +1111,7 @@ class SystemStatusPlugin : public plugin::PluginBase
autopilot_version_timer.stop();

// add/remove APM diag tasks
if (connected && disable_diag && m_uas->is_ardupilotmega()) {
if (connected && !disable_diag && m_uas->is_ardupilotmega()) {
UAS_DIAG(m_uas).add(mem_diag);
UAS_DIAG(m_uas).add(hwst_diag);
}
Expand Down