Offboard failsafe circumvented by mavsdk python architecture

[jeremyzff]

Issue:

MavSDK python's offboard mechanism circumvents offboard failsafe. Once offboard is started, a cothread starts sending offboard commands. A crash in the thread publishing the setpoint updates will result in the offboard thread continuing to send stale data, so the offboard mode failsafe is not activated. This can lead to the drone being stuck with some attitude command that is unsafe.

I recognize that normal mode switching will override offboard mode, but there are situations where things happen too fast. I've attached a code that demonstrates this issue- it takes off and does roll doublets. I've manually raised an exception to simulate some error, like some unexpected data showed up which throws an unhandled exception.

I also realize that writing VERY good code to catch all possible chances of exceptions is critical, but it seems unsafe that this mechanism basically prevents the normal offboard control timeout failsafe mechanism within PX4 from working correctly.

test_crash.zip

[julianoes]

Thanks @jeremyzff for the issue. This seems to be tricky indeed with Python asyncio (also see https://stackoverflow.com/questions/39351988/python-asyncio-program-wont-exit). I don't have an immediate simple solution. I need to think more about this. With the C++ mavsdk_server running in the background it's not straightforward.

Here again the example pasted above, changed to work with jMAVSim:

#!/usr/bin/env python3

# Warning: Only try this in simulation!
#          The direct attitude interface is a low level interface to be used
#          with caution. On real vehicles the thrust values are likely not
#          adjusted properly and you need to close the loop using altitude.

import asyncio

from mavsdk import System
from mavsdk.offboard import (Attitude, AttitudeRate, OffboardError)


hoverthrottle = 0.6
rollangle = 45.0
baseyaw = 90.0
yawrate = 200.0

async def print_statusmessages(drone):
    async for resp in drone.telemetry.status_text():
        print(f"{resp}")

async def rollsteps(drone):
    for i in [1.0, 0.75, 0.5, 0.25, 0.1]:


        print(f"roll step time {i}-- right ")
        await drone.offboard.set_attitude(Attitude(rollangle,0.0,baseyaw,hoverthrottle))
        await asyncio.sleep(i)

        print(f"roll step time {i}-- left")
        await drone.offboard.set_attitude(Attitude(-1.0*rollangle,0.0,baseyaw,hoverthrottle))
        await asyncio.sleep(i)

        print(f"roll step time {i}-- right")
        await drone.offboard.set_attitude(Attitude(rollangle,0.0,baseyaw,hoverthrottle))
        await asyncio.sleep(i)

        print(f"roll step time {i}-- left")
        await drone.offboard.set_attitude(-1*Attitude(rollangle,0.0,baseyaw,hoverthrottle))
        await asyncio.sleep(i)

        # Pretend an error happened here- gets stuck
        raise Exception('Whoops', 'Lets pretend I just processed bad data I didnt expect, throws an exception')

        print(f"roll step time {i}-- stop")
        await drone.offboard.set_attitude(Attitude(0.0,0.0,baseyaw,hoverthrottle))
        await asyncio.sleep(2)


async def run():
    """ Does Offboard control using attitude commands. """

    drone = System(mavsdk_server_address='localhost')

    await drone.connect()
    asyncio.ensure_future(print_statusmessages(drone))


    print("Waiting for drone to connect...")
    async for state in drone.core.connection_state():
        if state.is_connected:
            print(f"Drone discovered!")
            break

    print("-- Arming")
    await drone.action.arm()

    print("-- Setting initial setpoint")
    await drone.offboard.set_attitude(Attitude(0.0, 0.0, baseyaw, hoverthrottle))

    print("-- Starting offboard")
    try:
        await drone.offboard.start()
    except OffboardError as error:
        print(f"Starting offboard mode failed with error code: \
              {error._result.result}")
        #print("-- Disarming")
        #await drone.action.disarm()
        return

    await asyncio.sleep(10)

    await rollsteps(drone)




    print("-- Stopping offboard")
    try:
        await drone.offboard.stop()
    except OffboardError as error:
        print(f"Stopping offboard mode failed with error code: \
              {error._result.result}")

    try:
        #await drone.action.land()
        await drone.action.return_to_launch()
    except:
        print("LAND FAILED")

if __name__ == "__main__":
    loop = asyncio.get_event_loop()
    loop.run_until_complete(run())

The output I see is:

./test_crash.py
Waiting for mavsdk_server to be ready...
Connected to mavsdk_server!
Waiting for drone to connect...
Drone discovered!
-- Arming
-- Setting initial setpoint
StatusText: [type: INFO, text: Armed by external command]
-- Starting offboard
StatusText: [type: CRITICAL, text: Failsafe enabled: No manual control stick input]
StatusText: [type: INFO, text: Failsafe mode activated]
StatusText: [type: INFO, text: Failsafe mode deactivated]
StatusText: [type: INFO, text: Takeoff detected]
roll step time 1.0-- right 
roll step time 1.0-- left
roll step time 1.0-- right
roll step time 1.0-- left
Traceback (most recent call last):
  File "/home/julianoes/Downloads/test_crash/./test_crash.py", line 104, in <module>
    loop.run_until_complete(run())
  File "/usr/lib64/python3.9/asyncio/base_events.py", line 642, in run_until_complete
    return future.result()
  File "/home/julianoes/Downloads/test_crash/./test_crash.py", line 84, in run
    await rollsteps(drone)
  File "/home/julianoes/Downloads/test_crash/./test_crash.py", line 40, in rollsteps
    await drone.offboard.set_attitude(-1*Attitude(rollangle,0.0,baseyaw,hoverthrottle))
TypeError: unsupported operand type(s) for *: 'int' and 'Attitude'
^CException ignored in: <module 'threading' from '/usr/lib64/python3.9/threading.py'>
Traceback (most recent call last):
  File "/usr/lib64/python3.9/threading.py", line 1435, in _shutdown
    atexit_call()
  File "/usr/lib64/python3.9/concurrent/futures/thread.py", line 31, in _python_exit
    t.join()
  File "/usr/lib64/python3.9/threading.py", line 1053, in join
    self._wait_for_tstate_lock()
  File "/usr/lib64/python3.9/threading.py", line 1069, in _wait_for_tstate_lock
    elif lock.acquire(block, timeout):
KeyboardInterrupt: