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

Failure detector - Add flight termination comments, make format

This commit is contained in:
bresch 2019-01-30 14:42:08 +01:00 committed by Beat Küng
parent 0655f7603b
commit 59265f6663
5 changed files with 41 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/drivers/px4io/px4io.cpp
View File

@ -1031,7 +1031,7 @@ PX4IO::task_main()
/* Check if the flight termination circuit breaker has been updated */
_cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY);
/* Tell IO that it can terminate the flight if FMU is not responding */
/* Tell IO that it can terminate the flight if FMU is not responding or if a failure has been reported by the FailureDetector logic */
(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ENABLE_FLIGHTTERM, !_cb_flighttermination);
param_get(param_find("RC_RSSI_PWM_CHAN"), &_rssi_pwm_chan);

8
src/lib/circuit_breaker/circuit_breaker_params.c
View File

@ -104,7 +104,13 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0);
* Circuit breaker for flight termination
*
* Setting this parameter to 121212 will disable the flight termination action.
* --> The IO driver will not do flight termination if requested by the FMU
* If configured, flight termination can be triggered as a failsafe action of the following failures:
* - RC or data link is lost
* - The vehicle moves outside the geofence
* - The FailureDetector reports a failure (e.g.: attitude failure).
* - GPS is lost, height estimate is not available and RC is lost
* Flight termination is also triggered on the IO processor if FMU is lost.
* => With this circuit breaker enabled, flight termination will never be exectued, even if configured.
* WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK
*
* @reboot_required true

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

@ -2207,6 +2207,7 @@ Commander::run()
/* Check for failure detector status */
const bool failure_detector_updated = _failure_detector.update();
if (failure_detector_updated) {
const uint8_t failure_status = _failure_detector.get_status();
@ -2218,9 +2219,9 @@ Commander::run()
}
if (armed.armed &&
failure_detector_updated &&
!_in_flight_termination &&
!status_flags.circuit_breaker_flight_termination_disabled) {
failure_detector_updated &&
!_in_flight_termination &&
!status_flags.circuit_breaker_flight_termination_disabled) {
if (status.failure_detector_status != 0) {

28
src/modules/commander/PreflightCheck.cpp
View File

@ -682,20 +682,22 @@ static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s
// Ignore failure detector check after arming.
return true;
} else {
if (status.failure_detector_status != 0) {
success = false;
}
if (report_fail) {
if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ROLL FAILURE DETECTED");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: PITCH FAILURE DETECTED");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: ALTITUDE FAILURE DETECTED");
}
if (status.failure_detector_status != 0) {
success = false;
if (report_fail) {
if (status.failure_detector_status & vehicle_status_s::FAILURE_ROLL) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: Roll failure detected");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_PITCH) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: Pitch failure detected");
}
if (status.failure_detector_status & vehicle_status_s::FAILURE_ALT) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: Altitude failure detected");
}
}
}

28
src/modules/px4iofirmware/mixer.cpp
View File

@ -201,33 +201,33 @@ mixer_tick(void)
*
*/
should_arm = (
(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */
&& (
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */
|| (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
)
);
(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */
&& (
((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and FMU is armed */
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) /* and there is valid input via or mixer */
|| (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) /* or direct PWM is set */
)
);
should_arm_nothrottle = (
(r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) /* IO initialised without error */
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) /* and IO is armed */
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) /* and there is valid input via or mixer */
);
);
should_always_enable_pwm = (
(r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)
);
(r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK)
&& (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)
);
/*
* Check if failsafe termination is set - if yes,
* set the force failsafe flag once entering the first
* failsafe condition.
*/
if ( /* Flight termination is allowed */
if ( /* Flight termination is allowed */
REG_TO_BOOL(r_setup_flightterm) &&
/* If we ended up not changing the default mixer */
(source == MIX_DISARMED) &&