/**
* @brief Configure the Mems to compass application.
* @param None
* @retval None
*/
void Demo_CompassConfig(void)
{
LSM303DLHCMag_InitTypeDef LSM303DLHC_InitStructure;
LSM303DLHCAcc_InitTypeDef LSM303DLHCAcc_InitStructure;
LSM303DLHCAcc_FilterConfigTypeDef LSM303DLHCFilter_InitStructure;
/* Configure MEMS magnetometer main parameters: temp, working mode, full Scale and Data rate */
LSM303DLHC_InitStructure.Temperature_Sensor = LSM303DLHC_TEMPSENSOR_DISABLE;
LSM303DLHC_InitStructure.MagOutput_DataRate =LSM303DLHC_ODR_30_HZ ;
LSM303DLHC_InitStructure.MagFull_Scale = LSM303DLHC_FS_8_1_GA;
LSM303DLHC_InitStructure.Working_Mode = LSM303DLHC_CONTINUOS_CONVERSION;
LSM303DLHC_MagInit(&LSM303DLHC_InitStructure);
/* Fill the accelerometer structure */
LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_NORMAL_MODE;
LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_50_HZ;
LSM303DLHCAcc_InitStructure.Axes_Enable= LSM303DLHC_AXES_ENABLE;
LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB;
LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE;
/* Configure the accelerometer main parameters */
LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
/* Fill the accelerometer LPF structure */
LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection =LSM303DLHC_HPM_NORMAL_MODE;
LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency = LSM303DLHC_HPFCF_16;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1 = LSM303DLHC_HPF_AOI1_DISABLE;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2 = LSM303DLHC_HPF_AOI2_DISABLE;
/* Configure the accelerometer LPF main parameters */
LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
}
/**
* @brief Configure the Mems to Accelerometer MEMS.
* @param None
* @retval None
*/
void Acc_Config(void)
{
LSM303DLHCAcc_InitTypeDef LSM303DLHCAcc_InitStructure;
LSM303DLHCAcc_FilterConfigTypeDef LSM303DLHCFilter_InitStructure;
/* Fill the accelerometer structure */
LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_NORMAL_MODE;
LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_50_HZ;
LSM303DLHCAcc_InitStructure.Axes_Enable= LSM303DLHC_AXES_ENABLE;
LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB;
LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE;
/* Configure the accelerometer main parameters */
LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
/* Fill the accelerometer LPF structure */
LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection =LSM303DLHC_HPM_NORMAL_MODE;
LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency = LSM303DLHC_HPFCF_16;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1 = LSM303DLHC_HPF_AOI1_DISABLE;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2 = LSM303DLHC_HPF_AOI2_DISABLE;
/* Configure the accelerometer LPF main parameters */
LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
}
Accelerometer::Accelerometer(LSM303DLHCAcc_InitTypeDef& accInit, LSM303DLHCAcc_FilterConfigTypeDef& filterConfig, uint16_t maxBufferSize) :
_maxBufferSize(maxBufferSize)
{
switch(accInit.AccOutput_DataRate)
{
case LSM303DLHC_ODR_1_HZ:
_deltaT = 1.0 / LSM_ACC_ODR_1;
break;
case LSM303DLHC_ODR_10_HZ:
_deltaT = 1.0 / LSM_ACC_ODR_10;
break;
case LSM303DLHC_ODR_25_HZ:
_deltaT = 1.0 / LSM_ACC_ODR_25;
break;
case LSM303DLHC_ODR_50_HZ:
_deltaT = 1.0 / LSM_ACC_ODR_50;
break;
case LSM303DLHC_ODR_100_HZ:
_deltaT = 1.0 / LSM_ACC_ODR_100;
break;
case LSM303DLHC_ODR_200_HZ:
_deltaT = 1.0 / LSM_ACC_ODR_200;
break;
case LSM303DLHC_ODR_400_HZ:
_deltaT = 1.0 / LSM_ACC_ODR_400;
break;
case LSM303DLHC_ODR_1344_HZ:
_deltaT = 1.0 / LSM_ACC_ODR_1344;
break;
default:
return;
}
/* Configure Mems LSM303DLHC Accelerometer */
LSM303DLHC_AccInit(&accInit);
LSM303DLHC_AccFilterConfig(&filterConfig);
selectMode(BypassMode);
// TODO: filter defaultne vypnuty, po zapnuti nepodava spravne hodnoty (preco? Lebo high pass filtruje dlhodobu gravitaciu?)
// useHighPassFilter(true);
// uint8_t aaa;
// LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_CTRL_REG2_A, &aaa, 1);
/* Initialize scale buffer */
_scaleBuffer.push_back(std::make_pair(accInit.AccFull_Scale, 0));
}
void accelerometerCompassInit()
{
LSM303DLHCMag_InitTypeDef LSM303DLHC_InitStructure;
LSM303DLHCAcc_InitTypeDef LSM303DLHCAcc_InitStructure;
LSM303DLHCAcc_FilterConfigTypeDef LSM303DLHCFilter_InitStructure;
uint8_t threshold;
/* Configure MEMS magnetometer main parameters: temp, working mode, full Scale and Data rate */
LSM303DLHC_InitStructure.Temperature_Sensor = LSM303DLHC_TEMPSENSOR_DISABLE;
LSM303DLHC_InitStructure.MagOutput_DataRate =LSM303DLHC_ODR_30_HZ ;
LSM303DLHC_InitStructure.MagFull_Scale = LSM303DLHC_FS_8_1_GA;
LSM303DLHC_InitStructure.Working_Mode = LSM303DLHC_CONTINUOS_CONVERSION;
LSM303DLHC_MagInit(&LSM303DLHC_InitStructure);
/* Fill the accelerometer structure */
LSM303DLHCAcc_InitStructure.Power_Mode = LSM303DLHC_NORMAL_MODE;
LSM303DLHCAcc_InitStructure.AccOutput_DataRate = LSM303DLHC_ODR_50_HZ;
LSM303DLHCAcc_InitStructure.Axes_Enable= LSM303DLHC_AXES_ENABLE;
LSM303DLHCAcc_InitStructure.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
LSM303DLHCAcc_InitStructure.BlockData_Update = LSM303DLHC_BlockUpdate_Continous;
LSM303DLHCAcc_InitStructure.Endianness=LSM303DLHC_BLE_LSB;
LSM303DLHCAcc_InitStructure.High_Resolution=LSM303DLHC_HR_ENABLE;
/* Configure the accelerometer main parameters */
LSM303DLHC_AccInit(&LSM303DLHCAcc_InitStructure);
/* Fill the accelerometer LPF structure */
LSM303DLHCFilter_InitStructure.HighPassFilter_Mode_Selection =LSM303DLHC_HPM_NORMAL_MODE;
LSM303DLHCFilter_InitStructure.HighPassFilter_CutOff_Frequency = LSM303DLHC_HPFCF_16;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI1 = LSM303DLHC_HPF_AOI1_DISABLE;
LSM303DLHCFilter_InitStructure.HighPassFilter_AOI2 = LSM303DLHC_HPF_AOI2_DISABLE;
/* Configure the accelerometer LPF main parameters */
LSM303DLHC_AccFilterConfig(&LSM303DLHCFilter_InitStructure);
LSM303DLHC_AccIT1Config(LSM303DLHC_IT1_AOI1, ENABLE);
threshold = 126;
LSM303DLHC_Write(ACC_I2C_ADDRESS, LSM303DLHC_INT1_THS_A, 1, &threshold);
LSM303DLHC_AccINT1InterruptConfig(LSM303DLHC_OR_COMBINATION,
(LSM303DLHC_X_HIGH |
LSM303DLHC_Y_HIGH |
LSM303DLHC_Z_HIGH), ENABLE);
}
/* akselerometer_init()
@Beskrivelse
Initialiserer akselerometerkretsen med følgende nøkkelparametre:
- Sampler akselerasjon med en frekvens på 100 Hz.
- Måler akselerasjon i alle 3 akser (x, y, z)
- +/- 2g måleområde.
- 12 bit oppløsning dvs. 1mg oppløsning
@Inngangsvariabler:
-
*/
void accelerometer_init(void){
/* Lager her en initialiseringstruktur som sendes inn i metoden LSM303DLHC_AccInit()
* fra biblioteket
*/
LSM303DLHCAcc_InitTypeDef aks_init;
aks_init.Power_Mode = LSM303DLHC_NORMAL_MODE; /*dvs ikke lowpower mode*/
aks_init.AccOutput_DataRate = LSM303DLHC_ODR_100_HZ;
/*Plattformen skal i utgangspunktet kun beveges i y-retning.
* Vil uansett måle akselerasjon i alle retninger.
* x-aksen peker i retning fra usb-portene på kortet mot kompassdiodene
* y-aksen peker i retning fra blå knapp(USER) til svart knapp(RESET)
* z-aksen peker oppover*/
aks_init.Axes_Enable = LSM303DLHC_AXES_ENABLE;
/* Velger her hvor stort mAleomrAde vi Onsker A bruke.
* Det kan tenkes at dette mA endres etter testing for A utnytte
* akselerometeret best mulig.
* Kan velge mellom +/- 2G, 4G,8G og 16G.*/
aks_init.AccFull_Scale = LSM303DLHC_FULLSCALE_2G;
/*Velger sA opplOsningen til mAlingene, starter med A bruke
* full opplOsning, dvs 12bit. Dette kan endres senere,
* dersom vi ser at vi ikke trenger full opplosning
*/
aks_init.High_Resolution = LSM303DLHC_HR_ENABLE;
/* Velger kontinuerlig sampling, i stedet for at sampling trigges av
* at MSB og LSB av utgangsregisteret i aks.meteret leses.
*/
aks_init.High_Resolution = LSM303DLHC_BlockUpdate_Continous;
/*Velger LSB i laveste adresse*/
aks_init.Endianness = LSM303DLHC_BLE_LSB;
/*Naa er oppsettet klart for aa sendes inn i initialiseringsmetoden fra
* biblioteket. Metoden LSM303DLHC_AccInit() setter også opp I2C kommunikasjon
* til ADC'en for oss. Sender en peker til aks_init inn i metoden. */
LSM303DLHC_AccInit(&aks_init);
} // end akselerometer_init()
