[r2]: / trunk / legacy / Libraries / SenseHat.library.md Maximize Restore History
540 lines (494 with data), 19.5 kB
<?xml version="1.0" encoding="utf-8"?>enumenumenum---
This Function Block is instanciated along with the Sense Hat device of the raspberry PI.
It's inputs and outputs allow to easily access the sensors, inputs and LED outputs of the
Sense Hat Device.
FUNCTION_BLOCK SenseHat EXTENDS i2c
VAR_INPUT
adwPixel: ARRAY [..] OF ;
xUpdatePixel: BOOL;
xUpdateJoystick: BOOL;
xUpdateLSM9DS1: BOOL;
xUpdateHTS221: BOOL;
xUpdateLPS25H: BOOL;
END_VAR
VAR_OUTPUT
xJoystickUp: BOOL;
xJoystickDown: BOOL;
xJoystickLeft: BOOL;
xJoystickRight: BOOL;
xJoystickEnter: BOOL;
rTemperatureHTS221: REAL;
rHumidity: REAL;
rTemperatureLPS25H: REAL;
lrPressure: LREAL;
rGyroX: REAL;
rGyroY: REAL;
rGyroZ: REAL;
rAccelX: REAL;
rAccelY: REAL;
rAccelZ: REAL;
rCompX: REAL;
rCompY: REAL;
rCompZ: REAL;
END_VAR
VAR
_sFilePixel: string;
_sFileJoy: string;
_sName: string;
_hJoystickFile: RTS_IEC_HANDLE;
_Joystick: JoystickInput;
_awWordPixel: ARRAY [..] OF ;
_Timer100: TON;
_rTemperature_S: REAL;
_rTemperature_C: REAL;
_rHumidity_S: REAL;
_rHumidity_C: REAL;
_usiMagAddress: USINT;
_rGyroScale: REAL;
_rAccelScale: REAL;
_rMagScale: REAL;
END_VAR
VAR_TEMP
_i: INT;
_hFile: RTS_IEC_HANDLE;
_diResult: DINT;
_xResult: BOOL;
_abyData: ARRAY [..] OF ;
_uiT0_C_8: UINT;
_uiT1_C_8: UINT;
_byH0_H_2: BYTE;
_byH1_H_2: BYTE;
_iT0_OUT: INT;
_iT1_OUT: INT;
_iH0_T0_OUT: INT;
_iH1_T0_OUT: INT;
_rT0: REAL;
_rT1: REAL;
_rH0: REAL;
_rH1: REAL;
END_VAR
SUPER^();
CASE _iState OF
0:
//Pixel map
_sName := '';
{analysis -26}
FOR _i := 0 TO 10 DO
_sFilePixel := CONCAT('/sys/class/graphics/fb', INT_TO_STRING(_i));
_sFilePixel := CONCAT(_sFilePixel, '/name');
{analysis -130}
_hFile := CmpCharDevice.CDOpen(_sFilePixel, CmpCharDevice.ACCESS_MODE.O_RDONLY, ADR(_diResult));
{analysis +130}
IF NOT(_hFile = RTS_INVALID_HANDLE) THEN
{analysis -19}
_diResult := CmpCharDevice.CDRead(_hFile, ADR(_sName), 12, ADR(_diResult));
{analysis +19}
_diResult := CmpCharDevice.CDClose(_hFile, ADR(_diResult));
IF _sName = 'RPi-Sense FB' THEN
_sFilePixel := CONCAT('/dev/fb', INT_TO_STRING(_i));
EXIT;
END_IF
END_IF
END_FOR
{analysis +26}
_iState := 10;
10:
//Joystick Input
_sName := '';
{analysis -26}
FOR _i := 0 TO 10 DO
_sFileJoy := CONCAT('/sys/class/input/event', INT_TO_STRING(_i));
_sFileJoy := CONCAT(_sFileJoy, '/device/name');
{analysis -130}
_hFile := CmpCharDevice.CDOpen(_sFileJoy, CmpCharDevice.ACCESS_MODE.O_RDONLY, ADR(_diResult));
{analysis +130}
IF NOT(_hFile = RTS_INVALID_HANDLE) THEN
{analysis -19}
_diResult := CmpCharDevice.CDRead(_hFile, ADR(_sName), 31, ADR(_diResult));
{analysis +19}
_diResult := CmpCharDevice.CDClose(_hFile, ADR(_diResult));
IF _sName = 'Raspberry Pi Sense HAT Joystick' THEN
_sFileJoy := CONCAT('/dev/input/event', INT_TO_STRING(_i));
EXIT;
END_IF
END_IF
END_FOR
{analysis +26}
_iState := 20;
20:
//Humidity and Temperature HTS221
usiAddress := 16#5F;
//Power up and set sample rate
_xResult := write8(16#20, 2#10000110);
//Pressure and Temperature LPS25H
usiAddress := 16#5C;
//Reset
_xResult := write8(16#20, 2#10100100);
//Gyro- and Accelereometer LSM9DS1
usiAddress := 16#6A;
//Reset
_xResult := write8(16#22, 2#11000000);
_iState := 25;
25:
//Short waiting for the actions done in state 40 to
//be done by the sense hat
_Timer100(IN:=TRUE);
IF _Timer100.Q THEN
_Timer100(IN:=FALSE);
_iState := 30;
END_IF
30:
//Humidity- and Temperature HTS221
usiAddress := 16#5F;
//Configure Averaging
_xResult := write8(16#10, 2#000101011);
// Get calibration data
// Temperature Calibration
{analysis -52}
IF ReadRegister(16#35, ADR(_abyData[1]), 1) = 1 THEN // + 16#80 ?
IF ReadRegister(16#32, ADR(_abyData), 1) = 1 THEN
_uiT0_C_8 := SHL(BYTE_TO_UINT(_abyData[1] AND 16#3), 8) OR BYTE_TO_UINT(_abyData[0]);
_rT0 := UINT_TO_REAL(_uiT0_C_8) / 8;
END_IF
END_IF
IF ReadRegister(16#33, ADR(_abyData), 1) = 1 THEN
_uiT1_C_8 := SHL(BYTE_TO_UINT(_abyData[1] AND 16#C), 6) OR BYTE_TO_UINT(_abyData[0]);
_rT1 := UINT_TO_REAL(_uiT1_C_8) / 8;
END_IF
IF ReadRegister(16#3C, ADR(_abyData[0]), 1) = 1 AND ReadRegister(16#3D, ADR(_abyData[1]), 1) = 1 THEN
_iT0_OUT := UINT_TO_INT(SHL(BYTE_TO_UINT(_abyData[1]), 8) OR BYTE_TO_UINT(_abyData[0]));
END_IF
IF ReadRegister(16#3E, ADR(_abyData[0]), 1) = 1 AND ReadRegister(16#3F, ADR(_abyData[1]), 1) = 1 THEN
_iT1_OUT := UINT_TO_INT(SHL(BYTE_TO_UINT(_abyData[1]), 8) OR BYTE_TO_UINT(_abyData[0]));
END_IF
// Humidity Calibration
IF ReadRegister(16#30, ADR(_byH0_H_2), 1) = 1 THEN
_rH0 := BYTE_TO_REAL(_byH0_H_2) / 2;
END_IF
IF ReadRegister(16#31, ADR(_byH1_H_2), 1) = 1 THEN
_rH1 := BYTE_TO_REAL(_byH1_H_2) / 2;
END_IF
IF ReadRegister(16#36, ADR(_abyData[0]), 1) = 1 AND ReadRegister(16#37, ADR(_abyData[1]), 1) = 1 THEN
_iH0_T0_OUT := UINT_TO_INT(SHL(BYTE_TO_UINT(_abyData[1]), 8) OR BYTE_TO_UINT(_abyData[0]));
END_IF
IF ReadRegister(16#3A, ADR(_abyData[0]), 1) = 1 AND ReadRegister(16#3B, ADR(_abyData[1]), 1) = 1 THEN
_iH1_T0_OUT := UINT_TO_INT(SHL(BYTE_TO_UINT(_abyData[1]), 8) OR BYTE_TO_UINT(_abyData[0]));
END_IF
{analysis +52}
{analysis -66}
{analysis -40}
IF _iT1_OUT-_iT0_OUT <> 0 THEN
_rTemperature_S := (_rT1-_rT0)/INT_TO_REAL(_iT1_OUT-_iT0_OUT);
END_IF
_rTemperature_C := _rT0-(_rTemperature_S*INT_TO_REAL(_iT0_OUT));
IF _iH1_T0_OUT-_iH0_T0_OUT <> 0 THEN
_rHumidity_S := (_rH1-_rH0)/INT_TO_REAL(_iH1_T0_OUT-_iH0_T0_OUT);
END_IF
_rHumidity_C := (_rH0)-(_rHumidity_S*INT_TO_REAL(_iH0_T0_OUT));
{analysis +40}
{analysis +66}
_iState := 40;
40:
//Pressure and Temperature LPS25H
usiAddress := 16#5C;
//Configure Averaging
_xResult := write8(16#10, 2#00001010);
//Configure FIFO
_xResult := write8(16#2E, 2#00000000);
//Set FIFO mode
_xResult := write8(16#21, 2#01000000);
_iState := 45;
45:
//Short waiting for the actions done in state 40 to
//be done by the sense hat
_Timer100(IN:=TRUE);
IF _Timer100.Q THEN
_Timer100(IN:=FALSE);
_iState := 50;
END_IF
50:
//Magnetsensor LSM9DS1
//Find out Mag-Address
usiAddress := 16#1C;
IF ReadRegister(16#0F, ADR(_abyData[0]), 1) = 1 AND _abyData[0] = 16#3D THEN
_usiMagAddress := usiAddress;
ELSE
usiAddress := 16#1D;
IF ReadRegister(16#0F, ADR(_abyData[0]), 1) = 1 AND _abyData[0] = 16#3D THEN
_usiMagAddress := usiAddress;
ELSE
usiAddress := 16#1E;
IF ReadRegister(16#0F, ADR(_abyData[0]), 1) = 1 AND _abyData[0] = 16#3D THEN
_usiMagAddress := usiAddress;
ELSE
usiAddress := 16#1F;
IF ReadRegister(16#0F, ADR(_abyData[0]), 1) = 1 AND _abyData[0] = 16#3D THEN
_usiMagAddress := usiAddress;
END_IF
END_IF
END_IF
END_IF
//Gyro LSM9DS1
usiAddress := 16#6A;
//Gyro set bandwidth and rate and scale
//500dps --> Scale 0.07
_xResult := write8(16#10, 2#01001011);
_rGyroScale := 0.0175;
//Gyro activate HP filter
_xResult := write8(16#12, 2#01000100);
//Accel LSM9DS1
usiAddress := 16#6A;
//Accel set bandwidth and rate and scale
_xResult := write8(16#20, 2#01110000);
_rAccelScale := 0.000122;
//Accel set no filters
_xResult := write8(16#21, 2#00000000);
//Mag LSM9DS1
usiAddress := _usiMagAddress;
//Mag set sample rate
_xResult := write8(16#20, 2#00010000);
//Mag set scale
_xResult := write8(16#21, 2#01000000);
_rMagScale := 0.029;
//Mag set continuous conversion mode
_xResult := write8(16#22, 2#00000000);
_iState := 100;
ELSE
_iState := 100;
END_CASE
METHOD Fb_Exit: BOOL
VAR_INPUT
bInCopyCode: BOOL;
END_VAR
VAR
diResult: DINT;
diFunRes: DINT;
END_VAR
IF NOT(_hJoystickFile = RTS_INVALID_HANDLE) THEN
diFunRes := CmpCharDevice.CDClose(_hJoystickFile, ADR(diResult));
_hJoystickFile := RTS_INVALID_HANDLE;
END_IF
METHOD AfterReadInputs: INT
VAR
diResult: DINT;
diFunRes: DINT;
byStatus: BYTE;
abyData: ARRAY [..] OF ;
iData: INT;
diData: DINT;
pbyE: pointer;
xSet: BOOL;
END_VAR
AfterReadInputs := SUPER^.AfterReadInputs();
IF _iState = 100 THEN
//Joystick Readout
IF xUpdateJoystick THEN
IF _hJoystickFile = RTS_INVALID_HANDLE THEN
{analysis -58}
{analysis -130}
_hJoystickFile := CmpCharDevice.CDOpen(_sFileJoy, CmpCharDevice.ACCESS_MODE.O_RDONLY + DINT#4000 (* CmpCharDevice.ACCESS_MODE.O_NONBLOCK*), ADR(diResult));
{analysis +130}
{analysis +58}
END_IF
(* Ioctl was used, when "Blocking-Read-Out" was done:
--> EVIOCGKEY(len) for testing if joystick data is available; we need to call Ioctl with an ADR operator
so disable the corresponding compiler warning
{warning disable C0033}
CmpCharDevice.CDIoctl(hFile, UDINT_TO_DINT(RASPI._IOC(2, pbyE^, 16#18, SIZEOF(ariData))), ADR(ariData), ADR(udiResult));
{warning restore C0033}
IF udiResult = 0 AND ariData[1] + ariData[6] > 0 THEN ... *)
IF NOT(_hJoystickFile = RTS_INVALID_HANDLE) THEN
diResult := 0;
WHILE diResult = 0 DO
{analysis -19}
diFunRes := CmpCharDevice.CDRead(_hJoystickFile, ADR(_Joystick), SIZEOF(_Joystick), ADR(diResult));
{analysis +19}
IF diResult <> 0 THEN //no data to read
EXIT;
END_IF
IF _Joystick.eEvent = JoystickEvent.EV_KEY THEN
xSet := FALSE;
IF _Joystick.eState = JoystickState.STATE_PRESS THEN
xSet := TRUE;
ELSIF _Joystick.eState = JoystickState.STATE_RELEASE THEN
xSet := FALSE;
ELSE
CONTINUE;
END_IF
///Set booleans
{analysis -75}
{analysis -76}
CASE _Joystick.eKey OF
JoystickKey.KEY_UP:
xJoystickUp := xSet;
JoystickKey.KEY_DOWN:
xJoystickDown := xSet;
JoystickKey.KEY_LEFT:
xJoystickLeft := xSet;
JoystickKey.KEY_RIGHT:
xJoystickRight:= xSet;
JoystickKey.KEY_ENTER:
xJoystickEnter := xSet;
END_CASE
{analysis +76}
{analysis +75}
END_IF
END_WHILE
END_IF
ELSE
IF NOT(_hJoystickFile = RTS_INVALID_HANDLE) THEN
diFunRes := CmpCharDevice.CDClose(_hJoystickFile, ADR(diResult));
_hJoystickFile := RTS_INVALID_HANDLE;
END_IF
END_IF
//Gyro, Accelerometer and Magnetometer LSM9DS1
IF xUpdateLSM9DS1 THEN
//Gyro and Accelerometer
{analysis -52}
usiAddress := 16#6A;
IF ReadRegister(16#17, ADR(byStatus), 1) = 1 THEN
IF (byStatus AND 2) > 0 THEN
//Gyro data available
IF ReadRegister(16#18, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#19, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rGyroX := INT_TO_REAL(iData) * _rGyroScale;
END_IF
IF ReadRegister(16#1A, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#1B, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rGyroY := INT_TO_REAL(iData) * _rGyroScale;
END_IF
IF ReadRegister(16#1C, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#1D, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rGyroZ := INT_TO_REAL(iData) * _rGyroScale;
END_IF
END_IF
IF (byStatus AND 1) > 0 THEN
//Accel data available
IF ReadRegister(16#28, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#29, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rAccelX := INT_TO_REAL(iData) * _rAccelScale;
END_IF
IF ReadRegister(16#2A, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#2B, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rAccelY := INT_TO_REAL(iData) * _rAccelScale;
END_IF
IF ReadRegister(16#2C, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#2D, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rAccelZ := INT_TO_REAL(iData) * _rAccelScale;
END_IF
END_IF
END_IF
//Magnetometer
usiAddress := _usiMagAddress;
IF ReadRegister(16#27, ADR(byStatus), 1) = 1 THEN
IF (byStatus AND 1) > 0 THEN
IF ReadRegister(16#28, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#29, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rCompX := INT_TO_REAL(iData) * _rMagScale;
END_IF
END_IF
IF (byStatus AND 2) > 0 THEN
IF ReadRegister(16#2A, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#2B, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rCompY := INT_TO_REAL(iData) * _rMagScale;
END_IF
END_IF
IF (byStatus AND 4) > 0 THEN
IF ReadRegister(16#2C, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#2D, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rCompZ := INT_TO_REAL(iData) * _rMagScale;
END_IF
END_IF
END_IF
{analysis +52}
END_IF
//Pressure and Temperature LPS25h
IF xUpdateLPS25H THEN
{analysis -52}
usiAddress := 16#5C;
IF ReadRegister(16#27, ADR(byStatus), 1) = 1 THEN
IF (byStatus AND 2) > 0 THEN
IF ReadRegister(16#28, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#29, ADR(abyData[1]), 1) = 1 AND ReadRegister(16#2A, ADR(abyData[2]), 1) = 1 THEN
diData := UDINT_TO_DINT(SHL(BYTE_TO_UDINT(abyData[2]), 16) OR SHL(BYTE_TO_UDINT(abyData[1]), 8) OR BYTE_TO_UDINT(abyData[0]));
lrPressure := DINT_TO_LREAL(diData) / 4096;
END_IF
END_IF
IF (byStatus AND 1) > 0 THEN
IF ReadRegister(16#2B, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#2C, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rTemperatureLPS25H := INT_TO_REAL(iData) / 480 + 42.5;
END_IF
END_IF
END_IF
{analysis +52}
END_IF
//Humidity and Temperature HTS221
IF xUpdateHTS221 THEN
usiAddress := 16#5F;
{analysis -52}
IF ReadRegister(16#27, ADR(byStatus), 1) = 1 THEN
IF (byStatus AND 2) > 0 THEN
IF ReadRegister(16#28, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#29, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rHumidity := INT_TO_REAL(iData) * _rHumidity_S + _rHumidity_C;
END_IF
END_IF
IF (byStatus AND 1) > 0 THEN
IF ReadRegister(16#2A, ADR(abyData[0]), 1) = 1 AND ReadRegister(16#2B, ADR(abyData[1]), 1) = 1 THEN
iData := UINT_TO_INT(SHL(BYTE_TO_UINT(abyData[1]), 8) OR BYTE_TO_UINT(abyData[0]));
rTemperatureHTS221 := INT_TO_REAL(iData) * _rTemperature_S + _rTemperature_C;
END_IF
END_IF
END_IF
{analysis +52}
END_IF
END_IF
METHOD BeforeWriteOutputs: INT
VAR
i: INT;
hFile: RTS_IEC_HANDLE;
diResult: DINT;
diFunRes: DINT;
END_VAR
BeforeWriteOutputs := SUPER^.BeforeWriteOutputs();
IF _iState = 100 THEN
//Pixel
IF xUpdatePixel THEN
//Convert 32Bit ARGB value to 16Bit RGB565
{analysis -52}
FOR i := 0 TO 63 DO
_awWordPixel[i] := UDINT_TO_WORD(SHL(SHR(adwPixel[i] AND 16#00FF0000, 16 + 3) AND 16#1F, 11) + //R
SHL(SHR(adwPixel[i] AND 16#0000FF00, 8 + 2) AND 16#3F, 5) + //G
(SHR(adwPixel[i] AND 16#000000FF, 3) AND 16#1F)); //B
END_FOR
{analysis +52}
{analysis -130}
hFile := CmpCharDevice.CDOpen(_sFilePixel, CmpCharDevice.ACCESS_MODE.O_WRONLY, ADR(diResult));
{analysis +130}
IF NOT(hFile = RTS_INVALID_HANDLE) THEN
{analysis -19}
diFunRes := CmpCharDevice.CDWrite(hFile, ADR(_awWordPixel), 128, ADR(diResult));
{analysis +19}
diFunRes := CmpCharDevice.CDClose(hFile, ADR(diResult));
END_IF
END_IF
END_IF