dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: move esc checks to arming check

This commit is contained in:
Beat Küng 2022-08-12 13:44:39 +02:00 committed by Daniel Agar
parent c9037f115b
commit f197c8884d
9 changed files with 249 additions and 200 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_status_flags.msg
View File

@ -29,8 +29,6 @@ bool local_velocity_valid # set to true by the commander app if the quality of
bool global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
bool gps_position_valid
bool home_position_valid # indicates a valid home position (a valid home position is not always a valid launch)
bool escs_error # set to true if one or more ESCs reporting esc_status are offline
bool escs_failure # set to true if one or more ESCs reporting esc_status has a failure
bool offboard_control_signal_lost

154
src/modules/commander/Commander.cpp
View File

@ -532,38 +532,6 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
return "";
};
using esc_fault_reason_t = events::px4::enums::esc_fault_reason_t;
static_assert(esc_report_s::ESC_FAILURE_COUNT == (static_cast<uint8_t>(esc_fault_reason_t::_max) + 1)
, "ESC fault flags mismatch!");
static constexpr const char *esc_fault_reason_str(esc_fault_reason_t esc_fault_reason)
{
switch (esc_fault_reason) {
case esc_fault_reason_t::over_current: return "over current";
case esc_fault_reason_t::over_voltage: return "over voltage";
case esc_fault_reason_t::motor_over_temp: return "motor critical temperature";
case esc_fault_reason_t::over_rpm: return "over RPM";
case esc_fault_reason_t::inconsistent_cmd: return "control failure";
case esc_fault_reason_t::motor_stuck: return "motor stall";
case esc_fault_reason_t::failure_generic: return "hardware failure";
case esc_fault_reason_t::motor_warn_temp: return "motor over temperature";
case esc_fault_reason_t::esc_warn_temp: return "over temperature";
case esc_fault_reason_t::esc_over_temp: return "critical temperature";
}
return "";
};
using navigation_mode_t = events::px4::enums::navigation_mode_t;
@ -2014,34 +1982,6 @@ Commander::run()
}
}
if (_esc_status_sub.updated()) {
/* ESCs status changed */
esc_status_check();
} else if (_param_escs_checks_required.get() != 0) {
if (!_vehicle_status_flags.escs_error) {
if ((_last_esc_status_updated != 0) && (hrt_elapsed_time(&_last_esc_status_updated) > 700_ms)) {
/* Detect timeout after first telemetry packet received
* Some DShot ESCs are unresponsive for ~550ms during their initialization, so we use a timeout higher than that
*/
mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry timeout\t");
events::send(events::ID("commander_esc_telemetry_timeout"), events::Log::Critical,
"ESCs telemetry timeout");
_vehicle_status_flags.escs_error = true;
} else if (_last_esc_status_updated == 0 && hrt_elapsed_time(&_boot_timestamp) > 5000_ms) {
/* Detect if esc telemetry is not connected after reboot */
mavlink_log_critical(&_mavlink_log_pub, "ESCs telemetry not connected\t");
events::send(events::ID("commander_esc_telemetry_not_con"), events::Log::Critical,
"ESCs telemetry not connected");
_vehicle_status_flags.escs_error = true;
}
}
}
_home_position.update(_param_com_home_en.get(), !_arm_state_machine.isArmed() && _vehicle_land_detected.landed);
_vehicle_status_flags.home_position_valid = _home_position.valid();
@ -3578,100 +3518,6 @@ Commander::offboard_control_update()
}
}
void Commander::esc_status_check()
{
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';
int online_bitmask = (1 << esc_status.esc_count) - 1;
// Check if ALL the ESCs are online
if (online_bitmask == esc_status.esc_online_flags) {
_vehicle_status_flags.escs_error = false;
_last_esc_online_flags = esc_status.esc_online_flags;
} else if (_last_esc_online_flags == esc_status.esc_online_flags) {
// Avoid checking the status if the flags are the same or if the mixer has not yet been loaded in the ESC driver
_vehicle_status_flags.escs_error = true;
} else if (esc_status.esc_online_flags < _last_esc_online_flags) {
// Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warnings at boot
for (int index = 0; index < esc_status.esc_count; index++) {
if ((esc_status.esc_online_flags & (1 << index)) == 0) {
snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1);
esc_fail_msg[sizeof(esc_fail_msg) - 1] = '\0';
events::px4::enums::suggested_action_t action = _arm_state_machine.isArmed() ?
events::px4::enums::suggested_action_t::land :
events::px4::enums::suggested_action_t::none;
uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1;
events::send<uint8_t, events::px4::enums::suggested_action_t>(events::ID("commander_esc_offline"),
events::Log::Critical, "ESC{1} offline. {2}", motor_index, action);
}
}
mavlink_log_critical(&_mavlink_log_pub, "%soffline. %s\t", esc_fail_msg,
_arm_state_machine.isArmed() ? "Land now!" : "");
_last_esc_online_flags = esc_status.esc_online_flags;
_vehicle_status_flags.escs_error = true;
}
_vehicle_status_flags.escs_failure = false;
for (int index = 0; index < esc_status.esc_count; index++) {
_vehicle_status_flags.escs_failure |= esc_status.esc[index].failures > 0;
if (esc_status.esc[index].failures != _last_esc_failure[index]) {
for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(esc_fault_reason_t::_max);
fault_index++) {
if (esc_status.esc[index].failures & (1 << fault_index)) {
esc_fault_reason_t fault_reason_index = static_cast<esc_fault_reason_t>(fault_index);
const char *user_action = nullptr;
events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none;
if (fault_reason_index == esc_fault_reason_t::motor_warn_temp
|| fault_reason_index == esc_fault_reason_t::esc_warn_temp) {
user_action = "Reduce throttle";
action = events::px4::enums::suggested_action_t::reduce_throttle;
} else {
user_action = "Land now!";
action = events::px4::enums::suggested_action_t::land;
}
mavlink_log_emergency(&_mavlink_log_pub, "ESC%d: %s. %s \t", index + 1,
esc_fault_reason_str(fault_reason_index), _arm_state_machine.isArmed() ? user_action : "");
events::send<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>
(events::ID("commander_esc_fault"), {events::Log::Emergency, events::LogInternal::Warning},
"ESC {1}: {2}. {3}", index + 1, fault_reason_index, action);
}
}
}
_last_esc_failure[index] = esc_status.esc[index].failures;
}
}
_last_esc_status_updated = esc_status.timestamp;
}
void Commander::send_parachute_command()
{
vehicle_command_s vcmd{};

10
src/modules/commander/Commander.hpp
View File

@ -69,7 +69,6 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/cpuload.h>
#include <uORB/topics/distance_sensor.h>
#include <uORB/topics/esc_status.h>
#include <uORB/topics/geofence_result.h>
#include <uORB/topics/iridiumsbd_status.h>
#include <uORB/topics/manual_control_setpoint.h>
@ -136,8 +135,6 @@ private:
*/
void data_link_check();
void esc_status_check();
void manual_control_check();
bool handle_command(const vehicle_command_s &cmd);
@ -217,8 +214,6 @@ private:
// Engine failure
(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required,
(ParamInt<px4::params::COM_FLIGHT_UUID>) _param_flight_uuid,
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
@ -293,10 +288,6 @@ private:
hrt_abstime _high_latency_datalink_heartbeat{0};
hrt_abstime _high_latency_datalink_lost{0};
int _last_esc_online_flags{-1};
int _last_esc_failure[esc_status_s::CONNECTED_ESC_MAX] {};
hrt_abstime _last_esc_status_updated{0};
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
hrt_abstime _battery_failsafe_timestamp{0};
Hysteresis _auto_disarm_landed{false};
@ -348,7 +339,6 @@ private:
// Subscriptions
uORB::Subscription _action_request_sub {ORB_ID(action_request)};
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)};
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};

1
src/modules/commander/HealthAndArmingChecks/CMakeLists.txt
View File

@ -42,6 +42,7 @@ px4_add_library(health_and_arming_checks
checks/imuConsistencyCheck.cpp
checks/airspeedCheck.cpp
checks/distanceSensorChecks.cpp
checks/escCheck.cpp
checks/rcCalibrationCheck.cpp
checks/powerCheck.cpp
checks/estimatorCheck.cpp

3
src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.hpp
View File

@ -44,6 +44,7 @@
#include "checks/baroCheck.hpp"
#include "checks/cpuResourceCheck.hpp"
#include "checks/distanceSensorChecks.hpp"
#include "checks/escCheck.hpp"
#include "checks/estimatorCheck.hpp"
#include "checks/failureDetectorCheck.hpp"
#include "checks/gyroCheck.hpp"
@ -98,6 +99,7 @@ private:
BaroChecks _baro_checks;
CpuResourceChecks _cpu_resource_checks;
DistanceSensorChecks _distance_sensor_checks;
EscChecks _esc_checks;
EstimatorChecks _estimator_checks;
FailureDetectorChecks _failure_detector_checks;
GyroChecks _gyro_checks;
@ -118,6 +120,7 @@ private:
&_baro_checks,
&_cpu_resource_checks,
&_distance_sensor_checks,
&_esc_checks,
&_estimator_checks,
&_failure_detector_checks,
&_gyro_checks,

186
src/modules/commander/HealthAndArmingChecks/checks/escCheck.cpp Normal file
View File

@ -0,0 +1,186 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "escCheck.hpp"
#include <px4_platform_common/events.h>
#include <uORB/topics/actuator_motors.h>
using namespace time_literals;
using esc_fault_reason_t = events::px4::enums::esc_fault_reason_t;
static_assert(esc_report_s::ESC_FAILURE_COUNT == (static_cast<uint8_t>(esc_fault_reason_t::_max) + 1)
, "ESC fault flags mismatch!");
static constexpr const char *esc_fault_reason_str(esc_fault_reason_t esc_fault_reason)
{
switch (esc_fault_reason) {
case esc_fault_reason_t::over_current: return "over current";
case esc_fault_reason_t::over_voltage: return "over voltage";
case esc_fault_reason_t::motor_over_temp: return "motor critical temperature";
case esc_fault_reason_t::over_rpm: return "over RPM";
case esc_fault_reason_t::inconsistent_cmd: return "control failure";
case esc_fault_reason_t::motor_stuck: return "motor stall";
case esc_fault_reason_t::failure_generic: return "hardware failure";
case esc_fault_reason_t::motor_warn_temp: return "motor over temperature";
case esc_fault_reason_t::esc_warn_temp: return "over temperature";
case esc_fault_reason_t::esc_over_temp: return "critical temperature";
}
return "";
};
void EscChecks::checkAndReport(const Context &context, Report &reporter)
{
const hrt_abstime now = hrt_absolute_time();
const hrt_abstime esc_telemetry_timeout =
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;
if (_esc_status_sub.copy(&esc_status) && now - esc_status.timestamp < esc_telemetry_timeout) {
checkEscStatus(context, reporter, esc_status);
reporter.setIsPresent(health_component_t::motors_escs);
} else if (_param_escs_checks_required.get()
&& now - _start_time > 5_s) { // Wait a bit after startup to allow esc's to init
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_escs_telem_missing"),
events::Log::Critical, "ESC telemetry missing");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: ESC telemetry missing");
}
}
}
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;
if (esc_status.esc_count > 0) {
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 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!" : "");
}
}
for (int index = 0; index < esc_status.esc_count; index++) {
if (esc_status.esc[index].failures != 0) {
for (uint8_t fault_index = 0; fault_index <= static_cast<uint8_t>(esc_fault_reason_t::_max); fault_index++) {
if (esc_status.esc[index].failures & (1 << fault_index)) {
esc_fault_reason_t fault_reason_index = static_cast<esc_fault_reason_t>(fault_index);
const char *user_action = "";
events::px4::enums::suggested_action_t action = events::px4::enums::suggested_action_t::none;
if (context.isArmed()) {
if (fault_reason_index == esc_fault_reason_t::motor_warn_temp
|| fault_reason_index == esc_fault_reason_t::esc_warn_temp) {
user_action = "Reduce throttle";
action = events::px4::enums::suggested_action_t::reduce_throttle;
} else {
user_action = "Land now!";
action = events::px4::enums::suggested_action_t::land;
}
}
uint8_t motor_index = esc_status.esc[index].actuator_function - actuator_motors_s::ACTUATOR_FUNCTION_MOTOR1 + 1;
/* EVENT
* @description
* {3}
*
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure<uint8_t, events::px4::enums::esc_fault_reason_t, events::px4::enums::suggested_action_t>(
required_modes, health_component_t::motors_escs, events::ID("check_escs_fault"),
events::Log::Critical, "ESC {1}: {2}", motor_index, fault_reason_index, action);
if (reporter.mavlink_log_pub()) {
mavlink_log_emergency(reporter.mavlink_log_pub(), "ESC%d: %s. %s \t", motor_index,
esc_fault_reason_str(fault_reason_index), user_action);
}
}
}
}
}
}
}

59
src/modules/commander/HealthAndArmingChecks/checks/escCheck.hpp Normal file
View File

@ -0,0 +1,59 @@
/****************************************************************************
*
* Copyright (c) 2022 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include "../Common.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/topics/esc_status.h>
class EscChecks : public HealthAndArmingCheckBase
{
public:
EscChecks() = default;
~EscChecks() = default;
void checkAndReport(const Context &context, Report &reporter) override;
private:
void checkEscStatus(const Context &context, Report &reporter, const esc_status_s &esc_status);
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
const hrt_abstime _start_time{hrt_absolute_time()};
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_escs_checks_required
)
};

33
src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp
View File

@ -161,39 +161,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter)
}
}
// ESC checks
if (_param_com_arm_chk_escs.get() && reporter.failsafeFlags().escs_error) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_system_escs_offline"),
events::Log::Error, "One or more ESCs are offline");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: One or more ESCs are offline");
}
}
if (_param_com_arm_chk_escs.get() && reporter.failsafeFlags().escs_failure) {
/* EVENT
* @description
* <profile name="dev">
* This check can be configured via <param>COM_ARM_CHK_ESCS</param> parameter.
* </profile>
*/
reporter.healthFailure(NavModes::All, health_component_t::motors_escs, events::ID("check_system_escs_failure"),
events::Log::Error, "One or more ESCs have a failure");
if (reporter.mavlink_log_pub()) {
mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: One or more ESCs have a failure");
}
}
reporter.setIsPresent(health_component_t::motors_escs); // TODO: based on telemetry
// VTOL in transition
if (context.status().is_vtol && !context.isArmed()) {
if (context.status().in_transition_mode) {

1
src/modules/commander/HealthAndArmingChecks/checks/systemCheck.hpp
View File

@ -54,7 +54,6 @@ private:
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_com_arm_mis_req,
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
(ParamBool<px4::params::COM_ARM_CHK_ESCS>) _param_com_arm_chk_escs,
(ParamInt<px4::params::COM_ARM_AUTH_REQ>) _param_com_arm_auth_req,
(ParamInt<px4::params::GF_ACTION>) _param_gf_action
)