Failsafe

This page descibes how to setup the failsafe behavior.

During an AUTO mission:

Datalink enabled (COM_DL_LOSS_EN == 1):

  • Datalink ok, RC ok: MISSION
  • Datalink ok, RC lost: MISSION
  • Datalink lost, RC ok: RTGS
  • Datalink lost, RC lost: RTGS

Datalink disabled (COM_DL_LOSS_EN == 0):

  • Datalink ok, RC ok: MISSION
  • Datalink ok, RC lost: MISSION
  • Datalink lost, RC ok: MISSION
  • Datalink lost, RC lost: RCRECOVER

And after the mission is finished

Datalink enabled (COM_DL_LOSS_EN == 1):

  • Datalink ok, RC ok: MISSION (loitering)
  • Datalink ok, RC lost: MISSION (loitering)
  • Datalink lost, RC ok: RTGS
  • Datalink lost, RC lost: RTGS

Datalink disabled (COM_DL_LOSS_EN == 0):

  • Datalink ok, RC ok: MISSION (loitering)
  • Datalink ok, RC lost: RCRECOVER
  • Datalink lost, RC ok: MISSION (loitering)
  • Datalink lost, RC lost: RCRECOVER

OBC mode

If you fly a larger vehicle under some sort of aviation rule set (which e.g. includes killing throttle under certain conditions) you will be interested in the OBC mode failsafe: State diagram

QGC

  • enable outpput of heartbeats in File/Settings/MAvlink

Params

for OBC failsafe mode the following params are important. Make sure they are set to the correct values indicated below:

  • NAV_DLL_OBC = 1 (obc data link loss mode enabled)
  • NAV_DLL_CH_T = 120 (comms hold timeout in s)
  • NAV_DLL_CH_LAT (comms hold lat, check sign!)
  • NAV_DLL_CH_LON (comms hold lon)
  • NAV_DLL_CH_ALT (comms hold alt)
  • NAV_AH_LAT (airfield home lat, check sign!)
  • NAV_AH_LON (airfield home lon)
  • NAV_AH_ALT (airfield home alt)
  • NAV_DLL_AH_T = 120 time to loiter at airfield home in seconds, if within this time the state is not switched to manual (for landing) flight termination is initiated.
  • NAV_GPSF_LT = 30 (open loop loiter time after gps failure in s)
  • NAV_GPSF_P (pitch for open loop loiter)
  • NAV_GPSF_R (roll for open loop loiter)
  • NAV_GPSF_TR (throttle for open loop loiter)
  • GF_ON = 1 (geofence on)
  • GF_ALTMODE = 1 (use amsl to check altitude)
  • GF_SOURCE = 1 (use gps as source and not global position) (check with regulations!)
  • GF_COUNT = 3 (number of subsequent 'outside' detections needed). Set to small value but not 0 to avoid termination on gps glitch
  • COM_DL_LOSS_EN = 1
  • COM_DL_LOSS_T = 10 (timeout until data link loss triggers)
  • COM_DL_REG_T = 4 (?) (hysteresis timeout until the state of the datalink is set to regained), not not set too low as we risk counting up data link failures and we can only have 2
  • NAV_DLL_N = 2 (number of data link losses which are allowed, at DL loss NAV_DLL_N + 1 the system will fly to airfield home )
  • NAV_RCL_OBC = 1 (obc rc loss mode enabled)
  • NAV_RCL_LT = 120 (amount of loitering at current position before termination)
  • COM_RC_LOSS_T = 0.5, timeout in seconds before rc loss triggers

parameter files for OBC

SD card / startup script

In /etc/extras.txt add

pwm failsafe -c 1 -p 1100
pwm failsafe -c 2 -p 1100
pwm failsafe -c 4 -p 900
pwm failsafe -c 5 -p 1900
pwm terminatefail on

This sets the PWM values which are sent for when flighttermination is activated. Depending on the servo direction some channels may need the reverse value. (The above is the correct setting for a 'wing wing' aircraft)

Use the following to check if the settings are applied correctly:

pwm info

Implementation status

The wip branch is obcfailsafe and obcfailsafe

The implementation splits the failsafe process into 2 parts: detection and actions. Mostly detection occurs in the commander. If a failure is detected the corresponding flag in vehicle_status is set (for example the datalink loss detection). Depending on this flag the set_nav_state function then sets the state of the navigator (see here). The 'action' is then implemented in one of the navigation_mode classes in the navigator

General

Detection

Geofence

Done, tested in HIL, see Geofence, corresponding action is 'Flight Termination'

Engine failure

FIXME: add detecion in commander, corresponding action is engine failure mode, vehicle_status flag already exists

Done and tested, corresponding action is 'obc data link loss mode'

FIXME: handle datalink loss prelaunch and during takeoff?

GPS loss detection

Simplistic detection here here, needs testing and extension, corrseponding mode is 'obc gps loss mode'

invoke flight termination of data link and gps is lost in auto

Loss of barometric altitude

A warning is issued in QGC if the baro fails

RC Loss

well tested, corresponding action is OBC rc loss

Actions

Flight termination

bench tested, works when FMU halts or on request by user or app (for example geofence). You can use this qgc widget for manual activation. There is the CBRK_FLIGHTTER circuit breaker to disable the actual termination in the IO driver

engine failure mode

Navigator mode implemented here. Tested in HIL

Navigator mode implemented here. Testing status: basic functionality validated in HIL, needs more output in qgc for the user, need to test data link regain in all states

FIXME: new 1 900 feet AMSL min alt as of obc rules 1.3

OBC GPS loss mode

Navigator mode is implemented and passed initial HIL tests

Controlled flight termination

FIXME This mode should land the aircraft. Could be implemented as follows if mission exist and contains landing jump to waypoint before landing else do flight termination

OBC rc loss

Loitering at current altitude and after NAV_RCL_LT flight termination if no recovery

Testing

enable the circuit breaker CBRK_FLIGHTTERM (value: 121212) to avoid the flight termination mode

General Notes

Detection

  • detection triggers correctly
  • no flase positives
  • warning in qgc is clear

Actions

  • correct behaviour
  • understandable qgc output

DL loss can be triggered by disabling the heartbeat emission in QGC. Make sure to set airfield home and comms hold with the params (listed above) inside the testing area!

  1. failure with no recovery, after COM_DL_LOSS_T s AC should go to comms hold and wait there for NAV_DLL_CH_T s, then it should fly to airfield home
  2. failure with recovery: renable the heartbeats at various stages
    1. before the end of the loiter at comms hold the mission should continue
    2. after the end of the loiter at comms hold the mission should not continue and the AC should fly airfield home, also if the failure is simulated another time (disable heartbeats again) the AC should still continue to airfield home (and not to comms hold)
  3. multiple failures: trigger multiple failures: after at the NAV_DLL_N + 1 failures the system should fly to airfield home

Flight termination

If testing in the air: enable the circuit breaker CBRK_FLIGHTTERM (value: 121212)'

  • arm airframe and fly or hold it on ground
  • With the widget test if operational in all modes.
  • Test flight termination of IO board when fmu fails (TODO)

Geofence

See also Geofence

  • Add a geofence.txt to the sd card.
  • If testing in the air: enable the circuit breaker CBRK_FLIGHTTERM (value: 121212)
  • Arm
  • Fly or walk over geofence

Engine Failure mode

  • You can test this mode with the widget.
  • After the activation the aircraft should loiter down.
  • Test if geofence still works
  • Test if termination still works

GPS Failure mode

  • You can test this mode with the widget.
  • After the activation the aircraft should open loop loiter for NAV_GPSF_LT seconds and then terminate
  • check recovery
  • Test if geofence still works
  • Test if termination still works

RC Loss mode

  • You can test this mode with the widget or by powering off the RC.
  • After the activation the aircraft should loiter for NAV_RCL_LT seconds and then terminate
  • check recovery
  • Test if geofence still works
  • Test if termination still works

See also

FIXME Scrutineering docs

Translations of this page:


Quick Links

QR Code: URL of current page