From 27a748c6a6154cd608dac400bfa3ed6c870e77e4 Mon Sep 17 00:00:00 2001 From: Oleg Kalachev Date: Thu, 27 Sep 2018 03:48:53 +0300 Subject: [PATCH] selfcheck: add angular velocity check --- clever/src/selfcheck.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/clever/src/selfcheck.py b/clever/src/selfcheck.py index 456dc625..470e887d 100755 --- a/clever/src/selfcheck.py +++ b/clever/src/selfcheck.py @@ -120,9 +120,21 @@ def check_velocity(): horiz = math.hypot(velocity.twist.linear.x, velocity.twist.linear.y) vert = velocity.twist.linear.z if abs(horiz) > 0.1: - failure('Horizontal velocity estimation is %s m/s; is the copter staying still?' % horiz) + failure('Horizontal velocity estimation is %.2f m/s; is copter staying still?' % horiz) if abs(vert) > 0.1: - failure('Vertical velocity estimation is %s m/s; is the copter staying still?' % vert) + failure('Vertical velocity estimation is %.2f m/s; is copter staying still?' % vert) + + angular = velocity.twist.angular + ANGULAR_VELOCITY_LIMIT = 0.01 + if abs(angular.x) > ANGULAR_VELOCITY_LIMIT: + failure('Pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?', + angular.x, math.degrees(angular.x)) + if abs(angular.y) > ANGULAR_VELOCITY_LIMIT: + failure('Pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?', + angular.y, math.degrees(angular.y)) + if abs(angular.z) > ANGULAR_VELOCITY_LIMIT: + failure('Pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?', + angular.z, math.degrees(angular.z)) except rospy.ROSException: failure('No velocity estimation')