forked from Archive/PX4-Autopilot
Commander: added esc_status prearm checks
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
parent
d06c679252
commit
187a025dfe
|
@ -14,7 +14,7 @@ bool condition_local_velocity_valid # set to true by the commander app if the q
|
|||
bool condition_local_altitude_valid
|
||||
bool condition_power_input_valid # set if input power is valid
|
||||
bool condition_battery_healthy # set if battery is available and not low
|
||||
|
||||
bool condition_escs_error # set to true in case of standard ESCs that do not provide status OR if all the ESCs are online
|
||||
bool circuit_breaker_engaged_power_check
|
||||
bool circuit_breaker_engaged_airspd_check
|
||||
bool circuit_breaker_engaged_enginefailure_check
|
||||
|
@ -22,6 +22,7 @@ bool circuit_breaker_engaged_gpsfailure_check
|
|||
bool circuit_breaker_flight_termination_disabled
|
||||
bool circuit_breaker_engaged_usb_check
|
||||
bool circuit_breaker_engaged_posfailure_check # set to true when the position valid checks have been disabled
|
||||
bool circuit_breaker_engaged_escs_check
|
||||
|
||||
bool offboard_control_signal_found_once
|
||||
bool offboard_control_signal_lost
|
||||
|
@ -36,5 +37,3 @@ bool usb_connected # status of the USB power supp
|
|||
|
||||
bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter
|
||||
bool avoidance_system_valid # Status of the obstacle avoidance system
|
||||
|
||||
uint8 number_of_actuators # Number of active actuators
|
||||
|
|
|
@ -95,6 +95,7 @@
|
|||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
#include <uORB/topics/vtol_vehicle_status.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
typedef enum VEHICLE_MODE_FLAG {
|
||||
VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
|
||||
|
@ -149,6 +150,7 @@ static uint8_t _last_sp_man_arm_switch = 0;
|
|||
|
||||
static struct vtol_vehicle_status_s vtol_status = {};
|
||||
static struct cpuload_s cpuload = {};
|
||||
static struct esc_status_s esc_status = {};
|
||||
|
||||
static bool last_overload = false;
|
||||
|
||||
|
@ -1198,6 +1200,7 @@ Commander::run()
|
|||
param_t _param_arm_switch_is_button = param_find("COM_ARM_SWISBTN");
|
||||
param_t _param_rc_override = param_find("COM_RC_OVERRIDE");
|
||||
param_t _param_arm_mission_required = param_find("COM_ARM_MIS_REQ");
|
||||
param_t _param_escs_checks_required = param_find("COM_ARM_CHK_ESCS");
|
||||
param_t _param_flight_uuid = param_find("COM_FLIGHT_UUID");
|
||||
param_t _param_takeoff_finished_action = param_find("COM_TAKEOFF_ACT");
|
||||
|
||||
|
@ -1243,6 +1246,7 @@ Commander::run()
|
|||
PX4_ERR("Failed to register power notification callback");
|
||||
}
|
||||
|
||||
|
||||
get_circuit_breaker_params();
|
||||
|
||||
/* armed topic */
|
||||
|
@ -1275,6 +1279,7 @@ Commander::run()
|
|||
uORB::Subscription subsys_sub{ORB_ID(subsystem_info)};
|
||||
uORB::Subscription system_power_sub{ORB_ID(system_power)};
|
||||
uORB::Subscription vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
|
||||
uORB::Subscription esc_status_sub{ORB_ID(esc_status)};
|
||||
|
||||
geofence_result_s geofence_result {};
|
||||
|
||||
|
@ -1329,6 +1334,10 @@ Commander::run()
|
|||
param_get(_param_arm_mission_required, &arm_mission_required_param);
|
||||
arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT));
|
||||
|
||||
int32_t arm_escs_checks_required_param = 0;
|
||||
param_get(_param_escs_checks_required, &arm_escs_checks_required_param);
|
||||
arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT;
|
||||
|
||||
status.rc_input_mode = rc_in_off;
|
||||
|
||||
// user adjustable duration required to assert arm/disarm via throttle/rudder stick
|
||||
|
@ -1458,6 +1467,9 @@ Commander::run()
|
|||
arm_requirements = (arm_without_gps_param == 1) ? ARM_REQ_NONE : ARM_REQ_GPS_BIT;
|
||||
param_get(_param_arm_mission_required, &arm_mission_required_param);
|
||||
arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT));
|
||||
param_get(_param_escs_checks_required, &arm_escs_checks_required_param);
|
||||
arm_requirements |= (arm_escs_checks_required_param == 0) ? ARM_REQ_NONE : ARM_REQ_ESCS_CHECK_BIT;
|
||||
|
||||
|
||||
/* flight mode slots */
|
||||
param_get(_param_fmode_1, &_flight_mode_slots[0]);
|
||||
|
@ -1610,6 +1622,19 @@ Commander::run()
|
|||
}
|
||||
}
|
||||
|
||||
if (orb_exists(ORB_ID(esc_status), 0) == PX4_OK) {
|
||||
|
||||
if (esc_status_sub.updated()) {
|
||||
/* ESCs status changed */
|
||||
esc_status_sub.copy(&esc_status);
|
||||
esc_status_check();
|
||||
}
|
||||
|
||||
} else {
|
||||
status_flags.condition_escs_error = false;
|
||||
}
|
||||
|
||||
|
||||
estimator_check(&status_changed);
|
||||
airspeed_use_check();
|
||||
|
||||
|
@ -4347,3 +4372,37 @@ Commander::offboard_control_update(bool &status_changed)
|
|||
}
|
||||
|
||||
}
|
||||
|
||||
void Commander::esc_status_check()
|
||||
{
|
||||
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) {
|
||||
status_flags.condition_escs_error = false;
|
||||
_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 from uavcan_main
|
||||
else if (_last_esc_online_flags == esc_status.esc_online_flags || esc_status.esc_count == 0) {
|
||||
status_flags.condition_escs_error = true;
|
||||
}
|
||||
|
||||
// Only warn the user when an ESC goes from ONLINE to OFFLINE. This is done to prevent showing Offline ESCs warning messages at boot
|
||||
else if (esc_status.esc_online_flags < _last_esc_online_flags) {
|
||||
|
||||
for (int index = 0; index < esc_status.esc_count; index++) {
|
||||
if ((bool)!(esc_status.esc_online_flags & (1 << index))) {
|
||||
snprintf(esc_fail_msg + strlen(esc_fail_msg), sizeof(esc_fail_msg) - strlen(esc_fail_msg), "ESC%d ", index + 1);
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_log_critical(&mavlink_log_pub, "%soffline", esc_fail_msg);
|
||||
|
||||
_last_esc_online_flags = esc_status.esc_online_flags;
|
||||
status_flags.condition_escs_error = true;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -217,6 +217,8 @@ private:
|
|||
|
||||
void battery_status_check();
|
||||
|
||||
void esc_status_check();
|
||||
|
||||
/**
|
||||
* Checks the status of all available data links and handles switching between different system telemetry states.
|
||||
*/
|
||||
|
@ -240,6 +242,8 @@ private:
|
|||
hrt_abstime _high_latency_datalink_heartbeat{0};
|
||||
hrt_abstime _high_latency_datalink_lost{0};
|
||||
|
||||
int _last_esc_online_flags{-1};
|
||||
|
||||
uORB::Subscription _battery_sub{ORB_ID(battery_status)};
|
||||
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
||||
float _battery_current{0.0f};
|
||||
|
|
|
@ -926,3 +926,13 @@ PARAM_DEFINE_INT32(COM_ASPD_FS_DLY, 0);
|
|||
* @group Commander
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_FLT_PROFILE, 0);
|
||||
|
||||
/**
|
||||
* Require all the ESCs to be detected to arm.
|
||||
*
|
||||
* This param is specific for ESCs reporting status. Normal ESCs configurations are not affected by the change of this param.
|
||||
*
|
||||
* @group Commander
|
||||
* @boolean
|
||||
*/
|
||||
PARAM_DEFINE_INT32(COM_ARM_CHK_ESCS, 1);
|
||||
|
|
|
@ -1010,6 +1010,14 @@ bool prearm_check(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &s
|
|||
|
||||
}
|
||||
|
||||
if (status_flags.condition_escs_error && (arm_requirements & ARM_REQ_ESCS_CHECK_BIT)) {
|
||||
if (prearm_ok && reportFailures) {
|
||||
mavlink_log_critical(mavlink_log_pub, "Arming denied! One or more ESCs are offline");
|
||||
prearm_ok = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
return prearm_ok;
|
||||
}
|
||||
|
||||
|
|
|
@ -51,6 +51,7 @@
|
|||
#include <uORB/topics/safety.h>
|
||||
#include <uORB/topics/commander_state.h>
|
||||
#include <uORB/topics/vehicle_status_flags.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
|
||||
typedef enum {
|
||||
TRANSITION_DENIED = -1,
|
||||
|
@ -73,6 +74,7 @@ typedef enum {
|
|||
ARM_REQ_MISSION_BIT = (1 << 0),
|
||||
ARM_REQ_ARM_AUTH_BIT = (1 << 1),
|
||||
ARM_REQ_GPS_BIT = (1 << 2),
|
||||
ARM_REQ_ESCS_CHECK_BIT = (1 << 3)
|
||||
} arm_requirements_t;
|
||||
|
||||
extern const char *const arming_state_names[];
|
||||
|
|
Loading…
Reference in New Issue