From 458c775bfd559e9f4c634966b730b393be1b5654 Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Tue, 13 Jan 2026 21:42:56 -0900 Subject: [PATCH] fix ifdef, adjust esc isInRange check --- .../commander/HealthAndArmingChecks/checks/escCheck.cpp | 5 ++--- src/modules/mavlink/mavlink_receiver.cpp | 6 +++++- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp index 5fa9b4388e..61657b9803 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp @@ -105,13 +105,12 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e const NavModes required_modes = _param_escs_checks_required.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; - // Check if one or more the ESCs are offline if (online_bitmask != esc_status.esc_online_flags) { for (int i = 0; i < esc_status_s::CONNECTED_ESC_MAX; i++) { @@ -119,7 +118,7 @@ void EscChecks::checkEscStatus(const Context &context, Report &reporter, const e uint8_t actuator_function = esc_status.esc[i].actuator_function; bool is_motor = math::isInRange(actuator_function, actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1, - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR12); + actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + actuator_motors_s::NUM_CONTROLS - 1); bool is_online = esc_status.esc_online_flags & (1 << i); if (is_motor && !is_online) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 57ed503307..116127349e 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -602,11 +602,15 @@ void MavlinkReceiver::handle_message_command_both(mavlink_message_t *msg, const uint16_t message_id = (uint16_t)roundf(vehicle_command.param1); +#if defined(MAVLINK_MSG_ID_ESC_EEPROM) // NOTE: ESC_EEPROM message request handling is deferred - DShot driver handles and triggers reading if (message_id == MAVLINK_MSG_ID_ESC_EEPROM) { PX4_INFO("publishing MAV_CMD_REQUEST_MESSAGE for MAVLINK_MSG_ID_ESC_EEPROM"); _cmd_pub.publish(vehicle_command); - +else + if (1) + // no-op +#endif } else if (message_id == MAVLINK_MSG_ID_MESSAGE_INTERVAL) { get_message_interval((int)(cmd_mavlink.param2 + 0.5f));