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