From e3dbb6a4ed2e533571416f894901f8877161d0b8 Mon Sep 17 00:00:00 2001 From: Arnaud Vergnet Date: Tue, 10 Nov 2020 16:47:15 +0100 Subject: [PATCH] add accelerometer functions --- MyDrivers/ADC.c | 19 +++++++++-------- MyDrivers/ADC.h | 8 +++---- Services/Accelerometer.c | 45 ++++++++++++++++++++++++++++++++++++++++ Services/Accelerometer.h | 9 ++++++++ Services/ServoMotor.c | 2 +- Src/Roll.c | 24 +++++++++++++++++++++ Src/Roll.h | 6 ++++++ Src/main.c | 37 +++++++++++++++++++++++---------- 8 files changed, 125 insertions(+), 25 deletions(-) diff --git a/MyDrivers/ADC.c b/MyDrivers/ADC.c index 7fbd2d0..e1c6b13 100644 --- a/MyDrivers/ADC.c +++ b/MyDrivers/ADC.c @@ -1,7 +1,7 @@ #include "ADC.h" #include "stm32f1xx_ll_bus.h" // Pour horloge -void ADC_conf(ADC_TypeDef *adc, uint32_t voie) +void ADC_conf(ADC_TypeDef *adc) { if (adc == ADC1) { LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_ADC1); @@ -13,10 +13,9 @@ void ADC_conf(ADC_TypeDef *adc, uint32_t voie) RCC->CFGR |= RCC_CFGR_ADCPRE_DIV6; // Fixe le nombre de conversion à 1 - adc->SQR1&= ADC_SQR1_L; + adc->SQR1 &= ADC_SQR1_L; - // Indique la voie a convertir - adc->SQR3|= voie; + // Calibration adc->CR2 |= ADC_CR2_CAL_Msk; @@ -30,23 +29,25 @@ void ADC_start(ADC_TypeDef *adc) } -uint16_t ADC_readRaw(ADC_TypeDef *adc) +uint16_t ADC_readRaw(ADC_TypeDef *adc, int channel) { + // Indique la voie a convertir + adc->SQR3 = channel; // Lancement de la conversion adc->CR2 |= ADC_CR2_ADON; while(!(ADC1->SR & ADC_SR_EOC)) {} - + return ADC1->DR & ADC_DR_DATA_Msk; } -double ADC_convertToVolt(uint16_t value) +float ADC_convertToVolt(uint16_t value) { return ((double) value) / 4095.0 * 3.3; } -double ADC_readVolt(ADC_TypeDef *adc) +float ADC_readVolt(ADC_TypeDef *adc, int channel) { - return ADC_convertToVolt(ADC_readRaw(adc)); + return ADC_convertToVolt(ADC_readRaw(adc, channel)); } diff --git a/MyDrivers/ADC.h b/MyDrivers/ADC.h index b73929d..a7ebee7 100644 --- a/MyDrivers/ADC.h +++ b/MyDrivers/ADC.h @@ -3,13 +3,13 @@ #include "stm32f1xx_ll_adc.h" -void ADC_conf(ADC_TypeDef *adc, uint32_t voie); +void ADC_conf(ADC_TypeDef *adc); void ADC_start(ADC_TypeDef *adc); -uint16_t ADC_readRaw(ADC_TypeDef *adc); -double ADC_readVolt(ADC_TypeDef *adc); +uint16_t ADC_readRaw(ADC_TypeDef *adc, int channel); +float ADC_readVolt(ADC_TypeDef *adc, int channel); -double ADC_convertToVolt(uint16_t value); +float ADC_convertToVolt(uint16_t value); #endif diff --git a/Services/Accelerometer.c b/Services/Accelerometer.c index 022679c..99005e1 100644 --- a/Services/Accelerometer.c +++ b/Services/Accelerometer.c @@ -1 +1,46 @@ #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 MAX_G = 2.13; // 1 g +//const float MIN_G = 1.17; // -1 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; +} diff --git a/Services/Accelerometer.h b/Services/Accelerometer.h index 67bee39..58ce421 100644 --- a/Services/Accelerometer.h +++ b/Services/Accelerometer.h @@ -1,4 +1,13 @@ #ifndef ACCELEROMETER_H #define ACCELEROMETER_H +#include "GPIO.h" +#include "stm32f1xx_ll_adc.h" + +void Accelerometer_conf(ADC_TypeDef *adc, GPIO_TypeDef * gpio, int pinx, int piny); + +void Accelerometer_start(ADC_TypeDef *adc); + +int Accelerometer_getAngle(ADC_TypeDef *adc, int channel); + #endif diff --git a/Services/ServoMotor.c b/Services/ServoMotor.c index 70e636c..3fd03b9 100644 --- a/Services/ServoMotor.c +++ b/Services/ServoMotor.c @@ -4,7 +4,7 @@ void ServoMotor_conf(TIM_TypeDef * timer, int channel, GPIO_TypeDef * gpio, int pin) { - Timer_pwmo_conf(timer, channel, SERVO_MOTO_FREQ, 0.5); + Timer_pwmo_conf(timer, channel, SERVO_MOTO_FREQ, 0); GPIO_conf(gpio, pin, LL_GPIO_MODE_ALTERNATE, LL_GPIO_OUTPUT_PUSHPULL, LL_GPIO_PULL_UP); } diff --git a/Src/Roll.c b/Src/Roll.c index 3dd9b42..8e2bc0d 100644 --- a/Src/Roll.c +++ b/Src/Roll.c @@ -1 +1,25 @@ #include "Roll.h" +#include "Accelerometer.h" + +ADC_TypeDef * ROLL_ADC = ADC1; +const int ROLL_X_CHANNEL = LL_ADC_CHANNEL_11; +const int ROLL_Y_CHANNEL = LL_ADC_CHANNEL_10; +GPIO_TypeDef * ROLL_GPIO = GPIOC; +const int ROLL_X_PIN = LL_GPIO_PIN_0; +const int ROLL_Y_PIN = LL_GPIO_PIN_1; + +void Roll_conf(void) +{ + Accelerometer_conf(ROLL_ADC, ROLL_GPIO, ROLL_X_PIN, ROLL_Y_PIN); +} + +void Roll_start(void) +{ + Accelerometer_start(ROLL_ADC); +} + +void Roll_background(void) +{ + const int xAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_X_CHANNEL); + const int yAngle = Accelerometer_getAngle(ROLL_ADC, ROLL_Y_CHANNEL); +} diff --git a/Src/Roll.h b/Src/Roll.h index e6969d7..2b48a8c 100644 --- a/Src/Roll.h +++ b/Src/Roll.h @@ -1,4 +1,10 @@ #ifndef ROLL_H #define ROLL_H +void Roll_conf(void); + +void Roll_start(void); + +void Roll_background(void); + #endif diff --git a/Src/main.c b/Src/main.c index 77bdaaf..d55b4a8 100644 --- a/Src/main.c +++ b/Src/main.c @@ -21,36 +21,55 @@ #include "stm32f1xx_ll_system.h" // utile dans la fonction SystemClock_Config #include "Sail.h" +#include "Roll.h" + #include "Scheduler.h" #include "ADC.h" #include "GPIO.h" +#include "Accelerometer.h" void SystemClock_Config(void); /* Private functions ---------------------------------------------------------*/ int counter = 1; -int adcRaw = 0; -double adcVolt = 0.0; +int adcRaw1 = 0; +int adcRaw2 = 0; +double adcVolt1 = 0.0; +double adcVolt2 = 0.0; + +int angle = 0; void backgroundTask() { counter++; Sail_background(); - adcRaw = ADC_readRaw(ADC1); - adcVolt = ADC_convertToVolt(adcRaw); + Roll_background(); + + angle = Accelerometer_getAngle(ADC1, LL_ADC_CHANNEL_11); + + // DEBUG + adcRaw1 = ADC_readRaw(ADC1, LL_ADC_CHANNEL_11); + adcRaw2 = ADC_readRaw(ADC1, LL_ADC_CHANNEL_10); + adcVolt1 = ADC_convertToVolt(adcRaw1); + adcVolt2 = ADC_convertToVolt(adcRaw2); } - void configurePeripherals() { Sail_conf(); - ADC_conf(ADC1, 12); + Roll_conf(); + + // DEBUG + ADC_conf(ADC1); } void startPeripherals() { Sail_start(); + Roll_start(); + + // DEBUG ADC_start(ADC1); } @@ -65,11 +84,6 @@ int main(void) SystemClock_Config(); /* Add your application code here */ - // Configuration chronomètre - // Chrono_Conf(TIM3); - // Lancement chronomètre - // Chrono_Start(); - // time = Chrono_Read(); configurePeripherals(); startPeripherals(); @@ -78,6 +92,7 @@ int main(void) while (1) { + // Send to display here } }