static void init(void)
{
STM_EVAL_PBInit(BUTTON_USER, BUTTON_MODE_GPIO);
r3dfb_init();
LCD_SetColors(0x0000, 0xffff);
LCD_SetFont(&Font8x8);
itoa(meshes[mesh].count / 3, info_str, 10);
strcat(info_str, " tris");
r3d_viewport(1, 1, R3DFB_PIXEL_WIDTH - 1, R3DFB_PIXEL_HEIGHT - 1);
r3d_backface_culling = R3D_ENABLE;
r3d_primitive_winding = windings[mesh];
projection = mat4_perspective(60.0f, (float)R3DFB_PIXEL_WIDTH / (float)R3DFB_PIXEL_HEIGHT, 0.5f, 5.0f);
view = mat4_lookat(vec3(0.0f, 0.25f, 1.5f), vec3(0.0f, 0.0f, 0.0f), vec3(0.0f, 1.0f, 0.0f));
L3GD20_InitTypeDef L3GD20_InitStructure;
L3GD20_InitStructure.Power_Mode = L3GD20_MODE_ACTIVE;
L3GD20_InitStructure.Output_DataRate = L3GD20_OUTPUT_DATARATE_1;
L3GD20_InitStructure.Axes_Enable = L3GD20_AXES_ENABLE;
L3GD20_InitStructure.Band_Width = L3GD20_BANDWIDTH_4;
L3GD20_InitStructure.BlockData_Update = L3GD20_BlockDataUpdate_Continous;
L3GD20_InitStructure.Endianness = L3GD20_BLE_LSB;
L3GD20_InitStructure.Full_Scale = L3GD20_FULLSCALE_250;
L3GD20_Init(&L3GD20_InitStructure);
L3GD20_FilterConfigTypeDef L3GD20_FilterStructure;
L3GD20_FilterStructure.HighPassFilter_Mode_Selection = L3GD20_HPM_NORMAL_MODE_RES;
L3GD20_FilterStructure.HighPassFilter_CutOff_Frequency = L3GD20_HPFCF_0;
L3GD20_FilterConfig(&L3GD20_FilterStructure);
L3GD20_FilterCmd(L3GD20_HIGHPASSFILTER_ENABLE);
}
void L3GD20_Configuration()
{
// L3GD20 Configuration
L3GD20_InitTypeDef L3GD20_InitStructure;
L3GD20_InitStructure.Power_Mode = L3GD20_MODE_ACTIVE;
L3GD20_InitStructure.Output_DataRate = L3GD20_OUTPUT_DATARATE_1;
L3GD20_InitStructure.Axes_Enable = L3GD20_AXES_ENABLE;
L3GD20_InitStructure.Band_Width = L3GD20_BANDWIDTH_4;
L3GD20_InitStructure.BlockData_Update = L3GD20_BlockDataUpdate_Continous;
L3GD20_InitStructure.Endianness = L3GD20_BLE_LSB;
L3GD20_InitStructure.Full_Scale = L3GD20_FULLSCALE_250;
L3GD20_Init(&L3GD20_InitStructure);
/*
// L3GD20 Filter Configuration
L3GD20_FilterConfigTypeDef L3GD20Filter_InitStructure;
L3GD20Filter_InitStructure.HighPassFilter_Mode_Selection = L3GD20_HPM_NORMAL_MODE;
L3GD20Filter_InitStructure.HighPassFilter_CutOff_Frequency = L3GD20_HPFCF_4;
L3GD20_FilterConfig(&L3GD20Filter_InitStructure);
*/
// L3GD20 Reboot
L3GD20_RebootCmd();
}
Gyroscope::Gyroscope(L3GD20_InitTypeDef& gyroInit, L3GD20_FilterConfigTypeDef& filterConfig, uint16_t maxBufferSize) :
_maxBufferSize(maxBufferSize)
{
switch(gyroInit.Output_DataRate)
{
case L3GD20_OUTPUT_DATARATE_1:
_deltaT = 1.0 / L3G_ODR_95;
break;
case L3GD20_OUTPUT_DATARATE_2:
_deltaT = 1.0 / L3G_ODR_190;
break;
case L3GD20_OUTPUT_DATARATE_3:
_deltaT = 1.0 / L3G_ODR_380;
break;
case L3GD20_OUTPUT_DATARATE_4:
_deltaT = 1.0 / L3G_ODR_760;
break;
default:
return;
}
/* Configure Mems L3GD20 */
L3GD20_Init(&gyroInit);
L3GD20_FilterConfig(&filterConfig);
selectMode(BypassMode);
useHighPassFilter(true);
/* Initialize scale buffer */
_scaleBuffer.push_back(std::make_pair(gyroInit.Full_Scale, 0));
}
static void rr(void)
{
L3GD20_InitTypeDef L3GD20_InitStructure;
L3GD20_InitStructure.Power_Mode = L3GD20_MODE_ACTIVE;
L3GD20_InitStructure.Output_DataRate = L3GD20_OUTPUT_DATARATE_1;
L3GD20_InitStructure.Axes_Enable = L3GD20_AXES_ENABLE;
L3GD20_InitStructure.Band_Width = L3GD20_BANDWIDTH_4;
L3GD20_InitStructure.BlockData_Update = L3GD20_BlockDataUpdate_Continous;
L3GD20_InitStructure.Endianness = L3GD20_BLE_LSB;
L3GD20_InitStructure.Full_Scale = L3GD20_FULLSCALE_250;
L3GD20_Init(&L3GD20_InitStructure);
L3GD20_FilterConfigTypeDef L3GD20_FilterStructure;
L3GD20_FilterStructure.HighPassFilter_Mode_Selection = L3GD20_HPM_NORMAL_MODE_RES;
L3GD20_FilterStructure.HighPassFilter_CutOff_Frequency = L3GD20_HPFCF_0;
L3GD20_FilterConfig(&L3GD20_FilterStructure);
L3GD20_FilterCmd(L3GD20_HIGHPASSFILTER_ENABLE);
}
/**
* @brief Configure the Mems to gyroscope application.
* @param None
* @retval None
*/
static void Demo_GyroConfig(void)
{
L3GD20_InitTypeDef L3GD20_InitStructure;
L3GD20_FilterConfigTypeDef L3GD20_FilterStructure;
/* Configure Mems L3GD20 */
#if defined(USE_MEMS)
L3GD20_InitStructure.Power_Mode = L3GD20_MODE_ACTIVE;
#else
L3GD20_InitStructure.Power_Mode = L3GD20_MODE_POWERDOWN;
#endif
L3GD20_InitStructure.Output_DataRate = L3GD20_OUTPUT_DATARATE_1;
L3GD20_InitStructure.Axes_Enable = 0x00
#if defined(USE_MEMS_Y)
| 0x01
#endif
#if defined(USE_MEMS_X)
| 0x02
#endif
#if defined(USE_MEMS_Z)
| 0x04
#endif
;
L3GD20_InitStructure.Band_Width = L3GD20_BANDWIDTH_4;
L3GD20_InitStructure.BlockData_Update = L3GD20_BlockDataUpdate_Continous;
L3GD20_InitStructure.Endianness = L3GD20_BLE_LSB;
L3GD20_InitStructure.Full_Scale = L3GD20_FULLSCALE_500;
L3GD20_Init(&L3GD20_InitStructure);
L3GD20_FilterStructure.HighPassFilter_Mode_Selection =L3GD20_HPM_NORMAL_MODE_RES;
L3GD20_FilterStructure.HighPassFilter_CutOff_Frequency = L3GD20_HPFCF_0;
L3GD20_FilterConfig(&L3GD20_FilterStructure) ;
L3GD20_FilterCmd(L3GD20_HIGHPASSFILTER_ENABLE);
}
