#!/bin/bash
# OpenRex sensor reading

finish_test_now() {
  tput cud 8
  exit;
}

# kill proces if Ctrl+C is detected
trap finish_test_now 2

# gyroscope setup
i2cset -y 0 0x20 0x13 0x02 # set active state
i2cset -y 0 0x20 0x0D 0x00 # set full-scale range
i2cset -y 0 0x20 0x15 0x00 # disable double range selection

# accelerometer setup
i2cset -y 0 0x1C 0x02A 0x01 # set active state
i2cset -y 0 0x1C 0x05B 0x03 # enable both sensors

gyr_sens=62.5 # nominal sensitivity for 2000 dps range
acc_sens=0.244 # nominal sensitivity for 2g accelerometer mode
mag_sens=0.1 # nominal sensitivity for magnetometer

tput clear
echo "OpenRex Sensors Measurements"

while true
do
	# read data from temperature sensor
	temp_sen=$(i2cget -f -y 1 0x48 0x00 w | mawk '{printf("%.1f\n", (a=(("0x"substr($1,5,2)substr($1,3,1))*0.0625))>128?a-256:a)}')
	
	# read CPU temperature
	temp_cpu=$(cat /sys/devices/virtual/thermal/thermal_zone0/temp )
	
	# read humidity sensor data
	hum=$( cat /sys/bus/i2c/devices/1-0040/humidity1_input )
	temp_hum=$( cat /sys/bus/i2c/devices/1-0040/temp1_input )

	# read 16-bit gyroscope measurements
	gyro_x=$(( $( i2cget -f -y 0 0x20 0x01 ) << 8 | $( i2cget -f -y 0 0x20 0x02 ) ))
	gyro_y=$(( $( i2cget -f -y 0 0x20 0x03 ) << 8 | $( i2cget -f -y 0 0x20 0x04 ) ))
	gyro_z=$(( $( i2cget -f -y 0 0x20 0x05 ) << 8 | $( i2cget -f -y 0 0x20 0x06 ) ))

	# read 14-bit acceleration measurements
	acc_x=$(( $( i2cget -f -y 0 0x1C 0x01 ) << 6 | $( i2cget -f -y 0 0x1C 0x02 ) >> 2))
	acc_y=$(( $( i2cget -f -y 0 0x1C 0x03 ) << 6 | $( i2cget -f -y 0 0x1C 0x04 ) >> 2))
	acc_z=$(( $( i2cget -f -y 0 0x1C 0x05 ) << 6 | $( i2cget -f -y 0 0x1C 0x06 ) >> 2))
	
	# read 16-bit magnetic measurements	
	mag_x=$(( $( i2cget -f -y 0 0x1C 0x01 ) << 8 | $( i2cget -f -y 0 0x1C 0x02 ) ))
	mag_y=$(( $( i2cget -f -y 0 0x1C 0x03 ) << 8 | $( i2cget -f -y 0 0x1C 0x04 ) ))
	mag_z=$(( $( i2cget -f -y 0 0x1C 0x05 ) << 8 | $( i2cget -f -y 0 0x1C 0x06 ) ))
	
	# convert 2's complement if needed
	if [ "$gyro_x" -gt 32768 ]; then
	  gyro_x=$(echo "(65536-$gyro_x)*(-1)" | bc -l)
	fi
	if [ "$gyro_y" -gt 32768 ]; then
	  gyro_y=$(echo "(65536-$gyro_y)*(-1)" | bc -l)
	fi
	if [ "$gyro_z" -gt 32768 ]; then
	  gyro_z=$(echo "(65536-$gyro_z)*(-1)" | bc -l)
	fi
	
	if [ "$acc_x" -gt 8192 ]; then
	  acc_x=$(echo "(16384-$acc_x)*(-1)" | bc -l)
	fi
	if [ "$acc_y" -gt 8192 ]; then
	  acc_y=$(echo "(16384-$acc_y)*(-1)" | bc -l)
	fi
	if [ "$acc_z" -gt 8192 ]; then
	  acc_z=$(echo "(16384-$acc_z)*(-1)" | bc -l)
	fi
	
	if [ "$mag_x" -gt 32768 ]; then
	  mag_x=$(echo "(65536-$mag_x)*(-1)" | bc -l)
	fi
	if [ "$mag_y" -gt 32768 ]; then
	  mag_y=$(echo "(65536-$mag_y)*(-1)" | bc -l)
	fi
	if [ "$mag_z" -gt 32768 ]; then
	  mag_z=$(echo "(65536-$mag_z)*(-1)" | bc -l)
	fi

	# convert values
	gyro_x=$(echo "$gyro_x*$gyr_sens/1000" | bc -l)
	gyro_y=$(echo "$gyro_y*$gyr_sens/1000" | bc -l)
	gyro_z=$(echo "$gyro_z*$gyr_sens/1000" | bc -l)
	
	acc_x=$(echo "$acc_x*$acc_sens" | bc -l)
	acc_y=$(echo "$acc_y*$acc_sens" | bc -l)
	acc_z=$(echo "$acc_z*$acc_sens" | bc -l)
	
	mag_x=$(echo "$mag_x*$mag_sens" | bc -l)
	mag_y=$(echo "$mag_y*$mag_sens" | bc -l)
	mag_z=$(echo "$mag_z*$mag_sens" | bc -l)
	
	hum=$(echo "-6+125*$hum/65536" | bc -l)
	temp_hum=$(echo "-46.85+175.72*$temp_hum/65536" | bc -l)
	
	printf "                       TMP101   CPU HTC21 \n"
	printf " Temperature    [C]   : %5.0f %5.0f %5.0f \n" $temp_sen $temp_cpu $temp_hum
	printf " Rel. humidity  [per] :             %5.0f \n" $hum
	printf "                            X     Y     Z \n"
	printf " Angular rate   [dps] : %5.0f %5.0f %5.0f \n" $gyro_x $gyro_y $gyro_z
	printf " Acceleration   [mg]  : %5.0f %5.0f %5.0f \n" $acc_x $acc_y $acc_z
	printf " Magnetic field [uT]  : %5.0f %5.0f %5.0f \r" $mag_x $mag_y $mag_z
	tput cuu 6
	sleep 0.1
done