44 lines
970 B
C
44 lines
970 B
C
#include "Accelerometer.h"
|
|
#include "ADC.h"
|
|
#include "math.h"
|
|
|
|
#define M_PI 3.14159265358979323846
|
|
|
|
// g-sel1 et g-sel1 à 0 => range de 2.5g
|
|
// Donc sensibilité de +- 480 mv/g
|
|
|
|
|
|
const float ZERO_G = 1.65; // 0 g
|
|
const float SENSITIVITY = 0.48;
|
|
|
|
void Accelerometer_conf(ADC_TypeDef *adc, GPIO_TypeDef * gpio, int pinx, int piny)
|
|
{
|
|
ADC_conf(adc);
|
|
GPIO_conf(gpio, pinx, LL_GPIO_MODE_ANALOG, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP);
|
|
GPIO_conf(gpio, piny, LL_GPIO_MODE_ANALOG, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP);
|
|
}
|
|
|
|
void Accelerometer_start(ADC_TypeDef *adc)
|
|
{
|
|
ADC_start(adc);
|
|
}
|
|
|
|
float voltsToG(float volts)
|
|
{
|
|
float readG = (volts - ZERO_G) / SENSITIVITY;
|
|
if (readG > 1.0)
|
|
readG = 1.0;
|
|
else if (readG < -1.0)
|
|
readG = -1.0;
|
|
return readG;
|
|
}
|
|
|
|
int Accelerometer_getAngle(ADC_TypeDef *adc, int channel)
|
|
{
|
|
const float readG = voltsToG(ADC_readVolt(adc, channel));
|
|
|
|
float angleRad = asin(readG);
|
|
int angleDeg = angleRad * (180/M_PI);
|
|
|
|
return angleDeg;
|
|
}
|