fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-27 09:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

escCheck: rework online check to properly report offline ESCs

Browse Source 
previous to this
09d79b221f
set `esc_online_flags` e.g. for UAVCAN ESCs which specific one is online and that then got compared to a mask where the first `esc_count` bits were set.

So if only ESC 5 is mapped and online you get the message "ESC 156 offline" because `esc_online_flags = 0b1000` gets compared to `online_bitmask = 0b1` based on `esc_count = 1` and the motor index is `esc[0].actuator_function = 0` wrapped using `0 - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1 = 156`.
This commit is contained in:
Matthias Grob
2025-12-16 16:26:28 +01:00
parent c4535683a7
commit f5a56ae42f
2 changed files with 24 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp
+23 -26
View File
@@ -82,7 +82,7 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
checkEscStatus(context, reporter, esc_status);
reporter.setIsPresent(health_component_t::motors_escs);
} else if (_param_escs_checks_required.get()
} else if (_param_com_arm_chk_escs.get()
&& now - _start_time > 5_s) { // Wait a bit after startup to allow esc's to init
/* EVENT
@@ -102,40 +102,37 @@ void EscChecks::checkAndReport(const Context &context, Report &reporter)
void EscChecks::checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status)
{
const NavModes required_modes = _param_escs_checks_required.get() ? NavModes::All : NavModes::None;
const NavModes required_modes = _param_com_arm_chk_escs.get() ? NavModes::All : NavModes::None;
if (esc_status.esc_count > 0) {
// Check if one or more the ESCs are offline
char esc_fail_msg[50];
esc_fail_msg[0] = '\0';
int online_bitmask = (1 << esc_status.esc_count) - 1;
for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; ++i) {
const bool mapped = math::isInRange(esc_status.esc[i].actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1,
uint8_t(actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1));
const bool offline = (esc_status.esc_online_flags & (1 << i)) == 0;
// Check if one or more the ESCs are offline
if (online_bitmask != esc_status.esc_online_flags) {
for (int index = 0; index < esc_status.esc_count; index++) {
if ((esc_status.esc_online_flags & (1 << index)) == 0) {
uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1;
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t>(required_modes, health_component_t::motors_escs, events::ID("check_escs_offline"),
events::Log::Critical, "ESC {1} offline", motor_index);
snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", motor_index);
esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
}
}
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
if (mapped && offline) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t>(required_modes, health_component_t::motors_escs, events::ID("check_escs_offline"),
events::Log::Critical, "ESC {1} offline", i + 1);
snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", i + 1);
esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
}
}
for (int index = 0; index < esc_status.esc_count; index++) {
if ((esc_fail_msg[0] != '\0') && reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "%soffline. %s\t", esc_fail_msg, context.isArmed() ? "Land now!" : "");
}
for (int index = 0; index < math::min(esc_status.esc_count, esc_status_s::CONNECTED_ESC_MAX); ++index) {
if (esc_status.esc[index].failures != 0) {
src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp
+1 -1
View File
@@ -54,6 +54,6 @@ private:
const hrt_abstime _start_time{hrt_absolute_time()};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs
)
};