def read_gyroscope(sensorId):
insertDebugLog(NOTICE, "Read gyroscope: {}".format(sensorId),
CDH, int(time.time()))
if not SensorManager.isCorrectSensor(sensorId, GYRO):
raise Exception('Incorrect sensor specified')
SensorManager.mux_select(sensorId)
address = SensorEntropy.addr(sensorId)
gyro_reg = SensorEntropy.reg(GYRO)
# Get the values from the sensor
reg_x_h = gyro_reg['X-H']
reg_x_l = gyro_reg['X-L']
reg_y_h = gyro_reg['Y-H']
reg_y_l = gyro_reg['Y-L']
reg_z_h = gyro_reg['Z-H']
reg_z_l = gyro_reg['Z-L']
try:
valX = (SensorManager.bus.read_byte_data(address, reg_x_h) << 8) \
| SensorManager.bus.read_byte_data(address, reg_x_l)
sleep(0.1)
valY = (SensorManager.bus.read_byte_data(address, reg_y_h) << 8) \
| SensorManager.bus.read_byte_data(address, reg_y_l)
sleep(0.1)
valZ = (SensorManager.bus.read_byte_data(address, reg_z_h) << 8) \
| SensorManager.bus.read_byte_data(address, reg_z_l)
sleep(0.1)
except(IOError, OSError):
print('[READ] Error reading from gyroscope at address ' + \
str(address))
insertDebugLog(NOTICE, "[READ] Error reading from gyroscope: {}".format(sensorId),
CDH, int(time.time()))
return None
# Apply two's complement
valX = utility.twos_to_int(valX)
valY = utility.twos_to_int(valY)
valZ = utility.twos_to_int(valZ)
sleep(0.1)
# Log data
value = (valX, valY, valZ)
sub = SensorEntropy.subsystem(sensorId)
insertTelemetryLog(sensorId, value, sub, int(time.time()))
return value
def read_magnetometer(sensorId):
#insertDebugLog(NOTICE, "Read magnetometer: {}".format(sensorId), CDH, int(time.time()))
if not SensorManager.isCorrectSensor(sensorId, MAG):
raise Exception('Incorrect sensor specified')
SensorManager.mux_select(sensorId)
address = SensorEntropy.addr(sensorId)
mag_reg = SensorEntropy.reg(MAG)
# Get the values from the sensor
reg_x_h = mag_reg['X-H']
reg_x_l = mag_reg['X-L']
reg_y_h = mag_reg['Y-H']
reg_y_l = mag_reg['Y-L']
reg_z_h = mag_reg['Z-H']
reg_z_l = mag_reg['Z-L']
try:
valX = (SensorManager.bus.read_byte_data(address, reg_x_h) << 8) \
| SensorManager.bus.read_byte_data(address, reg_x_l)
sleep(0.1)
valY = (SensorManager.bus.read_byte_data(address, reg_y_h) << 8) \
| SensorManager.bus.read_byte_data(address, reg_y_l)
sleep(0.1)
valZ = (SensorManager.bus.read_byte_data(address, reg_z_h) << 8) \
| SensorManager.bus.read_byte_data(address, reg_z_l)
except(IOError, OSError):
print('[READ] Error reading from magnetometer at address ' + \
str(address))
insertDebugLog(NOTICE, "[READ] Error reading from magnetometer: {}".format(sensorId),
CDH, int(time.time()))
return None
# Update the values to be of two compliment
valX = utility.twos_to_int(valX, 16);
valY = utility.twos_to_int(valY, 16);
valZ = utility.twos_to_int(valZ, 16);
"""
# Change valX and valY to radians
import math
radians = math.atan2(valY, valX)
radians += -0.0197
# Compensate for errors
if radians < 0:
radians += 2*math.pi
if radians > 2*math.pi:
radians -= 2*math.pi
# Turn radians into degrees
degrees = math.floor(radians * 180 / math.pi)
# Log data
value = (radians, degrees)
"""
value = (valX, valY, valZ)
sub = SensorEntropy.subsystem(sensorId)
insertTelemetryLog(sensorId, value, sub, int(time.time()))
return value