src/lib/GSENSOR/stk8baxx.cpp

   1 #ifdef HAS_GSENSOR_STK8xxx
   2 #include "stk8baxx.h"
   3 #include "logging.h"
   4
   5 #define PID_SIZE        16
   6 uint8_t chipid_temp = 0x00;
   7 uint8_t stk8xxx_pid_list[PID_SIZE] = {STK8xxx_CHIPID_VAL, STK8BA50_X_CHIPID_VAL, STK8BA5X_CHIPID_VAL, STK8327_CHIPID_VAL};
   8
   9 void STK8xxx::ReadAccRegister(uint8_t reg, uint8_t *data)
  10 {
  11     Wire.beginTransmission(STK8xxx_SLAVE_ADDRESS);
  12     Wire.write(reg);
  13     Wire.endTransmission();
  14
  15     Wire.requestFrom(STK8xxx_SLAVE_ADDRESS, 1);    // request 1 bytes from slave device
  16     *data = Wire.read(); // receive a byte
  17 }
  18
  19 void STK8xxx::WriteAccRegister(uint8_t reg, uint8_t data)
  20 {
  21     Wire.beginTransmission(STK8xxx_SLAVE_ADDRESS);
  22     Wire.write(reg);
  23     Wire.write(data);
  24     Wire.endTransmission();
  25 }
  26
  27 /*
  28  * Anymotion init register
  29  * Must be placed after the software reset
  30  *
  31  */
  32 void STK8xxx::STK8xxx_Anymotion_init()
  33 {
  34         unsigned char ARegAddr, ARegWriteValue;
  35
  36         /* Enable X Y Z-axis any-motion (slope) interrupt */
  37     ARegAddr       = STK8xxx_REG_INTEN1;
  38     ARegWriteValue = STK8xxx_VAL_SLP_EN_X | STK8xxx_VAL_SLP_EN_Y | STK8xxx_VAL_SLP_EN_Z;
  39     WriteAccRegister(ARegAddr, ARegWriteValue);
  40
  41         /* Set anymotion Interrupt trigger threshold */
  42     ARegAddr       = STK8xxx_REG_SLOPETHD;
  43     ARegWriteValue = STK8xxx_VAL_SLP_DFLT;
  44     WriteAccRegister(ARegAddr, ARegWriteValue);
  45
  46         /* Enable any-motion */
  47     ARegAddr       = STK8xxx_REG_SIGMOT2;
  48     ARegWriteValue = STK8xxx_VAL_ANY_MOT_EN;
  49     WriteAccRegister(ARegAddr, ARegWriteValue);
  50
  51         /* Map any-motion (slope) interrupt to INT1 */
  52     ARegAddr       = STK8xxx_REG_INTMAP1;
  53     ARegWriteValue = STK8xxx_VAL_ANYMOT2INT1;
  54     WriteAccRegister(ARegAddr, ARegWriteValue);
  55 }
  56
  57 /*
  58  * Sigmotion init register
  59  * Must be placed after the software reset
  60  *
  61  */
  62 void STK8xxx::STK8xxx_Sigmotion_init()
  63 {
  64         unsigned char SRegAddr, SRegWriteValue;
  65
  66         /* Enable X Y Z-axis sig-motion (slope) interrupt */
  67     SRegAddr       = STK8xxx_REG_INTEN1;
  68     SRegWriteValue = STK8xxx_VAL_SLP_EN_X | STK8xxx_VAL_SLP_EN_Y | STK8xxx_VAL_SLP_EN_Z;
  69     WriteAccRegister(SRegAddr, SRegWriteValue);
  70
  71         /* Set sig-motion Interrupt trigger threshold */
  72     SRegAddr       = STK8xxx_REG_SLOPETHD;
  73     SRegWriteValue = STK8xxx_VAL_SLP_DFLT;
  74     WriteAccRegister(SRegAddr, SRegWriteValue);
  75
  76         /* Enable significant motion */
  77     SRegAddr       = STK8xxx_REG_SIGMOT2;
  78     SRegWriteValue = STK8xxx_VAL_SIG_MOT_EN;
  79     WriteAccRegister(SRegAddr, SRegWriteValue);
  80
  81         /* Map significant motion interrupt to INT1 */
  82     SRegAddr       = STK8xxx_REG_INTMAP1;
  83     SRegWriteValue = STK8xxx_VAL_SIGMOT2INT1;
  84     WriteAccRegister(SRegAddr, SRegWriteValue);
  85 }
  86
  87 /*
  88  * Diable motion register
  89  * Must be placed after the software reset
  90  *
  91  */
  92 void STK8xxx::STK8xxx_Disable_Motion()
  93 {
  94         unsigned char ARegAddr, ARegWriteValue;
  95
  96         /* Disable X Y Z-axis motion (slope) interrupt */
  97     ARegAddr       = STK8xxx_REG_INTEN1;
  98     ARegWriteValue = 0x00;
  99     WriteAccRegister(ARegAddr, ARegWriteValue);
 100
 101         /* Disable motion */
 102     ARegAddr       = STK8xxx_REG_SIGMOT2;
 103     ARegWriteValue = 0x00;
 104     WriteAccRegister(ARegAddr, ARegWriteValue);
 105 }
 106
 107
 108 /* Disable Gsensor */
 109 void STK8xxx::STK8xxx_Suspend_mode()
 110 {
 111         unsigned char RegAddr, RegWriteValue;
 112
 113     /* suspend mode enable */
 114         RegAddr       = STK8xxx_REG_POWMODE;
 115     RegWriteValue = STK8xxx_VAL_SUSPEND;
 116     WriteAccRegister(RegAddr, RegWriteValue);
 117 }
 118
 119 bool STK8xxx::STK8xxx_Check_chipid()
 120 {
 121         uint8_t RegAddr = STK_REG_CHIPID;
 122     int i = 0, pid_num = (sizeof(stk8xxx_pid_list) / sizeof(stk8xxx_pid_list[0]));
 123
 124     ReadAccRegister(RegAddr, &chipid_temp);
 125     for (i = 0; i < pid_num; i++)
 126     {
 127         if (chipid_temp == stk8xxx_pid_list[i])
 128         {
 129                 DBGLN("read stkchip id ok, chip_id = 0x%x", chipid_temp);
 130             return true;
 131         }
 132     }
 133         ERRLN("read stkchip id fail!");
 134     return false;
 135 }
 136
 137 /*
 138  * Initializes an example function
 139  * The initial configuration is in active mode.
 140  * STK8xxx is in the gear of ODR=2000Hz, and the range is set to +/-4G
 141  *
 142  */
 143 int STK8xxx::STK8xxx_Initialization()
 144 {
 145     unsigned char RegAddr, RegWriteValue;
 146
 147         if(!STK8xxx_Check_chipid())
 148         {
 149                 return -1;
 150     }
 151
 152     /* soft-reset */
 153         RegAddr       = STK8xxx_REG_SWRST;
 154     RegWriteValue = STK8xxx_VAL_RST_DFLTS;
 155     WriteAccRegister(RegAddr, RegWriteValue);
 156         delay(50);//unit ms
 157
 158     /* set range, resolution */
 159     RegAddr       = STK8xxx_REG_RANGESEL;
 160     RegWriteValue = STK8xxx_RANGE_4G; // range = +/-4g
 161     WriteAccRegister(RegAddr, RegWriteValue);
 162
 163         /* set power mode */
 164         RegAddr           = STK8xxx_REG_POWMODE;
 165         RegWriteValue = STK8xxx_VAL_SLEEP_05;   // active mode
 166         WriteAccRegister(RegAddr, RegWriteValue);
 167
 168         /* set bandwidth */
 169     RegAddr       = STK8xxx_REG_BWSEL;
 170     RegWriteValue = STK8xxx_VAL_BW_1000; // bandwidth = 1000Hz
 171     WriteAccRegister(RegAddr, RegWriteValue);
 172
 173         //STK8xxx_Anymotion_init();
 174         //STK8xxx_Sigmotion_init();
 175
 176     /* set i2c watch dog */
 177     RegAddr       = STK8xxx_REG_INTFCFG;
 178     RegWriteValue = STK8xxx_VAL_I2C_WDT_EN; // enable watch dog
 179     WriteAccRegister(RegAddr, RegWriteValue);
 180
 181     /* int config */
 182     RegAddr       = STK8xxx_REG_INTCFG1;
 183     RegWriteValue = STK8xxx_VAL_INT_LV; // INT1/INT2 push-pull, active high
 184     WriteAccRegister(RegAddr, RegWriteValue);
 185
 186         return chipid_temp;
 187 }
 188
 189 int STK8xxx::STK8xxx_Get_Sensitivity()
 190 {
 191     int sensitivity = 0;
 192         if(0x86 == chipid_temp)
 193         {
 194            //resolution = 10 bit
 195        sensitivity = 1 << 9;
 196         }
 197     else
 198     {
 199        //resolution = 12 bit
 200        sensitivity = 1 << 11;
 201         }
 202     //range = +/-4g
 203     sensitivity = sensitivity / 4;
 204     return sensitivity;
 205 }
 206
 207 /* Read data from registers */
 208 void STK8xxx::STK8xxx_Getregister_data(float *X_DataOut, float *Y_DataOut, float *Z_DataOut)
 209 {
 210     uint8_t RegAddr, RegReadValue[2];
 211     int16_t x, y, z;
 212
 213     RegAddr             = STK8xxx_REG_XOUT1;
 214     RegReadValue[0] = 0x00;
 215         ReadAccRegister(RegAddr, &RegReadValue[0]);
 216     RegAddr             = STK8xxx_REG_XOUT2;
 217     RegReadValue[1] = 0x00;
 218         ReadAccRegister(RegAddr, &RegReadValue[1]);
 219         x = (short int)(RegReadValue[1] << 8 | RegReadValue[0]);
 220
 221         RegAddr         = STK8xxx_REG_YOUT1;
 222     RegReadValue[0] = 0x00;
 223         ReadAccRegister(RegAddr, &RegReadValue[0]);
 224     RegAddr             = STK8xxx_REG_YOUT2;
 225     RegReadValue[1] = 0x00;
 226         ReadAccRegister(RegAddr, &RegReadValue[1]);
 227         y = (short int)(RegReadValue[1] << 8 | RegReadValue[0]);
 228
 229         RegAddr         = STK8xxx_REG_ZOUT1;
 230     RegReadValue[0] = 0x00;
 231         ReadAccRegister(RegAddr, &RegReadValue[0]);
 232     RegAddr             = STK8xxx_REG_ZOUT2;
 233     RegReadValue[1] = 0x00;
 234         ReadAccRegister(RegAddr, &RegReadValue[1]);
 235         z = (short int)(RegReadValue[1] << 8 | RegReadValue[0]);
 236
 237         if(0x86 == chipid_temp)
 238         {
 239                 //resolution = 10 bit
 240         x >>= 6;
 241         *X_DataOut = (float) x / STK8xxx_Get_Sensitivity();
 242
 243         y >>= 6;
 244         *Y_DataOut = (float) y / STK8xxx_Get_Sensitivity();
 245
 246         z >>= 6;
 247         *Z_DataOut = (float) z / STK8xxx_Get_Sensitivity();
 248         }
 249     else
 250     {
 251         //resolution = 12 bit
 252         x >>= 4;
 253         *X_DataOut = (float) x / STK8xxx_Get_Sensitivity();
 254
 255         y >>= 4;
 256         *Y_DataOut = (float) y / STK8xxx_Get_Sensitivity();
 257
 258         z >>= 4;
 259         *Z_DataOut = (float) z / STK8xxx_Get_Sensitivity();
 260         }
 261 }
 262 #endif