diff --git a/.gitignore b/.gitignore index f805e81..57071ce 100644 --- a/.gitignore +++ b/.gitignore @@ -1,33 +1,22 @@ -# Object files -*.o -*.ko -*.obj -*.elf +/Debug/ -# Precompiled Headers -*.gch -*.pch +**.o +**.d +**.elf +**.bin +**.xml +**.list -# Libraries -*.lib -*.a -*.la -*.lo +**/Debug/ -# Shared objects (inc. Windows DLLs) -*.dll -*.so -*.so.* -*.dylib +**/language.settings.xml -# Executables -*.exe -*.out -*.app -*.i*86 -*.x86_64 -*.hex - -# Debug files -*.dSYM/ -*.su +/Release/ +**.o +**.d +**.elf +**.bin +**.xml +**.list + +**/Release/ \ No newline at end of file diff --git a/UAV-ControlSystem/.cproject b/UAV-ControlSystem/.cproject index ab71b73..24530a6 100644 --- a/UAV-ControlSystem/.cproject +++ b/UAV-ControlSystem/.cproject @@ -215,5 +215,4 @@ - diff --git a/UAV-ControlSystem/inc/drivers/adc.h b/UAV-ControlSystem/inc/drivers/adc.h index 03e63f1..918b5d5 100644 --- a/UAV-ControlSystem/inc/drivers/adc.h +++ b/UAV-ControlSystem/inc/drivers/adc.h @@ -6,6 +6,7 @@ */ /********************************************************************** * NAME: adc.h * + * AUTHOR: Philip Johansson * * PURPOSE: Set up and read from ADC * * INFORMATION: * * How to use this driver is explained in page 107 of HAL driver * @@ -28,27 +29,29 @@ #include "stm32f4xx.h" + /*********************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Configuration of ADC * +* INFORMATION: Also initializes * ***********************************************************************/ void adc_configure(); +/*********************************************************************** +* BRIEF: Add the wanted channels to a list * +* INFORMATION: Not initialized here but later * +***********************************************************************/ +void adc_pin_add(uint32_t adc_channel); /*********************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Read ADC channel * +* INFORMATION: Returns a mean value from "ADB_BUFFER_LENGTH" samples. * ***********************************************************************/ -void adc_pin_config(ADC_HandleTypeDef * g_AdcHandle, uint32_t adc_channel); - +uint32_t adc_read(uint32_t adc_channel); /*********************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: When ADC is configured this function starts the DMA sampling * +* INFORMATION: Third arg is size. When its full it starts over. * ***********************************************************************/ -uint32_t adc_read_int(ADC_HandleTypeDef * g_AdcHandle); - - - +void adc_start(); #endif /* DRIVERS_ADC_H_ */ diff --git a/UAV-ControlSystem/inc/drivers/motors.h b/UAV-ControlSystem/inc/drivers/motors.h index bc98b25..282fba0 100644 --- a/UAV-ControlSystem/inc/drivers/motors.h +++ b/UAV-ControlSystem/inc/drivers/motors.h @@ -8,18 +8,24 @@ #ifndef DRIVERS_MOTORS_H_ #define DRIVERS_MOTORS_H_ +/* Struct of motor output protocol */ +typedef enum +{ +PWM, Oneshot125 +}motorOutput; + /************************************************************************** * BRIEF: Initializing a motor (maps a pwm signal to a certain pin) * * INFORMATION: The motor becomes active, Each motor configuration is changed in the "stm32f4xx_revo.h" file * * Example - pwmEnableMotor(MOTOR_1) * **************************************************************************/ -void pwmEnableMotor(uint8_t motor); +void pwmEnableMotor(uint8_t motor, motorOutput motorOutput); /************************************************************************** * BRIEF: Enables all motors (maps a pwm signal to all pins which will be connected to a motor) * * INFORMATION: All motors become active **************************************************************************/ -void pwmEnableAllMotors(void); +void pwmEnableAllMotors(motorOutput motorOutput); /************************************************************************** * BRIEF: Deactivates a motor (Disables the pwm signal for the motor pin ) diff --git a/UAV-ControlSystem/inc/drivers/pwm.h b/UAV-ControlSystem/inc/drivers/pwm.h index 8fb5337..5b89116 100644 --- a/UAV-ControlSystem/inc/drivers/pwm.h +++ b/UAV-ControlSystem/inc/drivers/pwm.h @@ -21,7 +21,7 @@ * period = Period of PWM signal * pulse = the "duty cycle" of the pwm signal * **************************************************************************/ -void pwmInit(GPIO_TypeDef * GPIO, uint16_t pin, TIM_TypeDef * tim, uint32_t Channel, uint16_t period, uint16_t pulse); +void pwmInit(GPIO_TypeDef * GPIO, uint16_t pin, TIM_TypeDef * tim, uint32_t Channel, uint16_t period, uint16_t pulse, uint32_t prescaler); /************************************************************************** * BRIEF: setPwmPulse changes a certain pulse for an initialized pwm signal * diff --git a/UAV-ControlSystem/inc/drivers/system_clock.h b/UAV-ControlSystem/inc/drivers/system_clock.h new file mode 100644 index 0000000..8538130 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/system_clock.h @@ -0,0 +1,50 @@ +/************************************************************************** +* NAME: system_clock.h * +* AUTHOR: Lennart Eriksson * +* PURPOSE: Enable and handle system clock functionality * +* INFORMATION: * +* This file initilizes the system clock and handles delays, get time * +* in both micro- and milliseconds. * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +**************************************************************************/ + + +#ifndef DRIVERS_SYSTEM_CLOCK_H_ +#define DRIVERS_SYSTEM_CLOCK_H_ + + +/*********************************************************************** +* BRIEF: Starts the system clock at 100MHz * +* INFORMATION: In the current version it works with ADC and DMA * +***********************************************************************/ +void system_clock_config(void); + +/*********************************************************************** +* BRIEF: Get the time since the system started in milliseconds +* INFORMATION: return the time in milliseconds +***********************************************************************/ +uint32_t clock_get_ms(); + +/*********************************************************************** +* BRIEF: Get the time since the system started in microseconds +* INFORMATION: return the time in microseconds +***********************************************************************/ +uint32_t clock_get_us(); + +/*********************************************************************** +* BRIEF: stall the system for number of milliseconds +* INFORMATION: +***********************************************************************/ +void clock_delay_ms(uint32_t ms); + +/*********************************************************************** +* BRIEF: Stall the system for a number of microseconds +* INFORMATION: +***********************************************************************/ +void clock_delay_us(uint32_t us); + + +#endif /* DRIVERS_SYSTEM_CLOCK_H_ */ diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h new file mode 100644 index 0000000..48e4968 --- /dev/null +++ b/UAV-ControlSystem/inc/system_variables.h @@ -0,0 +1,32 @@ +/* + /********************************************************************** + * NAME: adc.h * + * AUTHOR: Philip Johansson * + * PURPOSE: Set up and read from ADC * + * INFORMATION: * + * Here we gather common variables for the system as a whole * + * * + * GLOBAL VARIABLES: * + * Variable Type Description * + * -------- ---- ----------- * + * * + **********************************************************************/ + +#ifndef SYSTEM_VARIABLES_H_ +#define SYSTEM_VARIABLES_H_ + + + +#define ADC_STATE +#include "stm32f4xx.h" + + + + + + + + + + +#endif /* SYSTEM_VARIABLES_H_ */ diff --git a/UAV-ControlSystem/inc/utilities.h b/UAV-ControlSystem/inc/utilities.h new file mode 100644 index 0000000..35d18a0 --- /dev/null +++ b/UAV-ControlSystem/inc/utilities.h @@ -0,0 +1,39 @@ +/* + * utilities.h + * + * Created on: 16 sep. 2016 + * Author: Philip + */ + /********************************************************************** + * NAME: utilities.c * + * AUTHOR: Philip Johansson * + * PURPOSE: Set up and read from ADC * + * INFORMATION: * + * Here we gather usable functions by the whole system * + * * + * GLOBAL VARIABLES: * + * Variable Type Description * + * -------- ---- ----------- * + * * + **********************************************************************/ + +#ifndef UTILITIES_H_ +#define UTILITIES_H_ + +#include +#include "stm32f4xx_it.h" + +/*********************************************************************** +* BRIEF: Sums elements of array until index of second arg * +* INFORMATION: Returns the sum * +***********************************************************************/ +uint32_t accumulate(uint32_t list[], int length); + +/*********************************************************************** +* BRIEF: A function that can be called on exceptions * +* INFORMATION: If an exception happens its easier to find it in an * +* infinite loop * +***********************************************************************/ +void Error_Handler(void); + +#endif /* UTILITIES_H_ */ diff --git a/UAV-ControlSystem/src/drivers/adc.c b/UAV-ControlSystem/src/drivers/adc.c index 2a1e415..4509f14 100644 --- a/UAV-ControlSystem/src/drivers/adc.c +++ b/UAV-ControlSystem/src/drivers/adc.c @@ -1,77 +1,231 @@ /* * ADC.c - * - * Created on: 13 sep. 2016 - * Author: Philip - */ -#include + /********************************************************************** + * NAME: adc.c * + * AUTHOR: Philip Johansson * + * PURPOSE: Set up and read from ADC * + * INFORMATION: * + * How to use this driver is explained in page 107 of HAL driver * + * Enable the ADC clock * + * Enable the GPIO clock for the pin wanted * + * Configure the GPIO pin as analog input * + * Configure the ADC speed (prescaler/sampling time) * + * Enable continuous measurement mode * + * * + * Read more at: www.visualgdb.com/tutorials/arm/stm32/adc/ * + * * + * GLOBAL VARIABLES: * + * Variable Type Description * + * -------- ---- ----------- * + * * + **********************************************************************/ +#include "drivers/adc.h" +#include "system_variables.h" +#include "utilities.h" +#include + +/* A buffer for the ADC to write with DMA */ +enum{ ADC_BUFFER_LENGTH = 10 }; // TODO: Make this define instead of enum +uint32_t g_ADCBuffer[160] = {0}; // We allocate more than needed. Gives 10 readings on 16ch + +/* Handler for the ADC */ +ADC_HandleTypeDef g_AdcHandle; + +/* DMA handler */ +DMA_HandleTypeDef g_DmaHandle; + +/* Stores a list (in order) of the channels added + * OBS: Index/size defined by channel_counter */ +uint32_t channels_added[16]; +/* Increments when new ADC input is added */ +int channel_counter = 0; + +/* Stores the rank of every channel */ +uint32_t channel_ranks[16] = {0}; -void adc_configure() +void adc_pin_conf(uint32_t adc_channel, int adc_rank); + + +/*********************************************************************** +* BRIEF: Read ADC channel * +* INFORMATION: Returns a mean value from "ADB_BUFFER_LENGTH" samples. * +***********************************************************************/ +uint32_t adc_read(uint32_t adc_channel) { - __GPIOC_CLK_ENABLE(); - __ADC1_CLK_ENABLE(); + int rank = channel_ranks[adc_channel]; + int sum = 0; + for (int i = rank - 1; i < ADC_BUFFER_LENGTH * channel_counter; i += channel_counter ) + sum += g_ADCBuffer[i]; + + return (sum/ADC_BUFFER_LENGTH); } -void adc_pin_config(ADC_HandleTypeDef * g_AdcHandle, uint32_t adc_channel) + +/*********************************************************************** +* BRIEF: DMA configuration * +* INFORMATION: Is triggered automatically, enables clocks * +***********************************************************************/ +void HAL_ADC_MspInit(ADC_HandleTypeDef * hadc) { - //ADC_HandleTypeDef g_AdcHandle; + + //static DMA_HandleTypeDef g_DmaHandle; + + // Enable DMA, ADC and related GPIO ports clock + __DMA2_CLK_ENABLE(); + __ADC1_CLK_ENABLE(); + __GPIOA_CLK_ENABLE(); + __GPIOC_CLK_ENABLE(); + + // DMA2 Stream4 channel1 (physically mapped to ADC1) configuration + g_DmaHandle.Instance = DMA2_Stream4; + g_DmaHandle.Init.Channel = DMA_CHANNEL_0; + g_DmaHandle.Init.Direction = DMA_PERIPH_TO_MEMORY; + g_DmaHandle.Init.PeriphInc = DMA_PINC_DISABLE; + g_DmaHandle.Init.MemInc = DMA_MINC_ENABLE; + g_DmaHandle.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; + g_DmaHandle.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; + g_DmaHandle.Init.Mode = DMA_CIRCULAR; + g_DmaHandle.Init.Priority = DMA_PRIORITY_HIGH; + g_DmaHandle.Init.FIFOMode = DMA_FIFOMODE_DISABLE; + g_DmaHandle.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_HALFFULL; + g_DmaHandle.Init.MemBurst = DMA_MBURST_SINGLE; + g_DmaHandle.Init.PeriphBurst = DMA_PBURST_SINGLE; + HAL_DMA_Init(&g_DmaHandle); + + // Associate the initialized DMA handle to the the ADC handle + //__HAL_LINKDMA(adc_handler, DMA_Handle, g_DmaHandle); + __HAL_LINKDMA(&g_AdcHandle, DMA_Handle, g_DmaHandle); + + /* These are for interrupt which we are not using for ADC/DMA */ + //NVIC configuration for DMA transfer complete interrupt + //HAL_NVIC_SetPriority(DMA2_Stream4_IRQn, 15, 15); + //HAL_NVIC_EnableIRQ(DMA2_Stream4_IRQn); +} + + +/*********************************************************************** +* BRIEF: Configuration of ADC * +* INFORMATION: Also initializes * +***********************************************************************/ +void adc_configure() +{ + + /* Not using the IRQs but here is how they can be declared */ + // __ADC1_CLK_ENABLE(); + // + // HAL_NVIC_SetPriority(ADC_IRQn, 0,0); + // HAL_NVIC_EnableIRQ(ADC_IRQn); + + g_AdcHandle.Instance = ADC1; + g_AdcHandle.Init.ClockPrescaler = ADC_CLOCKPRESCALER_PCLK_DIV8; + g_AdcHandle.Init.Resolution = ADC_RESOLUTION12b; + g_AdcHandle.Init.ScanConvMode = ENABLE; //DISABLE; + g_AdcHandle.Init.ContinuousConvMode = ENABLE; + g_AdcHandle.Init.DiscontinuousConvMode = DISABLE; + g_AdcHandle.Init.NbrOfDiscConversion = 0; + g_AdcHandle.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE; //ADC_EXTERNALTRIGCONV_T1_CC1; + g_AdcHandle.Init.ExternalTrigConv = ADC_EXTERNALTRIGCONVEDGE_NONE; + g_AdcHandle.Init.DataAlign = ADC_DATAALIGN_RIGHT; + g_AdcHandle.Init.NbrOfConversion = channel_counter; //adc_rank; + g_AdcHandle.Init.DMAContinuousRequests = ENABLE;//DISABLE; + g_AdcHandle.Init.EOCSelection = EOC_SINGLE_CONV; + + + if (HAL_ADC_Init(&g_AdcHandle) != HAL_OK) + Error_Handler(); + for (int i = 0; i < channel_counter; i++) + adc_pin_conf(channels_added[i], i + 1); +} + +/*********************************************************************** +* BRIEF: Add the wanted channels to a list * +* INFORMATION: Not initialized here but later * +***********************************************************************/ +void adc_pin_add(uint32_t adc_channel) +{ + channels_added[channel_counter] = adc_channel; + channel_counter++; +} + +/*********************************************************************** +* BRIEF: Configures the ADC channel and ads it to the handler * +* INFORMATION: Each added channel gets an incrementing rank * +***********************************************************************/ +void adc_pin_conf(uint32_t adc_channel, int adc_rank) +{ + + /* Variable used to initialize the GPIO */ GPIO_InitTypeDef gpioInit; + /* Variable to assign the IO to its Port ex PA, PB etc */ + GPIO_TypeDef * gpio_port; - // TODO!!!! - if (adc_channel == ADC_CHANNEL_12) gpioInit.Pin = GPIO_PIN_2; - else gpioInit.Pin = GPIO_PIN_1; - + /* Configuration dependent on channel - TODO complete this switch */ + switch (adc_channel) + { + case ADC_CHANNEL_0: + gpio_port = GPIOA; + gpioInit.Pin = GPIO_PIN_0; + break; + case ADC_CHANNEL_1: + gpio_port = GPIOA; + gpioInit.Pin = GPIO_PIN_1; + break; + case ADC_CHANNEL_2: + gpio_port = GPIOA; + gpioInit.Pin = GPIO_PIN_2; + break; + case ADC_CHANNEL_3: + case ADC_CHANNEL_4: + case ADC_CHANNEL_5: + case ADC_CHANNEL_6: + case ADC_CHANNEL_7: + case ADC_CHANNEL_8: + case ADC_CHANNEL_9: + case ADC_CHANNEL_10: + case ADC_CHANNEL_11: + gpio_port = GPIOC; + gpioInit.Pin = GPIO_PIN_1; + break; + case ADC_CHANNEL_12: + gpio_port = GPIOC; + gpioInit.Pin = GPIO_PIN_2; + break; + case ADC_CHANNEL_13: + case ADC_CHANNEL_14: + case ADC_CHANNEL_15: + break; + } gpioInit.Mode = GPIO_MODE_ANALOG; gpioInit.Pull = GPIO_NOPULL; - HAL_GPIO_Init(GPIOC,&gpioInit); + HAL_GPIO_Init(gpio_port,&gpioInit); - HAL_NVIC_SetPriority(ADC_IRQn, 0,0); - HAL_NVIC_EnableIRQ(ADC_IRQn); + ADC_ChannelConfTypeDef adc_channel_conf; - ADC_ChannelConfTypeDef adcChannel; + adc_channel_conf.Channel = adc_channel; // ex ADC_CHANNEL_12 + adc_channel_conf.Rank = (uint32_t)adc_rank; + adc_channel_conf.SamplingTime = ADC_SAMPLETIME_480CYCLES; + adc_channel_conf.Offset = 0; + channel_ranks[adc_channel] = (uint32_t)adc_rank; - g_AdcHandle->Instance = ADC1; - g_AdcHandle->Init.ClockPrescaler = ADC_CLOCKPRESCALER_PCLK_DIV2; - g_AdcHandle->Init.Resolution = ADC_RESOLUTION12b; - g_AdcHandle->Init.ScanConvMode = DISABLE; - g_AdcHandle->Init.ContinuousConvMode = ENABLE; - g_AdcHandle->Init.DiscontinuousConvMode = DISABLE; - g_AdcHandle->Init.NbrOfDiscConversion = 0; - g_AdcHandle->Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONV_T1_CC1; - g_AdcHandle->Init.DataAlign = ADC_DATAALIGN_RIGHT; - g_AdcHandle->Init.NbrOfConversion = 1; - g_AdcHandle->Init.DMAContinuousRequests = ENABLE; - g_AdcHandle->Init.EOCSelection = DISABLE; - - HAL_ADC_Init(g_AdcHandle); - - adcChannel.Channel = adc_channel; // ex ADC_CHANNEL_12 - adcChannel.Rank = 1; - adcChannel.SamplingTime = ADC_SAMPLETIME_480CYCLES; - adcChannel.Offset = 0; - - if (HAL_ADC_ConfigChannel(g_AdcHandle, &adcChannel) != HAL_OK) - { - asm("bkpt 255"); - } - - HAL_ADC_Start(g_AdcHandle); - + if (HAL_ADC_ConfigChannel(&g_AdcHandle, &adc_channel_conf) != HAL_OK) + Error_Handler(); } -uint32_t adc_read_int(ADC_HandleTypeDef * g_AdcHandle) + +/*********************************************************************** +* BRIEF: When ADC is configured this function starts the DMA sampling * +* INFORMATION: Third arg is size. When its full it starts over. * +***********************************************************************/ +void adc_start() { - uint32_t value = 0; - if (HAL_ADC_PollForConversion(g_AdcHandle, 100) == HAL_OK) - { - value = HAL_ADC_GetValue(g_AdcHandle); - } - return value; + if (HAL_ADC_Start_DMA(&g_AdcHandle, (uint32_t*)&g_ADCBuffer, channel_counter * ADC_BUFFER_LENGTH) != HAL_OK) + Error_Handler(); } + diff --git a/UAV-ControlSystem/src/drivers/motors.c b/UAV-ControlSystem/src/drivers/motors.c index 4e84c15..8ec92bb 100644 --- a/UAV-ControlSystem/src/drivers/motors.c +++ b/UAV-ControlSystem/src/drivers/motors.c @@ -4,8 +4,8 @@ * INFORMATION: * * GLOBAL VARIABLES: * * Variable Type Description * -* MOTOR_PWM_INIT_PERIODE define The initializing periode of the pwm signal to each motor * -* MOTOR_PWM_INIT_PULSE define The initializing pulse of the pwm signal to each motor +* MotorPWMPeriode define The initializing periode of the pwm signal to each motor * +* MotorPWMInitPulse define The initializing pulse of the pwm signal to each motor * **************************************************************************/ #include "stm32f4xx.h" @@ -13,8 +13,8 @@ #include "drivers/pwm.h" #include "drivers/motors.h" -#define MOTOR_PWM_INIT_PERIODE 65535 -#define MOTOR_PWM_INIT_PULSE MOTOR_PWM_INIT_PERIODE/2 +const int MotorPWMPeriode = 2000; //Micro seconds +const int MotorPWMInitPulse = 1000; /* A struct of a pwm motor profile */ typedef struct @@ -25,6 +25,16 @@ typedef struct uint32_t channel; //TIM_CHANNEL_1/TIM_CHANNEL_1/.. }motorProfile; +/************************************************************************** +* BRIEF: Returns the final pulse of a motor driver call +* INFORMATION: The pulse is not allowed to be higher then 94 % of the total periode of the pwm signal, otherwise the pwm won't perform correctly +* Example - pwmEnableMotor(MOTOR_1) +**************************************************************************/ +uint16_t checkPulse(uint16_t pulse) +{ + return ((pulse/MotorPWMPeriode)*100 < 94)? pulse: MotorPWMPeriode*0.94; +} + /************************************************************************** * BRIEF: Returns a profile of a certain motor * INFORMATION: Each Motor has a certain profile which includes a pin, port, timer and a timer channel @@ -104,25 +114,51 @@ motorProfile getMotorProfile(uint8_t motor) return profile; } +/************************************************************************** +* BRIEF: Sets the motor output protocol to be either oneshot125 or pwm +* INFORMATION: +* Example - uint32_t prescaler = setMotorOutput(Oneshot125) +*************************************************************************/ +uint32_t setMotorOutput(motorOutput motorOutput) +{ + uint32_t prescaler; + + switch(motorOutput) + { + case PWM: + prescaler = 88; + break; + case Oneshot125: + prescaler = 11; + break; + default: + prescaler = 11; + break; + } + + return prescaler; +} + /************************************************************************** * BRIEF: Initializing a motor (maps a pwm signal to a certain pin) * * INFORMATION: The motor becomes active, Each motor configuration is changed in the "stm32f4xx_revo.h" file * * Example - pwmEnableMotor(MOTOR_1) * **************************************************************************/ -void pwmEnableMotor(uint8_t motor) +void pwmEnableMotor(uint8_t motor, motorOutput motorOutput) { motorProfile profile = getMotorProfile(motor); + uint32_t prescaler = setMotorOutput(motorOutput); - pwmInit(profile.port, profile.pin, profile.tim, profile.channel, MOTOR_PWM_INIT_PERIODE, MOTOR_PWM_INIT_PULSE); + pwmInit(profile.port, profile.pin, profile.tim, profile.channel, MotorPWMPeriode, MotorPWMInitPulse, prescaler); } /************************************************************************** * BRIEF: Enables all motors (maps a pwm signal to all pins which will be connected to a motor) * * INFORMATION: All motors become active **************************************************************************/ -void pwmEnableAllMotors(void) +void pwmEnableAllMotors(motorOutput motorOutput) { - for (uint8_t i = 1; i < 11; i++ ) pwmEnableMotor(i); + for (uint8_t i = 1; i < 11; i++ ) pwmEnableMotor(i, motorOutput); } /************************************************************************** @@ -152,11 +188,10 @@ void pwmDeactivateMotor(uint8_t motor) /************************************************************************** * BRIEF: Activates all motors (Activates a pwm signal to all motor pins to it's last state)* * * INFORMATION: * -* Example - pwmActivateAllMotors() * **************************************************************************/ void pwmActivateAllMotors(void) { - for (uint8_t i = 1; i < 11; i++ ) pwmDeactivateMotor(i); + for (uint8_t i = 1; i < 11; i++ ) pwmActivateMotor(i); } /************************************************************************** @@ -166,7 +201,7 @@ void pwmActivateAllMotors(void) **************************************************************************/ void pwmDeactivateAllMotors(void) { - for (uint8_t i = 1; i < 11; i++ ) pwmActivateMotor(i); + for (uint8_t i = 1; i < 11; i++ ) pwmDeactivateMotor(i); } /************************************************************************** @@ -178,7 +213,7 @@ void pwmAdjustSpeedOfMotor(uint8_t motor, uint16_t pulse) { motorProfile profile = getMotorProfile(motor); - setPwmPulse(profile.channel, profile.tim, pulse); + setPwmPulse(profile.channel, profile.tim, checkPulse(pulse)); } /************************************************************************** @@ -188,8 +223,8 @@ void pwmAdjustSpeedOfMotor(uint8_t motor, uint16_t pulse) **************************************************************************/ void pwmAdjustSpeedOfMotorDutyCycle(uint8_t motor, uint16_t DutyCycle) { - uint16_t pulse = (DutyCycle*MOTOR_PWM_INIT_PERIODE)/100; /* Converts the DutyCycle to a pulse */ + uint16_t pulse = (DutyCycle*MotorPWMPeriode)/100; /* Converts the DutyCycle to a pulse */ motorProfile profile = getMotorProfile(motor); - setPwmPulse(profile.channel, profile.tim, pulse); + setPwmPulse(profile.channel, profile.tim, checkPulse(pulse)); } diff --git a/UAV-ControlSystem/src/drivers/pwm.c b/UAV-ControlSystem/src/drivers/pwm.c index 33c7efa..cec7b99 100644 --- a/UAV-ControlSystem/src/drivers/pwm.c +++ b/UAV-ControlSystem/src/drivers/pwm.c @@ -47,7 +47,7 @@ pwmProfile get_Pwm_Profile(GPIO_TypeDef * GPIO, uint16_t pin, uint32_t Channel, * period = Period of PWM signal * pulse = the "duty cycle" of the pwm signal * **************************************************************************/ -void pwmInit(GPIO_TypeDef * GPIO, uint16_t pin, TIM_TypeDef * tim, uint32_t Channel, uint16_t period, uint16_t pulse) +void pwmInit(GPIO_TypeDef * GPIO, uint16_t pin, TIM_TypeDef * tim, uint32_t Channel, uint16_t period, uint16_t pulse, uint32_t prescaler) { pwmProfile profile; @@ -81,10 +81,9 @@ void pwmInit(GPIO_TypeDef * GPIO, uint16_t pin, TIM_TypeDef * tim, uint32_t Chan TIM_HandleTypeDef TimHandle; - uint32_t uwPrescalerValue =2; TimHandle.Instance = profile.tim; /* Sets timer */ TimHandle.Init.Period = period; /* Sets period of pwm */ - TimHandle.Init.Prescaler = uwPrescalerValue; + TimHandle.Init.Prescaler = prescaler; /* pwm prescaler of clk */ TimHandle.Init.ClockDivision = 0; TimHandle.Init.CounterMode = TIM_COUNTERMODE_UP; diff --git a/UAV-ControlSystem/src/drivers/system_clock.c b/UAV-ControlSystem/src/drivers/system_clock.c new file mode 100644 index 0000000..87b32e3 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/system_clock.c @@ -0,0 +1,111 @@ +/************************************************************************** +* NAME: system_clock.c * +* PURPOSE: Enable and handle system clock functionality * +* INFORMATION: * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +**************************************************************************/ + +#include +#include "stm32f4xx.h" + +//the number of clock cycles per microsecond +static uint32_t usTicks = 0; + +/*********************************************************************** +* BRIEF: Starts the system clock at 100MHz * +* INFORMATION: In the current version it works with ADC and DMA * +***********************************************************************/ +void SystemClock_Config(void) +{ + RCC_ClkInitTypeDef RCC_ClkInitStruct; + RCC_OscInitTypeDef RCC_OscInitStruct; + + //Enable GPIO clocks + __HAL_RCC_GPIOA_CLK_ENABLE(); + __HAL_RCC_GPIOB_CLK_ENABLE(); + __HAL_RCC_GPIOC_CLK_ENABLE(); + + //Enable power clock + __HAL_RCC_PWR_CLK_ENABLE(); + + __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2); + + RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; + RCC_OscInitStruct.HSEState = RCC_HSE_ON; + RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; + RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; + RCC_OscInitStruct.PLL.PLLM = 8; + RCC_OscInitStruct.PLL.PLLN = 288; + RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; + RCC_OscInitStruct.PLL.PLLQ = 6; + HAL_RCC_OscConfig(&RCC_OscInitStruct); + + RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2); + RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; + RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; + RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; + RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; + HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4); + + + if (HAL_GetREVID() == 0x1001) + __HAL_FLASH_PREFETCH_BUFFER_ENABLE(); + + //Set the number of sycles per microsecond + usTicks = SystemCoreClock / 1000000; + +} + +/*********************************************************************** +* BRIEF: Get the time since the system started in microseconds +* INFORMATION: return the time in microseconds +***********************************************************************/ +uint32_t clock_get_us() +{ + register uint32_t ms, cycle_cnt; + + //make sure that the system tick interrupt does not happen during the time we fetch microsecond from the register + do { + ms = get_ms(); + cycle_cnt = SysTick->VAL; + + //If the SysTick timer expired during the previous instruction, we need to give it a little time for that + //interrupt to be delivered before we can recheck sysTickUptime: + asm volatile("\tnop\n"); + } while (ms != get_ms()); + + assert_param(us >= 1000); + + return (ms * 1000) + (usTicks * 1000 - cycle_cnt) / usTicks; +} + +/*********************************************************************** +* BRIEF: Get the time since the system started in milliseconds +* INFORMATION: return the time in milliseconds +***********************************************************************/ +uint32_t clock_get_ms() +{ + return HAL_GetTick(); +} + +/*********************************************************************** +* BRIEF: stall the system for number of milliseconds +* INFORMATION: +***********************************************************************/ +void clock_delay_ms(uint32_t ms) +{ + HAL_Delay(ms); +} + +/*********************************************************************** +* BRIEF: Stall the system for a number of microseconds +* INFORMATION: +***********************************************************************/ +void clock_delay_us(uint32_t us) +{ + volatile uint32_t time = clock_get_us() + us; + while(clock_get_us() < time); +} diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 6d68a42..92fba26 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -5,13 +5,18 @@ * @version V1.0 * @date 01-December-2013 * @brief Default main function. + * Awesome ADC fix start project here to be good awesome!! ****************************************************************************** */ -#include +#include "drivers/adc.h" +#include "drivers/system_clock.h" #include "stm32f4xx.h" #include "stm32f4xx_revo.h" +#include "system_variables.h" +#include "utilities.h" +#include /* Private function prototypes -----------------------------------------------*/ static void SystemClock_Config(void); @@ -26,32 +31,9 @@ int g_MeasurementNumber; int main(void) { - // Comment? + // Initialize the Hardware Abstraction Layer HAL_Init(); - int i = 1; - - TIM_Base_InitTypeDef t; - - /* Configure the system clock to 100 MHz */ - SystemClock_Config(); - - adc_configure(); - //adc_pin_config(&adc_testinput_handle,ADC_CHANNEL_12); - adc_pin_config(&adc_testinput_handle,ADC_CHANNEL_11); - - - - /*##-1- Enable GPIOA Clock (to be able to program the configuration registers) */ - __HAL_RCC_GPIOA_CLK_ENABLE(); - - GPIO_InitTypeDef gpinit; - gpinit.Pin = GPIO_PIN_0; - gpinit.Mode = GPIO_MODE_OUTPUT_PP; - gpinit.Pull = GPIO_PULLUP; - gpinit.Speed = GPIO_SPEED_FAST; - - /*##-2- Configure PA05 IO in output push-pull mode to drive external LED ###*/ //GPIO_InitStruct.Pin = GPIO_PIN_5 | GPIO_PIN_6; @@ -59,101 +41,64 @@ int main(void) //GPIO_InitStruct.Pull = GPIO_PULLUP; //GPIO_InitStruct.Speed = GPIO_SPEED_FAST; HAL_GPIO_Init(GPIOA, &gpinit); + // Configure the system clock to 100 MHz + system_clock_config(); - /*##-3- Toggle PA05 IO in an infinite loop #################################*/ - while (1) - { - HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_0); + int i = 1; - i++; - /* Insert a 100ms delay */ - HAL_Delay(100); + //Add ADC Channels + adc_pin_add(ADC_CHANNEL_0); + adc_pin_add(ADC_CHANNEL_1); + adc_pin_add(ADC_CHANNEL_12); + //Configure the ADCs + adc_configure(); - // ADC part - //g_ADCValue = adc_read_int(&adc_testinput_handle); - HAL_Delay(10); - g_ADC2 = adc_read_int(&adc_test2input_handle); - g_MeasurementNumber++; - } + /* This is done in system_clock_config for all GPIO clocks */ + //__GPIOB_CLK_ENABLE(); + + GPIO_InitTypeDef gpinit; + gpinit.Pin = GPIO_PIN_5; + gpinit.Mode = GPIO_MODE_OUTPUT_PP; + gpinit.Pull = GPIO_PULLUP; + gpinit.Speed = GPIO_SPEED_HIGH; + HAL_GPIO_Init(GPIOB, &gpinit); + + adc_start(); + + int num = 2000; + int j = 0; + volatile uint32_t time_us[num]; + + while (1) + { + i++; + + //g_ADCValue = accumulate(g_ADCBuffer,ADC_BUFFER_LENGTH) / ADC_BUFFER_LENGTH; + //HAL_Delay(100); + int g_ADCValue = adc_read(ADC_CHANNEL_0); + int g_ADCValue1 = adc_read(ADC_CHANNEL_1); + int g_ADCValue12 = adc_read(ADC_CHANNEL_12); + + int offTime = g_ADCValue; + int onTime = 4096 - offTime; + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5,GPIO_PIN_SET); + for (int i = 0; i < onTime; i++) + { + asm("nop"); + } + HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5,GPIO_PIN_RESET); + for (int i = 0; i < offTime; i++) + { + asm("nop"); + } + + //Get time in microseconds + if(j < num) + time_us[j++] = clock_get_us(); + } for(;;); } -/** - * @brief System Clock Configuration - * The system Clock is configured as follow : - * System Clock source = PLL (HSI) - * SYSCLK(Hz) = 100000000 - * HCLK(Hz) = 100000000 - * AHB Prescaler = 1 - * APB1 Prescaler = 2 - * APB2 Prescaler = 1 - * HSI Frequency(Hz) = 16000000 - * PLL_M = 16 - * PLL_N = 400 - * PLL_P = 4 - * PLL_Q = 7 - * VDD(V) = 3.3 - * Main regulator output voltage = Scale2 mode - * Flash Latency(WS) = 3 - * @param None - * @retval None - */ -static void SystemClock_Config(void) -{ - RCC_ClkInitTypeDef RCC_ClkInitStruct; - RCC_OscInitTypeDef RCC_OscInitStruct; - - /* Enable Power Control clock */ - __HAL_RCC_PWR_CLK_ENABLE(); - - /* The voltage scaling allows optimizing the power consumption when the device is - clocked below the maximum system frequency, to update the voltage scaling value - regarding system frequency refer to product datasheet. */ - __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2); - - /* Enable HSI Oscillator and activate PLL with HSI as source */ - RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI; - RCC_OscInitStruct.HSIState = RCC_HSI_ON; - RCC_OscInitStruct.HSICalibrationValue = 0x10; - RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; - RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; - RCC_OscInitStruct.PLL.PLLM = 16; - RCC_OscInitStruct.PLL.PLLN = 400; - RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV4; - RCC_OscInitStruct.PLL.PLLQ = 7; - if(HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) - { - Error_Handler(); - } - - /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 - clocks dividers */ - RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2); - RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; - RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; - RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2; - RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1; - if(HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK) - { - Error_Handler(); - } -} - - - - - -/** - * @brief This function is executed in case of error occurrence. - * @param None - * @retval None - */ -static void Error_Handler(void) -{ - while(1) - { - } -} diff --git a/UAV-ControlSystem/src/utilities.c b/UAV-ControlSystem/src/utilities.c new file mode 100644 index 0000000..a125951 --- /dev/null +++ b/UAV-ControlSystem/src/utilities.c @@ -0,0 +1,42 @@ +/* + * utilities.c + /********************************************************************** + * NAME: utilities.c * + * AUTHOR: Philip Johansson * + * PURPOSE: Set up and read from ADC * + * INFORMATION: * + * Here we gather usable functions by the whole system * + * * + * GLOBAL VARIABLES: * + * Variable Type Description * + * -------- ---- ----------- * + * * + **********************************************************************/ + +#include "utilities.h" + +/*********************************************************************** +* BRIEF: Sums elements of array until index of second arg * +* INFORMATION: Returns the sum * +***********************************************************************/ +uint32_t accumulate(uint32_t * list, int length) +{ + int value = 0; + for (int i = 0; i < length; i++) + { + value += list[i]; + } + return value; +} + +/*********************************************************************** +* BRIEF: A function that can be called on exceptions * +* INFORMATION: If an exception happens its easier to find it in an * +* infinite loop * +***********************************************************************/ +void Error_Handler(void) +{ + while(1) + { + } +} diff --git a/revolution_hal_lib/.cproject b/revolution_hal_lib/.cproject index 838ba5a..e0849f1 100644 --- a/revolution_hal_lib/.cproject +++ b/revolution_hal_lib/.cproject @@ -41,6 +41,9 @@ + + + diff --git a/revolution_hal_lib/.settings/language.settings.xml b/revolution_hal_lib/.settings/language.settings.xml deleted file mode 100644 index 04c131c..0000000 --- a/revolution_hal_lib/.settings/language.settings.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/revolution_hal_lib/Debug/.gitignore b/revolution_hal_lib/Debug/.gitignore deleted file mode 100644 index 26f9e50..0000000 --- a/revolution_hal_lib/Debug/.gitignore +++ /dev/null @@ -1,5 +0,0 @@ -/HAL_Driver/ -/makefile -/objects.list -/objects.mk -/sources.mk