mavlink
Offboard failsafe circumvented by mavsdk python architecture
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.
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:
