Commander: offboard failsafe cleanup

[jlecoeur]

Cleanup the handling of offboard loss in Commander:

• add DISABLED, LOCKDOWN and TERMINATE options
• use enums to avoid nested if statements
• move failsafe handling to dedicated functions
• offboard loss timeout: it seems that the timeout was ignored if COM_OBL_ACT or COM_OBL_RC_ACT were set out of range? This is no longer the case.

Other changes in this PR:

• re-enable implicit-fallthrough warning. This is useful to write correct switch blocks.
• move COM_POSCTL_NAVL to px4::params and use enum as well

[julianoes]

Thanks @jlecoeur, I'll review this soon.

[jlecoeur]

Thanks @julianoes for the review. There is no doubt Commander would need a much deeper cleanup. I think this is already a good step towards understandable offboard failsafe handling logic. The added failsafe options may not have the cleanest implementation, but at least they are consistent with the existing link loss actions.

[julianoes]

@jlecoeur have you done some SITL testing to make sure there is nothing stupid with the changes?

[jlecoeur]

Not for all cases. It would be cool to be able to test all that in CI. For example fly a mission with MAVSDK, then artificially cut the up-link and monitor the behaviour for the various values failsafe parameters.

[julianoes]

Ok you have done some sanity checking then, good.
Agreed, we should.

[julianoes]

Reviewed, looks certainly better than before.

[jlecoeur]

Ok you have done some sanity checking then, good.
Agreed, we should.

I am installing MAVSDK to perform a few more.

[jlecoeur]

Test results with jMAVSim and the MAVSDK example offboard_velocity shut down mid-flight:

• COM_RC_IN_MODE=1 (no RC), NAV_DLL_ACT=0 (disabled)
  • COM_OBL_ACT=-1 (Disabled): continued upward at constant velocity (on last setpoint)
  • COM_OBL_ACT=0 (Land mode): started to land
  • COM_OBL_ACT=1 (Hold mode): held position
  • COM_OBL_ACT=2 (Return mode): returned above home and started to land
  • COM_OBL_ACT=3 (Terminate): Fell from the the sky, then disarmed
  • COM_OBL_ACT=4 (Lockdown): Stays in Offboard and drifts away. The actuators were still moving. I would have expected a flip and/or freefall here because the actuators should be locked
• COM_RC_IN_MODE=2 (virtual RC), NAV_DLL_ACT=0 (disabled)
  • COM_OBL_RC_ACT=-1 (Disabled): Offboard continues
  • COM_OBL_RC_ACT=0 (Position): Position engaged
  • COM_OBL_RC_ACT=1 (Altitude): Altitude engaged
  • COM_OBL_RC_ACT=2 (Manual): Manual engaged
  • COM_OBL_RC_ACT=3 (Return): RTL worked
  • COM_OBL_RC_ACT=4 (Land): Switched to land (it's ugly because it should stop first and then land)
  • COM_OBL_RC_ACT=5 (Hold): Works
  • COM_OBL_RC_ACT=6 (Terminate): Fell from the sky, still showing armed. (Is that correct?)
  • COM_OBL_RC_ACT=7 (Lockdown): Stays in Offboard and drifts away.

[jlecoeur]

@julianoes I have tested the main cases without RC (COM_OBL_ACT). I could not test the behaviour with RC present (COM_OBL_RC_ACT) because I do not have a joystick at hand.

It is OK except for the TERMINATE and LOCKDOWN options, which did not behave the way I expected. This is annoying because they are implemented the same way as the data link loss feature. The question is: is this also broken?

Also, I have not tested the subcases, for example if LAND is selected as failsafe action, but the gps position is lost, then it should try to descend, or terminate if that is not possible.

At the moment, I am leaning towards removing the LOCKDOWN and TERMINATE options, however this should be addressed because TERMINATE is used as a fallback if the desired failsafe is not possible (ex: global position not available).

[julianoes]

Let me try to test the RC cases, I should find a joystick!

[julianoes]

@jlecoeur btw. you can use the integration tests for this sort of thing:

cmake -Bbuild -DCMAKE_BUILD_TYPE=Debug -H. && cmake --build build && build/src/integration_tests/integration_tests_runner --gtest_filter="SitlTest.OffboardVelocityNED"

[julianoes]

It looks like lockdown and terminate need to be checked in general.

[julianoes]

COM_OBL_ACT=3 (Terminate): QGC displayed MANUAL ARMED, then INFO [commander] Freefall detected, then communication was lost. Is this the correct behaviour?

I did not see that. It just dropped from the sky, then detected landed and disarmed.

[jlecoeur]

Indeed, I tried again (with the example and the test runner) and it did just what you said.

[jlecoeur]

Without RC, the behaviour for terminate looks fine to me. The lockdown behaviour is bad, it behaves like altitude-control, here is a log https://review.px4.io/plot_app?log=d9e09744-f11c-4634-b699-c7d95a7ce22b

[julianoes]

I wonder about two things:

1. Do we need to disable some circuit breaker to make lockdown work?
2. What is the function of armed->force_failsafe, this doesn't seem to be applied consistently.

[julianoes]

1. Do we need to disable some circuit breaker to make lockdown work?

No, CBRK_FLIGHTTERM does not apply.

[jlecoeur]

2\. What is the function of `armed->force_failsafe`, this doesn't seem to be applied consistently.

https://github.com/PX4/Firmware/blob/504eb65689578da605c6ee8e2f8af5001ce0b55a/src/drivers/px4fmu/fmu.cpp#L1193-L1198

[julianoes]

Ok, it looks like lockdown is not supposed to work with HITL/SITL:

https://github.com/PX4/Firmware/blob/5c68880b564d5bc31debc8cb653555f80b919a8d/src/drivers/pwm_out_sim/PWMSim.cpp#L320-L324

To me it seems like lockdown should not actually be a "failsafe" feature but only a simulation option.

[jlecoeur]

Maybe you are right. But then how can we lock actuators to disarmed position (!= failsafe position) in the non-simulated case, cf #12713

I am starting to think that the intent of Terminate, Failsafe and Lockdown has probably evolved over time. It really should be clarified and not used interchangeably as it seems to be the case now.

[julianoes]

I agree. However, lockdown was always supposed to be a HITL "feature".

Overall it's a big mess that we should clean up.

For now, I'd say, let's merge this cleanup.

[jlecoeur]

Ok