#!/usr/bin/env python
# license removed for brevity
import serial
import rospy
from sensor_msgs.msg import Imu
import math
import tf
from median_filter import MedianFilter
from common_algos.constants import *
import traceback
um6_imu = Imu()
velocity_mf = MedianFilter(max_queue_size=5, min_value=0.0005, max_value=2.05)
def get_angle(section, coef=1.00424):
data = ord(section[3]) + ord(section[4]) * 256 * 256 + ord(section[5]) * 256
data = data - (data >> 23) * 1024 * 1024 * 16
data = data * coef / 100
return data
def get_velocity(section):
data = ord(section[0]) + ord(section[1]) * 256 * 256 + ord(section[2]) * 256
data = data - (data >> 23) * 1024 * 1024 * 16
data = data * 2500.0 / 8388608
return data
def check(section):
sum = 0
for i in range(6):
sum = sum + ord(section[i])
