fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 15:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: added esc_status prearm checks

Browse Source 
Signed-off-by: Claudio Micheli <claudio@auterion.com>
This commit is contained in:
Claudio Micheli
2019-05-31 13:16:10 +02:00
committed by Beat Küng
parent d06c679252
commit 187a025dfe
6 changed files with 85 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+59
View File
@@ -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;
}
}