Raspberry BASIC

« previous next »
Pages: [1]

Author Topic: Sense HAT Unchained  (Read 4130 times)

John Spikowski

  • Moderator
  • *****
  • Posts: 234
    • View Profile
    • ScriptBasic
Sense HAT Unchained
« on: April 27, 2019, 06:12:51 AM »
Sense HAT Unchained

The LED and Joystick functions will be in a separate extension module called SLED.

Code: C
  1. /* Raspberry Pi Sense HAT
  2. UXLIBS: -lpthread -lm -lc /usr/lib/gcc/arm-linux-gnueabihf/6/libgcc.a
  3. */
  4.  
  5. #include <stdio.h>
  6. #include <unistd.h>
  7. #include <stdlib.h>
  8. #include <stdint.h>
  9. #include <string.h>
  10. #include <fcntl.h>
  11. #include <sys/ioctl.h>
  12. #include <linux/i2c-dev.h>
  13. #include "../../basext.h"
  14.  
  15.  
  16. /***************************
  17.  Extension Module Functions
  18. ***************************/
  19.  
  20. typedef struct _ModuleObject {
  21.   void *HandleArray;
  22. }ModuleObject,*pModuleObject;
  23.  
  24.  
  25. besVERSION_NEGOTIATE
  26.   return (int)INTERFACE_VERSION;
  27. besEND
  28.  
  29.  
  30. besSUB_START
  31.   pModuleObject p;
  32.  
  33.   besMODULEPOINTER = besALLOC(sizeof(ModuleObject));
  34.   if( besMODULEPOINTER == NULL )return 0;
  35.  
  36.   p = (pModuleObject)besMODULEPOINTER;
  37.   return 0;
  38. besEND
  39.  
  40.  
  41. besSUB_FINISH
  42.   pModuleObject p;
  43.  
  44.   p = (pModuleObject)besMODULEPOINTER;
  45.   if( p == NULL )return 0;
  46.   return 0;
  47. besEND
  48.  
  49.  
  50. /***************
  51.  GPIO Functions
  52. ***************/
  53.  
  54. // I2C file handles
  55. static int file_bus = -1; // i2c bus
  56. static int file_hum = -1; // humidity/temp sensor
  57. static int file_pres = -1; // pressure sensor
  58. static int file_acc = -1; // accelerometer/gyro
  59. static int file_mag = -1; // magnetometer
  60.  
  61. static int i2cRead(int iHandle, unsigned char ucAddr, unsigned char *buf, int iLen);
  62. static int i2cWrite(int iHandle, unsigned char ucAddr, unsigned char *buf, int iLen);
  63. // humidity/temp calibration values
  64. static int H0_rH_x2, H1_rH_x2, T0_degC_x8;
  65. static int T1_degC_x8, H0_T0_OUT;
  66. static int H1_T0_OUT, T0_OUT, T1_OUT;
  67.  
  68. static int i2cRead(int iHandle, unsigned char ucAddr, unsigned char *buf, int iLen)
  69. {
  70. int rc;
  71.  
  72.   rc = write(iHandle, &ucAddr, 1);
  73.   if (rc == 1)
  74.   {
  75.     rc = read(iHandle, buf, iLen);
  76.   }
  77.   return rc;
  78. } /* i2cRead() */
  79.  
  80. static int i2cWrite(int iHandle, unsigned char ucAddr, unsigned char *buf, int iLen)
  81. {
  82. unsigned char ucTemp[512];
  83. int rc;
  84.  
  85.   if (iLen > 511 || iLen < 1 || buf == NULL)
  86.     return -1; // invalid write
  87.  
  88.   ucTemp[0] = ucAddr; // send the register number first
  89.   memcpy(&ucTemp[1], buf, iLen); // followed by the data
  90.   rc = write(iHandle, ucTemp, iLen+1);
  91.   return rc-1;
  92.  
  93. } /* i2cWrite() */
  94.  
  95.  
  96. besFUNCTION(shInit)
  97.   int iChannel;
  98.  
  99.   besARGUMENTS("i")
  100.     &iChannel
  101.   besARGEND
  102.  
  103.   unsigned char ucTemp[32];
  104.   char filename[32];
  105.  
  106.   sprintf(filename, "/dev/i2c-%d", iChannel);
  107.   if ((file_bus = open(filename, O_RDWR)) < 0)
  108.   {
  109.     fprintf(stderr, "Failed to open the i2c bus\n");
  110.     besRETURN_LONG(-1);
  111.   }
  112.  
  113.  
  114.   file_acc = open(filename, O_RDWR);
  115.   if (ioctl(file_acc, I2C_SLAVE, 0x6a) < 0)
  116.   {
  117.     fprintf(stderr, "Failed to acquire bus for accelerometer\n");
  118.     goto badexit;
  119.   }
  120.  
  121.   file_mag = open(filename, O_RDWR);
  122.   if (ioctl(file_mag, I2C_SLAVE, 0x1c) < 0)
  123.   {
  124.     fprintf(stderr, "Failed to acquire bus for magnetometer\n");
  125.     goto badexit;
  126.   }
  127.  
  128.   file_hum = open(filename, O_RDWR);
  129.   if (ioctl(file_hum, I2C_SLAVE, 0x5f) < 0)
  130.   {
  131.     fprintf(stderr, "Failed to acquire bus for Humidity sensor\n");
  132.     goto badexit;
  133.   }
  134.   file_pres = open(filename, O_RDWR);
  135.   if (ioctl(file_pres, I2C_SLAVE, 0x5C) < 0)
  136.   {
  137.     fprintf(stderr, "Failed to aquire bus for Pressure sensor\n");
  138.     goto badexit;
  139.   }
  140.   // Prepare humidity sensor
  141.   i2cRead(file_hum, 0x10, ucTemp, 1); // AV_CONF
  142.   ucTemp[0] &= 0xc0;
  143.   ucTemp[0] |= 0x1b; // avgt=16, avgh=32
  144.   i2cWrite(file_hum, 0x10, ucTemp, 1);
  145.  
  146.   i2cRead(file_hum, 0x20+0x80, ucTemp, 3); // get CTRL_REG 1-3
  147.   ucTemp[0] &= 0x78; // keep reserved bits
  148.   ucTemp[0] |= 0x81; // turn on + 1Hz sample rate
  149.   ucTemp[1] &= 0x7c; // turn off heater + boot + one shot
  150.   i2cWrite(file_hum, 0x20+0x80, ucTemp, 3); // turn on + set sample rate
  151.  
  152.   // Get the H/T calibration values
  153.   i2cRead(file_hum, 0x30+0x80, ucTemp, 16);
  154.   H0_rH_x2 = ucTemp[0];
  155.   H1_rH_x2 = ucTemp[1];
  156.   T0_degC_x8 = ucTemp[2];
  157.   T1_degC_x8 = ucTemp[3];
  158.   T0_degC_x8 |= ((ucTemp[5] & 0x3) << 8); // 2 msb bits
  159.   T1_degC_x8 |= ((ucTemp[5] & 0xc) << 6);
  160.   H0_T0_OUT = ucTemp[6] | (ucTemp[7] << 8);
  161.   H1_T0_OUT = ucTemp[10] | (ucTemp[11] << 8);
  162.   T0_OUT = ucTemp[12] | (ucTemp[13] << 8);
  163.   T1_OUT = ucTemp[14] | (ucTemp[15] << 8);
  164.   if (H0_T0_OUT > 32767) H0_T0_OUT -= 65536; // signed
  165.   if (H1_T0_OUT > 32767) H1_T0_OUT -= 65536;
  166.   if (T0_OUT > 32767) T0_OUT -= 65536;
  167.   if (T1_OUT > 32767) T1_OUT -= 65536;
  168.  
  169.   // prepare pressure sensor
  170.   ucTemp[0] = 0x90; // turn on and set 1Hz update
  171.   i2cWrite(file_pres, 0x20, ucTemp, 1);
  172.  
  173.   // Init magnetometer
  174.   ucTemp[0] = 0x48; // output data rate/power mode
  175.   ucTemp[1] = 0x00; // default scale
  176.   ucTemp[2] = 0x00; // continuous conversion
  177.   ucTemp[3] = 0x08; // high performance mode
  178.   i2cWrite(file_mag, 0x20+0x80, ucTemp, 4);
  179.  
  180.   // Init accelerometer/gyroscope
  181.   ucTemp[0] = 0x60; // 119hz accel
  182.   i2cWrite(file_acc, 0x20, ucTemp, 1);
  183.   ucTemp[0] = 0x38; // enable gyro on all axes
  184.   i2cWrite(file_acc, 0x1e, ucTemp, 1);
  185.         ucTemp[0] = 0x28; // data rate + full scale + bw selection
  186.   // bits:        ODR_G2 | ODR_G1 | ODR_G0 | FS_G1 | FS_G0 | 0 | BW_G1 | BW_G0
  187.   // 0x28 = 14.9hz, 500dps
  188.   i2cWrite(file_acc, 0x10, ucTemp, 1); // gyro ctrl_reg1
  189.  
  190.   besRETURN_LONG(1);
  191.  
  192.   // problems opening the I2C handles/addresses
  193.   badexit:
  194.   if (file_hum != -1)
  195.   {
  196.     close(file_hum);
  197.     file_hum = -1;
  198.   }
  199.   if (file_pres != -1)
  200.   {
  201.     close(file_pres);
  202.     file_pres = -1;
  203.   }
  204.   if (file_acc != -1)
  205.   {
  206.     close(file_acc);
  207.     file_acc = -1;
  208.   }
  209.   if (file_mag != -1)
  210.   {
  211.     close(file_mag);
  212.     file_mag = -1;
  213.   }
  214.   besRETURN_LONG(0);
  215. besEND
  216.  
  217.  
  218. besFUNCTION(shGetAccel)
  219.   VARIABLE Argument1;
  220.   VARIABLE Argument2;
  221.   VARIABLE Argument3;
  222.   unsigned long __refcount_;
  223.   LEFTVALUE Lval1;
  224.   LEFTVALUE Lval2;
  225.   LEFTVALUE Lval3;
  226.  
  227.   Argument1 = besARGUMENT(1);
  228.   besLEFTVALUE(Argument1,Lval1);
  229.   besRELEASE(*Lval1);
  230.   *Lval1 = NULL;
  231.  
  232.   Argument2 = besARGUMENT(2);
  233.   besLEFTVALUE(Argument2,Lval2);
  234.   besRELEASE(*Lval2);
  235.   *Lval2 = NULL;
  236.  
  237.   Argument3 = besARGUMENT(3);
  238.   besLEFTVALUE(Argument3,Lval3);
  239.   besRELEASE(*Lval3);
  240.   *Lval3 = NULL;
  241.  
  242.   unsigned char ucTemp[8];
  243.   int rc;
  244.  
  245.   *Lval1 = besNEWLONG;
  246.   *Lval2 = besNEWLONG;
  247.   *Lval3 = besNEWLONG;
  248.  
  249.   rc = i2cRead(file_acc, 0x28+0x80, ucTemp, 6);
  250.   if (rc == 6)
  251.   {
  252.     int x, y, z;
  253.  
  254.     x = ucTemp[0] + (ucTemp[1] << 8);
  255.     y = ucTemp[2] + (ucTemp[3] << 8);
  256.     z = ucTemp[4] + (ucTemp[5] << 8);
  257.     // fix the signed values
  258.     if (x > 32767) x -= 65536;
  259.     if (y > 32767) y -= 65536;
  260.     if (z > 32767) z -= 65536;
  261.  
  262.     LONGVALUE(*Lval1) = x;
  263.     LONGVALUE(*Lval2) = y;
  264.     LONGVALUE(*Lval3) = z;
  265.     besRETURN_LONG(1);
  266.   }
  267.   besRETURN_LONG(0);
  268. besEND
  269.  
  270.  
  271. besFUNCTION(shGetGyro)
  272.   VARIABLE Argument1;
  273.   VARIABLE Argument2;
  274.   VARIABLE Argument3;
  275.   unsigned long __refcount_;
  276.   LEFTVALUE Lval1;
  277.   LEFTVALUE Lval2;
  278.   LEFTVALUE Lval3;
  279.  
  280.   Argument1 = besARGUMENT(1);
  281.   besLEFTVALUE(Argument1,Lval1);
  282.   besRELEASE(*Lval1);
  283.   *Lval1 = NULL;
  284.  
  285.   Argument2 = besARGUMENT(2);
  286.   besLEFTVALUE(Argument2,Lval2);
  287.   besRELEASE(*Lval2);
  288.   *Lval2 = NULL;
  289.  
  290.   Argument3 = besARGUMENT(3);
  291.   besLEFTVALUE(Argument3,Lval3);
  292.   besRELEASE(*Lval3);
  293.   *Lval3 = NULL;
  294.  
  295.   unsigned char ucTemp[8];
  296.   int rc;
  297.  
  298.   *Lval1 = besNEWLONG;
  299.   *Lval2 = besNEWLONG;
  300.   *Lval3 = besNEWLONG;
  301.  
  302.   rc = i2cRead(file_acc, 0x18+0x80, ucTemp, 6);
  303.   if (rc == 6)
  304.   {
  305.     LONGVALUE(*Lval1) = ucTemp[0] + (ucTemp[1] << 8);
  306.     LONGVALUE(*Lval2) = ucTemp[2] + (ucTemp[3] << 8);
  307.     LONGVALUE(*Lval3) = ucTemp[4] + (ucTemp[5] << 8);
  308.     besRETURN_LONG(1);
  309.   }
  310.   besRETURN_LONG(0);
  311. besEND
  312.  
  313.  
  314. besFUNCTION(shGetMagneto)
  315.   VARIABLE Argument1;
  316.   VARIABLE Argument2;
  317.   VARIABLE Argument3;
  318.   unsigned long __refcount_;
  319.   LEFTVALUE Lval1;
  320.   LEFTVALUE Lval2;
  321.   LEFTVALUE Lval3;
  322.  
  323.   Argument1 = besARGUMENT(1);
  324.   besLEFTVALUE(Argument1,Lval1);
  325.   besRELEASE(*Lval1);
  326.   *Lval1 = NULL;
  327.  
  328.   Argument2 = besARGUMENT(2);
  329.   besLEFTVALUE(Argument2,Lval2);
  330.   besRELEASE(*Lval2);
  331.   *Lval2 = NULL;
  332.  
  333.   Argument3 = besARGUMENT(3);
  334.   besLEFTVALUE(Argument3,Lval3);
  335.   besRELEASE(*Lval3);
  336.   *Lval3 = NULL;
  337.  
  338.   unsigned char ucTemp[8];
  339.   int rc;
  340.  
  341.   *Lval1 = besNEWLONG;
  342.   *Lval2 = besNEWLONG;
  343.   *Lval3 = besNEWLONG;
  344.  
  345.   rc = i2cRead(file_mag, 0x28+0x80, ucTemp, 6);
  346.   if (rc == 6)
  347.   {
  348.     int x, y, z;
  349.     x = ucTemp[0] + (ucTemp[1] << 8);
  350.     y = ucTemp[2] + (ucTemp[3] << 8);
  351.     z = ucTemp[4] + (ucTemp[5] << 8);
  352.     // fix signed values
  353.     if (x > 32767) x -= 65536;
  354.     if (y > 32767) y -= 65536;
  355.     if (z > 32767) z -= 65536;
  356.  
  357.     LONGVALUE(*Lval1) = x;
  358.     LONGVALUE(*Lval2) = y;
  359.     LONGVALUE(*Lval3) = z;
  360.     besRETURN_LONG(1);
  361.   }
  362.   besRETURN_LONG(0);
  363. besEND
  364.  
  365. besFUNCTION(shGetPressure)
  366.   VARIABLE Argument1;
  367.   VARIABLE Argument2;
  368.   unsigned long __refcount_;
  369.   LEFTVALUE Lval1;
  370.   LEFTVALUE Lval2;
  371.  
  372.   Argument1 = besARGUMENT(1);
  373.   besLEFTVALUE(Argument1,Lval1);
  374.   besRELEASE(*Lval1);
  375.   *Lval1 = NULL;
  376.  
  377.   Argument2 = besARGUMENT(2);
  378.   besLEFTVALUE(Argument2,Lval2);
  379.   besRELEASE(*Lval2);
  380.   *Lval2 = NULL;
  381.  
  382.   unsigned char ucTemp[8];
  383.   int rc, P, T;
  384.  
  385.   *Lval1 = besNEWLONG;
  386.   *Lval2 = besNEWLONG;
  387.  
  388.   if (file_pres != -1)
  389.   {
  390.     rc = i2cRead(file_pres, 0x28+0x80, ucTemp, 5);
  391.     if (rc == 5)
  392.     {
  393.       P = ucTemp[0] + (ucTemp[1]<<8) + (ucTemp[2]<<16);
  394.       LONGVALUE(*Lval1) = P / 4096; //hPa
  395.       T = ucTemp[3] + (ucTemp[4] << 8);
  396.       if (T > 32767) T -= 65536; // twos compliment
  397.       T = 425 + (T / 48); // 42.5 + T value/480
  398.       LONGVALUE(*Lval2) = T;
  399.     }
  400.     besRETURN_LONG(1);
  401.   }
  402.   besRETURN_LONG(0);
  403. besEND
  404.  
  405.  
  406. besFUNCTION(shGetTempHumid)
  407.   VARIABLE Argument1;
  408.   VARIABLE Argument2;
  409.   unsigned long __refcount_;
  410.   LEFTVALUE Lval1;
  411.   LEFTVALUE Lval2;
  412.  
  413.   Argument1 = besARGUMENT(1);
  414.   besLEFTVALUE(Argument1,Lval1);
  415.   besRELEASE(*Lval1);
  416.   *Lval1 = NULL;
  417.  
  418.   Argument2 = besARGUMENT(2);
  419.   besLEFTVALUE(Argument2,Lval2);
  420.   besRELEASE(*Lval2);
  421.   *Lval2 = NULL;
  422.  
  423.   unsigned char ucTemp[4];
  424.   int rc;
  425.   int H_T_out, T_out, T0_degC, T1_degC;
  426.   int H0_rh, H1_rh;
  427.   int tmp;
  428.  
  429.   *Lval1 = besNEWLONG;
  430.   *Lval2 = besNEWLONG;
  431.  
  432.   rc = i2cRead(file_hum, 0x28+0x80, ucTemp, 4);
  433.   if (rc == 4)
  434.   {
  435.     H_T_out = ucTemp[0] + (ucTemp[1] << 8);
  436.     T_out = ucTemp[2] + (ucTemp[3] << 8);
  437.     if (H_T_out > 32767) H_T_out -=65536;
  438.     if (T_out > 32767) T_out -= 65536;
  439.     T0_degC = T0_degC_x8 / 8;
  440.     T1_degC = T1_degC_x8 / 8;
  441.     H0_rh = H0_rH_x2 / 2;
  442.     H1_rh = H1_rH_x2 / 2;
  443.     tmp = (H_T_out - H0_T0_OUT) * (H1_rh - H0_rh)*10;
  444.     LONGVALUE(*Lval2) = tmp / (H1_T0_OUT - H0_T0_OUT) + H0_rh*10;
  445.     tmp = (T_out - T0_OUT) * (T1_degC - T0_degC)*10;
  446.     LONGVALUE(*Lval1) = tmp / (T1_OUT - T0_OUT) + T0_degC*10;
  447.     besRETURN_LONG(1);
  448.   }
  449.   besRETURN_LONG(0); // not ready
  450. besEND
  451.  
  452. besFUNCTION(shShutdown)
  453.   // Close all I2C file handles
  454.   if (file_bus != -1) close(file_bus);
  455.   if (file_hum != -1) close(file_hum);
  456.   if (file_pres != -1) close(file_pres);
  457.   if (file_acc != -1) close(file_acc);
  458.   if (file_mag != -1) close(file_mag);
  459.   file_bus = file_hum = file_pres = file_acc = file_mag = -1;
  460. besEND
  461.  

Code: Script BASIC
  1. ' Raspberry Pi Sense HAT
  2.  
  3. MODULE SHAT
  4.  
  5. DECLARE SUB  ::Init          ALIAS  "shInit"          LIB  "shat"
  6. DECLARE SUB  ::GetAccel      ALIAS  "shGetAccel"      LIB  "shat"
  7. DECLARE SUB  ::GetGyro       ALIAS  "shGetGyro"       LIB  "shat"
  8. DECLARE SUB  ::GetMagneto    ALIAS  "shGetMagneto"    LIB  "shat"
  9. DECLARE SUB  ::GetPressure   ALIAS  "shGetPressure"   LIB  "shat"
  10. DECLARE SUB  ::GetTempHumid  ALIAS  "shGetTempHumid"  LIB  "shat"
  11. DECLARE SUB  ::Shutdown      ALIAS  "shShutdown"      LIB  "shat"
  12.  
  13. END MODULE
  14.  


Code: Script BASIC
  1. IMPORT shat.bas
  2.  
  3. SHAT::Init(1)
  4.  
  5. SHAT::GetPressure(pressure, ptemp)
  6. SHAT::GetTempHumid(htemp, humid)
  7. SHAT::GetAccel(Ax, Ay, Az)
  8. SHAT::GetMagneto(Mx, My, Mz)
  9. SHAT::GetGyro(Gx, Gy, Gz)
  10.  
  11. PRINT "Pressure: ", pressure, "\n"
  12. PRINT "P-Temp: ", ptemp,"\n"
  13. PRINT "Humidity: ", humid,"\n"
  14. PRINT "H-Temp: ", htemp,"\n"
  15. PRINT "Accel-x: ", Ax, "\n"
  16. PRINT "Accel-y: ", Ay, "\n"
  17. PRINT "Accel-z: ", Az, "\n"
  18. PRINT "Mag-x: ", Mx, "\n"
  19. PRINT "Mag-y: ", My, "\n"
  20. PRINT "Mag-z: ", Mz, "\n"
  21. PRINT "Gyro-x: ", Gx, "\n"
  22. PRINT "Gyro-y: ", Gy, "\n"
  23. PRINT "Gyro-z: ", Gz, "\n"
  24.  
  25. SHAT::Shutdown
  26.  


pi@RPi3B:~/sbrpi/examples $ time scriba testhat.sb
Pressure: 1026
P-Temp: 320
Humidity: 287
H-Temp: 325
Accel-x: -443
Accel-y: -186
Accel-z: 15830
Mag-x: -1050
Mag-y: 2290
Mag-z: -1314
Gyro-x: 211
Gyro-y: 65435
Gyro-z: 74

real   0m0.043s
user   0m0.014s
sys   0m0.013s
pi@RPi3B:~/sbrpi/examples $



« Last Edit: April 29, 2019, 02:52:54 AM by John Spikowski »
Logged
ScriptBasic Project Manager/Facilitator
Pages: [1]
« previous next »