fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-01 06:31:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander only copy actuator_controls if engine failure enabled

Browse Source
This commit is contained in:
Daniel Agar
2018-02-19 15:28:47 +01:00
committed by Lorenz Meier
parent 43c7f7edbe
commit 8fc659dcb2
1 changed files with 25 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+25 -23
View File
@@ -1453,8 +1453,6 @@ Commander::run()
/* Subscribe to actuator controls (outputs) */ /* Subscribe to actuator controls (outputs) */
int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
struct actuator_controls_s actuator_controls;
memset(&actuator_controls, 0, sizeof(actuator_controls));
/* Subscribe to vtol vehicle status topic */ /* Subscribe to vtol vehicle status topic */
int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status)); int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
@@ -1549,8 +1547,6 @@ Commander::run()
param_get(_param_rc_arm_hyst, &rc_arm_hyst); param_get(_param_rc_arm_hyst, &rc_arm_hyst);
rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC; rc_arm_hyst *= COMMANDER_MONITORING_LOOPSPERMSEC;
transition_result_t arming_ret;
int32_t datalink_loss_act = 0; int32_t datalink_loss_act = 0;
int32_t rc_loss_act = 0; int32_t rc_loss_act = 0;
int32_t datalink_loss_timeout = 10; int32_t datalink_loss_timeout = 10;
@@ -1607,7 +1603,7 @@ Commander::run()
while (!should_exit()) { while (!should_exit()) {
arming_ret = TRANSITION_NOT_CHANGED; transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
/* update parameters */ /* update parameters */
bool params_updated = false; bool params_updated = false;
@@ -2682,31 +2678,37 @@ Commander::run()
} }
} }
/* handle commands last, as the system needs to be updated to handle them */ // engine failure detection
// TODO: move out of commander
orb_check(actuator_controls_sub, &updated); orb_check(actuator_controls_sub, &updated);
if (updated) { if (updated) {
/* got command */
orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
/* Check engine failure /* Check engine failure
* only for fixed wing for now * only for fixed wing for now
*/ */
if (!status_flags.circuit_breaker_engaged_enginefailure_check && if (!status_flags.circuit_breaker_engaged_enginefailure_check &&
status.is_rotary_wing == false && !status.is_rotary_wing && !status.is_vtol && armed.armed) {
armed.armed &&
((actuator_controls.control[3] > ef_throttle_thres && actuator_controls_s actuator_controls = {};
battery.current_a / actuator_controls.control[3] < orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
ef_current2throttle_thres) ||
(status.engine_failure))) { const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
/* potential failure, measure time */ const float current2throttle = battery.current_a / throttle;
if (timestamp_engine_healthy > 0 &&
hrt_elapsed_time(&timestamp_engine_healthy) > if (((throttle > ef_throttle_thres) && (current2throttle < ef_current2throttle_thres))
ef_time_thres * 1e6f && || status.engine_failure) {
!status.engine_failure) {
status.engine_failure = true; const float elapsed = hrt_elapsed_time(&timestamp_engine_healthy) / 1e6f;
status_changed = true;
mavlink_log_critical(&mavlink_log_pub, "Engine Failure"); /* potential failure, measure time */
if ((timestamp_engine_healthy > 0) && (elapsed > ef_time_thres)
&& !status.engine_failure) {
status.engine_failure = true;
status_changed = true;
PX4_ERR("Engine Failure");
}
} }
} else { } else {