95 lines
2.5 KiB
C
95 lines
2.5 KiB
C
#include "Accelerometer.h"
|
||
|
||
#include "stm32f103xb.h"
|
||
#include "stm32f1xx_ll_adc.h"
|
||
#include "stm32f1xx_ll_bus.h"
|
||
#include "stm32f1xx_ll_rcc.h"
|
||
#include "stm32f1xx_ll_gpio.h"
|
||
#include <math.h>
|
||
|
||
#ifndef M_PI
|
||
#define M_PI 3.14159265358979323846
|
||
#endif
|
||
|
||
#define ANGLE_LIMIT_DEG 40
|
||
#define ANGLE_LIMIT_RAD (ANGLE_LIMIT_DEG / 180.0 * M_PI)
|
||
|
||
#define ZERO_G_VOLTAGE 1.65
|
||
#define ZERO_G_READING 2048
|
||
//#define ZERO_G_READING ((ZERO_G_VOLTAGE * 3.3) / 4095)
|
||
int zero_g =ZERO_G_READING;
|
||
|
||
#define VOLT_PER_G 0.48
|
||
double x;
|
||
double y;
|
||
double angle2;
|
||
int x_raw;
|
||
int y_raw;
|
||
|
||
void ACCELEROMETER_Init(void){
|
||
RCC -> CFGR |= (0x1<<15);
|
||
RCC-> CFGR &= ~ (0x1<<14);
|
||
LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_ADC1);
|
||
LL_APB1_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_GPIOC);
|
||
|
||
LL_GPIO_InitTypeDef pc0, pc1;
|
||
LL_ADC_InitTypeDef adc;
|
||
LL_ADC_REG_InitTypeDef adcReg;
|
||
|
||
LL_GPIO_StructInit(&pc0);
|
||
pc0.Pin = LL_GPIO_PIN_0;
|
||
pc0.Mode = LL_GPIO_MODE_ANALOG;
|
||
LL_GPIO_Init(GPIOA, &pc0);
|
||
|
||
LL_GPIO_StructInit(&pc1);
|
||
pc1.Pin = LL_GPIO_PIN_1;
|
||
pc1.Mode = LL_GPIO_MODE_ANALOG;
|
||
LL_GPIO_Init(GPIOA, &pc1);
|
||
|
||
|
||
adc.DataAlignment = LL_ADC_DATA_ALIGN_RIGHT;
|
||
adc.SequencersScanMode = LL_ADC_SEQ_SCAN_DISABLE;
|
||
LL_ADC_Init(ADC1, &adc);
|
||
|
||
adcReg.TriggerSource = LL_ADC_REG_TRIG_SOFTWARE ;
|
||
adcReg.SequencerLength = LL_ADC_REG_SEQ_SCAN_DISABLE;
|
||
adcReg.SequencerDiscont = LL_ADC_REG_SEQ_DISCONT_DISABLE;
|
||
adcReg.ContinuousMode = LL_ADC_REG_CONV_SINGLE;
|
||
adcReg.DMATransfer = LL_ADC_REG_DMA_TRANSFER_NONE;
|
||
|
||
LL_ADC_REG_Init(ADC1, &adcReg);
|
||
LL_ADC_Enable(ADC1);
|
||
}
|
||
|
||
double ACCELEROMETER_GetX(void){
|
||
LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_1, LL_ADC_CHANNEL_10);
|
||
LL_ADC_REG_StartConversionSWStart(ADC1);
|
||
while (LL_ADC_IsActiveFlag_EOS(ADC1) != 1);
|
||
x_raw = LL_ADC_REG_ReadConversionData12(ADC1)-ZERO_G_READING+280;
|
||
x = x_raw / VOLT_PER_G;
|
||
return x;
|
||
}
|
||
|
||
double ACCELEROMETER_GetY(void){
|
||
LL_ADC_REG_SetSequencerRanks(ADC1, LL_ADC_REG_RANK_1, LL_ADC_CHANNEL_11);
|
||
LL_ADC_REG_StartConversionSWStart(ADC1);
|
||
while (LL_ADC_IsActiveFlag_EOS(ADC1) != 1);
|
||
y_raw=LL_ADC_REG_ReadConversionData12(ADC1)-ZERO_G_READING+240;
|
||
y = y_raw/ VOLT_PER_G ;
|
||
return y;
|
||
}
|
||
|
||
int ACCELEROMETER_AngleGood(void){
|
||
x = ACCELEROMETER_GetX();
|
||
y = ACCELEROMETER_GetY();
|
||
angle2 = atan(x/y);
|
||
angle2=fabs(angle2);
|
||
if (angle2>ANGLE_LIMIT_RAD){
|
||
return 0;
|
||
}else {
|
||
return 1;
|
||
}
|
||
//le flag EOC n'est jamais mis <20> un ....
|
||
// Soit la conversion est mal faite soit on n'utilise pas bien la simulation
|
||
//soit on n'utilise pas bien isActiveFlag dans la boucle
|
||
}
|