#!/bin/bash

temp_sensitivity=326.8
accel_sensitivity=2048.0
gyro_sensitivity=16.4


deviceid=`i2cget -f -y 4 0x68 0x75 b` 
#echo "DeviceId:${deviceid}"

function icm20602_init() {
i2cset -f -y 4 0x68 0x6b 0x80
sleep 0.01
i2cset -f -y 4 0x68 0x6b 0x1
i2cset -f -y 4 0x68 0x6c 0x3F
i2cset -f -y 4 0x68 0x6a 0x0
i2cset -f -y 4 0x68 0x1a 0x5
i2cset -f -y 4 0x68 0x1b 0x18
i2cset -f -y 4 0x68 0x1c 0x18
i2cset -f -y 4 0x68 0x1d 0x03
i2cset -f -y 4 0x68 0x19 0x0
i2cset -f -y 4 0x68 0x6c 0x0
}


function temp_get() {
TempH=`i2cget -f -y 4 0x68 0x41 b` 
TempL=`i2cget -f -y 4 0x68 0x42 b`  
TempSorce=`awk -v H=$TempH -v L=$TempL 'BEGIN{printf("%d", lshift(H,8) + L)}'`
if [ $TempSorce -gt 32768 ]; then
	let TempSorce=TempSorce-65536
fi
Temp=`awk -v T=$TempSorce -v SENS=$temp_sensitivity 'BEGIN{printf("%.2f", T/SENS + 25)}'`

#echo "Temp: ${TempH} ${TempL} ${Temp}"

eval $1=$Temp
}


function accel_get() {
AccelX_H=`i2cget -f -y 4 0x68 0x3B b` 
AccelX_L=`i2cget -f -y 4 0x68 0x3C b`  
AccelXSorce=`awk -v H=$AccelX_H -v L=$AccelX_L 'BEGIN{printf("%d", lshift(H,8) + L)}'`

AccelY_H=`i2cget -f -y 4 0x68 0x3D b` 
AccelY_L=`i2cget -f -y 4 0x68 0x3E b`  
AccelYSorce=`awk -v H=$AccelY_H -v L=$AccelY_L 'BEGIN{printf("%d", lshift(H,8) + L)}'`

AccelZ_H=`i2cget -f -y 4 0x68 0x3F b` 
AccelZ_L=`i2cget -f -y 4 0x68 0x40 b`  
AccelZSorce=`awk -v H=$AccelZ_H -v L=$AccelZ_L 'BEGIN{printf("%d", lshift(H,8) + L)}'`

if [ $AccelXSorce -gt 32768 ]; then
	let AccelXSorce=AccelXSorce-65536
fi

if [ $AccelYSorce -gt 32768 ]; then
	let AccelYSorce=AccelYSorce-65536
fi

if [ $AccelZSorce -gt 32768 ]; then
	let AccelZSorce=AccelZSorce-65536
fi

AccelX=`awk -v A=$AccelXSorce -v SENS=$accel_sensitivity 'BEGIN{printf("%.2f", A/SENS)}'`
AccelY=`awk -v A=$AccelYSorce -v SENS=$accel_sensitivity 'BEGIN{printf("%.2f", A/SENS)}'`
AccelZ=`awk -v A=$AccelZSorce -v SENS=$accel_sensitivity 'BEGIN{printf("%.2f", A/SENS)}'`

#echo "AccelX: ${AccelX_H} ${AccelX_L} ${AccelX}"
#echo "AccelY: ${AccelY_H} ${AccelY_L} ${AccelY}"
#echo "AccelZ: ${AccelZ_H} ${AccelZ_L} ${AccelZ}"

eval $1=$AccelX
eval $2=$AccelY
eval $3=$AccelZ
}


function gyro_get() {
GyroX_H=`i2cget -f -y 4 0x68 0x43 b` 
GyroX_L=`i2cget -f -y 4 0x68 0x44 b`  
GyroXSorce=`awk -v H=$GyroX_H -v L=$GyroX_L 'BEGIN{printf("%d", lshift(H,8) + L)}'`

GyroY_H=`i2cget -f -y 4 0x68 0x45 b` 
GyroY_L=`i2cget -f -y 4 0x68 0x46 b`  
GyroYSorce=`awk -v H=$GyroY_H -v L=$GyroY_L 'BEGIN{printf("%d", lshift(H,8) + L)}'`

GyroZ_H=`i2cget -f -y 4 0x68 0x47 b` 
GyroZ_L=`i2cget -f -y 4 0x68 0x48 b`  
GyroZSorce=`awk -v H=$GyroZ_H -v L=$GyroZ_L 'BEGIN{printf("%d", lshift(H,8) + L)}'`

if [ $GyroXSorce -gt 32768 ]; then
	let GyroXSorce=GyroXSorce-65536
fi

if [ $GyroYSorce -gt 32768 ]; then
	let GyroYSorce=GyroYSorce-65536
fi

if [ $GyroZSorce -gt 32768 ]; then
	let GyroZSorce=GyroZSorce-65536
fi

GyroX=`awk -v A=$GyroXSorce -v SENS=$gyro_sensitivity 'BEGIN{printf("%.2f", A/SENS)}'`
GyroY=`awk -v A=$GyroYSorce -v SENS=$gyro_sensitivity 'BEGIN{printf("%.2f", A/SENS)}'`
GyroZ=`awk -v A=$GyroZSorce -v SENS=$gyro_sensitivity 'BEGIN{printf("%.2f", A/SENS)}'`

#echo "GyroX: ${GyroX_H} ${GyroX_L} ${GyroX}"
#echo "GyroY: ${GyroY_H} ${GyroY_L} ${GyroY}"
#echo "GyroZ: ${GyroZ_H} ${GyroZ_L} ${GyroZ}"

eval $1=$GyroX
eval $2=$GyroY
eval $3=$GyroZ
}


#icm20602_init


#temp_get Temp
#accel_get AccelX AccelY AccelZ
#gyro_get  GyroX  GyroY  GyroZ 

#echo "Temp:  ${Temp}"
#echo "Accel: ${AccelX} ${AccelY} ${AccelZ}"
#echo "Gyro:  ${GyroX} ${GyroY} ${GyroZ}"

#for (( i=0;i<100;i++)) 
#while [ true ]                                                                  
#do                                                                                              
#temp_get Temp
#accel_get AccelX AccelY AccelZ
#gyro_get  GyroX  GyroY  GyroZ 
#echo "Temp:  ${Temp}"
#echo "Accel: ${AccelX} ${AccelY} ${AccelZ}"
#echo "Gyro:  ${GyroX} ${GyroY} ${GyroZ}"
#sleep 0.5
#done





 
