diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py index 7048aa30..54ccd77d 100755 --- a/clever/src/selfcheck.py +++ b/clever/src/selfcheck.py @@ -1,4 +1,5 @@ #!/usr/bin/env python +# coding=utf-8 # Copyright (C) 2018 Copter Express Technologies # @@ -144,6 +145,45 @@ def mavlink_exec(cmd, timeout=3.0): return mavlink_recv +BOARD_ROTATIONS = { + 0: 'no rotation', + 1: 'yaw 45°', + 2: 'yaw 90°', + 3: 'yaw 135°', + 4: 'yaw 180°', + 5: 'yaw 225°', + 6: 'yaw 270°', + 7: 'yaw 315°', + 8: 'roll 180°', + 9: 'roll 180°, yaw 45°', + 10: 'roll 180°, yaw 90°', + 11: 'roll 180°, yaw 135°', + 12: 'pitch 180°', + 13: 'roll 180°, yaw 225°', + 14: 'roll 180°, yaw 270°', + 15: 'roll 180°, yaw 315°', + 16: 'roll 90°', + 17: 'roll 90°, yaw 45°', + 18: 'roll 90°, yaw 90°', + 19: 'roll 90°, yaw 135°', + 20: 'roll 270°', + 21: 'roll 270°, yaw 45°', + 22: 'roll 270°, yaw 90°', + 23: 'roll 270°, yaw 135°', + 24: 'pitch 90°', + 25: 'pitch 270°', + 26: 'roll 270°, yaw 270°', + 27: 'roll 180°, pitch 270°', + 28: 'pitch 90°, yaw 180', + 29: 'pitch 90°, roll 90°', + 30: 'yaw 293°, pitch 68°, roll 90°', + 31: 'pitch 90°, roll 270°', + 32: 'pitch 9°, yaw 180°', + 33: 'pitch 45°', + 34: 'pitch 315°', +} + + @check('FCU') def check_fcu(): try: @@ -189,6 +229,13 @@ def check_fcu(): else: failure('unknown selected estimator: %s', est) + rot = get_param('SENS_BOARD_ROT') + if rot is not None: + try: + info('board rotation: %s', BOARD_ROTATIONS[rot]) + except KeyError: + failure('unknown board rotation %s', rot) + except rospy.ROSException: failure('no MAVROS state (check wiring)')