#!/usr/bin/env python
import RPi.GPIO as gpio
import serial
import rospy
import sys
import struct as st
import binascii

from time import time
from sensor_msgs.msg import Imu, MagneticField

# Page 0 registers
CHIP_ID = 0x00
PAGE_ID = 0x07
ACCEL_DATA = 0x08
MAG_DATA = 0x0e
GYRO_DATA = 0x14
FUSED_EULER = 0x1a
FUSED_QUAT = 0x20
LIA_DATA = 0x28
GRAVITY_DATA = 0x2e
TEMP_DATA = 0x34
CALIB_STAT = 0x35
SYS_STATUS = 0x39
SYS_ERR = 0x3a
UNIT_SEL = 0x3b
OPER_MODE = 0x3d
PWR_MODE = 0x3e
SYS_TRIGGER = 0x3f
TEMP_SOURCE = 0x440
AXIS_MAP_CONFIG = 0x41
AXIS_MAP_SIGN = 0x42

ACC_OFFSET = 0x55
MAG_OFFSET = 0x5b
GYR_OFFSET = 0x61
ACC_RADIUS = 0x68
MAG_RADIUS = 0x69

# Page 1 registers
ACC_CONFIG = 0x08
MAG_CONFIG = 0x09
GYR_CONFIG0 = 0x0a
GYR_CONFIG1 = 0x0b

#  Operation modes
OPER_MODE_CONFIG = 0x00
OPER_MODE_ACCONLY = 0x01
OPER_MODE_MAGONLY = 0x02
OPER_MODE_GYROONLY = 0x03
OPER_MODE_ACCMAG = 0x04
OPER_MODE_ACCGYRO = 0x05
OPER_MODE_MAGGYRO = 0x06
OPER_MODE_AMG = 0x07
OPER_MODE_IMU = 0x08
OPER_MODE_COMPASS = 0x09
OPER_MODE_M4G = 0x0a
OPER_MODE_NDOF_FMC_OFF = 0x0b
OPER_MODE_NDOF = 0x0C

#  Power modes
PWR_MODE_NORMAL = 0x00
PWR_MODE_LOW = 0x01
PWR_MODE_SUSPEND  = 0x02

# Communication constants
BNO055_ID = 0xa0
START_BYTE_WR = 0xaa
START_BYTE_RESP = 0xbb
READ = 0x01
WRITE = 0x00


def imu_sub():
  rospy.init_node('imu/data', anonymous = true)
  rospy.Subscriber("bosch_imu_node", IMU)
  rospy.spin()

def imu_handle()
  lin_a_x = msg->imu_raw.linear_acceleration.x
  lin_a_y = msg->imu_raw.linear_acceleration.y
  lin_a_z = msg->imu_raw.linear_acceleration.z
  ang_v_x = msg->imu_raw.angular_velocity.x
  ang_v_y = msg->imu_raw.angular_velocity.y
  amg_v_z = msg->imu_raw.angular_velocity.z
  
def filter()
  alpha = .98
  beta = .02
  time_old = time.time()
  dt = time_new - time_old
  time_new = time.time()
  angle_x = alpha*(angle_x_last + ang_v_x*dt) + beta*lin_a_x
  angle_x_last = angle_x
  angle_y = alpha*(angle_y_last + ang_v_y*dt) + beta*lin_a_x
  angle_y_last = angle_y
  angle_z = alpha*(angle_z_last + ang_v_z*dt) + beta*lin_a_z
  angle_z_last = angle_z

if __name__ == '__main__':
  imu_sub()
  imu_handle()
  filter()
  
  
 
  
  


  
