Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-27 10:17:45 +08:00
Code Issues Actions 20 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 */ /* ESCs status changed */
esc_status_s esc_status; esc_status_check();
_esc_status_was_updated = true;
if (_esc_status_sub.copy(&esc_status)) { } else if (hrt_elapsed_time(&_last_esc_status_updated) > 700_ms) {
esc_status_check(esc_status); // 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(); estimator_check();
// Auto disarm when landed or kill switch engaged // Auto disarm when landed or kill switch engaged
if (_armed.armed) { 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)) { esc_status_s esc_status{};
// Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that
_esc_status_sub.copy(&esc_status);
if (esc_status.esc_count > 0) {
char esc_fail_msg[50]; char esc_fail_msg[50];
esc_fail_msg[0] = '\0'; 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) 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 avoidance_check();
void esc_status_check(const esc_status_s &esc_status); void esc_status_check();
void estimator_check(); void estimator_check();
@@ -334,7 +334,7 @@ private:
int _last_esc_online_flags{-1}; int _last_esc_online_flags{-1};
int _last_esc_failure[esc_status_s::CONNECTED_ESC_MAX] {0}; 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}; uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
float _battery_current{0.0f}; float _battery_current{0.0f};