forked from Archive/PX4-Autopilot
commander: move esc checks to arming check
This commit is contained in:
parent
c9037f115b
commit
f197c8884d
|
@ -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 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 gps_position_valid
|
||||||
bool home_position_valid # indicates a valid home position (a valid home position is not always a valid launch)
|
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
|
bool offboard_control_signal_lost
|
||||||
|
|
||||||
|
|
|
@ -532,38 +532,6 @@ static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_r
|
||||||
return "";
|
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;
|
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);
|
_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();
|
_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()
|
void Commander::send_parachute_command()
|
||||||
{
|
{
|
||||||
vehicle_command_s vcmd{};
|
vehicle_command_s vcmd{};
|
||||||
|
|
|
@ -69,7 +69,6 @@
|
||||||
#include <uORB/topics/battery_status.h>
|
#include <uORB/topics/battery_status.h>
|
||||||
#include <uORB/topics/cpuload.h>
|
#include <uORB/topics/cpuload.h>
|
||||||
#include <uORB/topics/distance_sensor.h>
|
#include <uORB/topics/distance_sensor.h>
|
||||||
#include <uORB/topics/esc_status.h>
|
|
||||||
#include <uORB/topics/geofence_result.h>
|
#include <uORB/topics/geofence_result.h>
|
||||||
#include <uORB/topics/iridiumsbd_status.h>
|
#include <uORB/topics/iridiumsbd_status.h>
|
||||||
#include <uORB/topics/manual_control_setpoint.h>
|
#include <uORB/topics/manual_control_setpoint.h>
|
||||||
|
@ -136,8 +135,6 @@ private:
|
||||||
*/
|
*/
|
||||||
void data_link_check();
|
void data_link_check();
|
||||||
|
|
||||||
void esc_status_check();
|
|
||||||
|
|
||||||
void manual_control_check();
|
void manual_control_check();
|
||||||
|
|
||||||
bool handle_command(const vehicle_command_s &cmd);
|
bool handle_command(const vehicle_command_s &cmd);
|
||||||
|
@ -217,8 +214,6 @@ private:
|
||||||
// Engine failure
|
// Engine failure
|
||||||
(ParamInt<px4::params::COM_ACT_FAIL_ACT>) _param_com_actuator_failure_act,
|
(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_FLIGHT_UUID>) _param_flight_uuid,
|
||||||
(ParamInt<px4::params::COM_TAKEOFF_ACT>) _param_takeoff_finished_action,
|
(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_heartbeat{0};
|
||||||
hrt_abstime _high_latency_datalink_lost{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};
|
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
||||||
hrt_abstime _battery_failsafe_timestamp{0};
|
hrt_abstime _battery_failsafe_timestamp{0};
|
||||||
Hysteresis _auto_disarm_landed{false};
|
Hysteresis _auto_disarm_landed{false};
|
||||||
|
@ -348,7 +339,6 @@ private:
|
||||||
// Subscriptions
|
// Subscriptions
|
||||||
uORB::Subscription _action_request_sub {ORB_ID(action_request)};
|
uORB::Subscription _action_request_sub {ORB_ID(action_request)};
|
||||||
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
|
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 _geofence_result_sub{ORB_ID(geofence_result)};
|
||||||
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
||||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||||
|
|
|
@ -42,6 +42,7 @@ px4_add_library(health_and_arming_checks
|
||||||
checks/imuConsistencyCheck.cpp
|
checks/imuConsistencyCheck.cpp
|
||||||
checks/airspeedCheck.cpp
|
checks/airspeedCheck.cpp
|
||||||
checks/distanceSensorChecks.cpp
|
checks/distanceSensorChecks.cpp
|
||||||
|
checks/escCheck.cpp
|
||||||
checks/rcCalibrationCheck.cpp
|
checks/rcCalibrationCheck.cpp
|
||||||
checks/powerCheck.cpp
|
checks/powerCheck.cpp
|
||||||
checks/estimatorCheck.cpp
|
checks/estimatorCheck.cpp
|
||||||
|
|
|
@ -44,6 +44,7 @@
|
||||||
#include "checks/baroCheck.hpp"
|
#include "checks/baroCheck.hpp"
|
||||||
#include "checks/cpuResourceCheck.hpp"
|
#include "checks/cpuResourceCheck.hpp"
|
||||||
#include "checks/distanceSensorChecks.hpp"
|
#include "checks/distanceSensorChecks.hpp"
|
||||||
|
#include "checks/escCheck.hpp"
|
||||||
#include "checks/estimatorCheck.hpp"
|
#include "checks/estimatorCheck.hpp"
|
||||||
#include "checks/failureDetectorCheck.hpp"
|
#include "checks/failureDetectorCheck.hpp"
|
||||||
#include "checks/gyroCheck.hpp"
|
#include "checks/gyroCheck.hpp"
|
||||||
|
@ -98,6 +99,7 @@ private:
|
||||||
BaroChecks _baro_checks;
|
BaroChecks _baro_checks;
|
||||||
CpuResourceChecks _cpu_resource_checks;
|
CpuResourceChecks _cpu_resource_checks;
|
||||||
DistanceSensorChecks _distance_sensor_checks;
|
DistanceSensorChecks _distance_sensor_checks;
|
||||||
|
EscChecks _esc_checks;
|
||||||
EstimatorChecks _estimator_checks;
|
EstimatorChecks _estimator_checks;
|
||||||
FailureDetectorChecks _failure_detector_checks;
|
FailureDetectorChecks _failure_detector_checks;
|
||||||
GyroChecks _gyro_checks;
|
GyroChecks _gyro_checks;
|
||||||
|
@ -118,6 +120,7 @@ private:
|
||||||
&_baro_checks,
|
&_baro_checks,
|
||||||
&_cpu_resource_checks,
|
&_cpu_resource_checks,
|
||||||
&_distance_sensor_checks,
|
&_distance_sensor_checks,
|
||||||
|
&_esc_checks,
|
||||||
&_estimator_checks,
|
&_estimator_checks,
|
||||||
&_failure_detector_checks,
|
&_failure_detector_checks,
|
||||||
&_gyro_checks,
|
&_gyro_checks,
|
||||||
|
|
|
@ -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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
|
@ -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
|
||||||
|
)
|
||||||
|
};
|
|
@ -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
|
// VTOL in transition
|
||||||
if (context.status().is_vtol && !context.isArmed()) {
|
if (context.status().is_vtol && !context.isArmed()) {
|
||||||
if (context.status().in_transition_mode) {
|
if (context.status().in_transition_mode) {
|
||||||
|
|
|
@ -54,7 +54,6 @@ private:
|
||||||
(ParamInt<px4::params::CBRK_USB_CHK>) _param_cbrk_usb_chk,
|
(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_MIS_REQ>) _param_com_arm_mis_req,
|
||||||
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
|
(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::COM_ARM_AUTH_REQ>) _param_com_arm_auth_req,
|
||||||
(ParamInt<px4::params::GF_ACTION>) _param_gf_action
|
(ParamInt<px4::params::GF_ACTION>) _param_gf_action
|
||||||
)
|
)
|
||||||
|
|
Loading…
Reference in New Issue