/**
* @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);
}
