Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 07:09:48 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

Commander:esc_checks minor improvements

Browse Source 
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
2021-03-30 11:29:23 +02:00
committed by Nuno Marques
parent 371fa98579
commit bff99f9794
2 changed files with 19 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+17 -16
View File
@@ -1957,19 +1957,22 @@ Commander::run()
}
}
if (_esc_status_sub.updated() || _esc_status_was_updated) {
if (_esc_status_sub.updated()) {
/* ESCs status changed */
esc_status_s esc_status;
_esc_status_was_updated = true;
esc_status_check();
if (_esc_status_sub.copy(&esc_status)) {
esc_status_check(esc_status);
} else if (hrt_elapsed_time(&_last_esc_status_updated) > 700_ms) {
// Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that
if (!_status_flags.condition_escs_error) {
mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout");
}
_status_flags.condition_escs_error = true;
}
estimator_check();
// Auto disarm when landed or kill switch engaged
if (_armed.armed) {
@@ -3936,10 +3939,13 @@ Commander::offboard_control_update()
}
}
void Commander::esc_status_check(const esc_status_s &esc_status)
void Commander::esc_status_check()
{
if ((esc_status.esc_count > 0) && (hrt_elapsed_time(&esc_status.timestamp) < 700_ms)) {
// Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that
esc_status_s esc_status{};
_esc_status_sub.copy(&esc_status);
if (esc_status.esc_count > 0) {
char esc_fail_msg[50];
esc_fail_msg[0] = '\0';
@@ -4018,14 +4024,9 @@ void Commander::esc_status_check(const esc_status_s &esc_status)
}
}
} else {
if (_esc_status_was_updated && !_status_flags.condition_escs_error) {
mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout");
}
_status_flags.condition_escs_error = true;
}
_last_esc_status_updated = esc_status.timestamp;
}
int Commander::print_usage(const char *reason)
src/modules/commander/Commander.hpp
+2 -2
View File
@@ -143,7 +143,7 @@ private:
void avoidance_check();
void esc_status_check(const esc_status_s &esc_status);
void esc_status_check();
void estimator_check();
@@ -334,7 +334,7 @@ private:
int _last_esc_online_flags{-1};
int _last_esc_failure[esc_status_s::CONNECTED_ESC_MAX] {0};
bool _esc_status_was_updated{false};
hrt_abstime _last_esc_status_updated{0};
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
float _battery_current{0.0f};