/** * @brief Set L3GD20 Interrupt configuration * @param L3GD20_InterruptConfig_TypeDef: pointer to a L3GD20_InterruptConfig_TypeDef * structure that contains the configuration setting for the L3GD20 Interrupt. * @retval None */ void L3GD20_INT1InterruptConfig(L3GD20_InterruptConfigTypeDef *L3GD20_IntConfigStruct) { uint8_t ctrl_cfr = 0x00, ctrl3 = 0x00; /* Read INT1_CFG register */ L3GD20_Read(&ctrl_cfr, L3GD20_INT1_CFG_ADDR, 1); /* Read CTRL_REG3 register */ L3GD20_Read(&ctrl3, L3GD20_CTRL_REG3_ADDR, 1); ctrl_cfr &= 0x80; ctrl3 &= 0xDF; /* Configure latch Interrupt request and axe interrupts */ ctrl_cfr |= (uint8_t)(L3GD20_IntConfigStruct->Latch_Request| \ L3GD20_IntConfigStruct->Interrupt_Axes); ctrl3 |= (uint8_t)(L3GD20_IntConfigStruct->Interrupt_ActiveEdge); /* Write value to MEMS INT1_CFG register */ L3GD20_Write(&ctrl_cfr, L3GD20_INT1_CFG_ADDR, 1); /* Write value to MEMS CTRL_REG3 register */ L3GD20_Write(&ctrl3, L3GD20_CTRL_REG3_ADDR, 1); }
//gyro updata void gryo_update() { uint8_t tmp[6] = {0}; int16_t a[3] = {0}; uint8_t tmpreg = 0; L3GD20_Read(&tmpreg, L3GD20_CTRL_REG4_ADDR, 1); L3GD20_Read(tmp, L3GD20_OUT_X_L_ADDR, 6); /* check in the control register 4 the data alignment (Big Endian or Little Endian)*/ if (!(tmpreg & 0x40)) { for (int i = 0; i < 3; i++) a[i] = (int16_t)(((uint16_t)tmp[2 * i + 1] << 8) | (uint16_t)tmp[2 * i]); } else { for (int i = 0; i < 3; i++) a[i] = (int16_t)(((uint16_t)tmp[2 * i] << 8) | (uint16_t)tmp[2 * i + 1]); } axes[1] = a[1] / 114.285f; //only need axes[1] // y: axes[1] }
static void qq(voud) { uint8_t tmp[6] = {0}; int16_t a[3] = {0}; uint8_t tmpreg = 0; L3GD20_Read(&tmpreg, L3GD20_CTRL_REG4_ADDR, 1); L3GD20_Read(tmp, L3GD20_OUT_X_L_ADDR, 6); /* check in the control register 4 the data alignment (Big Endian or Little Endian)*/ if (!(tmpreg & 0x40)) { for (int i = 0; i < 3; i++) a[i] = (int16_t)(((uint16_t)tmp[2 * i + 1] << 8) | (uint16_t)tmp[2 * i]); } else { for (int i = 0; i < 3; i++) a[i] = (int16_t)(((uint16_t)tmp[2 * i] << 8) | (uint16_t)tmp[2 * i + 1]); } for (int i = 0; i < 3; i++){ axes[i] = a[i] / 114.285f; // axes[i] += a[i]*delta / 114.285f; } LCD_DrawFullCircle(100,100+axes[2],20); }
void Gyroscope::changeScale(uint8_t scale) { uint8_t ctrl4, fifoCtrl; scale &= 0x30; L3GD20_Read(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1); bool fifoMode = (fifoCtrl & 0x70) != 0; /* Retrieve stored values before changing scale */ retrieveValues(); /* Read current value from CTRL_REG4 register */ L3GD20_Read(&ctrl4, L3GD20_CTRL_REG4_ADDR, 1); /* Change scale */ ctrl4 = (ctrl4 & ~0x30) | scale; /* Write new value to CTRL_REG4 regsister */ L3GD20_Write(&ctrl4, L3GD20_CTRL_REG4_ADDR, 1); /* Let L3GD20 perform changes */ sleep(CHANGE_DELAY); /* Discard values recorded during scale change */ if(fifoMode) clearFifo(); if (_scaleBuffer.back().second > 0) _scaleBuffer.push_back(std::make_pair(scale, 0)); else _scaleBuffer.back().first = scale; }
void Gyroscope::selectMode(Mode mode) { uint8_t ctrl5, fifoCtrl; uint8_t fifoEn, fifoMode; switch(mode) { case BypassMode: fifoEn = FIFO_DISABLED; fifoMode = BYPASS_MODE; break; case FifoMode: fifoEn = FIFO_ENABLED; fifoMode = FIFO_MODE; break; case StreamMode: fifoEn = FIFO_ENABLED; fifoMode = STREAM_MODE; break; default: return; } L3GD20_Read(&ctrl5, L3GD20_CTRL_REG5_ADDR, 1); ctrl5 = (ctrl5 & (~0x40)) | fifoEn; L3GD20_Write(&ctrl5, L3GD20_CTRL_REG5_ADDR, 1); L3GD20_Read(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1); fifoCtrl = (fifoCtrl & (~0xE0)) | fifoMode; L3GD20_Write(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1); // Clear FIFO from previously stored data if(fifoEn == FIFO_ENABLED) clearFifo(); }
void Demo_GyroReadAngRate (float* pfData) { uint8_t tmpbuffer[6] ={0}; int16_t RawData[3] = {0}; uint8_t tmpreg = 0; float sensitivity = 0; int i =0; L3GD20_Read(&tmpreg,L3GD20_CTRL_REG4_ADDR,1); L3GD20_Read(tmpbuffer,L3GD20_OUT_X_L_ADDR,6); /* check in the control register 4 the data alignment (Big Endian or Little Endian)*/ if(!(tmpreg & 0x40)) { for(i=0; i<3; i++) { RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i+1] << 8) + tmpbuffer[2*i]); } } else { for(i=0; i<3; i++) { RawData[i]=(int16_t)(((uint16_t)tmpbuffer[2*i] << 8) + tmpbuffer[2*i+1]); } } /* Switch the sensitivity value set in the CRTL4 */ switch(tmpreg & 0x30) { case 0x00: sensitivity=L3G_Sensitivity_250dps; break; case 0x10: sensitivity=L3G_Sensitivity_500dps; break; case 0x20: sensitivity=L3G_Sensitivity_2000dps; break; default: sensitivity=L3G_Sensitivity_250dps; } /* divide by sensitivity */ for(i=0; i<3; i++) { pfData[i]=(float)RawData[i]/sensitivity; } }
/** * @brief Get status for L3GD20 data * @param None * @retval Data status in a L3GD20 Data */ uint8_t L3GD20_GetDataStatus(void) { uint8_t tmpreg; /* Read STATUS_REG register */ L3GD20_Read(&tmpreg, L3GD20_STATUS_REG_ADDR, 1); return tmpreg; }
/** * @brief Enable or disable INT2 interrupt * @param InterruptState: State of INT1 Interrupt * This parameter can be: * @arg L3GD20_INT2INTERRUPT_DISABLE * @arg L3GD20_INT2INTERRUPT_ENABLE * @retval None */ void L3GD20_INT2InterruptCmd(uint8_t InterruptState) { uint8_t tmpreg; /* Read CTRL_REG3 register */ L3GD20_Read(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1); tmpreg &= 0xF7; tmpreg |= InterruptState; /* Write value to MEMS CTRL_REG3 regsister */ L3GD20_Write(&tmpreg, L3GD20_CTRL_REG3_ADDR, 1); }
/** * @brief Reboot memory content of L3GD20 * @param None * @retval None */ void L3GD20_RebootCmd(void) { uint8_t tmpreg; /* Read CTRL_REG5 register */ L3GD20_Read(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1); /* Enable or Disable the reboot memory */ tmpreg |= L3GD20_BOOT_REBOOTMEMORY; /* Write value to MEMS CTRL_REG5 regsister */ L3GD20_Write(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1); }
/** * @brief Enable or Disable High Pass Filter * @param HighPassFilterState: new state of the High Pass Filter feature. * This parameter can be: * @arg: L3GD20_HIGHPASSFILTER_DISABLE * @arg: L3GD20_HIGHPASSFILTER_ENABLE * @retval None */ void L3GD20_FilterCmd(uint8_t HighPassFilterState) { uint8_t tmpreg; /* Read CTRL_REG5 register */ L3GD20_Read(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1); tmpreg &= 0xEF; tmpreg |= HighPassFilterState; /* Write value to MEMS CTRL_REG5 regsister */ L3GD20_Write(&tmpreg, L3GD20_CTRL_REG5_ADDR, 1); }
static void update(void) { uint8_t tmp[6] = {0}; int16_t a[3] = {0}; uint8_t tmpreg = 0; L3GD20_Read(&tmpreg, L3GD20_CTRL_REG4_ADDR, 1); L3GD20_Read(tmp, L3GD20_OUT_X_L_ADDR, 6); /* check in the control register 4 the data alignment (Big Endian or Little Endian)*/ if (!(tmpreg & 0x40)) { for (int i = 0; i < 3; i++) a[i] = (int16_t)(((uint16_t)tmp[2 * i + 1] << 8) | (uint16_t)tmp[2 * i]); } else { for (int i = 0; i < 3; i++) a[i] = (int16_t)(((uint16_t)tmp[2 * i] << 8) | (uint16_t)tmp[2 * i + 1]); } if (STM_EVAL_PBGetState(BUTTON_USER)) { mesh = (mesh + 1) % 3; // next mesh r3d_primitive_winding = windings[mesh]; itoa(meshes[mesh].count / 3, info_str, 10); strcat(info_str, " tris"); while (STM_EVAL_PBGetState(BUTTON_USER)); // debounce } float delta = frametime - time; frametime = time; // make frametime a consistent time value during the frames for (int i = 0; i < 3; i++) axes[i] += (float)a[i] * delta / 114.285f; if (axes[0] < 0) axes[0] = 0; if (axes[0] > 180) axes[0] = 180; model = mat4_mul( mat4_rotation(axes[0], vec3(1.0f, 0.0f, 0.0f)), mat4_rotation(axes[1], vec3(0.0f, 1.0f, 0.0f))); if (mesh == 1) model = mat4_mul(model, mat4_scaling(vec3(0.5f, 0.5f, 0.5f))); mv = mat4_mul(view, model); mvp = mat4_mul(projection, mv); }
void Gyroscope::resetFifo() { uint8_t fifoCtrl; L3GD20_Read(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1); // Set to bypass mode to restart data collection fifoCtrl = (fifoCtrl & (~0xE0)); L3GD20_Write(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1); sleep(CHANGE_DELAY); // Change back to FIFO mode fifoCtrl = fifoCtrl | FIFO_MODE; L3GD20_Write(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1); sleep(CHANGE_DELAY); }
void Gyroscope::clearFifo() { uint8_t tmpbuffer[6] = {0}; uint8_t fifoSrc; bool fifoFull = false; do{ // FIFO overrun test L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1); fifoFull = fifoFull || (fifoSrc & 0x40) != 0; L3GD20_Read(tmpbuffer, L3GD20_OUT_X_L_ADDR, 6); // FIFO empty test L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1); }while ((fifoSrc & 0x20) == 0); /* FIFO needs reset after being full */ if (fifoFull) resetFifo(); else sleep(CHANGE_DELAY); }
/** * @brief Set High Pass Filter Modality * @param L3GD20_FilterStruct: pointer to a L3GD20_FilterConfigTypeDef structure * that contains the configuration setting for the L3GD20. * @retval None */ void L3GD20_FilterConfig(L3GD20_FilterConfigTypeDef *L3GD20_FilterStruct) { uint8_t tmpreg; /* Read CTRL_REG2 register */ L3GD20_Read(&tmpreg, L3GD20_CTRL_REG2_ADDR, 1); tmpreg &= 0xC0; /* Configure MEMS: mode and cutoff frquency */ tmpreg |= (uint8_t) (L3GD20_FilterStruct->HighPassFilter_Mode_Selection |\ L3GD20_FilterStruct->HighPassFilter_CutOff_Frequency); /* Write value to MEMS CTRL_REG2 regsister */ L3GD20_Write(&tmpreg, L3GD20_CTRL_REG2_ADDR, 1); }
// TODO: Bypass to stream support // Stream to FIFO support void Gyroscope::retrieveValues() { uint8_t tmpbuffer[6] = {0}; math3d::Vector3<int16_t> vec; uint8_t ctrl4, fifoCtrl, fifoSrc; int i = 0; L3GD20_Read(&fifoCtrl, L3GD20_FIFO_CTRL_REG_ADDR, 1); bool fifoMode = (fifoCtrl & 0x70) != 0; bool fifoFull = false; if (fifoMode){ L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1); /* Test FIFO empty bit */ if ((fifoSrc & 0x20) != 0) return; } if(_scaleBuffer.empty()) return; L3GD20_Read(&ctrl4, L3GD20_CTRL_REG4_ADDR, 1); do{ // FIFO overrun test if (fifoMode){ L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1); fifoFull = fifoFull || (fifoSrc & 0x40) != 0; } L3GD20_Read(tmpbuffer, L3GD20_OUT_X_L_ADDR, 6); /* Check in the control register 4 the data alignment (Big Endian or Little Endian) */ if(ctrl4 & 0x40){ for(i = 0; i < 3; i++){ vec[i] = (int16_t)(((uint16_t)tmpbuffer[2*i] << 8) + tmpbuffer[2*i+1]); } } else{ for(i = 0; i < 3; i++){ vec[i] = (int16_t)(((uint16_t)tmpbuffer[2*i+1] << 8) + tmpbuffer[2*i]); } } // Check if buffer size isn't larger than maximum if(_maxBufferSize > 0 && _dataBuffer.size() >= _maxBufferSize) discard(); /* Place retrieved values in local buffer */ _dataBuffer.push_back(vec); _scaleBuffer.back().second++; // FIFO empty test if (fifoMode){ // Let FIFO update sleep(10, microsecond); L3GD20_Read(&fifoSrc, L3GD20_FIFO_SRC_REG_ADDR, 1); } }while (fifoMode && (fifoSrc & 0x20) == 0); /* FIFO needs reset after being full */ if (fifoMode && fifoFull) resetFifo(); }
void gyroscope_read(uint8_t* data) { L3GD20_Read(data, L3GD20_OUT_X_L_ADDR, 6); }