예제 #1
0
    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
예제 #2
0
    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