From 0d3b5856250734fe3ae5c7a7f2c6425b0ff33662 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 12 Sep 2016 14:02:24 +0200 Subject: [PATCH 01/18] Adding scheduler files --- UAV-ControlSystem/.cproject | 11 ++++++- UAV-ControlSystem/inc/Scheduler/scheduler.h | 35 +++++++++++++++++++++ UAV-ControlSystem/inc/Scheduler/tasks.h | 13 ++++++++ UAV-ControlSystem/src/Scheduler/scheduler.c | 8 +++++ UAV-ControlSystem/src/Scheduler/tasks.c | 8 +++++ 5 files changed, 74 insertions(+), 1 deletion(-) create mode 100644 UAV-ControlSystem/inc/Scheduler/scheduler.h create mode 100644 UAV-ControlSystem/inc/Scheduler/tasks.h create mode 100644 UAV-ControlSystem/src/Scheduler/scheduler.c create mode 100644 UAV-ControlSystem/src/Scheduler/tasks.c diff --git a/UAV-ControlSystem/.cproject b/UAV-ControlSystem/.cproject index 951e1b5..7f73de9 100644 --- a/UAV-ControlSystem/.cproject +++ b/UAV-ControlSystem/.cproject @@ -41,6 +41,7 @@ + @@ -200,5 +201,13 @@ - + + + + + + + + + diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h new file mode 100644 index 0000000..5b7a665 --- /dev/null +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -0,0 +1,35 @@ +/************************************************************************** +* NAME: scheduler.h * +* PURPOSE: Defining the the scheduler to be used in the system to organize* +* the runtime for the tasks in the system based on priority. * +* INFORMATION: * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ + +#ifndef SCHEDULER_H_ +#define SCHEDULER_H_ + +#include + +/* Enum containing all the possible task priorities */ +typedef enum +{ + IDLE = 0, + LOW, + MEDIUM, + HIGH, + REALTIME, + MAX_PRIORITY = 255 +} taskPriority_t; + +typedef struct +{ + const char * taskName; /* Name of the task */ + const char * subTaskName; /*Needed?*/ + +}task_t; + + +#endif /* SCHEDULER_H_ */ diff --git a/UAV-ControlSystem/inc/Scheduler/tasks.h b/UAV-ControlSystem/inc/Scheduler/tasks.h new file mode 100644 index 0000000..bc6dacc --- /dev/null +++ b/UAV-ControlSystem/inc/Scheduler/tasks.h @@ -0,0 +1,13 @@ +/* + * tasks.h + * + * Created on: 12 sep. 2016 + * Author: holmis + */ + +#ifndef SCHEDULER_TASKS_H_ +#define SCHEDULER_TASKS_H_ + + + +#endif /* SCHEDULER_TASKS_H_ */ diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c new file mode 100644 index 0000000..fb63ebf --- /dev/null +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -0,0 +1,8 @@ +/* + * scheduler.c + * + * Created on: 12 sep. 2016 + * Author: holmis + */ + + diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c new file mode 100644 index 0000000..03a9d74 --- /dev/null +++ b/UAV-ControlSystem/src/Scheduler/tasks.c @@ -0,0 +1,8 @@ +/* + * tasks.c + * + * Created on: 12 sep. 2016 + * Author: holmis + */ + + From a70c874cca123d0fe85ee1be89821f6ccc1352cd Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 13 Sep 2016 13:57:33 +0200 Subject: [PATCH 02/18] Added all the files for scheduler and some code Some code and structure for the scheduler. --- UAV-ControlSystem/inc/Scheduler/scheduler.h | 105 +++++++++++++++- UAV-ControlSystem/inc/Scheduler/tasks.h | 15 ++- UAV-ControlSystem/src/Scheduler/scheduler.c | 132 +++++++++++++++++++- UAV-ControlSystem/src/Scheduler/tasks.c | 17 +-- UAV-ControlSystem/src/main.c | 3 + 5 files changed, 251 insertions(+), 21 deletions(-) diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index 5b7a665..c3d71eb 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -11,8 +11,11 @@ #ifndef SCHEDULER_H_ #define SCHEDULER_H_ +#include #include + + /* Enum containing all the possible task priorities */ typedef enum { @@ -24,12 +27,110 @@ typedef enum MAX_PRIORITY = 255 } taskPriority_t; + +/* Struct used to contain the information for each task */ typedef struct { - const char * taskName; /* Name of the task */ - const char * subTaskName; /*Needed?*/ + /* Basic task information */ + const char * taskName; /* Name of the task */ + const char * subTaskName; /*Needed?*/ + bool (*checkEventTriggered)(uint32_t currentDeltaTime); /* Function pointer to event trigger check, if used no standard scheduling */ + void (*taskFunction)(void); /* Pointer to the function that should be called to run the task */ + uint32_t desiredPeriod; /* The period the task wants to run in */ + const uint8_t staticPriority; /* Value used to increment the dynamic priority */ + + /* Scheduling variables */ + uint16_t dynamicPriority; /* Priority increases the longer the task have been idle, increased by staticPriority value */ + uint16_t taskAgeCycle; /* Helps to keep track of the "periods" for the task */ + uint32_t lastExecutedAt; /* last time of invocation */ + uint32_t lastSignaledAt; /* time of invocation event for event-driven tasks */ + + /* ToDo: potential statistic values of the task */ }task_t; +/* Task counter, ToDo: If more tasks are needed add them here first, defines are used to make sure the task is somewhere in the system */ +/* Important: If a new define for a task is added here it MUST be added in the tasks.c */ +typedef enum +{ + /* All the tasks that will be in the system, some are separated by IfDef */ + TASK_SYSTEM = 0, + TASK_GYROPID, + TASK_ACCELEROMETER, + TASK_ATTITUDE, + TASK_RX, + TASK_SERIAL, + TASK_BATTERY, +#ifdef BARO + TASK_BARO, +#endif +#ifdef COMPASS + TASK_COMPASS, +#endif +#ifdef GPS + TASK_GPS, +#endif +#ifdef SONAR + TASK_SONAR, +#endif +#if defined(BARO) || defined(SONAR) + TASK_ALTITUDE, +#endif +#if BEEPER + TASK_BEEPER +#endif + + /* Need to be the value directly after the tasks id. Keeps track of the count of the tasks */ + TASK_COUNT, + + /* Service task IDs */ + TASK_NONE = TASK_COUNT, + TASK_SELF +}taskId_t; + +/* Variables --------------------------------------------------------------------------------- */ + + + +/* Functions that operate can operate on the tasks ------------------------------------------- */ + +/************************************************************************** +* BRIEF: Enables or disables a specific task in the system * +* INFORMATION: Takes the id of the task(task_t) along with a true or * +* false value. If true the task will be added to the task queue, if false * +* it will be removed. * +**************************************************************************/ +void enableTask(taskId_t taskId, bool enabled); + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +uint32_t getTaskDeltaTime(taskId_t taskId); + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros); + + +/* Functions that handle the scheduler ------------------------------------------------------- */ + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +void scheduler(void); + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +void initScheduler(void); + + + + #endif /* SCHEDULER_H_ */ diff --git a/UAV-ControlSystem/inc/Scheduler/tasks.h b/UAV-ControlSystem/inc/Scheduler/tasks.h index bc6dacc..e82df8e 100644 --- a/UAV-ControlSystem/inc/Scheduler/tasks.h +++ b/UAV-ControlSystem/inc/Scheduler/tasks.h @@ -1,9 +1,12 @@ -/* - * tasks.h - * - * Created on: 12 sep. 2016 - * Author: holmis - */ +/************************************************************************** +* NAME: tasks.h * +* PURPOSE: Defining the the scheduler to be used in the system to organize* +* the runtime for the tasks in the system based on priority. * +* INFORMATION: * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ #ifndef SCHEDULER_TASKS_H_ #define SCHEDULER_TASKS_H_ diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index fb63ebf..1d24e13 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -1,8 +1,128 @@ -/* - * scheduler.c - * - * Created on: 12 sep. 2016 - * Author: holmis - */ +/************************************************************************** +* NAME: Scheduler.c * +* PURPOSE: Defining the the scheduler to be used in the system to organize* +* the runtime for the tasks in the system based on priority. * +* INFORMATION: * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ + +#include "Scheduler/scheduler.h" + +static uint32_t tasksReadyToRun; + +uint32_t currentTime = 0; +uint16_t averageSystmeLoad = 0; + +static int taskQueuePos = 0; +static int taskQueueSize = 0; + +static task_t * taskReadyQueue[TASK_COUNT + 1]; /* Array holding all the tasks that are ready to run in the system */ + + +/* Functions to operate on the task queue ------------------------------------------------------- */ + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +void taskReadyQueueClear(void) +{ + +} + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +bool taskReadyQueueContains(task_t *task) +{ + //Go through the taskReadyQueue to see if the current task is contained + for (int i = 0; i < taskQueueSize; i++) + { + if (taskReadyQueue[i] == task) + { + return true; + } + } + + return false; +} + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +bool taskReadyQueueAdd(task_t *task) +{ + +} + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +bool taskReadyQueueRemove(task_t *task) +{ + +} + + +/************************************************************************** +* BRIEF: Enables or disables a specific task in the system * +* INFORMATION: Takes the id of the task(task_t) along with a true or * +* false value. If true the task will be added to the task queue, if false * +* it will be removed. * +**************************************************************************/ +void enableTask(taskId_t taskId, bool enabled) +{ + +} + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +uint32_t getTaskDeltaTime(taskId_t taskId) +{ + +} + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros) +{ + +} + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +void initScheduler(void) +{ + +} + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +void scheduler(void) +{ + +} + + + + + + + + + diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c index 03a9d74..f2ca5a8 100644 --- a/UAV-ControlSystem/src/Scheduler/tasks.c +++ b/UAV-ControlSystem/src/Scheduler/tasks.c @@ -1,8 +1,11 @@ -/* - * tasks.c - * - * Created on: 12 sep. 2016 - * Author: holmis - */ - +/************************************************************************** +* NAME: tasks.c * +* PURPOSE: Defining the the scheduler to be used in the system to organize* +* the runtime for the tasks in the system based on priority. * +* INFORMATION: * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ +#include "Scheduler/tasks.h" diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index e4f449e..560e48e 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -10,6 +10,7 @@ #include "stm32f4xx.h" +#include "stm32f4xx_revo.h" /* Private function prototypes -----------------------------------------------*/ static void SystemClock_Config(void); @@ -21,6 +22,7 @@ int main(void) int i = 1; + TIM_Base_InitTypeDef t; /* Configure the system clock to 100 MHz */ SystemClock_Config(); @@ -35,6 +37,7 @@ int main(void) 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; //GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; From 8eda8ba66d8b267bd8b2077db9aa05488ba1aae5 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Thu, 15 Sep 2016 16:57:22 +0200 Subject: [PATCH 03/18] Added some more code to the scheduler Added more functionality to the scheduler. It should be able to select the task that should run and soon be able to create tasks. Not fully finished but its not that much left to add --- UAV-ControlSystem/inc/Scheduler/scheduler.h | 14 +- UAV-ControlSystem/src/Scheduler/scheduler.c | 165 ++++++++++++++++++-- 2 files changed, 166 insertions(+), 13 deletions(-) diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index c3d71eb..db8c95e 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -2,7 +2,7 @@ * NAME: scheduler.h * * PURPOSE: Defining the the scheduler to be used in the system to organize* * the runtime for the tasks in the system based on priority. * -* INFORMATION: * +* INFORMATION: Many elements and ideas obtained from beta/cleanflight * * GLOBAL VARIABLES: * * Variable Type Description * * -------- ---- ----------- * @@ -45,7 +45,13 @@ typedef struct uint32_t lastExecutedAt; /* last time of invocation */ uint32_t lastSignaledAt; /* time of invocation event for event-driven tasks */ - /* ToDo: potential statistic values of the task */ + /* Statistic values of the task */ + uint32_t averageExecutionTime; // Moving average over 6 samples, used to calculate guard interval + uint32_t taskLatestDeltaTime; // +#ifndef SKIP_TASK_STATISTICS + uint32_t maxExecutionTime; + uint32_t totalExecutionTime; // total time consumed by task since boot +#endif }task_t; @@ -90,7 +96,9 @@ typedef enum }taskId_t; /* Variables --------------------------------------------------------------------------------- */ - +extern task_t SystemTasks[TASK_COUNT]; /* All the tasks that exist in the system */ +extern uint16_t CpuLoad; /* The current load on the cpu Todo */ +extern uint16_t averageSystemLoadPercent; /* The average load on the system Todo */ /* Functions that operate can operate on the tasks ------------------------------------------- */ diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index 1d24e13..adb09a0 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -9,16 +9,23 @@ ***************************************************************************/ #include "Scheduler/scheduler.h" +#include "stm32f4xx_revo.h" -static uint32_t tasksReadyToRun; + +//Todo: change for several instances where 0 is used as null to the define value NULL + +static task_t *currentTask = 0; + +static uint32_t totalSchedulerPasses; /* Number of times the scheduler has been invoked */ +static uint32_t totalSchedulerReadyTasks; /* The amount of tasks that totally has been ready in the scheduler */ uint32_t currentTime = 0; uint16_t averageSystmeLoad = 0; -static int taskQueuePos = 0; -static int taskQueueSize = 0; +static int taskReadyQueuePos = 0; +static int taskReadyQueueSize = 0; -static task_t * taskReadyQueue[TASK_COUNT + 1]; /* Array holding all the tasks that are ready to run in the system */ +static task_t * taskReadyQueue[TASK_COUNT + 1]; /* Array holding all the tasks that are ready to run in the system + 1 for a NULL value at the end*/ /* Functions to operate on the task queue ------------------------------------------------------- */ @@ -29,7 +36,10 @@ static task_t * taskReadyQueue[TASK_COUNT + 1]; /* Array holding all the tasks **************************************************************************/ void taskReadyQueueClear(void) { - + //Empties the queue by settin all the positions to 0(NULL) + memset(taskReadyQueue, 0, sizeof(taskReadyQueue)); + taskReadyQueuePos = 0; + taskReadyQueueSize = 0; } /************************************************************************** @@ -39,7 +49,7 @@ void taskReadyQueueClear(void) bool taskReadyQueueContains(task_t *task) { //Go through the taskReadyQueue to see if the current task is contained - for (int i = 0; i < taskQueueSize; i++) + for (int i = 0; i < taskReadyQueueSize; i++) { if (taskReadyQueue[i] == task) { @@ -56,7 +66,60 @@ bool taskReadyQueueContains(task_t *task) **************************************************************************/ bool taskReadyQueueAdd(task_t *task) { + int i; /* Iterator */ + int j; /* Inner iterator used to sort based on desired period */ + //Check if the task is in the queue already + if (taskReadyQueueContains(task) || taskReadyQueueSize >= TASK_COUNT) + { + return false; + } + + //Try to add the task to the taskReadyQueue at a good position + //Order is: staticPriority > desired period > all else + //<= taskReadyQueueSize because if we go through the entire thing we should get a null value + //representing the position directly after the last one in the queue. + for (i = 0; i < taskReadyQueueSize; i++) + { + //check if the static prio of the current position in the queue is less than the task we want to add + if(taskReadyQueue[i]->staticPriority < task->staticPriority) + { + //order tasks based on lowest period first, within the ones with the same priority + //ToDo: check if setting j to i-1 initially will produce the same result, in that case add another iterator to get item from taskReadyQueue + for (j = i; taskReadyQueue[j]->staticPriority == task->staticPriority; j++ ) + { + //If the new task has a shorter period place in front + if (task->desiredPeriod <= taskReadyQueue[j]->desiredPeriod) + { + //remove one increment because otherwise it will be wrong + j--; + } + } + //task does not have shorter period than any other with the same priority, add behind them + j++; + memmove(&taskReadyQueue[j+1], &taskReadyQueue[j], sizeof(task) * (taskReadyQueueSize - j)); + taskReadyQueue[j] = task; + ++taskReadyQueueSize; + return true; + } + + } + //If this is true it needs to be placed at the very back of the queue + if (taskReadyQueue[i] == NULL ) + { + //if taskReadyQueueSize > 0 we need to increment the counter i + if (taskReadyQueueSize > 0) + { + i++; + } + + taskReadyQueue[i] = task; //ToDo: check if this works correctly, that a new NULL value will exist at i+1 + return true; + } + + + //Could not add the task in the anywhere in the queue + return false; } /************************************************************************** @@ -65,7 +128,14 @@ bool taskReadyQueueAdd(task_t *task) **************************************************************************/ bool taskReadyQueueRemove(task_t *task) { - + for (int i = 0; i < taskReadyQueueSize; ++i) { + if (taskReadyQueue[i] == task) { + memmove(&taskReadyQueue[i], &taskReadyQueue[i+1], sizeof(task) * (taskReadyQueueSize - i)); + --taskReadyQueueSize; + return true; + } + } + return false; } @@ -77,7 +147,14 @@ bool taskReadyQueueRemove(task_t *task) **************************************************************************/ void enableTask(taskId_t taskId, bool enabled) { - + if (taskId == TASK_SELF || taskId < TASK_COUNT) { + task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId]; + if (enabled && task->taskFunction) { + taskReadyQueueAdd(task); + } else { + taskReadyQueueRemove(task); + } + } } /************************************************************************** @@ -86,7 +163,12 @@ void enableTask(taskId_t taskId, bool enabled) **************************************************************************/ uint32_t getTaskDeltaTime(taskId_t taskId) { - + if (taskId == TASK_SELF || taskId < TASK_COUNT) { + task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId]; + return task->taskLatestDeltaTime; + } else { + return 0; + } } /************************************************************************** @@ -104,7 +186,9 @@ void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros) **************************************************************************/ void initScheduler(void) { - + /*Todo: what needs to be init for the scheduler, should all the tasks + * be added to the readyTasksqueue here or should it be done at in + * another place in the code. */ } /************************************************************************** @@ -113,6 +197,67 @@ void initScheduler(void) **************************************************************************/ void scheduler(void) { + /*ToDo: implement the function micros that should give the current + * time in microseconds. Not entirely sure how it should be obtained, + * needs to be looked into and solved otherwise no time can be gotten */ + currentTime = micros(); + task_t * taskToRun; + uint32_t currentDynamicPriority = 0; + uint32_t currentReadyTasks = 0; + float currentAgeCyckleExact = 0; + float ageCycleExact = 0; + + /* Check if a task has entered a new period and assign its new dynamic priority */ + /* Go through all the tasks to check if they should be able to run */ + for (int i = 0; i < taskReadyQueueSize; i++) + { + //Get the next task in the queue + taskToRun = taskReadyQueue[i]; + + /* Check if the task is an event, else its a time controlled task */ + if (taskToRun->checkEventTriggered != NULL) + { + //ToDO: Handle event tasks, they should get to operate at a higher priority + + } + else //Handle time controlled tasks + { + //Calculate a new age cycle for the task. It represents if the task is in a new "period" in relation to its last execution + //If the value is 0 it is still in the same "period" as the last execution + taskToRun->taskAgeCycle = (currentTime - taskToRun->lastExecutedAt) / taskToRun->desiredPeriod; + + //May not be used. Could be used to check what task is closest to its next period instance + ageCycleExact = ((float)currentTime - (float)taskToRun->lastExecutedAt) / (float)taskToRun->desiredPeriod; + + //Check if > 0, if it has entered a new "period" + if (taskToRun->taskAgeCycle > 0) + { + //Calculate the new dynamic priority for the task + taskToRun->dynamicPriority = 1 + taskToRun->taskAgeCycle * taskToRun->staticPriority; + currentReadyTasks ++; //Increase the number of ready tasks + + } + } + + //If the current task has the largest dynamic priority so far it could be a candidate for execution + if(taskToRun->dynamicPriority > currentDynamicPriority) + { + + } + else if(taskToRun->dynamicPriority == currentDynamicPriority) //The comparing tasks have the same priority + { + //if the tasks have the same priority check who is closest to its deadline and run that one + if (ageCycleExact > currentAgeCyckleExact) + { + //If the ageCycleExact is larger than the one of the current task we will change the priority of the one to run + + } + + } + + } /* for-loop end */ + + } From f2c39aecec9f2dac289b9c40a38291fef0f4f02f Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Fri, 16 Sep 2016 10:59:12 +0200 Subject: [PATCH 04/18] Just little just to change branch. --- UAV-ControlSystem/src/Scheduler/scheduler.c | 46 +++++++++++++++++---- UAV-ControlSystem/src/main.c | 1 + 2 files changed, 38 insertions(+), 9 deletions(-) diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index adb09a0..d9b9aad 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -202,10 +202,12 @@ void scheduler(void) * needs to be looked into and solved otherwise no time can be gotten */ currentTime = micros(); task_t * taskToRun; - uint32_t currentDynamicPriority = 0; + task_t * selectedTask; + uint16_t currentDynamicPriority = 0; uint32_t currentReadyTasks = 0; float currentAgeCyckleExact = 0; float ageCycleExact = 0; + bool isRealTime = false; /* Check if a task has entered a new period and assign its new dynamic priority */ /* Go through all the tasks to check if they should be able to run */ @@ -240,24 +242,50 @@ void scheduler(void) } //If the current task has the largest dynamic priority so far it could be a candidate for execution - if(taskToRun->dynamicPriority > currentDynamicPriority) + if(taskToRun->dynamicPriority >= currentDynamicPriority) { - - } - else if(taskToRun->dynamicPriority == currentDynamicPriority) //The comparing tasks have the same priority - { - //if the tasks have the same priority check who is closest to its deadline and run that one - if (ageCycleExact > currentAgeCyckleExact) + bool changeTask = false; //flag used to check if the task should be updated + /* If a realtime task is found the bool isRealTime will be set to true + * meaning no other tasks than realtime will be able to enter the function */ + if (taskToRun->staticPriority == REALTIME) //if it is a realtime task { - //If the ageCycleExact is larger than the one of the current task we will change the priority of the one to run + isRealTime = true; //toggle this flag so that no non realtime task can be selected this cycle + changeTask = true; + } + else if (!isRealTime) //If task is not realtime + { + if (taskToRun->dynamicPriority == currentDynamicPriority) //if the tasks have the same priority + { + if (ageCycleExact > currentAgeCyckleExact) //if the current tasks deadline is closer than selected task + { + changeTask = true; + currentAgeCyckleExact = ageCycleExact; //update the value + } + } + else + { + changeTask = true; + } } + if (changeTask == true) //if the task should change + { + selectedTask = taskToRun; //update the selected task + currentDynamicPriority = taskToRun->dynamicPriority; //update the current highest dynamic priority + } } } /* for-loop end */ + //Increase the + totalSchedulerReadyTasks += currentReadyTasks; + totalSchedulerPasses ++; + + + currentTask = selectedTask; + } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 560e48e..f6a143d 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -38,6 +38,7 @@ int main(void) 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; //GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; From 8181a131220eac9997e2c8bf9b05a5f31f38801f Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 19 Sep 2016 09:19:33 +0200 Subject: [PATCH 05/18] Small save commit Added more functionality to the scheduler, and also started to define some tasks. --- UAV-ControlSystem/inc/Scheduler/tasks.h | 15 +++- UAV-ControlSystem/src/Scheduler/scheduler.c | 27 ++++++- UAV-ControlSystem/src/Scheduler/tasks.c | 86 +++++++++++++++++++++ 3 files changed, 125 insertions(+), 3 deletions(-) diff --git a/UAV-ControlSystem/inc/Scheduler/tasks.h b/UAV-ControlSystem/inc/Scheduler/tasks.h index e82df8e..e7635db 100644 --- a/UAV-ControlSystem/inc/Scheduler/tasks.h +++ b/UAV-ControlSystem/inc/Scheduler/tasks.h @@ -11,6 +11,19 @@ #ifndef SCHEDULER_TASKS_H_ #define SCHEDULER_TASKS_H_ - +//All the task functions in the system, one for each task +void systemTaskSystem(void); +void systemTaskGyroPid(void); +void systemTaskAccelerometer(void); +void systemTaskAttitude(void); +void systemTaskRx(void); +void systemTaskSerial(void); +void systemTaskBattery(void); +void systemTaskBaro(void); +void systemTaskCompass(void); +void systemTaskGps(void); +void systemTaskSonar(void); +void systemTaskAltitude(void); +void systemTaskBeeper(void); #endif /* SCHEDULER_TASKS_H_ */ diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index d9b9aad..8b4b86d 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -177,7 +177,7 @@ uint32_t getTaskDeltaTime(taskId_t taskId) **************************************************************************/ void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros) { - + //ToDo: add implementation if necessary } /************************************************************************** @@ -189,6 +189,11 @@ void initScheduler(void) /*Todo: what needs to be init for the scheduler, should all the tasks * be added to the readyTasksqueue here or should it be done at in * another place in the code. */ + //Clear the queue + taskReadyQueueClear(); + + //add the basic system task + taskReadyQueueAdd(&SystemTasks[TASK_SYSTEM]); } /************************************************************************** @@ -283,9 +288,27 @@ void scheduler(void) totalSchedulerReadyTasks += currentReadyTasks; totalSchedulerPasses ++; - + //Assign the selected task to the pointer for the current task currentTask = selectedTask; + //If we have found a task to run, execute the function that the task is responsible for + if (selectedTask != NULL) + { + selectedTask->taskLatestDeltaTime = currentTime - selectedTask->lastExecutedAt; //calculate the time between now and last execution + selectedTask->lastExecutedAt = currentTime; //update the last execution time to this moment + selectedTask->dynamicPriority = 0; //reset the dynamic priority to 0 + + //handle the execution of the tasks function + const uint32_t timeBeforeExecution = micros(); + selectedTask->taskFunction(); //Run the function associated with the current task + const uint32_t TimeAfterExecution = micros(); + const uint32_t taskExecutionTime = TimeAfterExecution - timeBeforeExecution; //calculate the execution time of the task + + //Save statistical values + selectedTask->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32; + selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task + selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime); + } } diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c index f2ca5a8..17ec7a3 100644 --- a/UAV-ControlSystem/src/Scheduler/tasks.c +++ b/UAV-ControlSystem/src/Scheduler/tasks.c @@ -9,3 +9,89 @@ ***************************************************************************/ #include "Scheduler/tasks.h" +#include "Scheduler/scheduler.h" + +//initiate the values for all the tasks +//This information needs to be changed when functionality and setting for the tasks needs to be changed +task_t SystemTasks[TASK_COUNT] = + { + + [TASK_SYSTEM] = + { + + }, + + [TASK_GYROPID] = + { + + }, + + [TASK_ACCELEROMETER] = + { + + }, + + [TASK_ATTITUDE] = + { + + }, + + [TASK_RX] = + { + + }, + + [TASK_SERIAL] = + { + + }, + + [TASK_BATTERY] = + { + + }, + +#ifdef BARO + [TASK_BARO] = + { + + }, +#endif + +#ifdef COMPASS + [TASK_COMPASS] = + { + + }, +#endif + +#ifdef GPS + [TASK_GPS] = + { + + }, +#endif + +#ifdef SONAR + [TASK_SONAR] = + { + + }, +#endif + +#if defined(BARO) || defined(SONAR) + [TASK_ALTITUDE] = + { + + }, +#endif + +#if BEEPER + [TASK_BEEPER] = + { + + }, +#endif + + +}; From 7d00a65cd545fbcf5e1c9f5a33829546a433b963 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 19 Sep 2016 13:24:48 +0200 Subject: [PATCH 06/18] Save commit Small save commit for temporary branch change. Added some task definitions. --- UAV-ControlSystem/inc/Scheduler/scheduler.h | 14 ++-- UAV-ControlSystem/inc/Scheduler/tasks.h | 3 + UAV-ControlSystem/inc/stm32f4xx_revo.h | 12 +++ UAV-ControlSystem/src/Scheduler/scheduler.c | 5 +- UAV-ControlSystem/src/Scheduler/tasks.c | 93 +++++++++++++++------ 5 files changed, 93 insertions(+), 34 deletions(-) diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index db8c95e..92cab4c 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -13,18 +13,18 @@ #include #include - +#include "stm32f4xx_revo.h" /* Enum containing all the possible task priorities */ typedef enum { - IDLE = 0, - LOW, - MEDIUM, - HIGH, - REALTIME, - MAX_PRIORITY = 255 + PRIORITY_IDLE = 0, + PRIORITY_LOW, + PRIORITY_MEDIUM, + PRIORITY_HIGH, + PRIORITY_REALTIME, + PRIORITY_MAX_PRIORITY = 255 } taskPriority_t; diff --git a/UAV-ControlSystem/inc/Scheduler/tasks.h b/UAV-ControlSystem/inc/Scheduler/tasks.h index e7635db..c6d33b2 100644 --- a/UAV-ControlSystem/inc/Scheduler/tasks.h +++ b/UAV-ControlSystem/inc/Scheduler/tasks.h @@ -11,12 +11,15 @@ #ifndef SCHEDULER_TASKS_H_ #define SCHEDULER_TASKS_H_ +#include + //All the task functions in the system, one for each task void systemTaskSystem(void); void systemTaskGyroPid(void); void systemTaskAccelerometer(void); void systemTaskAttitude(void); void systemTaskRx(void); +bool systemTaskRxCheck(uint32_t currentDeltaTime); void systemTaskSerial(void); void systemTaskBattery(void); void systemTaskBaro(void); diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index fa46876..21a1758 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -92,12 +92,24 @@ /* Baro */ +//#define BARO /* Compass */ +//#define COMPASS /* GPS */ +//#define GPS + + + /* Sonar */ +//#define SONAR + + + /* Beeper */ +//#define BEEPER + /* Define all the moter of the system, servos + extra */ diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index 8b4b86d..b6e19d5 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -8,8 +8,9 @@ * -------- ---- ----------- * ***************************************************************************/ -#include "Scheduler/scheduler.h" #include "stm32f4xx_revo.h" +#include "Scheduler/scheduler.h" +#include "Scheduler/tasks.h" //Todo: change for several instances where 0 is used as null to the define value NULL @@ -252,7 +253,7 @@ void scheduler(void) bool changeTask = false; //flag used to check if the task should be updated /* If a realtime task is found the bool isRealTime will be set to true * meaning no other tasks than realtime will be able to enter the function */ - if (taskToRun->staticPriority == REALTIME) //if it is a realtime task + if (taskToRun->staticPriority == PRIORITY_REALTIME) //if it is a realtime task { isRealTime = true; //toggle this flag so that no non realtime task can be selected this cycle diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c index 17ec7a3..bff30e2 100644 --- a/UAV-ControlSystem/src/Scheduler/tasks.c +++ b/UAV-ControlSystem/src/Scheduler/tasks.c @@ -11,86 +11,129 @@ #include "Scheduler/tasks.h" #include "Scheduler/scheduler.h" +#define GetUpdateRateHz(x) (1000000 / x) //x is the desired hz value given in micro seconds //initiate the values for all the tasks //This information needs to be changed when functionality and setting for the tasks needs to be changed +// - Note: desiredPeriod is given in micro seconds, writing for example (1000000 / 10) means 10 hz rate task_t SystemTasks[TASK_COUNT] = { [TASK_SYSTEM] = { - + .taskName = "SYSTEM", + .taskFunction = systemTaskSystem, + .desiredPeriod = GetUpdateRateHz(10), //10 hz update rate (100 ms) + .staticPriority = PRIORITY_HIGH, }, [TASK_GYROPID] = { - + .taskName = "PID", + .subTaskName = "GYRO", + .taskFunction = systemTaskGyroPid, + .desiredPeriod = GetUpdateRateHz(1000), //1000 hz update rate (1 ms) + .staticPriority = PRIORITY_REALTIME, }, [TASK_ACCELEROMETER] = { - + .taskName = "ACCELEROMTER", + .taskFunction = systemTaskAccelerometer, + .desiredPeriod = GetUpdateRateHz(1000), //1000 hz update rate (1 ms) + .staticPriority = PRIORITY_MEDIUM, }, [TASK_ATTITUDE] = { - + .taskName = "ATTITUDE", + .taskFunction = systemTaskAttitude, + .desiredPeriod = GetUpdateRateHz(100), //100 hz update rate (10 ms) + .staticPriority = PRIORITY_MEDIUM, }, [TASK_RX] = { - + .taskName = "RX", + .taskFunction = systemTaskRx, //Event handler function, will check if a message is obtainable + .checkEventTriggered = systemTaskRxCheck, + .desiredPeriod = GetUpdateRateHz(50), //Standard scheduling should not be used if event based, used as fail safe + .staticPriority = PRIORITY_HIGH, }, [TASK_SERIAL] = { - + .taskName = "SERIAL", + .taskFunction = systemTaskSerial, + .desiredPeriod = GetUpdateRateHz(100), //100 hz update rate (10 ms) + .staticPriority = PRIORITY_LOW, }, [TASK_BATTERY] = { - + .taskName = "BATTERY", + .taskFunction = systemTaskBattery, + .desiredPeriod = GetUpdateRateHz(50), + .staticPriority = PRIORITY_MEDIUM, }, #ifdef BARO [TASK_BARO] = { - + .taskName = "BAROMETER", + .taskFunction = systemTaskBaro, + .desiredPeriod = GetUpdateRateHz(20), + .staticPriority = PRIORITY_LOW, }, #endif #ifdef COMPASS [TASK_COMPASS] = { - + .taskName = "COMPASS", + .taskFunction = systemTaskCompass, + .desiredPeriod = GetUpdateRateHz(10), + .staticPriority = PRIORITY_LOW, }, #endif #ifdef GPS - [TASK_GPS] = - { - - }, + [TASK_GPS] = + { + .taskName = "GPS", + .taskFunction = systemTaskGps, + .desiredPeriod = GetUpdateRateHz(10), + .staticPriority = PRIORITY_MEDIUM, + }, #endif #ifdef SONAR - [TASK_SONAR] = - { - - }, + [TASK_SONAR] = + { + .taskName = "SONAR", + .taskFunction = systemTaskSonar, + .desiredPeriod = GetUpdateRateHz(20), + .staticPriority = PRIORITY_LOW, + }, #endif #if defined(BARO) || defined(SONAR) - [TASK_ALTITUDE] = - { - - }, + [TASK_ALTITUDE] = + { + .taskName = "ALTITUDE", + .taskFunction = systemTaskAltitude, + .desiredPeriod = GetUpdateRateHz(40), + .staticPriority = PRIORITY_LOW, + }, #endif #if BEEPER - [TASK_BEEPER] = - { - - }, + [TASK_BEEPER] = + { + .taskName = "BEEPER", + .taskFunction = systemTaskBeeper, + .desiredPeriod = GetUpdateRateHz(100), + .staticPriority = PRIORITY_LOW, + }, #endif From db9ec943302ee4105fec62ed59d89339ab5a56ed Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Tue, 20 Sep 2016 15:42:05 +0200 Subject: [PATCH 07/18] Added additional things in the scheduler The tasks should now be able to be added to the ready queue when the schedual init. --- UAV-ControlSystem/inc/Scheduler/scheduler.h | 6 +- UAV-ControlSystem/src/Scheduler/scheduler.c | 83 ++++++++++++++++++++- 2 files changed, 85 insertions(+), 4 deletions(-) diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index 92cab4c..2f2f901 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -138,7 +138,11 @@ void scheduler(void); **************************************************************************/ void initScheduler(void); - +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +void initSchedulerTasks(void); #endif /* SCHEDULER_H_ */ diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index b6e19d5..278c9b0 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -43,6 +43,7 @@ void taskReadyQueueClear(void) taskReadyQueueSize = 0; } + /************************************************************************** * BRIEF: * * INFORMATION: * @@ -61,6 +62,7 @@ bool taskReadyQueueContains(task_t *task) return false; } + /************************************************************************** * BRIEF: * * INFORMATION: * @@ -123,6 +125,7 @@ bool taskReadyQueueAdd(task_t *task) return false; } + /************************************************************************** * BRIEF: * * INFORMATION: * @@ -158,6 +161,7 @@ void enableTask(taskId_t taskId, bool enabled) } } + /************************************************************************** * BRIEF: * * INFORMATION: * @@ -172,15 +176,68 @@ uint32_t getTaskDeltaTime(taskId_t taskId) } } + /************************************************************************** * BRIEF: * * INFORMATION: * **************************************************************************/ void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros) { - //ToDo: add implementation if necessary + if (taskId == TASK_SELF || taskId < TASK_COUNT) + { + task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId]; + task->desiredPeriod = MAX(100, newPeriodMicros); // Limit delay to 100us (10 kHz) to prevent scheduler clogging + } } + +/************************************************************************** +* BRIEF: * +* INFORMATION: * +**************************************************************************/ +void initSchedulerTasks(void) +{ + //Enable the tasks in the system + enableTask(TASK_SYSTEM, true); + + enableTask(TASK_GYROPID, true); + + enableTask(TASK_ACCELEROMETER, true); + + enableTask(TASK_ATTITUDE, true); + + enableTask(TASK_RX, true); + + enableTask(TASK_SERIAL, true); + + enableTask(TASK_BATTERY, true); + +#ifdef BARO + enableTask(TASK_BARO, true); +#endif + +#ifdef COMPASS + enableTask(TASK_COMPASS, true); +#endif + +#ifdef GPS + enableTask(TASK_GPS, true); +#endif + +#ifdef SONAR + enableTask(TASK_SONAR, true); +#endif + +#if defined(BARO) || defined(SONAR) + enableTask(TASK_ALTITUDE, true); +#endif + +#if BEEPER + enableTask(TASK_BEEPER, true); +#endif +} + + /************************************************************************** * BRIEF: * * INFORMATION: * @@ -193,10 +250,11 @@ void initScheduler(void) //Clear the queue taskReadyQueueClear(); - //add the basic system task - taskReadyQueueAdd(&SystemTasks[TASK_SYSTEM]); + //Init all the + initSchedulerTasks(); } + /************************************************************************** * BRIEF: * * INFORMATION: * @@ -226,6 +284,25 @@ void scheduler(void) if (taskToRun->checkEventTriggered != NULL) { //ToDO: Handle event tasks, they should get to operate at a higher priority + //ToDo: test if it works + if (taskToRun->dynamicPriority > 0) + { + taskToRun->taskAgeCycle = 1 + ((currentTime - taskToRun->lastSignaledAt) / taskToRun->desiredPeriod); + taskToRun->dynamicPriority = 1 + taskToRun->staticPriority * taskToRun->taskAgeCycle; + currentReadyTasks++; + } + else if (taskToRun->checkEventTriggered(currentTime - taskToRun->lastExecutedAt)) + { + taskToRun->lastSignaledAt = currentTime; + taskToRun->taskAgeCycle = 1; + taskToRun->dynamicPriority = 1 + taskToRun->staticPriority; + currentReadyTasks++; + } + else + { + taskToRun->taskAgeCycle = 0; + } + } else //Handle time controlled tasks From 3ff3219a6af30524bef8bfeba7c3f4db0ce4c5e1 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Wed, 21 Sep 2016 10:09:22 +0200 Subject: [PATCH 08/18] Very small only because of master update --- UAV-ControlSystem/src/tasks_main.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100644 UAV-ControlSystem/src/tasks_main.c diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c new file mode 100644 index 0000000..e180406 --- /dev/null +++ b/UAV-ControlSystem/src/tasks_main.c @@ -0,0 +1,16 @@ +/************************************************************************** +* NAME: tasks_main.c * +* PURPOSE: Holds the function implementations for the individual tasks +* that are invoked when a task is executed in the scheduler. Each task +* needs to have an associated function that has to be invoked when it is +* chosen as the task to run. Additionally optional event driven task +* functions must be implemented here as well. This file will include +* different drivers meaning that the task functions could jump around +* into other files before finishing its execution. +* INFORMATION: * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ + + From 049575f54f7e166b3c49fbb9c783ed84bf3bb13e Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Wed, 21 Sep 2016 10:59:09 +0200 Subject: [PATCH 09/18] Commit because update from master needed Added some functionallity to utilize debug tasks. Only to be used when testing the scheduler and never when the real system should run. --- UAV-ControlSystem/inc/Scheduler/scheduler.h | 7 ++ UAV-ControlSystem/inc/Scheduler/tasks.h | 7 ++ UAV-ControlSystem/inc/drivers/led.h | 13 +++ UAV-ControlSystem/inc/stm32f4xx_revo.h | 4 + UAV-ControlSystem/src/Scheduler/scheduler.c | 5 +- UAV-ControlSystem/src/Scheduler/tasks.c | 33 +++++++- UAV-ControlSystem/src/drivers/led.c | 8 ++ UAV-ControlSystem/src/main.c | 2 +- UAV-ControlSystem/src/tasks_main.c | 93 +++++++++++++++++++++ 9 files changed, 167 insertions(+), 5 deletions(-) create mode 100644 UAV-ControlSystem/inc/drivers/led.h create mode 100644 UAV-ControlSystem/src/drivers/led.c diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index 2f2f901..a058bba 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -87,6 +87,13 @@ typedef enum TASK_BEEPER #endif + //Debug tasks, ONLY to be used when testing +#ifdef USE_DEBUG_TASKS + TASK_DEBUG_1, + TASK_DEBUG_2, + TASK_DEBUG_3, +#endif + /* Need to be the value directly after the tasks id. Keeps track of the count of the tasks */ TASK_COUNT, diff --git a/UAV-ControlSystem/inc/Scheduler/tasks.h b/UAV-ControlSystem/inc/Scheduler/tasks.h index c6d33b2..69503e4 100644 --- a/UAV-ControlSystem/inc/Scheduler/tasks.h +++ b/UAV-ControlSystem/inc/Scheduler/tasks.h @@ -29,4 +29,11 @@ void systemTaskSonar(void); void systemTaskAltitude(void); void systemTaskBeeper(void); +//Tasks used only for testing purposes +#ifdef USE_DEBUG_TASKS +void systemTaskDebug_1(void); +void systemTaskDebug_2(void); +void systemTaskDebug_3(void); +#endif + #endif /* SCHEDULER_TASKS_H_ */ diff --git a/UAV-ControlSystem/inc/drivers/led.h b/UAV-ControlSystem/inc/drivers/led.h new file mode 100644 index 0000000..e344720 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/led.h @@ -0,0 +1,13 @@ +/* + * led.h + * + * Created on: 21 sep. 2016 + * Author: holmis + */ + +#ifndef DRIVERS_LED_H_ +#define DRIVERS_LED_H_ + + + +#endif /* DRIVERS_LED_H_ */ diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index 21a1758..6b11c39 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -91,6 +91,10 @@ #define MPU6000_SPI_INSTANCE SPI1 //Dont know if necessary for us to specify + /* Scheduler */ +#define USE_DEBUG_TASKS //Only to be used when testing scheduler, not when intending to run the whole system + + /* Baro */ //#define BARO diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index 278c9b0..581281b 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -11,6 +11,7 @@ #include "stm32f4xx_revo.h" #include "Scheduler/scheduler.h" #include "Scheduler/tasks.h" +#include "drivers/system_clock.h" //Todo: change for several instances where 0 is used as null to the define value NULL @@ -250,8 +251,8 @@ void initScheduler(void) //Clear the queue taskReadyQueueClear(); - //Init all the - initSchedulerTasks(); + //Init all the ToDo: add back when it is known that the scheduler works + //initSchedulerTasks(); } diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c index bff30e2..d690ce9 100644 --- a/UAV-ControlSystem/src/Scheduler/tasks.c +++ b/UAV-ControlSystem/src/Scheduler/tasks.c @@ -116,7 +116,7 @@ task_t SystemTasks[TASK_COUNT] = }, #endif -#if defined(BARO) || defined(SONAR) +#ifdef defined(BARO) || defined(SONAR) [TASK_ALTITUDE] = { .taskName = "ALTITUDE", @@ -126,7 +126,7 @@ task_t SystemTasks[TASK_COUNT] = }, #endif -#if BEEPER +#ifdef BEEPER [TASK_BEEPER] = { .taskName = "BEEPER", @@ -136,5 +136,34 @@ task_t SystemTasks[TASK_COUNT] = }, #endif + /* ----------------------------------------------------------------------------------------------------------------- */ + /* DEBUG ONLY TASKS, NOT TO BE INCLUDED WHEN RUNNING THE REAL SYSTEM ----------------------------------------------- */ +#ifdef USE_DEBUG_TASKS + + [TASK_DEBUG_1] = + { + .taskName = "DEBUG_1", + .taskFunction = systemTaskDebug_1, + .desiredPeriod = GetUpdateRateHz(100), + .staticPriority = PRIORITY_LOW, + }, + + [TASK_DEBUG_2] = + { + .taskName = "DEBUG_2", + .taskFunction = systemTaskDebug_2, + .desiredPeriod = GetUpdateRateHz(100), + .staticPriority = PRIORITY_LOW, + }, + + [TASK_DEBUG_3] = + { + .taskName = "DEBUG_3", + .taskFunction = systemTaskDebug_3, + .desiredPeriod = GetUpdateRateHz(100), + .staticPriority = PRIORITY_LOW, + }, + +#endif }; diff --git a/UAV-ControlSystem/src/drivers/led.c b/UAV-ControlSystem/src/drivers/led.c new file mode 100644 index 0000000..fca0fb8 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/led.c @@ -0,0 +1,8 @@ +/* + * led.c + * + * Created on: 21 sep. 2016 + * Author: holmis + */ + + diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 92fba26..275f49c 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -40,7 +40,7 @@ int main(void) //GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; //GPIO_InitStruct.Pull = GPIO_PULLUP; //GPIO_InitStruct.Speed = GPIO_SPEED_FAST; - HAL_GPIO_Init(GPIOA, &gpinit); + //HAL_GPIO_Init(GPIOA, &gpinit); // Configure the system clock to 100 MHz system_clock_config(); diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index e180406..26b9ae0 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -13,4 +13,97 @@ * -------- ---- ----------- * ***************************************************************************/ +#include "Scheduler/scheduler.h" +#include "Scheduler/tasks.h" +#include "stm32f4xx_revo.h" + +void systemTaskSystem(void) +{ + +} + +void systemTaskGyroPid(void) +{ + +} + +void systemTaskAccelerometer(void) +{ + +} + +void systemTaskAttitude(void) +{ + +} + +void systemTaskRx(void) +{ + +} + +bool systemTaskRxCheck(uint32_t currentDeltaTime) +{ + +} + +void systemTaskSerial(void) +{ + +} + +void systemTaskBattery(void) +{ + +} + +void systemTaskBaro(void) +{ + +} + +void systemTaskCompass(void) +{ + +} + +void systemTaskGps(void) +{ + +} + +void systemTaskSonar(void) +{ + +} + +void systemTaskAltitude(void) +{ + +} + +void systemTaskBeeper(void) +{ + +} + +/* TO BE USED ONLY WHEN TESTING/DEBUGIN TASK FUNCTIONALLITY, DONT USE WHEN RUNNING THE REAL SYSTEM!!!!!!!!!! */ +#ifdef USE_DEBUG_TASKS + +void systemTaskDebug_1(void) +{ + +} + +void systemTaskDebug_2(void) +{ + +} + +void systemTaskDebug_3(void) +{ + +} + +#endif /* End USE_DEBUG_TASKS */ From 7c37ef0b4795342ea0d673ba32a22369151f7e1e Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Thu, 22 Sep 2016 08:31:51 +0200 Subject: [PATCH 10/18] temp checkin --- UAV-ControlSystem/inc/drivers/I2C.h | 19 ++++++++++ UAV-ControlSystem/src/drivers/I2C.c | 56 +++++++++++++++++++++++++++++ 2 files changed, 75 insertions(+) create mode 100644 UAV-ControlSystem/inc/drivers/I2C.h create mode 100644 UAV-ControlSystem/src/drivers/I2C.c diff --git a/UAV-ControlSystem/inc/drivers/I2C.h b/UAV-ControlSystem/inc/drivers/I2C.h new file mode 100644 index 0000000..aed20ad --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/I2C.h @@ -0,0 +1,19 @@ +/* + * I2C.h + * + * Created on: 21 sep. 2016 + * Author: len12007 + */ + +#ifndef DRIVERS_I2C_H_ +#define DRIVERS_I2C_H_ + +#include "stm32f4xx.h" + + +void i2c_configure(void); + + + + +#endif /* DRIVERS_I2C_H_ */ diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c new file mode 100644 index 0000000..c2216ec --- /dev/null +++ b/UAV-ControlSystem/src/drivers/I2C.c @@ -0,0 +1,56 @@ +/* + * I2C.c + * + * Created on: 21 sep. 2016 + * Author: len12007 + */ + +#include "drivers/I2C.h" + + + + +void i2c_configure(uint32_t channel) +{ + + GPIO_InitTypeDef GPIO_InitStruct; + I2C_InitTypeDef I2C_InitStruct; + + // enable APB1 peripheral clock for I2C1 + RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); + // enable clock for SCL and SDA pins + RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); + + /* setup SCL and SDA pins + * You can connect I2C1 to two different + * pairs of pins: + * 1. SCL on PB6 and SDA on PB7 + * 2. SCL on PB8 and SDA on PB9 + */ + GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7; // we are going to use PB6 and PB7 + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; // set pins to alternate function + GPIO_InitStruct.Speed = GPIO_SPEED_LOW; // set GPIO speed +// GPIO_InitStruct.GPIO_OType = GPIO_OType_OD; // set output to open drain --> the line has to be only pulled low, not driven high + GPIO_InitStruct.Pull = GPIO_PULLUP; // enable pull up resistors + GPIO_InitStruct.Alternate = GPIO_AF4_I2C1; + GPIO_Init(GPIOB, &GPIO_InitStruct); // init GPIOB + + I2C_InitStruct.ClockSpeed = 100000; + I2C_InitStruct.DualAddressMode = + + // Connect I2C1 pins to AF + GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_I2C1); // SCL + GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_I2C1); // SDA + + // configure I2C1 + I2C_InitStruct.I2C_ClockSpeed = 100000; // 100kHz + I2C_InitStruct.I2C_Mode = I2C_Mode_I2C; // I2C mode + I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2; // 50% duty cycle --> standard + I2C_InitStruct.I2C_OwnAddress1 = 0x00; // own address, not relevant in master mode + I2C_InitStruct.I2C_Ack = I2C_Ack_Disable; // disable acknowledge when reading (can be changed later on) + I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; // set address length to 7 bit addresses + I2C_Init(I2C1, &I2C_InitStruct); // init I2C1 + + // enable I2C1 + I2C_Cmd(I2C1, ENABLE); +} From fc5ca4330c0088bf3966a33279756f550ca7eca4 Mon Sep 17 00:00:00 2001 From: Jonas Holmberg Date: Mon, 26 Sep 2016 08:53:50 +0200 Subject: [PATCH 11/18] Final adjustments Added the and adjusted the final part to the scheduler. Added ability to add task functionality that will run. Updated main file to its intended state. Further added led functinality along with led warnings. --- UAV-ControlSystem/inc/Scheduler/scheduler.h | 116 ++++++-- UAV-ControlSystem/inc/Scheduler/tasks.h | 24 +- UAV-ControlSystem/inc/drivers/led.h | 111 +++++++- UAV-ControlSystem/inc/stm32f4xx_revo.h | 16 +- UAV-ControlSystem/inc/system_variables.h | 1 - .../inc/utilities/math_helpers.h | 21 ++ UAV-ControlSystem/src/Scheduler/scheduler.c | 263 ++++++++++++++---- UAV-ControlSystem/src/Scheduler/tasks.c | 56 ++-- UAV-ControlSystem/src/drivers/led.c | 187 ++++++++++++- UAV-ControlSystem/src/main.c | 123 ++++---- UAV-ControlSystem/src/tasks_main.c | 73 +++-- .../src/utilities/math_helpers.c | 10 + 12 files changed, 788 insertions(+), 213 deletions(-) create mode 100644 UAV-ControlSystem/inc/utilities/math_helpers.h create mode 100644 UAV-ControlSystem/src/utilities/math_helpers.c diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h index a058bba..5c17864 100644 --- a/UAV-ControlSystem/inc/Scheduler/scheduler.h +++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h @@ -1,11 +1,22 @@ /************************************************************************** -* NAME: scheduler.h * -* PURPOSE: Defining the the scheduler to be used in the system to organize* -* the runtime for the tasks in the system based on priority. * -* INFORMATION: Many elements and ideas obtained from beta/cleanflight * +* NAME: scheduler.h * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Defining the the scheduler to be used in the system to * +* organize the runtime for the tasks in the system based on * +* priority. * +* INFORMATION: Many elements and ideas obtained from beta/cleanflight. * +* * * GLOBAL VARIABLES: * * Variable Type Description * -* -------- ---- ----------- * +* -------- ---- ----------- * +* SystemTasks task_t[] Contains all the tasks that can exist in * +* the system right now * +* * +* averageSystem uint16_t The current load of the system in percent.* +* LoadPercent May or may not display correctly. * +* * ***************************************************************************/ #ifndef SCHEDULER_H_ @@ -15,6 +26,7 @@ #include #include "stm32f4xx_revo.h" +#define taskAgeCycleCounterSize 16 /* Enum containing all the possible task priorities */ typedef enum @@ -28,6 +40,20 @@ typedef enum } taskPriority_t; +/* struct used to obtain task information */ +typedef struct { + const char * taskName; + const char * subTaskName; + bool isEnabled; + uint32_t desiredPeriod; + uint8_t staticPriority; + uint32_t maxExecutionTime; + uint32_t totalExecutionTime; + uint32_t averageExecutionTime; + uint32_t latestDeltaTime; +} taskInformation_t; + + /* Struct used to contain the information for each task */ typedef struct { @@ -102,54 +128,100 @@ typedef enum TASK_SELF }taskId_t; -/* Variables --------------------------------------------------------------------------------- */ +/* Variables -------------------------------------------------------------*/ extern task_t SystemTasks[TASK_COUNT]; /* All the tasks that exist in the system */ -extern uint16_t CpuLoad; /* The current load on the cpu Todo */ extern uint16_t averageSystemLoadPercent; /* The average load on the system Todo */ -/* Functions that operate can operate on the tasks ------------------------------------------- */ +/* Functions that operate can operate on the tasks -----------------------*/ /************************************************************************** -* BRIEF: Enables or disables a specific task in the system * -* INFORMATION: Takes the id of the task(task_t) along with a true or * -* false value. If true the task will be added to the task queue, if false * -* it will be removed. * +* BRIEF: Enables or disables a task to be able to run in the system. +* +* INFORMATION: Given an id for a task it can be added or removed from the +* active tasks in the system. Meaning tasks that can be selected by the +* scheduler to run. **************************************************************************/ void enableTask(taskId_t taskId, bool enabled); + /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Returns the delta time value of a task. +* +* INFORMATION: Given an id for a task it will return is delta time value. +* The time between its latest run and the one before. **************************************************************************/ uint32_t getTaskDeltaTime(taskId_t taskId); + /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Gives a task a new period. +* +* INFORMATION: Given an id for a task its period can be changed to a new +* desired value. **************************************************************************/ void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros); +/************************************************************************** +* BRIEF: Get some information of a task. +* +* INFORMATION: Given an id for a task its current values can be obtain. +* The variable taskInfo will point to the obtained information +* and act as an out variable. +**************************************************************************/ +void getTaskInfo(taskId_t taskId, taskInformation_t * taskInfo); + + /* Functions that handle the scheduler ------------------------------------------------------- */ /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: The main scheduler function. +* +* INFORMATION: This function is responsible for choosing the next task that +* the system should run. This is the main part of the system +* that will always run, and is the part of the code that will +* run in each iteration of the system loop. **************************************************************************/ void scheduler(void); + /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Initiates the scheduler and makes it ready to run. +* +* INFORMATION: The init will reset the ready queue of the system and add +* all the tasks that should be added. The tasks that are +* supposed to run with the current configuration of the system **************************************************************************/ void initScheduler(void); + /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Enables the tasks that should run. +* +* INFORMATION: All the tasks in the system that should be added to the run +* queue will be added when this function is invoked. **************************************************************************/ void initSchedulerTasks(void); +/************************************************************************** +* BRIEF: Returns an array of ageCyckle values observed when +* scheduling the tasks. +* +* INFORMATION: If a task has an age cycle greater than 1 it means it has +* missed one or more "periods". Each slot in the 16 value +* sized array matches a cycle age value observed for a task. +* Cycle age 1 or less is not counted since it would be to +* many and we don't care about when it is correct, this is only +* used to observe how many is wrong. Every task will generate +* a age cycle value each scheduling attempt. Each time a task +* missed his period the values in the array that this function +* return will increase. +**************************************************************************/ +#ifdef USE_TASK_AGE_CYCLE_STATISTICS +void getSchedulerAgeCycleData(uint32_t outValue[taskAgeCycleCounterSize]); +#endif + + #endif /* SCHEDULER_H_ */ diff --git a/UAV-ControlSystem/inc/Scheduler/tasks.h b/UAV-ControlSystem/inc/Scheduler/tasks.h index 69503e4..9b5ee73 100644 --- a/UAV-ControlSystem/inc/Scheduler/tasks.h +++ b/UAV-ControlSystem/inc/Scheduler/tasks.h @@ -1,8 +1,17 @@ /************************************************************************** -* NAME: tasks.h * -* PURPOSE: Defining the the scheduler to be used in the system to organize* -* the runtime for the tasks in the system based on priority. * -* INFORMATION: * +* NAME: tasks.h * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Defining the the scheduler to be used in the system to * +* organize the runtime for the tasks in the system based on * +* priority. * +* * +* INFORMATION: Adds the initial task values for each task that can be in * +* the system, in the c file. In the h file functions that * +* when called will* perform the action of its corresponding * +* task. * +* * * GLOBAL VARIABLES: * * Variable Type Description * * -------- ---- ----------- * @@ -13,6 +22,11 @@ #include +#define GetUpdateRateHz(x) (1000000 / x) //x is the desired hz value given in micro seconds +#define GetUpdateRateMs(x) (x*1000) //X is the desired ms value given as micro seconds +#define GetUpdateRateUs(x) (x) //X is the desired us value given as micro seconds (Its the same, used for clarification) +#define GetUpdateRateSeconds(x) (x * 1000000) //X is the desired second value given as micro seconds + //All the task functions in the system, one for each task void systemTaskSystem(void); void systemTaskGyroPid(void); @@ -30,10 +44,8 @@ void systemTaskAltitude(void); void systemTaskBeeper(void); //Tasks used only for testing purposes -#ifdef USE_DEBUG_TASKS void systemTaskDebug_1(void); void systemTaskDebug_2(void); void systemTaskDebug_3(void); -#endif #endif /* SCHEDULER_TASKS_H_ */ diff --git a/UAV-ControlSystem/inc/drivers/led.h b/UAV-ControlSystem/inc/drivers/led.h index e344720..72795ed 100644 --- a/UAV-ControlSystem/inc/drivers/led.h +++ b/UAV-ControlSystem/inc/drivers/led.h @@ -1,13 +1,112 @@ -/* - * led.h - * - * Created on: 21 sep. 2016 - * Author: holmis - */ +/************************************************************************** +* NAME: led.h * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Contains some led functionality. * +* * +* INFORMATION: Contains functions to configure pins to leds. It also * +* contains some led error codes that can be used in the * +* system, although at the time hardcoded to work only with * +* the revo on board leds. * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ #ifndef DRIVERS_LED_H_ #define DRIVERS_LED_H_ +//Update rate for the led +#define LED_UPDATE_TIMER 100000 + +//Different led warning approaches +typedef enum +{ + LEDWARNING_LED0_BLINK = 0, + LEDWARNING_LED1_BLINK, + LEDWARNING_LED0_ON, + LEDWARNING_LED1_ON, + LEDWARNING_LED0_BLINK_ONCE, + LEDWARNING_LED1_BLINK_ONCE, + LEDWARNING_OFF +}ledWarnings_t; + + +/************************************************************************** +* BRIEF: Enables pins so that they can be use for a Led +* +* INFORMATION: Given a pin and port enables that configuration to use led +**************************************************************************/ +void ledEnable(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Enables the two leds on board leds on the revolution board +* +* INFORMATION: +**************************************************************************/ +void ledReavoEnable(void); + + +/************************************************************************** +* BRIEF: Toggles a led +* +* INFORMATION: Given a pin and port, it attempts to toggle a led that +* should be linked with the given combination. +**************************************************************************/ +void ledToggle(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Turns on a led. +* +* INFORMATION: Given a pin and port, the function tries to turn on a led. +**************************************************************************/ +void ledOn(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Turns off a led. +* +* INFORMATION: Given a pin and port, the function tries to turn off a led. +**************************************************************************/ +void ledOff(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Turns on a led that is inverted +* +* INFORMATION: Given a pin and port, the function tries to turn on a led. +**************************************************************************/ +void ledOnInverted(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Turns off a led that is inverted +* +* INFORMATION: Given a pin and port, the function tries to turn off a led. +**************************************************************************/ +void ledOffInverted(uint16_t led_pin, GPIO_TypeDef* led_port); + + +/************************************************************************** +* BRIEF: Updates the warning leds in the system +* +* INFORMATION: Checks if any led warning should be active and if its the +* case perform some led activities on a specific led +**************************************************************************/ +void ledIndicatorsUpdate(void); + + +/************************************************************************** +* BRIEF: Change the warning type of a led +* +* INFORMATION: Change the warning type of led given a warningId that is +* obtained from ledWarnings_t. +**************************************************************************/ +void ledSetWarningType(uint16_t warningId); #endif /* DRIVERS_LED_H_ */ diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index 6b11c39..9fb748c 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -18,10 +18,10 @@ //Code here //Leds? (taken from betaflight REVO h file) -#define Led0_PIN GPIO_PIN_5 //Correct? -#define Led0_GPIO_PORT GPIOB //Correct? -#define Led1 GPIO_PIN_4 //Correct? -#define Led1_GPIO_PORT GPIOB //Correct? +#define Led0_PIN GPIO_PIN_5 //blue +#define Led0_GPIO_PORT GPIOB +#define Led1 GPIO_PIN_4 //yellow +#define Led1_GPIO_PORT GPIOB //Servo out, To be used for motors? @@ -79,7 +79,6 @@ #define USART6_RX_PIN GPIO_PIN_7 #define USART6_RX_PORT GPIOC #define USART6_TX_PIN GPIO_PIN_6 - #define USART6_TX_PORT GPIOC @@ -91,8 +90,15 @@ #define MPU6000_SPI_INSTANCE SPI1 //Dont know if necessary for us to specify + /* Led Warnings */ +#define USE_LEDS +#define USE_LED_WARNINGS +#define USE_LED_WARNINGS_MISSED_PERIOD +//#define USE_LED_WARNINGS_SYSTEM_LOAD + /* Scheduler */ #define USE_DEBUG_TASKS //Only to be used when testing scheduler, not when intending to run the whole system +#define USE_TASK_AGE_CYCLE_STATISTICS /* Baro */ diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h index 48e4968..2ae0606 100644 --- a/UAV-ControlSystem/inc/system_variables.h +++ b/UAV-ControlSystem/inc/system_variables.h @@ -1,4 +1,3 @@ -/* /********************************************************************** * NAME: adc.h * * AUTHOR: Philip Johansson * diff --git a/UAV-ControlSystem/inc/utilities/math_helpers.h b/UAV-ControlSystem/inc/utilities/math_helpers.h new file mode 100644 index 0000000..6a2c114 --- /dev/null +++ b/UAV-ControlSystem/inc/utilities/math_helpers.h @@ -0,0 +1,21 @@ +/* + * math_helpers.h + * + * Created on: 21 sep. 2016 + * Author: holmis + */ + +#ifndef UTILITIES_MATH_HELPERS_H_ +#define UTILITIES_MATH_HELPERS_H_ + + +#define M_PIf 3.14159265358979323846f + +#define RAD (M_PIf / 180.0f) + +#define MIN(a, b) ((a) < (b) ? (a) : (b)) +#define MAX(a, b) ((a) > (b) ? (a) : (b)) +#define ABS(x) ((x) > 0 ? (x) : -(x)) + + +#endif /* UTILITIES_MATH_HELPERS_H_ */ diff --git a/UAV-ControlSystem/src/Scheduler/scheduler.c b/UAV-ControlSystem/src/Scheduler/scheduler.c index 581281b..ff3b743 100644 --- a/UAV-ControlSystem/src/Scheduler/scheduler.c +++ b/UAV-ControlSystem/src/Scheduler/scheduler.c @@ -1,53 +1,75 @@ /************************************************************************** -* NAME: Scheduler.c * -* PURPOSE: Defining the the scheduler to be used in the system to organize* -* the runtime for the tasks in the system based on priority. * -* INFORMATION: * +* NAME: scheduler.c * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Defining the the scheduler to be used in the system to * +* organize the runtime for the tasks in the system based on * +* priority. * +* INFORMATION: Many elements and ideas obtained from beta/cleanflight. * +* * * GLOBAL VARIABLES: * * Variable Type Description * -* -------- ---- ----------- * +* -------- ---- ----------- * +* SystemTasks task_t[] Contains all the tasks that can exist in * +* the system right now * +* * +* averageSystem uint16_t The current load of the system in percent.* +* LoadPercent May or may not display correctly. * +* * ***************************************************************************/ #include "stm32f4xx_revo.h" #include "Scheduler/scheduler.h" #include "Scheduler/tasks.h" #include "drivers/system_clock.h" +#include "utilities/math_helpers.h" +#include +#include "drivers/led.h" -//Todo: change for several instances where 0 is used as null to the define value NULL +/* Variables ------------------------------------------- */ -static task_t *currentTask = 0; +static task_t *currentTask = NULL; //The current task that the is run -static uint32_t totalSchedulerPasses; /* Number of times the scheduler has been invoked */ -static uint32_t totalSchedulerReadyTasks; /* The amount of tasks that totally has been ready in the scheduler */ +static uint32_t totalSchedulerPasses = 0; /* Number of times the scheduler has been invoked */ +static uint32_t totalSchedulerReadyTasks = 0; /* The amount of tasks that totally has been ready in the scheduler */ -uint32_t currentTime = 0; +uint32_t currentTime = 0; //The current time in microseconds uint16_t averageSystmeLoad = 0; + // +static int taskReadyQueueSize = 0; //The number of tasks in the ready queue -static int taskReadyQueuePos = 0; -static int taskReadyQueueSize = 0; +static task_t * taskReadyQueue[TASK_COUNT + 1]; /* Array holding all the tasks that are ready to run in the system + 1 for a NULL value at the end*/ -static task_t * taskReadyQueue[TASK_COUNT + 1]; /* Array holding all the tasks that are ready to run in the system + 1 for a NULL value at the end*/ +uint16_t averageSystemLoadPercent = 0; //The load of the system in percent +static uint32_t tasksExecutionTimeUs = 0; //Total execution time of the task for one pass of the system task +//static uint32_t lastSystemLoadTimeValUs = 0; //Not used right now, would be used to calculate load with time vals + +uint32_t taskAgeCycleStatisitcs[taskAgeCycleCounterSize]; //Age cycle statistics array /* Functions to operate on the task queue ------------------------------------------------------- */ /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Clears the ready queue. * +* INFORMATION: Clears the entire queue and sets the size to the number of +* possible tasks that exist in the system. * **************************************************************************/ void taskReadyQueueClear(void) { //Empties the queue by settin all the positions to 0(NULL) memset(taskReadyQueue, 0, sizeof(taskReadyQueue)); - taskReadyQueuePos = 0; taskReadyQueueSize = 0; } /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Checks if a task already exists in the ready queue. +* * +* INFORMATION: Given a task it will be compared to the tasks in the queue. +* If it exist inside the queue the function will return true, +* otherwise it will return false. **************************************************************************/ bool taskReadyQueueContains(task_t *task) { @@ -65,8 +87,13 @@ bool taskReadyQueueContains(task_t *task) /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Adds a new task to the ready queue. +* * +* INFORMATION: This function will add a new task to the system if it does +* not already exist in it, or if the task queue is full (it +* should not be able to be that if every task is implemented +* correctly). Otherwise the task will be added to the queue +* based on priority and then period. * **************************************************************************/ bool taskReadyQueueAdd(task_t *task) { @@ -106,30 +133,26 @@ bool taskReadyQueueAdd(task_t *task) ++taskReadyQueueSize; return true; } - } + //If this is true it needs to be placed at the very back of the queue if (taskReadyQueue[i] == NULL ) { - //if taskReadyQueueSize > 0 we need to increment the counter i - if (taskReadyQueueSize > 0) - { - i++; - } - taskReadyQueue[i] = task; //ToDo: check if this works correctly, that a new NULL value will exist at i+1 + taskReadyQueueSize ++; return true; } - //Could not add the task in the anywhere in the queue return false; } /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Removes a task from the ready queue. +* * +* INFORMATION: Given a task it will remove it from the ready queue if it +* exist somewhere inside the queue. * **************************************************************************/ bool taskReadyQueueRemove(task_t *task) { @@ -145,10 +168,62 @@ bool taskReadyQueueRemove(task_t *task) /************************************************************************** -* BRIEF: Enables or disables a specific task in the system * -* INFORMATION: Takes the id of the task(task_t) along with a true or * -* false value. If true the task will be added to the task queue, if false * -* it will be removed. * +* BRIEF: The function that will run when the scheduler chooses to run +* the SYSTEM task. +* * +* INFORMATION: This task function is responsible for calculating the load +* of the system. More things can be added to this task if +* needed. * +**************************************************************************/ +void systemTaskSystem(void) +{ + /* Calculate system load */ + if (totalSchedulerPasses > 0) { + averageSystemLoadPercent = 100 * totalSchedulerReadyTasks / totalSchedulerPasses; + totalSchedulerPasses = 0; + totalSchedulerReadyTasks = 0; + /*uint32_t timeAtInstantUs = clock_get_us(); + uint32_t timeBetween = timeAtInstantUs - lastSystemLoadTimeValUs; + float divVal = ((float)tasksExecutionTimeUs / (float)timeBetween); + + averageSystemLoadPercent = 100* divVal; + lastSystemLoadTimeValUs = timeAtInstantUs;*/ + tasksExecutionTimeUs = 0; + +#ifdef USE_LED_WARNINGS_SYSTEM_LOAD + //ToDo: Test to light a led if the system is utilizing more than 100% + if (averageSystemLoadPercent >= 100) + { + ledSetWarningType(LEDWARNING_LED0_ON); + } + else if(averageSystemLoadPercent >= 80) + { + ledSetWarningType(LEDWARNING_LED0_BLINK); + } + else if(averageSystemLoadPercent >= 60) + { + ledSetWarningType(LEDWARNING_LED1_ON); + } + else if(averageSystemLoadPercent >= 40) + { + ledSetWarningType(LEDWARNING_LED1_BLINK); + } + else + { + ledSetWarningType(LEDWARNING_OFF); + } +#endif + } + +} + + +/************************************************************************** +* BRIEF: Enables or disables a task to be able to run in the system. +* +* INFORMATION: Given an id for a task it can be added or removed from the +* active tasks in the system. Meaning tasks that can be selected by the +* scheduler to run. **************************************************************************/ void enableTask(taskId_t taskId, bool enabled) { @@ -164,8 +239,10 @@ void enableTask(taskId_t taskId, bool enabled) /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Returns the delta time value of a task. +* +* INFORMATION: Given an id for a task it will return is delta time value. +* The time between its latest run and the one before. **************************************************************************/ uint32_t getTaskDeltaTime(taskId_t taskId) { @@ -179,8 +256,10 @@ uint32_t getTaskDeltaTime(taskId_t taskId) /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Gives a task a new period. +* +* INFORMATION: Given an id for a task its period can be changed to a new +* desired value. **************************************************************************/ void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros) { @@ -193,8 +272,53 @@ void rescheduleTask(taskId_t taskId, uint32_t newPeriodMicros) /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Returns an array of ageCyckle values observed when +* scheduling the tasks. +* +* INFORMATION: If a task has an age cycle greater than 1 it means it has +* missed one or more "periods". Each slot in the 16 value +* sized array matches a cycle age value observed for a task. +* Cycle age 1 or less is not counted since it would be to +* many and we don't care about when it is correct, this is only +* used to observe how many is wrong. Every task will generate +* a age cycle value each scheduling attempt. Each time a task +* missed his period the values in the array that this function +* return will increase. +**************************************************************************/ +#ifdef USE_TASK_AGE_CYCLE_STATISTICS +void getSchedulerAgeCycleData(uint32_t outValue[taskAgeCycleCounterSize]) +{ + outValue = taskAgeCycleStatisitcs; +} +#endif + + +/************************************************************************** +* BRIEF: Get some information of a task. +* +* INFORMATION: Given an id for a task its current values can be obtain. +* The variable taskInfo will point to the obtained information +* and act as an out variable. +**************************************************************************/ +void getTaskInfo(taskId_t taskId, taskInformation_t * taskInfo) +{ + taskInfo->taskName = SystemTasks[taskId].taskName; + taskInfo->subTaskName = SystemTasks[taskId].subTaskName; + taskInfo->isEnabled = taskReadyQueueContains(&SystemTasks[taskId]); + taskInfo->desiredPeriod = SystemTasks[taskId].desiredPeriod; + taskInfo->staticPriority = SystemTasks[taskId].staticPriority; + taskInfo->maxExecutionTime = SystemTasks[taskId].maxExecutionTime; + taskInfo->totalExecutionTime = SystemTasks[taskId].totalExecutionTime; + taskInfo->averageExecutionTime = SystemTasks[taskId].averageExecutionTime; + taskInfo->latestDeltaTime = SystemTasks[taskId].taskLatestDeltaTime; +} + + +/************************************************************************** +* BRIEF: Enables the tasks that should run. +* +* INFORMATION: All the tasks in the system that should be added to the run +* queue will be added when this function is invoked. **************************************************************************/ void initSchedulerTasks(void) { @@ -240,34 +364,46 @@ void initSchedulerTasks(void) /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: Initiates the scheduler and makes it ready to run. +* +* INFORMATION: The init will reset the ready queue of the system and add +* all the tasks that should be added. The tasks that are +* supposed to run with the current configuration of the system **************************************************************************/ void initScheduler(void) { - /*Todo: what needs to be init for the scheduler, should all the tasks - * be added to the readyTasksqueue here or should it be done at in - * another place in the code. */ //Clear the queue taskReadyQueueClear(); //Init all the ToDo: add back when it is known that the scheduler works - //initSchedulerTasks(); +#ifndef USE_DEBUG_TASKS + initSchedulerTasks(); +#endif + + //If we use debug tasks only init those +#ifdef USE_DEBUG_TASKS + enableTask(TASK_DEBUG_1, true); + enableTask(TASK_DEBUG_2, true); + enableTask(TASK_DEBUG_3, true); + enableTask(TASK_SYSTEM, true); +#endif } /************************************************************************** -* BRIEF: * -* INFORMATION: * +* BRIEF: The main scheduler function. +* +* INFORMATION: This function is responsible for choosing the next task that +* the system should run. This is the main part of the system +* that will always run, and is the part of the code that will +* run in each iteration of the system loop. **************************************************************************/ void scheduler(void) { - /*ToDo: implement the function micros that should give the current - * time in microseconds. Not entirely sure how it should be obtained, - * needs to be looked into and solved otherwise no time can be gotten */ - currentTime = micros(); + //Get the current time in Micro seconds + currentTime = clock_get_us(); task_t * taskToRun; - task_t * selectedTask; + task_t * selectedTask = NULL; uint16_t currentDynamicPriority = 0; uint32_t currentReadyTasks = 0; float currentAgeCyckleExact = 0; @@ -312,6 +448,15 @@ void scheduler(void) //If the value is 0 it is still in the same "period" as the last execution taskToRun->taskAgeCycle = (currentTime - taskToRun->lastExecutedAt) / taskToRun->desiredPeriod; +#ifdef USE_TASK_AGE_CYCLE_STATISTICS + if (taskToRun->taskAgeCycle > 1) + taskAgeCycleStatisitcs[taskToRun->taskAgeCycle] ++; //increment the statistic counter for age cycles if we miss period +#endif +#ifdef USE_LED_WARNINGS_MISSED_PERIOD + if (taskToRun->taskAgeCycle > 1) + ledSetWarningType(LEDWARNING_LED0_BLINK_ONCE); +#endif + //May not be used. Could be used to check what task is closest to its next period instance ageCycleExact = ((float)currentTime - (float)taskToRun->lastExecutedAt) / (float)taskToRun->desiredPeriod; @@ -321,12 +466,11 @@ void scheduler(void) //Calculate the new dynamic priority for the task taskToRun->dynamicPriority = 1 + taskToRun->taskAgeCycle * taskToRun->staticPriority; currentReadyTasks ++; //Increase the number of ready tasks - } } //If the current task has the largest dynamic priority so far it could be a candidate for execution - if(taskToRun->dynamicPriority >= currentDynamicPriority) + if(taskToRun->dynamicPriority >= currentDynamicPriority && taskToRun->taskAgeCycle >= 1) { bool changeTask = false; //flag used to check if the task should be updated /* If a realtime task is found the bool isRealTime will be set to true @@ -359,11 +503,10 @@ void scheduler(void) currentDynamicPriority = taskToRun->dynamicPriority; //update the current highest dynamic priority } } - } /* for-loop end */ - //Increase the + //Used for meassuring load totalSchedulerReadyTasks += currentReadyTasks; totalSchedulerPasses ++; @@ -378,11 +521,13 @@ void scheduler(void) selectedTask->dynamicPriority = 0; //reset the dynamic priority to 0 //handle the execution of the tasks function - const uint32_t timeBeforeExecution = micros(); + const uint32_t timeBeforeExecution = clock_get_us(); selectedTask->taskFunction(); //Run the function associated with the current task - const uint32_t TimeAfterExecution = micros(); + const uint32_t TimeAfterExecution = clock_get_us(); const uint32_t taskExecutionTime = TimeAfterExecution - timeBeforeExecution; //calculate the execution time of the task + tasksExecutionTimeUs += taskExecutionTime; //Add the task execution time for each task execution, will be used to + //Save statistical values selectedTask->averageExecutionTime = ((uint32_t)selectedTask->averageExecutionTime * 31 + taskExecutionTime) / 32; selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c index d690ce9..a08cda0 100644 --- a/UAV-ControlSystem/src/Scheduler/tasks.c +++ b/UAV-ControlSystem/src/Scheduler/tasks.c @@ -1,8 +1,17 @@ /************************************************************************** -* NAME: tasks.c * -* PURPOSE: Defining the the scheduler to be used in the system to organize* -* the runtime for the tasks in the system based on priority. * -* INFORMATION: * +* NAME: tasks.h * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Defining the the scheduler to be used in the system to * +* organize the runtime for the tasks in the system based on * +* priority. * +* * +* INFORMATION: Adds the initial task values for each task that can be in * +* the system, in the c file. In the h file functions that * +* when called will* perform the action of its corresponding * +* task. * +* * * GLOBAL VARIABLES: * * Variable Type Description * * -------- ---- ----------- * @@ -11,10 +20,16 @@ #include "Scheduler/tasks.h" #include "Scheduler/scheduler.h" -#define GetUpdateRateHz(x) (1000000 / x) //x is the desired hz value given in micro seconds -//initiate the values for all the tasks -//This information needs to be changed when functionality and setting for the tasks needs to be changed -// - Note: desiredPeriod is given in micro seconds, writing for example (1000000 / 10) means 10 hz rate + +/************************************************************************** +* Information - Initiate the values for all the tasks. This information +* needs to be changed when functionality and setting for the tasks needs to +* be changed. Each new task added to the system needs to be added here or +* it will not work. +* +* Note - desiredPeriod is given in micro seconds, writing for example +* (1000000 / 10) means 10 hz rate +***************************************************************************/ task_t SystemTasks[TASK_COUNT] = { @@ -72,7 +87,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "BATTERY", .taskFunction = systemTaskBattery, - .desiredPeriod = GetUpdateRateHz(50), + .desiredPeriod = GetUpdateRateHz(50), //50 hz update rate (20 ms) .staticPriority = PRIORITY_MEDIUM, }, @@ -81,7 +96,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "BAROMETER", .taskFunction = systemTaskBaro, - .desiredPeriod = GetUpdateRateHz(20), + .desiredPeriod = GetUpdateRateHz(20), //20 hz update rate (50 ms) .staticPriority = PRIORITY_LOW, }, #endif @@ -91,7 +106,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "COMPASS", .taskFunction = systemTaskCompass, - .desiredPeriod = GetUpdateRateHz(10), + .desiredPeriod = GetUpdateRateHz(10), //10 hz update rate (100 ms) .staticPriority = PRIORITY_LOW, }, #endif @@ -101,7 +116,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "GPS", .taskFunction = systemTaskGps, - .desiredPeriod = GetUpdateRateHz(10), + .desiredPeriod = GetUpdateRateHz(10), //10 hz update rate (100 ms) .staticPriority = PRIORITY_MEDIUM, }, #endif @@ -111,17 +126,17 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "SONAR", .taskFunction = systemTaskSonar, - .desiredPeriod = GetUpdateRateHz(20), + .desiredPeriod = GetUpdateRateHz(20), //20 hz update rate (50 ms) .staticPriority = PRIORITY_LOW, }, #endif -#ifdef defined(BARO) || defined(SONAR) +#if defined(BARO) || defined(SONAR) [TASK_ALTITUDE] = { .taskName = "ALTITUDE", .taskFunction = systemTaskAltitude, - .desiredPeriod = GetUpdateRateHz(40), + .desiredPeriod = GetUpdateRateHz(40), //40 hz update rate (25 ms) .staticPriority = PRIORITY_LOW, }, #endif @@ -131,7 +146,7 @@ task_t SystemTasks[TASK_COUNT] = { .taskName = "BEEPER", .taskFunction = systemTaskBeeper, - .desiredPeriod = GetUpdateRateHz(100), + .desiredPeriod = GetUpdateRateHz(100), //100 hz update rate (10 ms) .staticPriority = PRIORITY_LOW, }, #endif @@ -145,25 +160,24 @@ task_t SystemTasks[TASK_COUNT] = .taskName = "DEBUG_1", .taskFunction = systemTaskDebug_1, .desiredPeriod = GetUpdateRateHz(100), - .staticPriority = PRIORITY_LOW, + .staticPriority = PRIORITY_REALTIME, }, [TASK_DEBUG_2] = { .taskName = "DEBUG_2", .taskFunction = systemTaskDebug_2, - .desiredPeriod = GetUpdateRateHz(100), - .staticPriority = PRIORITY_LOW, + .desiredPeriod = GetUpdateRateHz(40), + .staticPriority = PRIORITY_MEDIUM, }, [TASK_DEBUG_3] = { .taskName = "DEBUG_3", .taskFunction = systemTaskDebug_3, - .desiredPeriod = GetUpdateRateHz(100), + .desiredPeriod = GetUpdateRateHz(20), .staticPriority = PRIORITY_LOW, }, - #endif }; diff --git a/UAV-ControlSystem/src/drivers/led.c b/UAV-ControlSystem/src/drivers/led.c index fca0fb8..cfda577 100644 --- a/UAV-ControlSystem/src/drivers/led.c +++ b/UAV-ControlSystem/src/drivers/led.c @@ -1,8 +1,183 @@ -/* - * led.c - * - * Created on: 21 sep. 2016 - * Author: holmis - */ +/************************************************************************** +* NAME: led.h * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Contains some led functionality. * +* * +* INFORMATION: Contains functions to configure pins to LEDs. It also * +* contains some led error codes that can be used in the * +* system, although at the time hardcoded to work only with * +* the revo on board leds. * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ + +#include "stm32f4xx.h" +#include "stm32f4xx_revo.h" +#include "drivers/system_clock.h" +#include "drivers/led.h" + +static int32_t ledTimer = 0; //Timer that keeps track of when it is ok to refresh the led warnings +static uint16_t currentLedWarning = LEDWARNING_OFF; //The current led warning that is activated in the system +/************************************************************************** +* BRIEF: Toggles a led +* +* INFORMATION: Given a pin and port, it attempts to toggle a led that +* should be linked with the given combination. +**************************************************************************/ +void ledToggle(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + HAL_GPIO_TogglePin(led_port, led_pin); +} + + +/************************************************************************** +* BRIEF: Turns on a led. +* +* INFORMATION: Given a pin and port, the function tries to turn on a led. +**************************************************************************/ +void ledOn(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + HAL_GPIO_WritePin(led_port, led_pin, GPIO_PIN_SET); +} + + +/************************************************************************** +* BRIEF: Turns off a led. +* +* INFORMATION: Given a pin and port, the function tries to turn off a led. +**************************************************************************/ +void ledOff(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + HAL_GPIO_WritePin(led_port, led_pin, GPIO_PIN_RESET); +} + + +/************************************************************************** +* BRIEF: Turns on a led that is inverted +* +* INFORMATION: Given a pin and port, the function tries to turn on a led. +**************************************************************************/ +void ledOnInverted(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + HAL_GPIO_WritePin(led_port, led_pin, GPIO_PIN_RESET); +} + + +/************************************************************************** +* BRIEF: Turns off a led that is inverted +* +* INFORMATION: Given a pin and port, the function tries to turn off a led. +**************************************************************************/ +void ledOffInverted(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + HAL_GPIO_WritePin(led_port, led_pin, GPIO_PIN_SET); +} + + +/************************************************************************** +* BRIEF: Enables pins so that they can be use for a Led +* +* INFORMATION: Given a pin and port enables that configuration to use led +**************************************************************************/ +void ledEnable(uint16_t led_pin, GPIO_TypeDef* led_port) +{ + GPIO_InitTypeDef gpinit; + gpinit.Pin = led_pin; + gpinit.Mode = GPIO_MODE_OUTPUT_PP; + gpinit.Pull = GPIO_PULLUP; + gpinit.Speed = GPIO_SPEED_HIGH; + HAL_GPIO_Init(led_port, &gpinit); +} + + +/************************************************************************** +* BRIEF: Enables the two leds on board leds on the revolution board +* +* INFORMATION: +**************************************************************************/ +void ledReavoEnable(void) +{ + ledEnable(Led0_PIN, GPIOB); + ledEnable(Led1, GPIOB); +} + + +/************************************************************************** +* BRIEF: Change the warning type of a led +* +* INFORMATION: Change the warning type of led given a warningId that is +* obtained from ledWarnings_t. +**************************************************************************/ +void ledSetWarningType(uint16_t warningId) +{ + currentLedWarning = warningId; +} + + +/************************************************************************** +* BRIEF: Refresh the led warnings +* +* INFORMATION: Given the current led warning perform associated action. +**************************************************************************/ +void ledIndicatorsRefresh() +{ + /* Use the two leds on the revo board for warnings, revo on board leds are inverted (off = on, vice versa) */ + switch(currentLedWarning) + { + case LEDWARNING_LED0_ON: + ledOnInverted(Led0_PIN, Led0_GPIO_PORT); + break; + case LEDWARNING_LED1_ON: + ledOnInverted(Led1, Led1_GPIO_PORT); + break; + case LEDWARNING_LED0_BLINK: + ledToggle(Led0_PIN, Led0_GPIO_PORT); + break; + case LEDWARNING_LED1_BLINK: + ledToggle(Led1, Led1_GPIO_PORT); + break; + case LEDWARNING_LED0_BLINK_ONCE: + ledOnInverted(Led0_PIN, Led0_GPIO_PORT); + ledSetWarningType(LEDWARNING_OFF); + break; + case LEDWARNING_LED1_BLINK_ONCE: + ledOnInverted(Led1, Led1_GPIO_PORT); + ledSetWarningType(LEDWARNING_OFF); + break; + case LEDWARNING_OFF: + default: + ledOffInverted(Led1, Led1_GPIO_PORT); + ledOffInverted(Led0_PIN, Led0_GPIO_PORT); + break; + } + + //Update the timer so that the led will not update unnecessarily to often + int32_t timeNow = clock_get_us(); + ledTimer = timeNow +LED_UPDATE_TIMER; +} + + +/************************************************************************** +* BRIEF: Updates the warning leds in the system +* +* INFORMATION: Checks if any led warning should be active and if its the +* case perform some led activities on a specific led +**************************************************************************/ +void ledIndicatorsUpdate(void) +{ + //get the current time + int32_t timeNow = clock_get_us(); + + //Can only refresh the leds if a certain time has passed + if (timeNow - ledTimer < 0) + return; + + //Refresh the led warnings + ledIndicatorsRefresh(); +} diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 275f49c..d169208 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -12,90 +12,89 @@ #include "drivers/adc.h" #include "drivers/system_clock.h" +#include "drivers/led.h" +#include "Scheduler/scheduler.h" #include "stm32f4xx.h" #include "stm32f4xx_revo.h" #include "system_variables.h" #include "utilities.h" #include -/* Private function prototypes -----------------------------------------------*/ -static void SystemClock_Config(void); -static void Error_Handler(void); -ADC_HandleTypeDef adc_testinput_handle; // For example battery voltage -ADC_HandleTypeDef adc_test2input_handle; -uint32_t g_ADCValue; -uint32_t g_ADC2; -int g_MeasurementNumber; - - -int main(void) +/************************************************************************** +* BRIEF: Should contain all the initializations of the system, needs to +* run before the scheduler. +* +* INFORMATION: Everything that will run somewhere in the system at some +* possible point and needs to be initialized to run properly, have to do it +* inside this function. +**************************************************************************/ +void init_system() { - // Initialize the Hardware Abstraction Layer + //Init the Hardware abstraction layer (HAL) HAL_Init(); - - /*##-2- Configure PA05 IO in output push-pull mode to drive external LED ###*/ - //GPIO_InitStruct.Pin = GPIO_PIN_5 | GPIO_PIN_6; - //GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP; - //GPIO_InitStruct.Pull = GPIO_PULLUP; - //GPIO_InitStruct.Speed = GPIO_SPEED_FAST; - //HAL_GPIO_Init(GPIOA, &gpinit); - // Configure the system clock to 100 MHz + //Configure the clock system_clock_config(); - int i = 1; +#ifdef USE_LEDS + //Initialize the on board leds + ledReavoEnable(); + ledOffInverted(Led0_PIN, Led0_GPIO_PORT); + ledOffInverted(Led1, Led1_GPIO_PORT); + //ledEnable(GPIO_PIN_0, GPIOA); +#endif - //Add ADC Channels - adc_pin_add(ADC_CHANNEL_0); - adc_pin_add(ADC_CHANNEL_1); - adc_pin_add(ADC_CHANNEL_12); +#ifdef BARO - //Configure the ADCs - adc_configure(); +#endif - /* This is done in system_clock_config for all GPIO clocks */ - //__GPIOB_CLK_ENABLE(); +#ifdef COMPASS - 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); +#endif - adc_start(); +#ifdef GPS - int num = 2000; - int j = 0; - volatile uint32_t time_us[num]; +#endif + +#ifdef SONAR + +#endif + +#if defined(BARO) || defined(SONAR) + +#endif + +#if BEEPER + +#endif + +} + +/************************************************************************** +* BRIEF: Main function of the system, every thing originates from this +* point. +* +* INFORMATION: The function is responsible for calling the Initialize +* function that will make the system ready to run. It is also responsible +* for starting and running the scheduler in a loop that never stops. +**************************************************************************/ +int main(void) +{ + //Init the system + init_system(); + + //Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler + initScheduler(); while (1) { - i++; + //Run the scheduler, responsible for distributing all the work of the running system + scheduler(); - //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(); +#ifdef USE_LED_WARNINGS + ledIndicatorsUpdate(); +#endif } for(;;); diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 26b9ae0..78ba9fb 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -1,36 +1,49 @@ /************************************************************************** -* NAME: tasks_main.c * -* PURPOSE: Holds the function implementations for the individual tasks -* that are invoked when a task is executed in the scheduler. Each task -* needs to have an associated function that has to be invoked when it is -* chosen as the task to run. Additionally optional event driven task -* functions must be implemented here as well. This file will include -* different drivers meaning that the task functions could jump around -* into other files before finishing its execution. -* INFORMATION: * +* NAME: tasks_main.c * +* * +* AUTHOR: Jonas Holmberg * +* * +* PURPOSE: Implement all the functions that will be called when * +* executing a task in the scheduler. * +* * +* INFORMATION: Holds the function implementations for the individual tasks* +* that are invoked when a task is executed in the scheduler. * +* Each task needs to have an associated function that has to * +* be invoked when it is chosen as the task to run. * +* Additionally optional event driven task functions must be * +* implemented here as well. This file will include different * +* drivers meaning that the task functions could jump around * +* into other files before finishing its execution. * +* * * GLOBAL VARIABLES: * * Variable Type Description * * -------- ---- ----------- * ***************************************************************************/ +#include +#include + #include "Scheduler/scheduler.h" #include "Scheduler/tasks.h" #include "stm32f4xx_revo.h" +/* Drivers */ +#include "drivers/led.h" +#include "drivers/adc.h" +#include "drivers/motors.h" +#include "drivers/pwm.h" +#include "drivers/system_clock.h" -void systemTaskSystem(void) -{ -} void systemTaskGyroPid(void) { - + //Read gyro and update PID and finally update the motors. The most important task in the system } void systemTaskAccelerometer(void) { - + //update the accelerometer data } void systemTaskAttitude(void) @@ -40,12 +53,14 @@ void systemTaskAttitude(void) void systemTaskRx(void) { - + //Interpret commands to the vehicle } bool systemTaskRxCheck(uint32_t currentDeltaTime) { - + //This task is what is controlling the event activation of the systemTaskRx + //check if there is anything that has be received. + return false; } void systemTaskSerial(void) @@ -55,32 +70,32 @@ void systemTaskSerial(void) void systemTaskBattery(void) { - + //Keep track of the battery level of the system } void systemTaskBaro(void) { - + //Obtain the barometer data } void systemTaskCompass(void) { - + //Obtain compass data } void systemTaskGps(void) { - + //Obtain gps data } void systemTaskSonar(void) { - + //obtain sonar data } void systemTaskAltitude(void) { - + //Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar } void systemTaskBeeper(void) @@ -93,17 +108,25 @@ void systemTaskBeeper(void) void systemTaskDebug_1(void) { - + //ledToggle(Led0_PIN, Led0_GPIO_PORT); + clock_delay_ms(8); + //ledToggle(Led0_PIN, Led0_GPIO_PORT); } void systemTaskDebug_2(void) { - + //ledToggle(Led1, Led1_GPIO_PORT); + //clock_delay_ms(15); + clock_delay_ms(8); + //ledToggle(Led1, Led1_GPIO_PORT); } void systemTaskDebug_3(void) { - + //ledToggle(GPIO_PIN_0, GPIOA); + //clock_delay_ms(20); + clock_delay_ms(8); + //ledToggle(GPIO_PIN_0, GPIOA); } #endif /* End USE_DEBUG_TASKS */ diff --git a/UAV-ControlSystem/src/utilities/math_helpers.c b/UAV-ControlSystem/src/utilities/math_helpers.c new file mode 100644 index 0000000..c382f74 --- /dev/null +++ b/UAV-ControlSystem/src/utilities/math_helpers.c @@ -0,0 +1,10 @@ +/* + * math_helpers.c + * + * Created on: 21 sep. 2016 + * Author: holmis + */ + +#include "utilities/math_helpers.h" + + From 30cb13cf49931f58e13ab4ca054fa37da7653b0b Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Tue, 27 Sep 2016 11:35:07 +0200 Subject: [PATCH 12/18] Intermediate storage point --- UAV-ControlSystem/inc/drivers/I2C.h | 27 +- UAV-ControlSystem/src/drivers/I2C.c | 125 +++++--- UAV-ControlSystem/src/drivers/i2c_test.c | 297 ++++++++++++++++++ UAV-ControlSystem/src/main.c | 99 +++--- .../HAL_Driver/Inc/stm32f4xx_hal_conf.h | 2 +- .../HAL_Driver/Src/stm32f4xx_hal_i2c.c | 3 +- 6 files changed, 468 insertions(+), 85 deletions(-) create mode 100644 UAV-ControlSystem/src/drivers/i2c_test.c diff --git a/UAV-ControlSystem/inc/drivers/I2C.h b/UAV-ControlSystem/inc/drivers/I2C.h index aed20ad..07750b3 100644 --- a/UAV-ControlSystem/inc/drivers/I2C.h +++ b/UAV-ControlSystem/inc/drivers/I2C.h @@ -10,10 +10,33 @@ #include "stm32f4xx.h" +/****************************************************************************** +* BRIEF: Configure the I2C bus to be used * +* INFORMATION: * +******************************************************************************/ +void i2c_configure(I2C_TypeDef* i2c, uint16_t sda_pin, uint16_t scl_pin, uint32_t my_address); -void i2c_configure(void); - +/****************************************************************************** +* BRIEF: Get data over the I2C bus * +* INFORMATION: * +* Since this system uses a 7 bit addressing mode we send the slave address * +* in the first bytes 7 MSbs and then the LSb is set to 1 indicating that * +* it is a receive command, after that the slave responds with a 1 bit ack and * +* the data is sent one byte at a time with an acknowledge in between * +* every byte, as per the standard. Returns true if successful * +******************************************************************************/ +bool i2c_receive(uint8_t slave_address, uint8_t* buffer, uint32_t length); +/****************************************************************************** +* BRIEF: Send data over the I2C bus * +* INFORMATION: * +* Since this system uses a 7 bit addressing mode we send the slave address * +* in the first bytes 7 MSbs and then the LSb is set to 0 indicating that * +* it is a send command, after that the slave responds with a 1 bit ack and * +* the data is sent one byte at a time with an acknowledge in between * +* every byte, as per the standard. Returns true if successful * +******************************************************************************/ +bool i2c_send(uint8_t slave_address, uint8_t* data, uint32_t length); #endif /* DRIVERS_I2C_H_ */ diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c index c2216ec..c93dd0a 100644 --- a/UAV-ControlSystem/src/drivers/I2C.c +++ b/UAV-ControlSystem/src/drivers/I2C.c @@ -7,50 +7,93 @@ #include "drivers/I2C.h" +// the I2C handle sending data over +I2C_HandleTypeDef I2C_handle; - - -void i2c_configure(uint32_t channel) +/****************************************************************************** +* BRIEF: Configure the I2C bus to be used * +* INFORMATION: * +******************************************************************************/ +void i2c_configure(I2C_TypeDef* i2c, uint16_t sda_pin, uint16_t scl_pin, uint32_t my_address) { + uint8_t i2c_af; + // get the correct alternate function for the selected I2C + if(i2c == I2C1) + i2c_af = GPIO_AF4_I2C1; + else if(i2c == I2C2) + i2c_af = GPIO_AF4_I2C2; + else if(i2c == I2C3) + i2c_af = GPIO_AF4_I2C3; + else + i2c_af = GPIO_AF4_I2C1; + + + + //Initialize pins for SCL and SDA GPIO_InitTypeDef GPIO_InitStruct; - I2C_InitTypeDef I2C_InitStruct; + __HAL_RCC_I2C1_CLK_ENABLE(); + __GPIOB_CLK_ENABLE(); + GPIO_InitStruct.Pin = sda_pin | scl_pin; + GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; + GPIO_InitStruct.Pull = GPIO_PULLUP; + GPIO_InitStruct.Speed = GPIO_SPEED_HIGH; + GPIO_InitStruct.Alternate = i2c_af; + HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); - // enable APB1 peripheral clock for I2C1 - RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C1, ENABLE); - // enable clock for SCL and SDA pins - RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOB, ENABLE); - - /* setup SCL and SDA pins - * You can connect I2C1 to two different - * pairs of pins: - * 1. SCL on PB6 and SDA on PB7 - * 2. SCL on PB8 and SDA on PB9 - */ - GPIO_InitStruct.Pin = GPIO_PIN_6 | GPIO_PIN_7; // we are going to use PB6 and PB7 - GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; // set pins to alternate function - GPIO_InitStruct.Speed = GPIO_SPEED_LOW; // set GPIO speed -// GPIO_InitStruct.GPIO_OType = GPIO_OType_OD; // set output to open drain --> the line has to be only pulled low, not driven high - GPIO_InitStruct.Pull = GPIO_PULLUP; // enable pull up resistors - GPIO_InitStruct.Alternate = GPIO_AF4_I2C1; - GPIO_Init(GPIOB, &GPIO_InitStruct); // init GPIOB - - I2C_InitStruct.ClockSpeed = 100000; - I2C_InitStruct.DualAddressMode = - - // Connect I2C1 pins to AF - GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_I2C1); // SCL - GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_I2C1); // SDA - - // configure I2C1 - I2C_InitStruct.I2C_ClockSpeed = 100000; // 100kHz - I2C_InitStruct.I2C_Mode = I2C_Mode_I2C; // I2C mode - I2C_InitStruct.I2C_DutyCycle = I2C_DutyCycle_2; // 50% duty cycle --> standard - I2C_InitStruct.I2C_OwnAddress1 = 0x00; // own address, not relevant in master mode - I2C_InitStruct.I2C_Ack = I2C_Ack_Disable; // disable acknowledge when reading (can be changed later on) - I2C_InitStruct.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; // set address length to 7 bit addresses - I2C_Init(I2C1, &I2C_InitStruct); // init I2C1 - - // enable I2C1 - I2C_Cmd(I2C1, ENABLE); + //Initialize I2C communication + I2C_handle.Instance = i2c; + I2C_handle.Init.ClockSpeed = 100000; + I2C_handle.Init.DutyCycle = I2C_DUTYCYCLE_2; + I2C_handle.Init.OwnAddress1 = my_address; + I2C_handle.Init.OwnAddress2 = 0; + I2C_handle.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + I2C_handle.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + I2C_handle.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + I2C_handle.Init.NoStretchMode = I2C_NOSTRETCH_ENABLE; + HAL_I2C_Init(&I2C_handle); +} + + +/****************************************************************************** +* BRIEF: Get data over the I2C bus * +* INFORMATION: * +* Since this system uses a 7 bit addressing mode we send the slave address * +* in the first bytes 7 MSbs and then the LSb is set to 1 indicating that * +* it is a receive command, after that the slave responds with a 1 bit ack and * +* the data is sent one byte at a time with an acknowledge in between * +* every byte, as per the standard. Returns true if successful * +******************************************************************************/ +bool i2c_receive(uint8_t slave_address, uint8_t* buffer, uint32_t length) +{ + uint8_t i = 0; + while(HAL_I2C_Master_Receive(&I2C_handle, (slave_address << 1) | 1, buffer, length, 1000)!= HAL_OK && i++ < 10) + {} + while (HAL_I2C_GetState(&I2C_handle) != HAL_I2C_STATE_READY) + {} + + return (i < 10); +} + +/*************************************************************************** +* BRIEF: Send data over the I2C bus * +* INFORMATION: * +* Since this system uses a 7 bit addressing mode we send the slave address * +* in the first bytes 7 MSbs and then the LSb is set to 0 indicating that * +* it is a send command, after that the slave responds with a 1 bit ack and * +* the data is sent one byte at a time with an acknowledge in between * +* every byte, as per the standard. Returns true if successful * +***************************************************************************/ +bool i2c_send(uint8_t slave_address, uint8_t* data, uint32_t length) +{ + uint8_t i = 0; + // try to send the data 10 times and if no acknowledge is received during these 10 messages we stop trying so that + // the system don't gets stuck forever because a slave is unreachable + while(HAL_I2C_Master_Transmit(&I2C_handle,(slave_address << 1), (uint8_t*)data, length, 1000) != HAL_OK && i++ < 10) + {} + + //Wait til the I2C bus is done with all sending + while (HAL_I2C_GetState(&I2C_handle) != HAL_I2C_STATE_READY){} + + return (i < 10); } diff --git a/UAV-ControlSystem/src/drivers/i2c_test.c b/UAV-ControlSystem/src/drivers/i2c_test.c new file mode 100644 index 0000000..dca5bc6 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/i2c_test.c @@ -0,0 +1,297 @@ +// +//#include "stm32f4xx.h" +// +////_____ M A C R O S +//#define TRUE 1 +//#define FALSE 0 +//#define F_CPU 4000000UL // 4 MHz external XTAL +//#define SCL_CLOCK 100000L // I2C clock in Hz +//#define ADDR_W 0xEE // Module address write mode +//#define ADDR_R 0xEF // Module address read mode +//#define CMD_RESET 0x1E // ADC reset command +//#define CMD_ADC_READ 0x00 // ADC read command +//#define CMD_ADC_CONV 0x40 // ADC conversion command +//#define CMD_ADC_D1 0x00 // ADC D1 conversion +//#define CMD_ADC_D2 0x10 // ADC D2 conversion +//#define CMD_ADC_256 0x00 // ADC OSR=256 +//#define CMD_ADC_512 0x02 // ADC OSR=512 +//#define CMD_ADC_1024 0x04 // ADC OSR=1024 +//#define CMD_ADC_2048 0x06 // ADC OSR=2048 +//#define CMD_ADC_4096 0x08 // ADC OSR=4096 +//#define CMD_PROM_RD 0xA0 // Prom read command +////_____ I N C L U D E S +//#include +////#include +////#include +////#include +//#include +////_____ D E F I N I T I O N S +//unsigned char i2c_start(unsigned char address); +//void i2c_stop(void); +//unsigned char i2c_write( unsigned char data ); +//unsigned char i2c_readAck(void); +//unsigned char i2c_readNak(void); +//void cmd_reset(void); +//unsigned long cmd_adc(char cmd); +//unsigned int cmd_prom(char coef_num); +//unsigned char crc4(unsigned int n_prom[]); +////******************************************************** +////! @brief send I2C start condition and the address byte +////! +////! @return 0 +////******************************************************** +//unsigned char i2c_start(unsigned char address) +//{ +// unsigned char twst; +// +// I2C1->CR1 = 1 << 8 | 1 << 0; +// +// TWCR = (1<>1]) & 0x00FF); +//else n_rem ^= (unsigned short) (n_prom[cnt>>1]>>8); +//for (n_bit = 8; n_bit > 0; n_bit--) +//{ +//if (n_rem & (0x8000)) +//{ +//n_rem = (n_rem << 1) ^ 0x3000; } +//else +//{ +//n_rem = (n_rem << 1); +//} +//} +//} +//n_rem= (0x000F & (n_rem >> 12)); // final 4-bit reminder is CRC code +//n_prom[7]=crc_read; // restore the crc_read to its original place +//return (n_rem ^ 0x0); +//} +////******************************************************** +////! @brief main program +////! +////! @return 0 +////******************************************************** +//int main (void) +//{ +//unsigned long D1; // ADC value of the pressure conversion +//unsigned long D2; // ADC value of the temperature conversion +//unsigned int C[8]; // calibration coefficients +//double P; // compensated pressure value +//double T; // compensated temperature value +//double dT; // difference between actual and measured temperature +//double OFF; // offset at actual temperature +//double SENS; // sensitivity at actual temperature +//int i; +//unsigned char n_crc; // crc value of the prom +// +//// setup the ports +//DDRA = 0xFE; +//DDRB = 0x0F; //SPI pins as input +//DDRC = 0x03; // I2C pins as output +//DDRD = 0x82; // RS out and tx out; +// +//PORTA = 0x1F; // I2C pin high +//PORTB = 0xF0; +//PORTC = 0x01; +//PORTD = 0x00; +// +//// initialize the I2C hardware module +//TWSR = 0; // no prescaler +//TWBR = ((F_CPU/SCL_CLOCK)-16)/2; // set the I2C speed +// +//D1=0; +//D2=0; +//cmd_reset(); // reset IC +//for (i=0;i<8;i++){ C[i]=cmd_prom(i);} // read coefficients +//n_crc=crc4(C); // calculate the CRC +//for(;;) // loop without stopping +//{ +//D2=cmd_adc(CMD_ADC_D2+CMD_ADC_4096); // read D2 +//D1=cmd_adc(CMD_ADC_D1+CMD_ADC_4096); // read D1 +//// calcualte 1st order pressure and temperature (MS5607 1st order algorithm) +//dT=D2-C[5]*pow(2,8); +//OFF=C[2]*pow(2,17)+dT*C[4]/pow(2,6); +//SENS=C[1]*pow(2,16)+dT*C[3]/pow(2,7); +// +//T=(2000+(dT*C[6])/pow(2,23))/100; +//P=(((D1*SENS)/pow(2,21)-OFF)/pow(2,15))/100; +//// place to use P, T, put them on LCD, send them trough RS232 interface... +//} +// +//return 0; +//} diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 3368f78..fe16f0b 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -12,6 +12,7 @@ #include "drivers/adc.h" #include "drivers/system_clock.h" +#include "drivers/I2C.h" #include "stm32f4xx.h" #include "system_variables.h" #include "utilities.h" @@ -28,59 +29,77 @@ int main(void) system_clock_config(); - int i = 1; + +/* COMPAS WORKING + i2c_configure(I2C1, GPIO_PIN_8, GPIO_PIN_9, 0x56); - //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(); + uint32_t address = 0b00011110; + uint8_t start_request_1[2] = { 0b00000000, 0b01110000 }; + uint8_t start_request_2[2] = { 0b00000001, 0b10100000 }; + uint8_t start_request_3[2] = { 0b00000010, 0b00000000 }; - /* 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); + uint8_t request_data[1] = { 0b00000110 }; + uint8_t reset_pointer_data[1] = { 0b00000011 }; + uint8_t response_data[6] = { 0x0 }; - adc_start(); - int num = 2000; - int j = 0; - volatile uint32_t time_us[num]; + i2c_send(address, &start_request_1, 2); + i2c_send(address, &start_request_2, 2); + i2c_send(address, &start_request_3, 2); + + // Delay for at least 6 ms for system startup to finish + HAL_Delay(10); while (1) { - i++; + i2c_send(address, &request_data, 1); + i2c_receive(address, &response_data, 6); + i2c_send(address, &reset_pointer_data, 1); - //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); + // HAL_Delay(100); + if(response_data[0] != 0) + response_data[0] = 0; + } +*/ - 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(); + i2c_configure(I2C1, GPIO_PIN_8, GPIO_PIN_9, 0x56); + uint32_t address = 0x77; + uint8_t reset_data[1] = { 0x1E }; + + uint8_t init_data[1] = { 0xA2 }; + uint8_t init_rsp_data[2] = { 0x00 }; + + uint8_t request_data[500] = { 0xFF }; + uint8_t response_data[2] = { 0x0 }; + +// while(count--) + i2c_send(address, address, 1); + i2c_send(reset_data[0], &reset_data, 1); + + HAL_Delay(50); + + for(int i = 0; i < 6; i++) + { + if(i2c_send(address, &init_data, 1) == true) + i2c_receive(address, &response_data, 2); + init_data[0] += 2; + } + + + HAL_Delay(20); + + while (1) + { + if(i2c_send(address, &request_data, 1) == true) + i2c_receive(address, &response_data, 2); + +// HAL_Delay(100); + if(response_data[0] != 0) + response_data[0] = 0; } for(;;); diff --git a/revolution_hal_lib/HAL_Driver/Inc/stm32f4xx_hal_conf.h b/revolution_hal_lib/HAL_Driver/Inc/stm32f4xx_hal_conf.h index ecf1646..010c103 100644 --- a/revolution_hal_lib/HAL_Driver/Inc/stm32f4xx_hal_conf.h +++ b/revolution_hal_lib/HAL_Driver/Inc/stm32f4xx_hal_conf.h @@ -392,7 +392,7 @@ * If expr is true, it returns no value. * @retval None */ - #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) + #define assert_param(expr) ((expr) ? (void)0 : a,ssert_failed((uint8_t *)__FILE__ __LINE__)) /* Exported functions ------------------------------------------------------- */ void assert_failed(uint8_t* file, uint32_t line); #else diff --git a/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c b/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c index 1c0383d..008158e 100644 --- a/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c +++ b/revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c @@ -4763,7 +4763,8 @@ static HAL_StatusTypeDef I2C_WaitOnFlagUntilTimeout(I2C_HandleTypeDef *hi2c, uin /* Check for the Timeout */ if(Timeout != HAL_MAX_DELAY) { - if((Timeout == 0U)||((HAL_GetTick() - tickstart ) > Timeout)) + uint32_t time = HAL_GetTick(); + if((Timeout == 0U)||((time - tickstart ) > Timeout)) { hi2c->PreviousState = I2C_STATE_NONE; hi2c->State= HAL_I2C_STATE_READY; From d8a9adff6d3f6ccd289eb1783b6a314112322146 Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Fri, 30 Sep 2016 12:20:10 +0200 Subject: [PATCH 13/18] USART implementation This is an implementation of the USART but the DMA transfer is not supported, all other functionalities are working including i.e. polling, regular processor transmit and dma double buffer --- UAV-ControlSystem/inc/drivers/usart.h | 213 ++++++++++++++ UAV-ControlSystem/src/drivers/usart.c | 399 ++++++++++++++++++++++++++ 2 files changed, 612 insertions(+) create mode 100644 UAV-ControlSystem/inc/drivers/usart.h create mode 100644 UAV-ControlSystem/src/drivers/usart.c diff --git a/UAV-ControlSystem/inc/drivers/usart.h b/UAV-ControlSystem/inc/drivers/usart.h new file mode 100644 index 0000000..0448942 --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/usart.h @@ -0,0 +1,213 @@ +/************************************************************************** +* NAME: usart.h +* AUTHOR: Lennart Eriksson +* PURPOSE: USART implementation for sending data +* INFORMATION: +* This file includes 2 types of USARTS, regular polling or DMA based +* the polling version of the USART uses the processor in order to get +* messages while the DMA has Direct Memory Access and does not need the +* processor to receive the messages and can copy the entire message once. +* The DMA is implemented using a double buffer with fixed sizes of the +* buffers in order to work with fixed data sizes of the messages. The up +* side of this is that the system can read a complete message and not +* where the DMA is not writing on the same place. Though this means that +* the message sizes needs to be know on before hand +* +* GLOBAL VARIABLES: +* Variable Type Description +* -------- ---- ----------- +**************************************************************************/ + +#ifndef DRIVERS_USART_H_ +#define DRIVERS_USART_H_ + +#include "stm32f4xx.h" + +// Enumeration for USART stop bits +typedef enum stop_bits +{ + STOP_BITS_1 = 0, + STOP_BITS_0_5, + STOP_BITS_2, + STOP_BITS_1_5 +} stop_bits; + +// Enuymeration for USART parity +typedef enum partiy +{ + PARITY_NONE = 0x0, + PARITY_EVEN = 0x2, + PARITY_ODD = 0x3 +} parity; + +// Struct to be used for regular USART with polling +typedef struct usart_profile +{ + USART_TypeDef* usart_instance; // The USART used to send or receive data +} usart_profile; + +// struct to be used for dma receiving of USART messages +typedef struct usart_dma_profile +{ + usart_profile usart_pro; // The USART profile to be used + DMA_Stream_TypeDef* dma_usart_rx_instance; // The DMA profile corresponding to the rx buffer + DMA_Stream_TypeDef* dma_usart_tx_instance; // The DMA profile corresponding to the tx buffer + uint8_t* dma_rx_buffer1; // The first rx buffer used in double buffering + uint8_t* dma_rx_buffer2; // The second rx buffer used in double buffering + uint8_t* dma_tx_buffer; // The tx buffer used for sending messages +} usart_dma_profile; + + + +/*********************************************************************** +* BRIEF: Initialize the USART with DMA reception of messages +* INFORMATION: +* Initialize the specified USART and enable the DMA for it so that the +* messages can be received without utilizing any processor load. This +* function returns false if any error occurred during the initialization +* and true of everything went well +***********************************************************************/ +bool usart_init_dma(USART_TypeDef* usart_inst, + usart_dma_profile* profile_out, + uint32_t baud_rate, + stop_bits stopbits, + parity parity_mode, + uint32_t dma_rx_buffersize, + uint32_t dma_tx_buffersize); + + +/*********************************************************************** +* BRIEF: Initialize a regular USART that can be used for polling +* INFORMATION: +* Initialize the specified USART in order to get polling and regular +* transmit of messages to work. If the initialization fails this function +* returns false and otherwise it returns true +***********************************************************************/ +bool usart_init(USART_TypeDef* usart_inst, + usart_profile* profile_out, + uint32_t baud_rate, + stop_bits stopbits, + parity parity_mode); + +/*********************************************************************** +* BRIEF: Send message over USART +* INFORMATION: +* Try to send a message over USART, if the time exceeds the specified +* timeout the transmit will be stopped. This function returns the number +* of bytes that was successfully sent down to the USART bus even if +* the timeout was reached before it was done. +***********************************************************************/ +uint32_t usart_transmit(usart_profile *profile, + uint8_t* buffer, + uint32_t size, + uint32_t timeout_us); + +/*********************************************************************** +* BRIEF: return if the USART has any data to be received in the buffer +* INFORMATION: +***********************************************************************/ +bool usart_poll_data_ready(usart_profile* profile); + +/*********************************************************************** +* BRIEF: Poll messages from the USART +* INFORMATION: +* Try to get a message from the USART, if the time spent receiving +* exceeds the specified timeout the function will return the buffer +* that has been received to that point. The function returns the number +* of bytes received even if the timeout was exceeded. +***********************************************************************/ +uint32_t usart_poll(usart_profile *profile, + uint8_t* buffer, + uint32_t size, + uint32_t timeout_us); + +/*********************************************************************** +* BRIEF: Get the DMA buffer that was most recently completed +* INFORMATION: +* This function will return the buffer that the DMA most recently +* completed so that the DMA can continue writing to the second buffer +* without interfering with the rest of the system +***********************************************************************/ +uint8_t* usart_get_dma_buffer(usart_dma_profile *profile); + +/*********************************************************************** +* BRIEF: NOT IMPLEMENTED YET +* INFORMATION: +* Use the usart_transmit function instead with the +* usart_dma_profile.usart_pro as input argument instead +***********************************************************************/ +void usart_transmit_dma(usart_dma_profile *profile, + uint8_t* buffer, + uint32_t size); + +#endif /* DRIVERS_USART_H_ */ + +// -------------------- EXAMPLE OF USART POLL ------------------------- +/* +int main() +{ + //Initialize the system + init_system(); + + //Set up a usart profile + usart_profile usart_p; + + //Initiate the profile for specified USART + usart_init(USART1, &usart_p, 115200, STOP_BITS_1, PARITY_NONE); + + //The data buffer used + uint8_t data = 0x00; + + while(1) + { + //poll data from the USART + uint32_t num_received = usart_poll(&usart_p, &data, 1, 1000); + //if data was received send the data back over the USART + if(num_received > 0x00) + { + usart_transmit(&usart_p, &data, 1, 1000); + } + } +} +*/ + +// -------------------- EXAMPLE OF USART DMA ------------------------- +/* +#define USART_DMA_SIZE 4 + +int main(void) +{ + //Init the system + init_system(); + + //Set up a usart DMA profile + usart_dma_profile dma_p; + + //Initiate USART DMA profile + usart_init_dma(USART1, &dma_p, 115200, STOP_BITS_1, PARITY_NONE, USART_DMA_SIZE, 0); + + //Temporary array to compare against + uint8_t tmp[USART_DMA_SIZE] = { 0x00 }; + + while(1) + { + uint8_t* buf; + // Get the dma finished buffer + buf = usart_get_dma_buffer(&dma_p); + bool change = false; + //compare against the previous buffer and copy the elements + for(int i = 0; i < USART_DMA_SIZE; i++) + { + if(tmp[i] != (tmp[i] = buf[i])) + change |= true; + } + + //if the buffer has changed print the buffer back over USART + if(change) + { + usart_transmit(&dma_p, tmp, USART_DMA_SIZE, 1000); + } + } +} +*/ + diff --git a/UAV-ControlSystem/src/drivers/usart.c b/UAV-ControlSystem/src/drivers/usart.c new file mode 100644 index 0000000..37e5eeb --- /dev/null +++ b/UAV-ControlSystem/src/drivers/usart.c @@ -0,0 +1,399 @@ +/************************************************************************** +* NAME: usart.c +* AUTHOR: Lennart Eriksson +* PURPOSE: USART implementation for sending data +* INFORMATION: +* This file includes 2 types of USARTS, regular polling or DMA based +* the polling version of the USART uses the processor in order to get +* messages while the DMA has Direct Memory Access and does not need the +* processor to receive the messages and can copy the entire message once. +* The DMA is implemented using a double buffer with fixed sizes of the +* buffers in order to work with fixed data sizes of the messages. The up +* side of this is that the system can read a complete message and not +* where the DMA is not writing on the same place. Though this means that +* the message sizes needs to be know on before hand +* +* GLOBAL VARIABLES: +* Variable Type Description +* -------- ---- ----------- +**************************************************************************/ + +#include "drivers/usart.h" +#include "stm32f4xx_revo.h" +#include "drivers/system_clock.h" + + +//Define registers for the USART since the HAL library does not work with sending +//data at the moment + +// CR1 +#define UE 0x2000 // Usart Enabled +#define M 0x0000 // word length 8 +#define RE 0x0004 // Receive enabled +#define TE 0x0008 // Transmit enabled +#define PARITY_OFFSET 9 //Offset to parity bits + +//CR2 +#define STOP_OFFSET 12 // offset to stop bits in CR2 + +//CR3 +#define DMAR 0x0040 // Enable DMA rx +#define DMAT 0x0080 // Enable DMA tx + +//BRR +#define USART_BRR(_PCLK_, _BAUD_) ((_PCLK_ /(_BAUD_ * 16)) * 16) // Calculate BRR from the desired baud rate + + +/*********************************************************************** +* BRIEF: Initialize the USART with DMA reception of messages +* INFORMATION: +* Initialize the specified USART and enable the DMA for it so that the +* messages can be received without utilizing any processor load. This +* function returns false if any error occurred during the initialization +* and true of everything went well +***********************************************************************/ +bool usart_init_dma(USART_TypeDef* usart_inst, // The USART instance to be used, i.e. USART1, USART3 or USART6 for the REVO card + usart_dma_profile* profile_out, // The USART profile that will be used when sending or receiving data + uint32_t baud_rate, // The baud rate that the USART will communicate with + stop_bits stopbits, // The number of stop bits that the USART should have + parity parity_mode, // The parity that the USART should have + uint32_t dma_rx_buffersize, // The buffer size for the DMA rx buffers + uint32_t dma_tx_buffersize) // The buffer size for the DMA tx buffers +{ + // Local variables used when extracting the different USARTs + DMA_Stream_TypeDef *dma_rx_instance, *dma_tx_instance; + uint32_t channel; + + // Check if the instance is either USART1, USART3 of USART6 since + // those are the only ones available on the REVO card + if(usart_inst == USART1) + { + dma_rx_instance = DMA2_Stream2; + dma_tx_instance = DMA2_Stream5; + channel = DMA_CHANNEL_4; + } + else if(usart_inst == USART3) + { + dma_rx_instance = DMA1_Stream1; + dma_tx_instance = DMA1_Stream3; + channel = DMA_CHANNEL_4; + } + else if(usart_inst == USART6) + { + dma_rx_instance = DMA2_Stream2; + dma_tx_instance = DMA2_Stream6; + channel = DMA_CHANNEL_5; + } + else + return false; // If any other USART is sent in return false + + // Enable the correct clock if it has not been enabled already + if(__HAL_RCC_DMA2_IS_CLK_DISABLED() && (usart_inst == USART6 || usart_inst == USART1)) + __HAL_RCC_DMA2_CLK_ENABLE(); + if(__HAL_RCC_DMA1_IS_CLK_DISABLED()&& usart_inst == USART3) + __HAL_RCC_DMA1_CLK_ENABLE(); + + // Initialize the regular usart before adding on the DMA + if(!usart_init(usart_inst, &profile_out->usart_pro, baud_rate, stopbits, parity_mode)) + return false; // If the initialization did not complete return false + + // set the USART profile buffers and initialize them to 0x00 for every element + profile_out->dma_rx_buffer1 = malloc(sizeof(uint8_t) * dma_rx_buffersize); + profile_out->dma_rx_buffer2 = malloc(sizeof(uint8_t) * dma_rx_buffersize); + profile_out->dma_tx_buffer = malloc(sizeof(uint8_t) * dma_tx_buffersize); + memset(profile_out->dma_rx_buffer1, 0x00, dma_rx_buffersize); + memset(profile_out->dma_rx_buffer2, 0x00, dma_rx_buffersize); + memset(profile_out->dma_tx_buffer, 0x00, dma_tx_buffersize); + + // Set the DMA instances in the USART profile + profile_out->dma_usart_rx_instance = dma_rx_instance; + profile_out->dma_usart_tx_instance = dma_tx_instance; + + // Enable the DMA on the USARTon register level + profile_out->usart_pro.usart_instance->CR3 |= DMAR | DMAT; + + // This is only a dummy that is used by the DMA linking later on + USART_HandleTypeDef usart; + usart.Instance = usart_inst; + + // Set up the DMA handle for the USART rx + DMA_HandleTypeDef g_DmaHandle_rx; + g_DmaHandle_rx.Instance = dma_rx_instance; // the rx instance + g_DmaHandle_rx.Init.Channel = channel; // the rx channel + g_DmaHandle_rx.Init.Direction = DMA_PERIPH_TO_MEMORY; // set data direction to peripheral to memory + g_DmaHandle_rx.Init.PeriphInc = DMA_PINC_DISABLE; // peripheral increment data pointer disabled + g_DmaHandle_rx.Init.MemInc = DMA_MINC_ENABLE; // Memory increment data pointer enabled + g_DmaHandle_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD; // Align data words on the peripheral + g_DmaHandle_rx.Init.MemDataAlignment = DMA_MDATAALIGN_WORD; // Align data words on the memory + g_DmaHandle_rx.Init.Mode = DMA_SxCR_DBM; // Enable Double buffer mode + g_DmaHandle_rx.Init.Priority = DMA_PRIORITY_HIGH; // Set the receive priority to high + g_DmaHandle_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE; // Disable fifo mode + g_DmaHandle_rx.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_HALFFULL; // Set fifo threshold to half full although this is probably not used + g_DmaHandle_rx.Init.MemBurst = DMA_MBURST_SINGLE; // In double buffer mode the burst must always be single + g_DmaHandle_rx.Init.PeriphBurst = DMA_PBURST_SINGLE; // In double buffer mode the burst must always be single + + // Initialize the DMA handle + HAL_DMA_Init(&g_DmaHandle_rx); + + // Link the DMA to the USART instance + __HAL_LINKDMA(&usart, hdmarx ,g_DmaHandle_rx); + + + //Set up the DMA handle for the USART tx + DMA_HandleTypeDef g_DmaHandle_tx; + g_DmaHandle_tx.Instance = dma_tx_instance; + g_DmaHandle_tx.Init.Direction = DMA_MEMORY_TO_PERIPH; + g_DmaHandle_tx.Init.Channel = channel; + HAL_DMA_Init(&g_DmaHandle_tx); + __HAL_LINKDMA(&usart, hdmatx ,g_DmaHandle_tx); + + +// g_DmaHandle.Instance = dma_tx_instance; +// g_DmaHandle.Init.Direction = DMA_MEMORY_TO_PERIPH; +// g_DmaHandle.Init.Mode = 0x00; +// g_DmaHandle.Init.FIFOMode = DMA_FIFOMODE_ENABLE; +// g_DmaHandle.Init.Priority = DMA_PRIORITY_MEDIUM; +// +// HAL_DMA_Init(&g_DmaHandle); +// +// __HAL_LINKDMA(&usart, hdmatx ,g_DmaHandle); + + + + //Setup DMA buffers + + // Disable the DMA, must be done before writing to the addresses below + dma_rx_instance->CR &= ~(DMA_SxCR_EN); + + dma_rx_instance->NDTR = dma_rx_buffersize; // Set the buffer size + dma_rx_instance->PAR = (uint32_t)&profile_out->usart_pro.usart_instance->DR; // Set the address to the USART data register + dma_rx_instance->M0AR = (uint32_t)profile_out->dma_rx_buffer1; // Set the address to the first DMA buffer + dma_rx_instance->M1AR = (uint32_t)profile_out->dma_rx_buffer2; // Set the address to the second DMA buffer + dma_rx_instance->CR &= ~(0xF << 11); // Set the data size to 8 bit values + + //Enable the DMA again to start receiving data from the USART + dma_rx_instance->CR |= DMA_SxCR_EN; + + + dma_tx_instance->CR &= ~(DMA_SxCR_EN); + + dma_tx_instance->NDTR = dma_tx_buffersize; + dma_tx_instance->PAR = (uint32_t)&profile_out->usart_pro.usart_instance->DR; + dma_tx_instance->M0AR = (uint32_t)profile_out->dma_tx_buffer; + dma_tx_instance->CR &= ~(0xF << 11); + + + dma_tx_instance->CR |= DMA_SxCR_EN; + + return true; // everything went as planned and the USART should be ready to use +} + +/*********************************************************************** +* BRIEF: Initialize a regular USART that can be used for polling +* INFORMATION: +* Initialize the specified USART in order to get polling and regular +* transmit of messages to work. If the initialization fails this function +* returns false and otherwise it returns true +***********************************************************************/ +bool usart_init(USART_TypeDef* usart_inst, // The USART instance to be used, i.e. USART1, USART3 or USART6 for the REVO board + usart_profile* profile_out, // The USART profile that will be used when sending or receiving data + uint32_t baud_rate, // The baud rate that the USART will communicate with + stop_bits stopbits, // The number of stop bits that the USART should have + parity parity_mode) // The parity that the USART should have +{ + // set the USART profiles USART instance + profile_out->usart_instance = usart_inst; + + // Local variables used when extracting the different USARTs + uint32_t rx_pin, tx_pin, af_func; + GPIO_TypeDef* gpioport; + + // Check if the instance is either USART1, USART3 of USART6 since + // those are the only ones available on the REVO card + if(usart_inst == USART1) + { + rx_pin = USART1_RX_PIN; + tx_pin = USART1_TX_PIN; + af_func = GPIO_AF7_USART1; + gpioport = USART1_RX_PORT; + + //Enable clock if not already enabled + if(__HAL_RCC_USART1_IS_CLK_DISABLED()) + __HAL_RCC_USART1_CLK_ENABLE(); + } + else if(usart_inst == USART3) + { + rx_pin = USART3_RX_PIN; + tx_pin = USART3_TX_PIN; + af_func = GPIO_AF7_USART3; + gpioport = USART3_RX_PORT; + + //Enable clock if not already enabled + if(__HAL_RCC_USART3_IS_CLK_DISABLED()) + __HAL_RCC_USART3_CLK_ENABLE(); + } + else if(usart_inst == USART6) + { + rx_pin = USART6_RX_PIN; + tx_pin = USART6_TX_PIN; + af_func = GPIO_AF8_USART6; + gpioport = USART6_RX_PORT; + + //Enable clock if not already enabled + if(__HAL_RCC_USART6_IS_CLK_DISABLED()) + __HAL_RCC_USART6_CLK_ENABLE(); + } + else + return false;// If any other USART is sent in return false + + + // PIN Initialization for the USART + GPIO_InitTypeDef gpio; + gpio.Pin = rx_pin | tx_pin; + gpio.Pull = GPIO_NOPULL; + gpio.Mode = GPIO_MODE_AF_PP; + gpio.Speed = GPIO_SPEED_HIGH; + gpio.Alternate = af_func; + HAL_GPIO_Init(gpioport, &gpio); + + // USART initialization + // This is on register level since the HAL library did not work as expected + usart_inst->CR1 = UE | M | RE | TE | (parity_mode << PARITY_OFFSET); + usart_inst->CR2 = stopbits << STOP_OFFSET; + usart_inst->BRR = USART_BRR(HAL_RCC_GetPCLK2Freq(), baud_rate); + + return true; // Everything went as planned and the USART is enabled +} + +/*********************************************************************** +* BRIEF: Send message over USART +* INFORMATION: +* Try to send a message over USART, if the time exceeds the specified +* timeout the transmit will be stopped. This function returns the number +* of bytes that was successfully sent down to the USART bus even if +* the timeout was reached before it was done. +***********************************************************************/ +uint32_t usart_transmit(usart_profile *profile, // The USART profile to send data from + uint8_t* buffer, // The buffer to send + uint32_t size, // The size of the buffer to send + uint32_t timeout_us) // If time exceeds this microsecond value the function will return +{ + // Calculate at what time the function should stop sending and return if not done + uint32_t time_to_leave = clock_get_us() + timeout_us; + int i; + // Send all messages in the buffer + for(i = 0; i < size; i++) + { + profile->usart_instance->DR = buffer[i]; + // Wait for the buffer to be emptied and sent over the usart, if the timeout value is reached leave this function + while (!(profile->usart_instance->SR & 0x40) && time_to_leave > clock_get_us()); + + // If the overrun error is active clear it before continue + if(profile->usart_instance->SR & 0x08) + { + profile->usart_instance->SR; + profile->usart_instance->DR; + } + + // if the timeout is reached return the number of bytes that was successfully sent over USART + if(time_to_leave <= clock_get_us()) + return i; + } + + //return the number of Bytes sent on the USART, this should be the same size as the provided buffer size + return i; +} + +/*********************************************************************** +* BRIEF: NOT IMPLEMENTED YET +* INFORMATION: +* Use the usart_transmit function instead with the +* usart_dma_profile.usart_pro as input argument instead +***********************************************************************/ +void usart_transmit_dma(usart_dma_profile *profile, uint8_t* buffer, uint32_t size) +{ + // this is only a try to get the system working so there is no definite proof that this + // is the correct way. This is only kept if it were to be implemented to see what have been + // done. + + /* + for(int i = 0; i < size; i++) + { + profile->dma_tx_buffer[i] = buffer[i]; + } + + profile->dma_usart_tx_instance->CR &= ~(DMA_SxCR_EN); + + profile->dma_usart_tx_instance->NDTR = size; + + profile->dma_usart_tx_instance->CR |= DMA_SxCR_EN; + */ +} + +/*********************************************************************** +* BRIEF: return if the USART has any data to be received in the buffer +* INFORMATION: +***********************************************************************/ +bool usart_poll_data_ready(usart_profile* profile) +{ + // check if the Read Data Register Not Empty (RXNE) is set + if(profile->usart_instance->SR & 0x20) + return true; + + return false; +} + +/*********************************************************************** +* BRIEF: Poll messages from the USART +* INFORMATION: +* Try to get a message from the USART, if the time spent receiving +* exceeds the specified timeout the function will return the buffer +* that has been received to that point. The function returns the number +* of bytes received even if the timeout was exceeded. +***********************************************************************/ +uint32_t usart_poll(usart_profile *profile, // The USART profile to receive data from + uint8_t* buffer, // The buffer to put the data in + uint32_t size, // The expected size of the data + uint32_t timeout_us) // If time exceeds this microsecond value the function will return +{ + // Calculate at what time the function should stop sending and return if not done + uint32_t time_to_leave = clock_get_us() + timeout_us; + + int i = 0; + // While the timeout is not exceeded and we have data to read run this loop + while(time_to_leave > clock_get_us() && i < size) + { + // Check if data is ready to be received + if(usart_poll_data_ready(profile)) + { + // Copy the data from the data register to the buffer + buffer[i++] = profile->usart_instance->DR & 0xFF; + + // Wait until the status register gets ready again + while (profile->usart_instance->SR & 0x20); + } + } + + //return the number of bytes received + return i; +} + +/*********************************************************************** +* BRIEF: Get the DMA buffer that was most recently completed +* INFORMATION: +* This function will return the buffer that the DMA most recently +* completed so that the DMA can continue writing to the second buffer +* without interfering with the rest of the system +***********************************************************************/ +uint8_t* usart_get_dma_buffer(usart_dma_profile *profile) +{ + // Check which buffer the DMA is writing to at the moment and return the other buffer + if(profile->dma_usart_rx_instance->CR & DMA_SxCR_CT) + { + return profile->dma_rx_buffer1; + } + else + { + return profile->dma_rx_buffer2; + } +} From cd6087f3d482dae52924caf8f43da419401face1 Mon Sep 17 00:00:00 2001 From: Robert Skoglund Date: Mon, 3 Oct 2016 08:41:16 +0200 Subject: [PATCH 14/18] Working SPI gyroacc --- UAV-ControlSystem/inc/drivers/accel_gyro.h | 183 +++++++ UAV-ControlSystem/src/drivers/accel_gyro.c | 553 +++++++++++++++++++++ 2 files changed, 736 insertions(+) create mode 100644 UAV-ControlSystem/inc/drivers/accel_gyro.h create mode 100644 UAV-ControlSystem/src/drivers/accel_gyro.c diff --git a/UAV-ControlSystem/inc/drivers/accel_gyro.h b/UAV-ControlSystem/inc/drivers/accel_gyro.h new file mode 100644 index 0000000..43ecb2c --- /dev/null +++ b/UAV-ControlSystem/inc/drivers/accel_gyro.h @@ -0,0 +1,183 @@ +/* + * accelgyro.h + * + * Created on: 16 sep. 2016 + * Author: rsd12002 + */ + +/********************************************************************** + * NAME: accelgyro.h * + * AUTHOR: rsd12002 * + * PURPOSE: Set up and read from the gyroscope and accelerometer * + * INFORMATION: * + * How to use this driver is explained in page 810 of HAL driver * + * Enable the SPI clock * + * Enable the clock for the SPI GPIOs * + * Configure the MISO, MOSI, SCK pins as alternate function push-pull * + * Program the Mode, Direction, Data size, Baudrate Prescaler, * + * NSS management, Clock polarity and phase, FirstBit and * + * CRC configuration * + * * + * GLOBAL VARIABLES: * + * Variable Type Description * + * -------- ---- ----------- * + * * + **********************************************************************/ + +#ifndef DRIVERS_ACCEL_GYRO_H_ +#define DRIVERS_ACCEL_GYRO_H_ + +#include +#include "stm32f4xx.h" +#include "stm32f4xx_revo.h" + +#define MPU6000_CONFIG 0x1A + +// Bits +#define MPU_CLK_SEL_PLLGYROZ 0x03 +#define BITS_FS_250DPS 0x00 +#define BITS_FS_500DPS 0x08 +#define BITS_FS_1000DPS 0x10 +#define BITS_FS_2000DPS 0x18 +#define BITS_FS_2G 0x00 +#define BITS_FS_4G 0x08 +#define BITS_FS_8G 0x10 +#define BITS_FS_16G 0x18 +#define BITS_FS_MASK 0x18 +#define BITS_DLPF_CFG_256HZ 0x00 +#define BITS_DLPF_CFG_188HZ 0x01 +#define BITS_DLPF_CFG_98HZ 0x02 +#define BITS_DLPF_CFG_42HZ 0x03 +#define BITS_DLPF_CFG_20HZ 0x04 +#define BITS_DLPF_CFG_10HZ 0x05 +#define BITS_DLPF_CFG_5HZ 0x06 +#define BIT_I2C_IF_DIS 0x10 +#define BIT_H_RESET 0x80 +#define BIT_FIFO_RESET 0x04 +#define BIT_GYRO 3 +#define BIT_ACC 2 + +#define MPU6000_WHO_AM_I_CONST 0x68 + +// RA = Register Address +#define MPU_RA_PRODUCT_ID 0x0C +#define MPU_RA_SMPLRT_DIV 0x19 +#define MPU_RA_CONFIG 0x1A +#define MPU_RA_GYRO_CONFIG 0x1B +#define MPU_RA_ACCEL_CONFIG 0x1C +#define MPU_RA_FIFO_EN 0x23 +#define MPU_RA_ACCEL_XOUT_H 0x3B +#define MPU_RA_ACCEL_XOUT_L 0x3C +#define MPU_RA_ACCEL_YOUT_H 0x3D +#define MPU_RA_ACCEL_YOUT_L 0x3E +#define MPU_RA_ACCEL_ZOUT_H 0x3F +#define MPU_RA_ACCEL_ZOUT_L 0x40 +#define MPU_RA_GYRO_XOUT_H 0x43 +#define MPU_RA_GYRO_XOUT_L 0x44 +#define MPU_RA_GYRO_YOUT_H 0x45 +#define MPU_RA_GYRO_YOUT_L 0x46 +#define MPU_RA_GYRO_ZOUT_H 0x47 +#define MPU_RA_GYRO_ZOUT_L 0x48 +#define MPU_RA_SIGNAL_PATH_RESET 0x68 +#define MPU_RA_USER_CTRL 0x6A +#define MPU_RA_PWR_MGMT_1 0x6B +#define MPU_RA_PWR_MGMT_2 0x6C +#define MPU_RA_FIFO_COUNTH 0x72 +#define MPU_RA_FIFO_COUNTL 0x73 +#define MPU_RA_FIFO_R_W 0x74 +#define MPU_RA_WHO_AM_I 0x75 + +// Product ID Description for MPU6000 +#define MPU6000ES_REV_C4 0x14 +#define MPU6000ES_REV_C5 0x15 +#define MPU6000ES_REV_D6 0x16 +#define MPU6000ES_REV_D7 0x17 +#define MPU6000ES_REV_D8 0x18 +#define MPU6000_REV_C4 0x54 +#define MPU6000_REV_C5 0x55 +#define MPU6000_REV_D6 0x56 +#define MPU6000_REV_D7 0x57 +#define MPU6000_REV_D8 0x58 +#define MPU6000_REV_D9 0x59 +#define MPU6000_REV_D10 0x5A + +#define YAW_ROT_0 + +typedef struct gyro_t { + int16_t gyroX, gyroY, gyroZ; /* Gyroscope data converted into °/s */ + int16_t offsetX, offsetY, offsetZ; /* Gyroscope offset values */ + float scale; /* Scale factor */ +} gyro_t; + +typedef struct accel_t { + float accelXconv, accelYconv, accelZconv; /* Converted accelerometer data into G (9.82 m/s^2) */ + int16_t accelXraw, accelYraw, accelZraw; /* Raw accelerometer data */ + int16_t offsetX, offsetY, offsetZ; /* Accelerometer offset raw values */ + uint16_t accel1G; /* Sensitivity factor */ +} accel_t; + +/*********************************************************************** + * BRIEF: SPI1_Init initializes the SPI1 instance with predefined values* + * INFORMATION: * + * Mode = SPI_MODE_MASTER; * + * Direction = SPI_DIRECTION_2LINES; * + * DataSize = SPI_DATASIZE_8BIT; * + * CLKPolarity = SPI_POLARITY_LOW; * + * CLKPhase = SPI_PHASE_1EDGE; * + * NSS = SPI_NSS_HARD_OUTPUT; * + * BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128; * + * FirstBit = SPI_FIRSTBIT_MSB; * + * TIMode = SPI_TIMODE_DISABLE; * + * CRCCalculation = SPI_CRCCALCULATION_DISABLED; * + ***********************************************************************/ +HAL_StatusTypeDef spi1_init(SPI_HandleTypeDef* hspi); + +/*********************************************************************** + * BRIEF: mpu6000_Init initializes the gyroscope and accelerometer * + * INFORMATION: * + * Utilizing the GyroZ clock * + * I2C bus disabled * + * Sample rate division = 0 * + * 256Hz DLPF * + * Full scale range of the gyroscope = ± 2000°/s * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_init(SPI_HandleTypeDef *hspi, gyro_t* gyro, accel_t* accel); + +/*********************************************************************** + * BRIEF: mpu6000_ReadGyro reads the three axis of the gyroscope and * + * stores the data, in °/s format, in the gyro struct * + * INFORMATION: * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_gyro(SPI_HandleTypeDef *hspi, gyro_t* gyro); + +/*********************************************************************** + * BRIEF: mpu6000_ReadGyro reads the three axis of the accelerometer * + * INFORMATION: * + * The data is both saved in raw format and in converted into the * + * number of Gs (9.82 m/s^2) the accelerometer is sensing * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_accel(SPI_HandleTypeDef *hspi, accel_t* accel); + +/*********************************************************************** + * BRIEF: mpu6000_ReadFIFO read the X, Y, and Z gyro axis from the * + * FIFO queue * + * INFORMATION: * + * Reads every complete set of gyro data (6 bytes) from the queue and * + * stores it it data_out * + * returns: * + * -1 if SPI transmission error occurs * + * -2 if FIFO queue overflow * + * -3 if FIFO queue doesn't contain any complete set of gyro data * + * else the number of bytes read from the FIFO queue * + ***********************************************************************/ +uint16_t mpu6000_read_fifo(SPI_HandleTypeDef* hspi, gyro_t* gyro, int16_t* data_out); + +/*********************************************************************** + * BRIEF: mpu6000_WhoAmI requests the product ID of the mpu6000 to * + * confirm device * + * INFORMATION: * + * returns true if correct device if found * + ***********************************************************************/ +bool mpu6000_who_am_i(SPI_HandleTypeDef *hspi); + +#endif /* DRIVERS_ACCEL_GYRO_H_ */ diff --git a/UAV-ControlSystem/src/drivers/accel_gyro.c b/UAV-ControlSystem/src/drivers/accel_gyro.c new file mode 100644 index 0000000..be17729 --- /dev/null +++ b/UAV-ControlSystem/src/drivers/accel_gyro.c @@ -0,0 +1,553 @@ +/* + * gyro.c + * + * Created on: 16 sep. 2016 + * Author: rsd12002 + */ + +#include + +/*********************************************************************** + * BRIEF: SPI1_Init initializes the SPI1 instance with predefined values* + * INFORMATION: * + * Mode = SPI_MODE_MASTER; * + * Direction = SPI_DIRECTION_2LINES; * + * DataSize = SPI_DATASIZE_8BIT; * + * CLKPolarity = SPI_POLARITY_LOW; * + * CLKPhase = SPI_PHASE_1EDGE; * + * NSS = SPI_NSS_HARD_OUTPUT; * + * BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128; * + * FirstBit = SPI_FIRSTBIT_MSB; * + * TIMode = SPI_TIMODE_DISABLE; * + * CRCCalculation = SPI_CRCCALCULATION_DISABLED; * + ***********************************************************************/ +HAL_StatusTypeDef spi1_init(SPI_HandleTypeDef* hspi) +{ + HAL_StatusTypeDef status; + + hspi->Instance = MPU6000_SPI_INSTANCE; + hspi->Init.Mode = SPI_MODE_MASTER; + hspi->Init.Direction = SPI_DIRECTION_2LINES; + hspi->Init.DataSize = SPI_DATASIZE_8BIT; + hspi->Init.CLKPolarity = SPI_POLARITY_LOW; + hspi->Init.CLKPhase = SPI_PHASE_1EDGE; + hspi->Init.NSS = SPI_NSS_HARD_OUTPUT; + /* mpu6000 SCLK Clock Frequency max 1 MHz */ + hspi->Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128; + hspi->Init.FirstBit = SPI_FIRSTBIT_MSB; + hspi->Init.TIMode = SPI_TIMODE_DISABLE; + hspi->Init.CRCCalculation = SPI_CRCCALCULATION_DISABLED; + + HAL_SPI_MspInit(hspi); + status = HAL_SPI_Init(hspi); + return status; +} + +/*********************************************************************** + * BRIEF: HAL_SPI_MspInit initializes the SPI1 clock and GPIO pins used * + * INFORMATION: * + * Is called automatically * + ***********************************************************************/ +void HAL_SPI_MspInit(SPI_HandleTypeDef *hspi) +{ + if(__SPI1_IS_CLK_DISABLED()) + __SPI1_CLK_ENABLE(); + + /**SPI1 GPIO Configuration + PA7 ------> SPI1_MOSI + PA6 ------> SPI1_MISO + PA5 ------> SPI1_SCK + PA4 ------> SPI1_NSS + */ + + GPIO_InitTypeDef gpioInit; + + gpioInit.Pin = GPIO_PIN_7 | GPIO_PIN_6 | GPIO_PIN_5; + gpioInit.Mode = GPIO_MODE_AF_PP; + gpioInit.Pull = GPIO_NOPULL; + gpioInit.Speed = GPIO_SPEED_LOW; + gpioInit.Alternate = GPIO_AF5_SPI1; + HAL_GPIO_Init(GPIOA, &gpioInit); + + gpioInit.Pin = GPIO_PIN_4; + gpioInit.Mode = GPIO_MODE_OUTPUT_PP; + gpioInit.Pull = GPIO_PULLUP; + gpioInit.Speed = GPIO_SPEED_LOW; + HAL_GPIO_Init(GPIOA, &gpioInit); +} + +/*********************************************************************** + * BRIEF: mpu6000_Transmit transmits the specified register and data * + * to the mpu6000 * + * INFORMATION: * + * data[0] = register * + * data[1] = command * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_transmit(SPI_HandleTypeDef *hspi, uint8_t* data) +{ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET); + err = HAL_SPI_Transmit(hspi, data, 2, 10); + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET); + + HAL_Delay(1); + return err; +} + +/*********************************************************************** + * BRIEF: mpu6000_TransmitReceive is used to request data from the * + * mpu6000 which it stores in data * + * INFORMATION: * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_transmit_receive(SPI_HandleTypeDef *hspi, uint8_t reg, uint8_t* data, uint8_t length) +{ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + + uint8_t tmpData[length+1]; /* Temporary data variable */ + tmpData[0] = reg | 0x80; /* Flips the request for data bit */ + + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET); + err = HAL_SPI_TransmitReceive(hspi, tmpData, tmpData, length+1, 10); + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET); + if(err != HAL_OK) + return err; + + HAL_Delay(1); + + if(length == 1) + { + *data = tmpData[1]; + } + else + { + for(int i = 1; i < length; i++) + { + data[i-1] = tmpData[i]; + } + } + return err; +} + +/*********************************************************************** + * BRIEF: mpu6000_read_offset reads and returns the offset of the * + * gyroscope and accelerometer * + * INFORMATION: * + * Is automatically called when mpu6000_init is called * + * The flight controller needs to be stationary when this function is * + * called * + * When the UAV is finished this data could be saved so that the * + * offset doesn't need to be read every time * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_offset(SPI_HandleTypeDef *hspi, gyro_t* gyro, accel_t* accel) +{ + uint8_t dataG[6]; /* Temporary data variable used to receive gyroscope data */ + uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + + err = mpu6000_transmit_receive(hspi, MPU_RA_ACCEL_XOUT_H, dataA, 6); + if(err != HAL_OK) + return err; + + err = mpu6000_transmit_receive(hspi, MPU_RA_GYRO_XOUT_H, dataG, 6); + if(err != HAL_OK) + return err; + +#ifdef YAW_ROT_0 + gyro->offsetX = -(((int16_t)dataG[0] << 8) | dataG[1]); + gyro->offsetY = -(((int16_t)dataG[2] << 8) | dataG[3]); + gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); + + accel->offsetX = -((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetY = -((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_90) + gyro->offsetX = -(((int16_t)dataG[2] << 8) | dataG[3]); + gyro->offsetY = (((int16_t)dataG[0] << 8) | dataG[1]); + gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); + + accel->offsetX = -((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetY = ((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_180) + gyro->offsetX = (((int16_t)dataG[0] << 8) | dataG[1]); + gyro->offsetY = (((int16_t)dataG[2] << 8) | dataG[3]); + gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); + + accel->offsetX = ((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetY = ((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#elif defined(YAW_ROT_270) + gyro->offsetX = (((int16_t)dataG[2] << 8) | dataG[3]); + gyro->offsetY = -(((int16_t)dataG[0] << 8) | dataG[1]); + gyro->offsetZ = (((int16_t)dataG[4] << 8) | dataG[5]); + + accel->offsetX = ((int16_t)dataA[2] << 8) | dataA[3]; + accel->offsetY = -((int16_t)dataA[0] << 8) | dataA[1]; + accel->offsetZ = accel->accel1G - (((int16_t)dataA[4] << 8) | dataA[5]); +#endif + + return err; +} + +/*********************************************************************** + * BRIEF: mpu6000_Init initializes the gyroscope and accelerometer * + * INFORMATION: * + * Utilizing the GyroZ clock * + * I2C bus disabled * + * Sample rate division = 0 * + * 256Hz Digital Low Pass Filter (DLPF) * + * Full scale range of the gyroscope = ± 2000°/s * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_init(SPI_HandleTypeDef *hspi, gyro_t* gyro, accel_t* accel) +{ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + uint8_t reg[2]; /* Register address and bit selection */ + + // Reset device + reg[0] = MPU_RA_PWR_MGMT_1; + reg[1] = BIT_H_RESET; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // Reset Signal Path + HAL_Delay(10); + reg[0] = MPU_RA_SIGNAL_PATH_RESET; + reg[1] = BIT_GYRO | BIT_ACC; + err = mpu6000_transmit(hspi, reg); + HAL_Delay(150); + if(err != HAL_OK) + return err; + + // Wake up device and select GyroZ clock (better performance) + reg[0] = MPU_RA_PWR_MGMT_1; + reg[1] = 0b00001000 | MPU_CLK_SEL_PLLGYROZ; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // Disable I2C bus + reg[0] = MPU_RA_USER_CTRL; + reg[1] = 0x50; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // No standby mode + reg[0] = MPU_RA_PWR_MGMT_2; + reg[1] = 0x00; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // Sample rate + reg[0] = MPU_RA_SMPLRT_DIV; + reg[1] = 0x00; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // Digital Low Pass Filter (DLPF) + reg[0] = MPU_RA_CONFIG; + reg[1] = BITS_DLPF_CFG_256HZ; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // FIFO + reg[0] = MPU_RA_FIFO_EN; + // Temperature GyroX GyroY GyroZ Accel Slave Slave Slave + reg[1] = 0 << 7 | 1 << 6 | 1 << 5 | 1 << 4 | 0 << 3 | 0 << 2 | 0 << 1 | 0 << 0; + err = mpu6000_transmit(hspi, reg); + if(err != HAL_OK) + return err; + + // Gyroscope 2000DPS + reg[0] = MPU_RA_GYRO_CONFIG; + reg[1] = BITS_FS_2000DPS; + err = mpu6000_transmit(hspi, reg); + gyro->scale = (1.0f / 16.4f); + if(err != HAL_OK) + return err; + + // Accelerometer 16±G + reg[0] = MPU_RA_ACCEL_CONFIG; + reg[1] = BITS_FS_16G; + err = mpu6000_transmit(hspi, reg); + accel->accel1G = 2048; // (32768/16)/G + if(err != HAL_OK) + return err; + + mpu6000_read_offset(hspi, gyro, accel); + + return HAL_OK; +} + +/*********************************************************************** + * BRIEF: mpu6000_ReadGyro reads the three axis of the gyroscope and * + * stores the data, in °/s format, in the gyro struct * + * INFORMATION: * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_gyro(SPI_HandleTypeDef *hspi, gyro_t* gyro) +{ + uint8_t data[6]; /* Temporary data variable used to receive gyroscope data */ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + + err = mpu6000_transmit_receive(hspi, MPU_RA_GYRO_XOUT_H, data, 6); + if(err != HAL_OK) + return err; + +#ifdef YAW_ROT_0 + gyro->gyroX = -(((int16_t)data[0] << 8) | data[1]); + gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; + gyro->gyroY = -(((int16_t)data[2] << 8) | data[3]); + gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; + gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); + gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; +#elif defined(YAW_ROT_90) + gyro->gyroX = -(((int16_t)data[2] << 8) | data[3]); + gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; + gyro->gyroY = (((int16_t)data[0] << 8) | data[1]); + gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; + gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); + gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; +#elif defined(YAW_ROT_180) + gyro->gyroX = (((int16_t)data[0] << 8) | data[1]); + gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; + gyro->gyroY = (((int16_t)data[2] << 8) | data[3]); + gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; + gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); + gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; +#elif defined(YAW_ROT_270) + gyro->gyroX = (((int16_t)data[2] << 8) | data[3]); + gyro->gyroX = (gyro->gyroX - gyro->offsetX) * gyro->scale; + gyro->gyroY = -(((int16_t)data[0] << 8) | data[1]); + gyro->gyroY = (gyro->gyroY - gyro->offsetY) * gyro->scale; + gyro->gyroZ = (((int16_t)data[4] << 8) | data[5]); + gyro->gyroZ = (gyro->gyroZ - gyro->offsetZ) * gyro->scale; +#else + No Yaw Direction Defined +#endif + + return err; +} + +/*********************************************************************** + * BRIEF: mpu6000_ReadGyro reads the three axis of the accelerometer * + * INFORMATION: * + * The data is both saved in raw format and in converted into the * + * number of G (9.82 m/s^2) the accelerometer is sensing * + ***********************************************************************/ +HAL_StatusTypeDef mpu6000_read_accel(SPI_HandleTypeDef *hspi, accel_t* accel) +{ + uint8_t data[6]; /* Temporary data variable used to receive accelerometer data */ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + + err = mpu6000_transmit_receive(hspi, MPU_RA_ACCEL_XOUT_H, data, 6); + +#ifdef YAW_ROT_0 + accel->accelXraw = -((int16_t)data[0] << 8) | data[1]; + accel->accelXraw = accel->accelXraw - accel->offsetX; + accel->accelYraw = -((int16_t)data[2] << 8) | data[3]; + accel->accelYraw = accel->accelYraw - accel->offsetY; + accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; + accel->accelZraw = accel->accelZraw + accel->offsetZ; + + accel->accelXconv = ((float)accel->accelXraw / accel->accel1G); + accel->accelYconv = ((float)accel->accelYraw / accel->accel1G); + accel->accelZconv = ((float)accel->accelZraw / accel->accel1G); +#elif defined(YAW_ROT_90) + accel->accelXraw = -((int16_t)data[2] << 8) | data[3]; + accel->accelXraw = accel->accelXraw - accel->offsetX; + accel->accelYraw = ((int16_t)data[0] << 8) | data[1]; + accel->accelYraw = accel->accelYraw - accel->offsetY; + accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; + accel->accelZraw = accel->accelZraw + accel->offsetZ; + + accel->accelXconv = (float)(accel->accelYraw / accel->accel1G); + accel->accelYconv = (float)(accel->accelXraw / accel->accel1G); + accel->accelZconv = (float)(accel->accelZraw / accel->accel1G); +#elif defined(YAW_ROT_180) + accel->accelXraw = ((int16_t)data[0] << 8) | data[1]; + accel->accelXraw = accel->accelXraw - accel->offsetX; + accel->accelYraw = ((int16_t)data[2] << 8) | data[3]; + accel->accelYraw = accel->accelYraw - accel->offsetY; + accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; + accel->accelZraw = accel->accelZraw + accel->offsetZ; + + accel->accelXconv = ((float)accel->accelXraw / accel->accel1G); + accel->accelYconv = ((float)accel->accelYraw / accel->accel1G); + accel->accelZconv = ((float)accel->accelZraw / accel->accel1G); +#elif defined(YAW_ROT_270) + accel->accelXraw = ((int16_t)data[2] << 8) | data[3]; + accel->accelXraw = accel->accelXraw - accel->offsetX; + accel->accelYraw = -((int16_t)data[0] << 8) | data[1]; + accel->accelYraw = accel->accelYraw - accel->offsetY; + accel->accelZraw = ((int16_t)data[4] << 8) | data[5]; + accel->accelZraw = accel->accelZraw + accel->offsetZ; + + accel->accelXconv = ((float)accel->accelYraw / accel->accel1G); + accel->accelYconv = ((float)accel->accelXraw / accel->accel1G); + accel->accelZconv = ((float)accel->accelZraw / accel->accel1G); +#endif + + return err; +} + +/*********************************************************************** + * BRIEF: mpu6000_ReadFIFO read the X, Y, and Z gyroscope axis from the * + * FIFO queue * + * INFORMATION: * + * Reads every complete set of gyro data (6 bytes) from the queue and * + * stores it it data_out * + * returns: * + * -1 if SPI transmission error occurs * + * -2 if FIFO queue overflow * + * -3 if FIFO queue doesn't contain any complete set of gyro data * + * else the number of bytes read from the FIFO queue * + ***********************************************************************/ +uint16_t mpu6000_read_fifo(SPI_HandleTypeDef* hspi, gyro_t* gyro, int16_t* data_out) +{ + uint16_t fifoCount = 0; /* Number of bytes in the FIFO queue */ + uint8_t countH = 0; /* Bits 8-16 of the number of bytes in the FIFO queue */ + uint8_t countL = 0; /* Bits 0-7 of the number of bytes in the FIFO queue */ + uint16_t bytesRead = 0; /* Number of bytes actually read from the FIFO queue */ + HAL_StatusTypeDef err = 0; /* SPI transmission status variable */ + uint8_t reg[2]; /* Register address and bit selection */ + + err = mpu6000_transmit_receive(hspi, MPU_RA_FIFO_COUNTH, &countH, 1); + if(err != HAL_OK) + return -1; + + err = mpu6000_transmit_receive(hspi, MPU_RA_FIFO_COUNTL, &countL, 1); + if(err != HAL_OK) + return -1; + + + fifoCount = (uint16_t)((countH << 8) | countL); + if (fifoCount == 1024) + { + reg[0] = MPU_RA_USER_CTRL; + reg[1] = BIT_FIFO_RESET; + mpu6000_transmit(hspi, reg); + return -2; + } + + if (fifoCount < 6) + return -3; + + /* Make sure that only complete sets of gyro data are read */ + bytesRead = (fifoCount/6)*6; + + uint8_t fifobuffer[bytesRead+1]; + + fifobuffer[0] = MPU_RA_FIFO_R_W | 0x80; + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_RESET); + err = HAL_SPI_TransmitReceive(hspi, fifobuffer, fifobuffer, bytesRead+1, 10); + HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4, GPIO_PIN_SET); + + if(err != HAL_OK) + return -1; + + uint8_t xL, xH, yL, yH, zL, zH; + for(int j = 1; 6+((j-1)*6) < bytesRead+1; j++) + { + xH = fifobuffer[1+((j-1)*6)]; + xL = fifobuffer[2+((j-1)*6)]; + yH = fifobuffer[3+((j-1)*6)]; + yL = fifobuffer[4+((j-1)*6)]; + zH = fifobuffer[5+((j-1)*6)]; + zL = fifobuffer[6+((j-1)*6)]; + +#ifdef YAW_ROT_0 + + + data_out[0+((j-1)*3)] = -(((int16_t)xH << 8) | xL); // X + data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; + + data_out[1+((j-1)*3)] = -(((int16_t)yH << 8) | yL); // Y + data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; + + data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z + data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; + +#elif defined(YAW_ROT_90) + data_out[0+((j-1)*3)] = -(((int16_t)yH << 8) | yL); // X + data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; + + data_out[1+((j-1)*3)] = (((int16_t)xH << 8) | xL); // Y + data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; + + data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z + data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; +#elif defined(YAW_ROT_180) + data_out[0+((j-1)*3)] = (((int16_t)xH << 8) | xL); // X + data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; + + data_out[1+((j-1)*3)] = (((int16_t)yH << 8) | yL); // Y + data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; + + data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z + data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; +#elif defined(YAW_ROT_270) + data_out[0+((j-1)*3)] = (((int16_t)yH << 8) | yL); // X + data_out[0+((j-1)*3)] = (data_out[0+((j-1)*3)] - gyro->offsetX) * gyro->scale; + + data_out[1+((j-1)*3)] = -(((int16_t)xH << 8) | xL); // Y + data_out[1+((j-1)*3)] = (data_out[1+((j-1)*3)] - gyro->offsetY) * gyro->scale; + + data_out[2+((j-1)*3)] = (((int16_t)zH << 8) | zL); // Z + data_out[2+((j-1)*3)] = (data_out[2+((j-1)*3)] - gyro->offsetZ) * gyro->scale; +#endif + } + return bytesRead; +} + +/*********************************************************************** + * BRIEF: mpu6000_WhoAmI requests the product ID of the mpu6000 to * + * confirm device * + * INFORMATION: * + * returns true if correct device and revision if found * + ***********************************************************************/ +bool mpu6000_who_am_i(SPI_HandleTypeDef *hspi) +{ + HAL_StatusTypeDef err; /* SPI transmission status variable */ + uint8_t data = 0; /* Received data is placed in this variable */ + + + err = mpu6000_transmit_receive(hspi, MPU_RA_WHO_AM_I, &data, 1); + if(err != HAL_OK) + return false; + + if (data != MPU6000_WHO_AM_I_CONST) + { + return false; + } + + + /* look for a product ID we recognize */ + err = mpu6000_transmit_receive(hspi, MPU_RA_PRODUCT_ID, &data, 1); + if(err != HAL_OK) + return false; + + // verify product revision + switch (data) + { + case MPU6000ES_REV_C4: + case MPU6000ES_REV_C5: + case MPU6000_REV_C4: + case MPU6000_REV_C5: + case MPU6000ES_REV_D6: + case MPU6000ES_REV_D7: + case MPU6000ES_REV_D8: + case MPU6000_REV_D6: + case MPU6000_REV_D7: + case MPU6000_REV_D8: + case MPU6000_REV_D9: + case MPU6000_REV_D10: + return true; + } + + return false; +} From 9e6ba36ac0e3cea31ce29e2dc2bdc49bdebe0bfe Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Mon, 3 Oct 2016 09:42:16 +0200 Subject: [PATCH 15/18] I2C working with compass only so far --- UAV-ControlSystem/inc/drivers/I2C.h | 89 ++++++- UAV-ControlSystem/inc/stm32f4xx_revo.h | 10 + UAV-ControlSystem/src/drivers/I2C.c | 95 +++++--- UAV-ControlSystem/src/drivers/i2c_test.c | 297 ----------------------- UAV-ControlSystem/src/main.c | 108 --------- 5 files changed, 151 insertions(+), 448 deletions(-) delete mode 100644 UAV-ControlSystem/src/drivers/i2c_test.c diff --git a/UAV-ControlSystem/inc/drivers/I2C.h b/UAV-ControlSystem/inc/drivers/I2C.h index 07750b3..01c7e65 100644 --- a/UAV-ControlSystem/inc/drivers/I2C.h +++ b/UAV-ControlSystem/inc/drivers/I2C.h @@ -1,9 +1,15 @@ -/* - * I2C.h - * - * Created on: 21 sep. 2016 - * Author: len12007 - */ +/*************************************************************************** +* NAME: I2C.h * +* AUTHOR: Lennart Eriksson * +* PURPOSE: Enabole the I2C Communication channel on the Revo board * +* INFORMATION: * +* This file initilizes the I2C communication that can be used by barometer * +* communication etc. * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ #ifndef DRIVERS_I2C_H_ #define DRIVERS_I2C_H_ @@ -12,9 +18,12 @@ /****************************************************************************** * BRIEF: Configure the I2C bus to be used * -* INFORMATION: * +* INFORMATION: This function only implements I2C1 or I2C2 which are available * +* on the REVO board * ******************************************************************************/ -void i2c_configure(I2C_TypeDef* i2c, uint16_t sda_pin, uint16_t scl_pin, uint32_t my_address); +bool i2c_configure(I2C_TypeDef* i2c, + I2C_HandleTypeDef* out_profile, + uint32_t my_address); /****************************************************************************** * BRIEF: Get data over the I2C bus * @@ -25,7 +34,10 @@ void i2c_configure(I2C_TypeDef* i2c, uint16_t sda_pin, uint16_t scl_pin, uint32_ * the data is sent one byte at a time with an acknowledge in between * * every byte, as per the standard. Returns true if successful * ******************************************************************************/ -bool i2c_receive(uint8_t slave_address, uint8_t* buffer, uint32_t length); +bool i2c_receive(I2C_HandleTypeDef* profile, + uint8_t slave_address, + uint8_t* buffer, + uint32_t length); /****************************************************************************** * BRIEF: Send data over the I2C bus * @@ -36,7 +48,64 @@ bool i2c_receive(uint8_t slave_address, uint8_t* buffer, uint32_t length); * the data is sent one byte at a time with an acknowledge in between * * every byte, as per the standard. Returns true if successful * ******************************************************************************/ -bool i2c_send(uint8_t slave_address, uint8_t* data, uint32_t length); +bool i2c_send(I2C_HandleTypeDef* profile, + uint8_t slave_address, + uint8_t* data, + uint32_t length); #endif /* DRIVERS_I2C_H_ */ + + + + +// ---------------------- I2C Working with the compas on the REVO board ------------------------------ + +/* +int main(void) +{ + + // Initialize the Hardware Abstraction Layer + HAL_Init(); + + // Configure the system clock to 100 MHz + system_clock_config(); + + I2C_HandleTypeDef i2c_profile; + + + //---- COMPAS WORKING ---- + i2c_configure(I2C1, &i2c_profile, 0x56); + + + + uint32_t address = 0b00011110; + uint8_t start_request_1[2] = { 0b00000000, 0b01110000 }; + uint8_t start_request_2[2] = { 0b00000001, 0b10100000 }; + uint8_t start_request_3[2] = { 0b00000010, 0b00000000 }; + + + uint8_t request_data[1] = { 0b00000110 }; + uint8_t reset_pointer_data[1] = { 0b00000011 }; + uint8_t response_data[6] = { 0x0 }; + + + i2c_send(&i2c_profile, address, &start_request_1, 2); + i2c_send(&i2c_profile, address, &start_request_2, 2); + i2c_send(&i2c_profile, address, &start_request_3, 2); + + // Delay for at least 6 ms for system startup to finish + HAL_Delay(10); + + while (1) + { + i2c_send(&i2c_profile, address, &request_data, 1); + i2c_receive(&i2c_profile, address, &response_data, 6); + i2c_send(&i2c_profile, address, &reset_pointer_data, 1); + + // HAL_Delay(100); + if(response_data[0] != 0) + response_data[0] = 0; + } +} +*/ diff --git a/UAV-ControlSystem/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h index fa46876..6854d54 100644 --- a/UAV-ControlSystem/inc/stm32f4xx_revo.h +++ b/UAV-ControlSystem/inc/stm32f4xx_revo.h @@ -83,6 +83,16 @@ #define USART6_TX_PORT GPIOC +/* I2C */ +#define I2C1_SDA_PIN GPIO_PIN_9 +#define I2C1_SCL_PIN GPIO_PIN_8 +#define I2C1_PORT GPIOB + +#define I2C2_SCL_PIN GPIO_PIN_10 +#define I2C2_SDA_PIN GPIO_PIN_11 +#define I2C2_PORT GPIOB + + /* Gyro */ #define GYRO #define MPU6000_CS_PIN GPIO_PIN_4 diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c index c93dd0a..f978e40 100644 --- a/UAV-ControlSystem/src/drivers/I2C.c +++ b/UAV-ControlSystem/src/drivers/I2C.c @@ -1,39 +1,59 @@ -/* - * I2C.c - * - * Created on: 21 sep. 2016 - * Author: len12007 - */ +/*************************************************************************** +* NAME: I2C.c * +* AUTHOR: Lennart Eriksson * +* PURPOSE: Enabole the I2C Communication channel on the Revo board * +* INFORMATION: * +* This file initilizes the I2C communication that can be used by barometer * +* communication etc. * +* * +* GLOBAL VARIABLES: * +* Variable Type Description * +* -------- ---- ----------- * +***************************************************************************/ #include "drivers/I2C.h" - -// the I2C handle sending data over -I2C_HandleTypeDef I2C_handle; +#include "stm32f4xx_revo.h" /****************************************************************************** * BRIEF: Configure the I2C bus to be used * * INFORMATION: * ******************************************************************************/ -void i2c_configure(I2C_TypeDef* i2c, uint16_t sda_pin, uint16_t scl_pin, uint32_t my_address) +bool i2c_configure(I2C_TypeDef *i2c, + I2C_HandleTypeDef *out_profile, + uint32_t my_address) { uint8_t i2c_af; + uint16_t sda_pin, scl_pin; + GPIO_TypeDef *i2c_port; - // get the correct alternate function for the selected I2C + // get the correct alternate function and pins for the selected I2C + // Only I2C1 and I2C2 is available on the REVO board if(i2c == I2C1) + { i2c_af = GPIO_AF4_I2C1; + i2c_port = I2C1_PORT; + sda_pin = I2C1_SDA_PIN; + scl_pin = I2C1_SCL_PIN; + if(__HAL_RCC_I2C1_IS_CLK_DISABLED()) + __HAL_RCC_I2C1_CLK_ENABLE(); + } else if(i2c == I2C2) + { i2c_af = GPIO_AF4_I2C2; - else if(i2c == I2C3) - i2c_af = GPIO_AF4_I2C3; + i2c_port = I2C2_PORT; + sda_pin = I2C2_SDA_PIN; + scl_pin = I2C2_SCL_PIN; + if(__HAL_RCC_I2C2_IS_CLK_DISABLED()) + __HAL_RCC_I2C2_CLK_ENABLE(); + } else - i2c_af = GPIO_AF4_I2C1; - + return false; // The provided I2C is not correct + if(__HAL_RCC_GPIOB_IS_CLK_DISABLED()) + __HAL_RCC_GPIOB_CLK_ENABLE(); //Initialize pins for SCL and SDA GPIO_InitTypeDef GPIO_InitStruct; - __HAL_RCC_I2C1_CLK_ENABLE(); - __GPIOB_CLK_ENABLE(); GPIO_InitStruct.Pin = sda_pin | scl_pin; GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; GPIO_InitStruct.Pull = GPIO_PULLUP; @@ -42,16 +62,19 @@ void i2c_configure(I2C_TypeDef* i2c, uint16_t sda_pin, uint16_t scl_pin, uint32_ HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); //Initialize I2C communication - I2C_handle.Instance = i2c; - I2C_handle.Init.ClockSpeed = 100000; - I2C_handle.Init.DutyCycle = I2C_DUTYCYCLE_2; - I2C_handle.Init.OwnAddress1 = my_address; - I2C_handle.Init.OwnAddress2 = 0; - I2C_handle.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; - I2C_handle.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; - I2C_handle.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; - I2C_handle.Init.NoStretchMode = I2C_NOSTRETCH_ENABLE; - HAL_I2C_Init(&I2C_handle); + out_profile->Instance = i2c; + out_profile->Init.ClockSpeed = 100000; + out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2; + out_profile->Init.OwnAddress1 = my_address; + out_profile->Init.OwnAddress2 = 0; + out_profile->Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; + out_profile->Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; + out_profile->Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; + out_profile->Init.NoStretchMode = I2C_NOSTRETCH_ENABLE; + if(HAL_I2C_Init(out_profile) != HAL_OK) + return false; + + return true; } @@ -64,12 +87,15 @@ void i2c_configure(I2C_TypeDef* i2c, uint16_t sda_pin, uint16_t scl_pin, uint32_ * the data is sent one byte at a time with an acknowledge in between * * every byte, as per the standard. Returns true if successful * ******************************************************************************/ -bool i2c_receive(uint8_t slave_address, uint8_t* buffer, uint32_t length) +bool i2c_receive(I2C_HandleTypeDef* profile, + uint8_t slave_address, + uint8_t* buffer, + uint32_t length) { uint8_t i = 0; - while(HAL_I2C_Master_Receive(&I2C_handle, (slave_address << 1) | 1, buffer, length, 1000)!= HAL_OK && i++ < 10) + while(HAL_I2C_Master_Receive(profile, (slave_address << 1) | 1, buffer, length, 1000)!= HAL_OK && i++ < 10) {} - while (HAL_I2C_GetState(&I2C_handle) != HAL_I2C_STATE_READY) + while (HAL_I2C_GetState(profile) != HAL_I2C_STATE_READY) {} return (i < 10); @@ -84,16 +110,19 @@ bool i2c_receive(uint8_t slave_address, uint8_t* buffer, uint32_t length) * the data is sent one byte at a time with an acknowledge in between * * every byte, as per the standard. Returns true if successful * ***************************************************************************/ -bool i2c_send(uint8_t slave_address, uint8_t* data, uint32_t length) +bool i2c_send(I2C_HandleTypeDef* profile, + uint8_t slave_address, + uint8_t* data, + uint32_t length) { uint8_t i = 0; // try to send the data 10 times and if no acknowledge is received during these 10 messages we stop trying so that // the system don't gets stuck forever because a slave is unreachable - while(HAL_I2C_Master_Transmit(&I2C_handle,(slave_address << 1), (uint8_t*)data, length, 1000) != HAL_OK && i++ < 10) + while(HAL_I2C_Master_Transmit(profile,(slave_address << 1), (uint8_t*)data, length, 1000) != HAL_OK && i++ < 10) {} //Wait til the I2C bus is done with all sending - while (HAL_I2C_GetState(&I2C_handle) != HAL_I2C_STATE_READY){} + while (HAL_I2C_GetState(profile) != HAL_I2C_STATE_READY){} return (i < 10); } diff --git a/UAV-ControlSystem/src/drivers/i2c_test.c b/UAV-ControlSystem/src/drivers/i2c_test.c deleted file mode 100644 index dca5bc6..0000000 --- a/UAV-ControlSystem/src/drivers/i2c_test.c +++ /dev/null @@ -1,297 +0,0 @@ -// -//#include "stm32f4xx.h" -// -////_____ M A C R O S -//#define TRUE 1 -//#define FALSE 0 -//#define F_CPU 4000000UL // 4 MHz external XTAL -//#define SCL_CLOCK 100000L // I2C clock in Hz -//#define ADDR_W 0xEE // Module address write mode -//#define ADDR_R 0xEF // Module address read mode -//#define CMD_RESET 0x1E // ADC reset command -//#define CMD_ADC_READ 0x00 // ADC read command -//#define CMD_ADC_CONV 0x40 // ADC conversion command -//#define CMD_ADC_D1 0x00 // ADC D1 conversion -//#define CMD_ADC_D2 0x10 // ADC D2 conversion -//#define CMD_ADC_256 0x00 // ADC OSR=256 -//#define CMD_ADC_512 0x02 // ADC OSR=512 -//#define CMD_ADC_1024 0x04 // ADC OSR=1024 -//#define CMD_ADC_2048 0x06 // ADC OSR=2048 -//#define CMD_ADC_4096 0x08 // ADC OSR=4096 -//#define CMD_PROM_RD 0xA0 // Prom read command -////_____ I N C L U D E S -//#include -////#include -////#include -////#include -//#include -////_____ D E F I N I T I O N S -//unsigned char i2c_start(unsigned char address); -//void i2c_stop(void); -//unsigned char i2c_write( unsigned char data ); -//unsigned char i2c_readAck(void); -//unsigned char i2c_readNak(void); -//void cmd_reset(void); -//unsigned long cmd_adc(char cmd); -//unsigned int cmd_prom(char coef_num); -//unsigned char crc4(unsigned int n_prom[]); -////******************************************************** -////! @brief send I2C start condition and the address byte -////! -////! @return 0 -////******************************************************** -//unsigned char i2c_start(unsigned char address) -//{ -// unsigned char twst; -// -// I2C1->CR1 = 1 << 8 | 1 << 0; -// -// TWCR = (1<>1]) & 0x00FF); -//else n_rem ^= (unsigned short) (n_prom[cnt>>1]>>8); -//for (n_bit = 8; n_bit > 0; n_bit--) -//{ -//if (n_rem & (0x8000)) -//{ -//n_rem = (n_rem << 1) ^ 0x3000; } -//else -//{ -//n_rem = (n_rem << 1); -//} -//} -//} -//n_rem= (0x000F & (n_rem >> 12)); // final 4-bit reminder is CRC code -//n_prom[7]=crc_read; // restore the crc_read to its original place -//return (n_rem ^ 0x0); -//} -////******************************************************** -////! @brief main program -////! -////! @return 0 -////******************************************************** -//int main (void) -//{ -//unsigned long D1; // ADC value of the pressure conversion -//unsigned long D2; // ADC value of the temperature conversion -//unsigned int C[8]; // calibration coefficients -//double P; // compensated pressure value -//double T; // compensated temperature value -//double dT; // difference between actual and measured temperature -//double OFF; // offset at actual temperature -//double SENS; // sensitivity at actual temperature -//int i; -//unsigned char n_crc; // crc value of the prom -// -//// setup the ports -//DDRA = 0xFE; -//DDRB = 0x0F; //SPI pins as input -//DDRC = 0x03; // I2C pins as output -//DDRD = 0x82; // RS out and tx out; -// -//PORTA = 0x1F; // I2C pin high -//PORTB = 0xF0; -//PORTC = 0x01; -//PORTD = 0x00; -// -//// initialize the I2C hardware module -//TWSR = 0; // no prescaler -//TWBR = ((F_CPU/SCL_CLOCK)-16)/2; // set the I2C speed -// -//D1=0; -//D2=0; -//cmd_reset(); // reset IC -//for (i=0;i<8;i++){ C[i]=cmd_prom(i);} // read coefficients -//n_crc=crc4(C); // calculate the CRC -//for(;;) // loop without stopping -//{ -//D2=cmd_adc(CMD_ADC_D2+CMD_ADC_4096); // read D2 -//D1=cmd_adc(CMD_ADC_D1+CMD_ADC_4096); // read D1 -//// calcualte 1st order pressure and temperature (MS5607 1st order algorithm) -//dT=D2-C[5]*pow(2,8); -//OFF=C[2]*pow(2,17)+dT*C[4]/pow(2,6); -//SENS=C[1]*pow(2,16)+dT*C[3]/pow(2,7); -// -//T=(2000+(dT*C[6])/pow(2,23))/100; -//P=(((D1*SENS)/pow(2,21)-OFF)/pow(2,15))/100; -//// place to use P, T, put them on LCD, send them trough RS232 interface... -//} -// -//return 0; -//} diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index fe16f0b..e69de29 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -1,108 +0,0 @@ -/** - ****************************************************************************** - * @file main.c - * @author Ac6 - * @version V1.0 - * @date 01-December-2013 - * @brief Default main function. - * Awesome ADC fix start project here to be good awesome!! - ****************************************************************************** -*/ - - -#include "drivers/adc.h" -#include "drivers/system_clock.h" -#include "drivers/I2C.h" -#include "stm32f4xx.h" -#include "system_variables.h" -#include "utilities.h" -#include - - -int main(void) -{ - - // Initialize the Hardware Abstraction Layer - HAL_Init(); - - // Configure the system clock to 100 MHz - system_clock_config(); - - - -/* COMPAS WORKING - i2c_configure(I2C1, GPIO_PIN_8, GPIO_PIN_9, 0x56); - - - - uint32_t address = 0b00011110; - uint8_t start_request_1[2] = { 0b00000000, 0b01110000 }; - uint8_t start_request_2[2] = { 0b00000001, 0b10100000 }; - uint8_t start_request_3[2] = { 0b00000010, 0b00000000 }; - - - uint8_t request_data[1] = { 0b00000110 }; - uint8_t reset_pointer_data[1] = { 0b00000011 }; - uint8_t response_data[6] = { 0x0 }; - - - i2c_send(address, &start_request_1, 2); - i2c_send(address, &start_request_2, 2); - i2c_send(address, &start_request_3, 2); - - // Delay for at least 6 ms for system startup to finish - HAL_Delay(10); - - while (1) - { - i2c_send(address, &request_data, 1); - i2c_receive(address, &response_data, 6); - i2c_send(address, &reset_pointer_data, 1); - - // HAL_Delay(100); - if(response_data[0] != 0) - response_data[0] = 0; - } -*/ - - - i2c_configure(I2C1, GPIO_PIN_8, GPIO_PIN_9, 0x56); - uint32_t address = 0x77; - uint8_t reset_data[1] = { 0x1E }; - - uint8_t init_data[1] = { 0xA2 }; - uint8_t init_rsp_data[2] = { 0x00 }; - - uint8_t request_data[500] = { 0xFF }; - uint8_t response_data[2] = { 0x0 }; - -// while(count--) - i2c_send(address, address, 1); - i2c_send(reset_data[0], &reset_data, 1); - - HAL_Delay(50); - - for(int i = 0; i < 6; i++) - { - if(i2c_send(address, &init_data, 1) == true) - i2c_receive(address, &response_data, 2); - init_data[0] += 2; - } - - - HAL_Delay(20); - - while (1) - { - if(i2c_send(address, &request_data, 1) == true) - i2c_receive(address, &response_data, 2); - -// HAL_Delay(100); - if(response_data[0] != 0) - response_data[0] = 0; - } - - for(;;); -} - - From 6983d53f8eb2972e508afbcb1629ce16dd6d29af Mon Sep 17 00:00:00 2001 From: Lennart Eriksson Date: Mon, 3 Oct 2016 09:51:09 +0200 Subject: [PATCH 16/18] Finalizing the I2C --- UAV-ControlSystem/inc/drivers/I2C.h | 6 +++--- UAV-ControlSystem/src/drivers/I2C.c | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/UAV-ControlSystem/inc/drivers/I2C.h b/UAV-ControlSystem/inc/drivers/I2C.h index 01c7e65..44fea89 100644 --- a/UAV-ControlSystem/inc/drivers/I2C.h +++ b/UAV-ControlSystem/inc/drivers/I2C.h @@ -68,8 +68,7 @@ int main(void) // Initialize the Hardware Abstraction Layer HAL_Init(); - // Configure the system clock to 100 MHz - system_clock_config(); + init_system(); I2C_HandleTypeDef i2c_profile; @@ -89,7 +88,8 @@ int main(void) uint8_t reset_pointer_data[1] = { 0b00000011 }; uint8_t response_data[6] = { 0x0 }; - + // This sequence starts the compass by first initializing it with the first 2 send + // The third is there to say that the system should be continous communication i2c_send(&i2c_profile, address, &start_request_1, 2); i2c_send(&i2c_profile, address, &start_request_2, 2); i2c_send(&i2c_profile, address, &start_request_3, 2); diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c index f978e40..ed643b0 100644 --- a/UAV-ControlSystem/src/drivers/I2C.c +++ b/UAV-ControlSystem/src/drivers/I2C.c @@ -59,7 +59,7 @@ bool i2c_configure(I2C_TypeDef *i2c, GPIO_InitStruct.Pull = GPIO_PULLUP; GPIO_InitStruct.Speed = GPIO_SPEED_HIGH; GPIO_InitStruct.Alternate = i2c_af; - HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); + HAL_GPIO_Init(i2c_port, &GPIO_InitStruct); //Initialize I2C communication out_profile->Instance = i2c; From ad9232147c76926814f2a31b047a70c13e53a73f Mon Sep 17 00:00:00 2001 From: philsson Date: Mon, 3 Oct 2016 11:45:19 +0200 Subject: [PATCH 17/18] Code cleanups and optimization --- UAV-ControlSystem/inc/config/eeprom.h | 13 +- UAV-ControlSystem/inc/system_variables.h | 4 +- UAV-ControlSystem/src/config/eeprom.c | 312 +++++++++++++++++++++-- UAV-ControlSystem/src/main.c | 7 + 4 files changed, 309 insertions(+), 27 deletions(-) diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 6237a15..3568d70 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -17,10 +17,19 @@ #ifndef CONFIG_EEPROM_H_ #define CONFIG_EEPROM_H_ - +#include "stm32f4xx.h" /* Defines where emulated EEPROM starts from - OBS! Also defined in LinkerScript.id */ -#define EEPROM_BASE_ADDR 0x080DAC00 //0x080E0000 +#define EEPROM_BASE_ADDR 0x080E0000 +#define EEPROM_PROFILE_SIZE 20 // Size in uint32_t + +typedef enum { + PROFILE_1 = 1, + PROFILE_2, + PROFILE_3 +} ACTIVE_PROFILE; + +extern uint8_t active_profile; /*********************************************************************** * BRIEF: Writes EEPROM data to FLASH * diff --git a/UAV-ControlSystem/inc/system_variables.h b/UAV-ControlSystem/inc/system_variables.h index 2ae0606..0cc221e 100644 --- a/UAV-ControlSystem/inc/system_variables.h +++ b/UAV-ControlSystem/inc/system_variables.h @@ -14,13 +14,13 @@ #ifndef SYSTEM_VARIABLES_H_ #define SYSTEM_VARIABLES_H_ - +#define EEPROM_SYS_VERSION 101 #define ADC_STATE #include "stm32f4xx.h" - +extern uint8_t pid_pitch_pk; diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index ca8d427..5b9d4cf 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -17,18 +17,68 @@ #include "config/eeprom.h" #include "drivers/adc.h" #include "drivers/uart1_inverter.h" +#include "system_variables.h" +#include "utilities.h" +/* Reads the EEPROM version from EEPROM - Is compared to EEPROM_SYS_VERSION */ +uint8_t stored_eeprom_identifier; + +/* Buffer where we temporarily store profiles we not use as we erase eeprom before write */ +uint32_t eeprom_profile_tmp_a[EEPROM_PROFILE_SIZE]; +uint32_t eeprom_profile_tmp_b[EEPROM_PROFILE_SIZE]; + + +/* Created CRC */ +uint8_t calculated_crc; +uint8_t read_crc; + +uint8_t active_profile = 0; + +typedef enum { + EEPROM_VERSION = 0, + + /* Counts the amount of system settings */ + EEPROM_HEADER_COUNT +} EEPROM_HEADER_ID_t; /*********************************************************************** -* BRIEF: List of all EEPROM values * +* BRIEF: List of all System EEPROM values * * INFORMATION: This content of this struct will be used in EEPROM * ***********************************************************************/ typedef enum { - EEPROM_ADC_SCALES = 0, + EEPROM_ACTIVE_PROFILE = 0, + EEPROM_ADC_SCALES, EEPROM_UART1_RX_INV, - EEPROM_COUNT // Needs to be the last element as is used as counter -} EEPROM_ID_t; + /* Counts the amount of system settings */ + EEPROM_SYS_COUNT +} EEPROM_SYS_ID_t; + +/*********************************************************************** +* BRIEF: List of all Profile EEPROM values * +* INFORMATION: This content of this struct will be used in EEPROM * +***********************************************************************/ +typedef enum { + EEPROM_PID_ROLL_KP = 0, + + /* Counts the amount of settings in profile */ + EEPROM_PROFILE_COUNT +} EEPROM_PROFILE_ID_t; + +typedef enum { + EEPROM_CRC = 0, + + /* Counts the amount of system settings */ + EEPROM_FOOTER_COUNT +} EEPROM_FOOTER_ID_t; + +uint8_t eeprom_blocksize_Arr[4] = { + EEPROM_HEADER_COUNT, + EEPROM_SYS_COUNT, + EEPROM_PROFILE_COUNT, + EEPROM_FOOTER_COUNT +}; + /*********************************************************************** @@ -41,15 +91,29 @@ typedef struct uint32_t writeTypeId; uint32_t size; //Size of the data void * dataPtr; //pointer to the data, that should be saved to EEPROM - uint32_t padding[]; -}EEPROM_DATA_t; +} EEPROM_DATA_t; + + /*********************************************************************** * BRIEF: Data info to be passed to the write function * * INFORMATION: passes size and data pointer * ***********************************************************************/ -EEPROM_DATA_t eepromArr[EEPROM_COUNT] = { +EEPROM_DATA_t eeprom_header_Arr[EEPROM_HEADER_COUNT] = { + [EEPROM_VERSION] = + { + .size = sizeof(stored_eeprom_identifier), + .dataPtr = &stored_eeprom_identifier, + } +}; + +EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = { + [EEPROM_ACTIVE_PROFILE] = + { + .size = sizeof(active_profile), + .dataPtr = &active_profile, + }, [EEPROM_ADC_SCALES] = { .size = sizeof(adcScaleStruct_t), @@ -62,15 +126,70 @@ EEPROM_DATA_t eepromArr[EEPROM_COUNT] = { } }; +EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = { + [EEPROM_PID_ROLL_KP] = + { + .size = sizeof(pid_pitch_pk), + .dataPtr = &pid_pitch_pk, + } +}; + +EEPROM_DATA_t eeprom_footer_Arr[EEPROM_FOOTER_COUNT] = { + [EEPROM_CRC] = + { + .size = sizeof(calculated_crc), + .dataPtr = &calculated_crc, + } +}; + +/* to loop through these arrays*/ +EEPROM_DATA_t * eeprom_Arr_list[4] = { + eeprom_header_Arr, + eeprom_sys_Arr, + eeprom_profile_Arr, + eeprom_footer_Arr +}; + +uint16_t eepromArrSize(EEPROM_DATA_t * data) +{ + int size = 0; + for (int j = 0; j < 4; j++) + { + if (data == eeprom_Arr_list[j]) + for (int i = 0; i < eeprom_blocksize_Arr[j]; i++) + { + size += data[i].size; + } + } + if (size == 0) + Error_Handler(); // We should never get here + return size; +} + + +/*********************************************************************** +* BRIEF: Increments the checksum * +* INFORMATION: * +***********************************************************************/ +uint8_t incrementChecksum(uint8_t crc, const void *data, uint32_t length) +{ + const uint8_t * addrIterator = (const uint8_t *)data; + const uint8_t * addrEnd = addrIterator + length; + + for (;addrIterator != addrEnd; addrIterator++) + crc ^= *addrIterator; + + return crc; +} /*********************************************************************** * BRIEF: Stores one data type at the time from eepromArr to EEPROM * * INFORMATION: Writes byte per byte until end of data * ***********************************************************************/ -void writeEepromExtension(uint32_t addr, uint32_t id) +void writeEepromExtension(uint32_t addr,EEPROM_DATA_t * data, uint32_t id) { - for (int i = 0; i < eepromArr[id].size; i ++) - HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, addr + i , *((uint8_t*)(eepromArr[id].dataPtr + i))); + for (int i = 0; i < data[id].size; i ++) + HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, addr + i , *((uint8_t*)(data[id].dataPtr + i))); } /*********************************************************************** @@ -85,16 +204,60 @@ void writeEEPROM() * point to and it can read the data directly to that value which is pointed to*/ uint32_t addrIterator = EEPROM_BASE_ADDR; + uint8_t crc = 0; // Initializing the checksum to 0 + HAL_FLASH_Unlock(); __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR ); - //FLASH_Erase_Sector(((uint32_t)11U),VOLTAGE_RANGE_3); FLASH_Erase_Sector(((uint32_t)FLASH_SECTOR_11),VOLTAGE_RANGE_3); - for (int i = 0; i < EEPROM_COUNT; i ++) + stored_eeprom_identifier = (uint8_t)EEPROM_SYS_VERSION; + + for (int j = 0; j < 4; j++) { - writeEepromExtension(addrIterator, i); - addrIterator += eepromArr[i].size; + for (int i = 0; i < eeprom_blocksize_Arr[j]; i++) + { + if (eeprom_Arr_list[j] == eeprom_footer_Arr) + calculated_crc = crc; + writeEepromExtension(addrIterator, eeprom_Arr_list[j], i); + addrIterator += eeprom_Arr_list[j][i].size; + crc = incrementChecksum(crc, eeprom_Arr_list[j][i].dataPtr, eeprom_Arr_list[j][i].size); + } } + +// +// /* Write the Header and calc CRC */ +// for (int i = 0; i < EEPROM_HEADER_COUNT; i ++) +// { +// writeEepromExtension(addrIterator, eeprom_header_Arr, i); +// addrIterator += eeprom_header_Arr[i].size; +// crc = incrementChecksum(crc, eeprom_header_Arr[i].dataPtr, eeprom_header_Arr[i].size); +// } +// +// /* Write the system settings and calc CRC */ +// for (int i = 0; i < EEPROM_SYS_COUNT; i ++) +// { +// writeEepromExtension(addrIterator, eeprom_sys_Arr, i); +// addrIterator += eeprom_sys_Arr[i].size; +// crc = incrementChecksum(crc, eeprom_sys_Arr[i].dataPtr, eeprom_sys_Arr[i].size); +// } +// /* Write the profile and calc CRC */ +// for (int i = 0; i < EEPROM_PROFILE_COUNT; i ++) +// { +// writeEepromExtension(addrIterator, eeprom_profile_Arr, i); +// addrIterator += eeprom_profile_Arr[i].size; +// crc = incrementChecksum(crc, eeprom_profile_Arr[i].dataPtr, eeprom_profile_Arr[i].size); +// } +// +// // Updating the calculated crc value before writing to EEPROM +// calculated_crc = crc; +// +// /* Write the CRC to footer */ +// for (int i = 0; i < EEPROM_FOOTER_COUNT; i ++) +// { +// writeEepromExtension(addrIterator, eeprom_footer_Arr, i); +// addrIterator += eeprom_footer_Arr[i].size; +// } + HAL_FLASH_Lock(); } @@ -102,12 +265,78 @@ void writeEEPROM() * BRIEF: Restores one data type at the time from eepromArr from EEPROM * * INFORMATION: * ***********************************************************************/ -void readEepromExtension(uint32_t addr, uint32_t id) +void readEepromExtension(uint32_t addr, EEPROM_DATA_t * data, uint32_t id) { - for (int i = 0; i < eepromArr[id].size; i++) - *(uint8_t*)(eepromArr[id].dataPtr + i) = *( uint8_t*)(addr + i * sizeof(uint8_t)); + for (int i = 0; i < data[id].size; i++) + *(uint8_t*)(data[id].dataPtr + i) = *( uint8_t*)(addr + i * sizeof(uint8_t)); } +// Verifies that eeprom is not corrupt and that the EEPROM version is correct +bool scanEEPROM(void) +{ + uint8_t crc = 0; + uint8_t old_crc = calculated_crc; // store to restore + uint32_t addrIterator = EEPROM_BASE_ADDR; + HAL_FLASH_Unlock(); + HAL_Delay(100); + + for (int j = 0; j < 4; j++) + { + for (int i = 0; i < eeprom_blocksize_Arr[j]; i++) + { + if ((eeprom_Arr_list[j] == eeprom_header_Arr) || (eeprom_Arr_list[j] == eeprom_footer_Arr)) + readEepromExtension(addrIterator, eeprom_Arr_list[j], i); + if (eeprom_Arr_list[j] != eeprom_footer_Arr) + crc = incrementChecksum(crc, (int*)addrIterator, eeprom_Arr_list[j][i].size); + addrIterator += eeprom_Arr_list[j][i].size; + } + } + +// /* Read the Header */ +// for (int i = 0; i < EEPROM_HEADER_COUNT; i ++) +// { +// readEepromExtension(addrIterator, eeprom_header_Arr, i); +// crc = incrementChecksum(crc, addrIterator, eeprom_header_Arr[i].size); +// addrIterator += eeprom_header_Arr[i].size; +// } +// +// /* Read the system array */ +// for (int i = 0; i < EEPROM_SYS_COUNT; i ++) +// { +// crc = incrementChecksum(crc, addrIterator, eeprom_sys_Arr[i].size); +// addrIterator += eeprom_sys_Arr[i].size; +// } +// +// /* Read the profile array */ +// for (int i = 0; i < EEPROM_PROFILE_COUNT; i ++) +// { +// crc = incrementChecksum(crc, addrIterator, eeprom_profile_Arr[i].size); +// addrIterator += eeprom_profile_Arr[i].size; +// } +// +// /* Read the Footer */ +// for (int i = 0; i < EEPROM_FOOTER_COUNT; i ++) +// { +// readEepromExtension(addrIterator, eeprom_footer_Arr, i); +// addrIterator += eeprom_footer_Arr[i].size; +// } + + HAL_FLASH_Lock(); + + + /* Check to see if CRC matches */ + if ( (crc == calculated_crc) && (stored_eeprom_identifier == (uint8_t)EEPROM_SYS_VERSION) ) + { + return true; + } + else + { + calculated_crc = old_crc; + //Error_Handler(); + // Reinitialize eeprom with default values. + return false; + } +} /*********************************************************************** * BRIEF: Reads EEPROM data from FLASH * @@ -116,12 +345,49 @@ void readEepromExtension(uint32_t addr, uint32_t id) void readEEPROM() { uint32_t addrIterator = EEPROM_BASE_ADDR; - HAL_FLASH_Unlock(); - HAL_Delay(100); - for (int i = 0; i < EEPROM_COUNT; i ++) + + /* This check has to be done as not to overwrite memory with bad data */ + if (scanEEPROM()) { - readEepromExtension(addrIterator, i); - addrIterator += eepromArr[i].size; + HAL_FLASH_Unlock(); + HAL_Delay(100); + + for (int j = 0; j < 3; j++) // 3 excludes the footer + { + for (int i = 0; i < eeprom_blocksize_Arr[j]; i++) + { + readEepromExtension(addrIterator, eeprom_Arr_list[j], i); + addrIterator += eeprom_Arr_list[j][i].size; + } + } + +// /* Read the Header */ +// for (int i = 0; i < EEPROM_HEADER_COUNT; i ++) +// { +// readEepromExtension(addrIterator, eeprom_header_Arr, i); +// addrIterator += eeprom_header_Arr[i].size; +// } +// +// /* Read the system array */ +// for (int i = 0; i < EEPROM_SYS_COUNT; i ++) +// { +// readEepromExtension(addrIterator, eeprom_sys_Arr, i); +// addrIterator += eeprom_sys_Arr[i].size; +// } +// +// /* Read the profile array */ +// for (int i = 0; i < EEPROM_PROFILE_COUNT; i ++) +// { +// readEepromExtension(addrIterator, eeprom_profile_Arr, i); +// addrIterator += eeprom_profile_Arr[i].size; +// } + + // Now there is no need to read the footer + + HAL_FLASH_Lock(); + } + else + { + } - HAL_FLASH_Lock(); } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index f88bb15..59acb38 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -19,6 +19,9 @@ #include #include "drivers/uart1_inverter.h" +// for test +uint8_t pid_pitch_pk = 0; + int main(void) { @@ -36,6 +39,10 @@ int main(void) for(;;) { writeEEPROM(); + adcScaleStruct_t.vcc_scale = 0; + adcScaleStruct_t.i_scale_right = 0; + adcScaleStruct_t.i_scale_left = 0; + uart1_rx_inverter = 0; readEEPROM(); adcScaleStruct_t.vcc_scale ++; adcScaleStruct_t.i_scale_right ++; From fd354ccf2d481ef2358f3e9476260ed3775b8e1d Mon Sep 17 00:00:00 2001 From: philsson Date: Tue, 4 Oct 2016 13:29:59 +0200 Subject: [PATCH 18/18] Final Philip EEPROM version. Now it will be merged with Master --- UAV-ControlSystem/inc/config/eeprom.h | 65 ++++- UAV-ControlSystem/src/config/eeprom.c | 338 ++++++++++++++------------ UAV-ControlSystem/src/main.c | 41 +++- 3 files changed, 269 insertions(+), 175 deletions(-) diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 3568d70..25f6d7d 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -21,27 +21,82 @@ /* Defines where emulated EEPROM starts from - OBS! Also defined in LinkerScript.id */ #define EEPROM_BASE_ADDR 0x080E0000 -#define EEPROM_PROFILE_SIZE 20 // Size in uint32_t +/* The size in bytes of the profile buffers. The error handler will be called if this is too small */ +#define EEPROM_PROFILE_SIZE 200 +/* The profiles one can choose from */ typedef enum { PROFILE_1 = 1, PROFILE_2, PROFILE_3 } ACTIVE_PROFILE; -extern uint8_t active_profile; +/* List of all header EEPROM values */ +typedef enum { + EEPROM_VERSION = 0, + + /* Counts the amount of system settings */ + EEPROM_HEADER_COUNT +} EEPROM_HEADER_ID_t; + +/* List of all system EEPROM values */ +typedef enum { + EEPROM_ACTIVE_PROFILE = 0, + EEPROM_ADC_SCALES, + EEPROM_UART1_RX_INV, + + /* Counts the amount of system settings */ + EEPROM_SYS_COUNT +} EEPROM_SYS_ID_t; + +/* List of all profile EEPROM values */ +typedef enum { + EEPROM_PID_ROLL_KP = 0, + + /* Counts the amount of settings in profile */ + EEPROM_PROFILE_COUNT +} EEPROM_PROFILE_ID_t; + +/* List of all footer EEPROM values */ +typedef enum { + EEPROM_CRC = 0, + + /* Counts the amount of system settings */ + EEPROM_FOOTER_COUNT +} EEPROM_FOOTER_ID_t; /*********************************************************************** -* BRIEF: Writes EEPROM data to FLASH * +* BRIEF: Writes EEPROM data to FLASH. Requires the next active profile * +* to be selected (current profile can be used as input) * * INFORMATION: passes all data directly from where they are defined * ***********************************************************************/ -void writeEEPROM(); +void writeEEPROM(ACTIVE_PROFILE new_active_profile); + +/*********************************************************************** +* BRIEF: Writes EEPROM data to FLASH without the need of setting next * +* active profile * +* INFORMATION: Keeps the current profile active * +***********************************************************************/ +void saveEEPROM(); /*********************************************************************** * BRIEF: Reads EEPROM data from FLASH * * INFORMATION: passes all data directly to where they are defined * ***********************************************************************/ -void readEEPROM(); +bool readEEPROM(); + +/*********************************************************************** +* BRIEF: Choose a profile between 1 .. 3 * +* INFORMATION: The current changes will be saved * +***********************************************************************/ +void setActiveProfile(ACTIVE_PROFILE profile); + +/*********************************************************************** +* BRIEF: Writes current profile values to all EEPROM profiles * +* INFORMATION: used when EEPROM is corrupt or there is a version * +* mismatch * +***********************************************************************/ +void resetEEPROM(void); #endif /* CONFIG_EEPROM_H_ */ diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index 5b9d4cf..78c8555 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -24,54 +24,25 @@ uint8_t stored_eeprom_identifier; /* Buffer where we temporarily store profiles we not use as we erase eeprom before write */ -uint32_t eeprom_profile_tmp_a[EEPROM_PROFILE_SIZE]; -uint32_t eeprom_profile_tmp_b[EEPROM_PROFILE_SIZE]; +uint8_t eeprom_profile_tmp_a[EEPROM_PROFILE_SIZE]; +uint8_t eeprom_profile_tmp_b[EEPROM_PROFILE_SIZE]; +/* The two temporary buffers are addressed through this array */ +uint8_t * eeprom_profile_tmp_Arr[2] = { + eeprom_profile_tmp_a, + eeprom_profile_tmp_b +}; /* Created CRC */ uint8_t calculated_crc; uint8_t read_crc; -uint8_t active_profile = 0; +/* Defines which profile is active. Should not be changed other than calling setActiveProfile() */ +ACTIVE_PROFILE active_profile = 1; // Between 1 .. 3 -typedef enum { - EEPROM_VERSION = 0, - /* Counts the amount of system settings */ - EEPROM_HEADER_COUNT -} EEPROM_HEADER_ID_t; - -/*********************************************************************** -* BRIEF: List of all System EEPROM values * -* INFORMATION: This content of this struct will be used in EEPROM * -***********************************************************************/ -typedef enum { - EEPROM_ACTIVE_PROFILE = 0, - EEPROM_ADC_SCALES, - EEPROM_UART1_RX_INV, - - /* Counts the amount of system settings */ - EEPROM_SYS_COUNT -} EEPROM_SYS_ID_t; - -/*********************************************************************** -* BRIEF: List of all Profile EEPROM values * -* INFORMATION: This content of this struct will be used in EEPROM * -***********************************************************************/ -typedef enum { - EEPROM_PID_ROLL_KP = 0, - - /* Counts the amount of settings in profile */ - EEPROM_PROFILE_COUNT -} EEPROM_PROFILE_ID_t; - -typedef enum { - EEPROM_CRC = 0, - - /* Counts the amount of system settings */ - EEPROM_FOOTER_COUNT -} EEPROM_FOOTER_ID_t; +/* List of all EEPROM lists */ uint8_t eeprom_blocksize_Arr[4] = { EEPROM_HEADER_COUNT, EEPROM_SYS_COUNT, @@ -81,11 +52,7 @@ uint8_t eeprom_blocksize_Arr[4] = { -/*********************************************************************** -* BRIEF: General EEPROM data info * -* INFORMATION: Each EEPROM parameter needs an instance of this struct * -* to be added in eepromArr * -***********************************************************************/ +/* General EEPROM data info. All data addressed needs both size and pointer */ typedef struct { uint32_t writeTypeId; @@ -96,10 +63,7 @@ typedef struct -/*********************************************************************** -* BRIEF: Data info to be passed to the write function * -* INFORMATION: passes size and data pointer * -***********************************************************************/ +/* Data pointers and sizes for header content */ EEPROM_DATA_t eeprom_header_Arr[EEPROM_HEADER_COUNT] = { [EEPROM_VERSION] = { @@ -108,6 +72,7 @@ EEPROM_DATA_t eeprom_header_Arr[EEPROM_HEADER_COUNT] = { } }; +/* Data pointers and sizes for sys content */ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = { [EEPROM_ACTIVE_PROFILE] = { @@ -126,6 +91,7 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = { } }; +/* Data pointers and sizes for profile content */ EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = { [EEPROM_PID_ROLL_KP] = { @@ -134,6 +100,7 @@ EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = { } }; +/* Data pointers and sizes for footer content */ EEPROM_DATA_t eeprom_footer_Arr[EEPROM_FOOTER_COUNT] = { [EEPROM_CRC] = { @@ -142,7 +109,7 @@ EEPROM_DATA_t eeprom_footer_Arr[EEPROM_FOOTER_COUNT] = { } }; -/* to loop through these arrays*/ +/* List of all EEPROM content arrays */ EEPROM_DATA_t * eeprom_Arr_list[4] = { eeprom_header_Arr, eeprom_sys_Arr, @@ -150,6 +117,11 @@ EEPROM_DATA_t * eeprom_Arr_list[4] = { eeprom_footer_Arr }; +/*********************************************************************** +* BRIEF: Reads Calculates the combined size of all elements * +* in an EEPROM_DATA_t array * +* INFORMATION: return value is in bytes * +***********************************************************************/ uint16_t eepromArrSize(EEPROM_DATA_t * data) { int size = 0; @@ -169,7 +141,7 @@ uint16_t eepromArrSize(EEPROM_DATA_t * data) /*********************************************************************** * BRIEF: Increments the checksum * -* INFORMATION: * +* INFORMATION: XOR operator byte per byte * ***********************************************************************/ uint8_t incrementChecksum(uint8_t crc, const void *data, uint32_t length) { @@ -193,10 +165,11 @@ void writeEepromExtension(uint32_t addr,EEPROM_DATA_t * data, uint32_t id) } /*********************************************************************** -* BRIEF: Writes EEPROM data to FLASH * +* BRIEF: Writes EEPROM data to FLASH. Requires the next active profile * +* to be selected (current profile can be used as input) * * INFORMATION: passes all data directly from where they are defined * ***********************************************************************/ -void writeEEPROM() +void writeEEPROM(uint8_t new_profile) { /* Add size and a pointer to the data for each value * Each value when saved will store the data that it points to. @@ -205,6 +178,9 @@ void writeEEPROM() uint32_t addrIterator = EEPROM_BASE_ADDR; uint8_t crc = 0; // Initializing the checksum to 0 + uint8_t profile_counter = 0; + uint8_t buff_counter = 0; + ACTIVE_PROFILE old_profile = active_profile; HAL_FLASH_Unlock(); __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR ); @@ -214,50 +190,44 @@ void writeEEPROM() for (int j = 0; j < 4; j++) { - for (int i = 0; i < eeprom_blocksize_Arr[j]; i++) + // We exclude reading of profiles not active to memory + if (!((j == 2) && (profile_counter != ((uint8_t)active_profile - 1) ))) { - if (eeprom_Arr_list[j] == eeprom_footer_Arr) - calculated_crc = crc; - writeEepromExtension(addrIterator, eeprom_Arr_list[j], i); - addrIterator += eeprom_Arr_list[j][i].size; - crc = incrementChecksum(crc, eeprom_Arr_list[j][i].dataPtr, eeprom_Arr_list[j][i].size); + if (j == 1) // Writing sys we store the NEW profile + active_profile = new_profile; + for (int i = 0; i < eeprom_blocksize_Arr[j]; i++) + { + if (eeprom_Arr_list[j] == eeprom_footer_Arr) + calculated_crc = crc; + writeEepromExtension(addrIterator, eeprom_Arr_list[j], i); + addrIterator += eeprom_Arr_list[j][i].size; + crc = incrementChecksum(crc, eeprom_Arr_list[j][i].dataPtr, eeprom_Arr_list[j][i].size); + } + if (j == 1) + active_profile = old_profile; // restore the old profile + } + else + { + if (buff_counter >= 2) + Error_Handler(); + + int eeprom_profile_arr_size = eepromArrSize(eeprom_profile_Arr); + for (int i = 0; i < eeprom_profile_arr_size; i ++) + HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, addrIterator + i , *((uint8_t*)(eeprom_profile_tmp_Arr[buff_counter] + i))); + + crc = incrementChecksum(crc, eeprom_profile_tmp_Arr[buff_counter], eepromArrSize(eeprom_profile_Arr)); + buff_counter++; // move on to next buffer + + // Adding size of eeprom array + addrIterator += eeprom_profile_arr_size; + } + // Halt iterator j @2 (profile) for 3 iterations (3 profiles) + if ((j == 2) && (profile_counter < 2)) + { + profile_counter++; + j--; // We do this to keep j = 2 for three iterations (looping through all profiles) } } - -// -// /* Write the Header and calc CRC */ -// for (int i = 0; i < EEPROM_HEADER_COUNT; i ++) -// { -// writeEepromExtension(addrIterator, eeprom_header_Arr, i); -// addrIterator += eeprom_header_Arr[i].size; -// crc = incrementChecksum(crc, eeprom_header_Arr[i].dataPtr, eeprom_header_Arr[i].size); -// } -// -// /* Write the system settings and calc CRC */ -// for (int i = 0; i < EEPROM_SYS_COUNT; i ++) -// { -// writeEepromExtension(addrIterator, eeprom_sys_Arr, i); -// addrIterator += eeprom_sys_Arr[i].size; -// crc = incrementChecksum(crc, eeprom_sys_Arr[i].dataPtr, eeprom_sys_Arr[i].size); -// } -// /* Write the profile and calc CRC */ -// for (int i = 0; i < EEPROM_PROFILE_COUNT; i ++) -// { -// writeEepromExtension(addrIterator, eeprom_profile_Arr, i); -// addrIterator += eeprom_profile_Arr[i].size; -// crc = incrementChecksum(crc, eeprom_profile_Arr[i].dataPtr, eeprom_profile_Arr[i].size); -// } -// -// // Updating the calculated crc value before writing to EEPROM -// calculated_crc = crc; -// -// /* Write the CRC to footer */ -// for (int i = 0; i < EEPROM_FOOTER_COUNT; i ++) -// { -// writeEepromExtension(addrIterator, eeprom_footer_Arr, i); -// addrIterator += eeprom_footer_Arr[i].size; -// } - HAL_FLASH_Lock(); } @@ -271,59 +241,52 @@ void readEepromExtension(uint32_t addr, EEPROM_DATA_t * data, uint32_t id) *(uint8_t*)(data[id].dataPtr + i) = *( uint8_t*)(addr + i * sizeof(uint8_t)); } -// Verifies that eeprom is not corrupt and that the EEPROM version is correct +/*********************************************************************** +* BRIEF: Copies a profile from EEPROM into a buffer * +* INFORMATION: Run once for every buffer * +***********************************************************************/ +void readEepromBuff(uint32_t addr, uint8_t * eeprom_profile_tmp, uint32_t length) +{ + for (int i = 0; i < length; i++) + *(uint8_t*)(eeprom_profile_tmp + i) = *(uint8_t*)(addr + i * sizeof(uint8_t)); +} + +/*********************************************************************** +* BRIEF: Verifies EEPROM only * +* INFORMATION: compares EEPROM version and makes CRC checks * +* Return value is true if okay * +***********************************************************************/ bool scanEEPROM(void) { uint8_t crc = 0; uint8_t old_crc = calculated_crc; // store to restore + uint8_t profile_counter = 0; // To iterate through the profiles uint32_t addrIterator = EEPROM_BASE_ADDR; HAL_FLASH_Unlock(); HAL_Delay(100); - for (int j = 0; j < 4; j++) + for (int j = 0; j < 4; j++) // 3 to skip footer from CRC calculation { for (int i = 0; i < eeprom_blocksize_Arr[j]; i++) { + // Store data only for Header (EEPROM version) and footer (CRC) while scanning if ((eeprom_Arr_list[j] == eeprom_header_Arr) || (eeprom_Arr_list[j] == eeprom_footer_Arr)) readEepromExtension(addrIterator, eeprom_Arr_list[j], i); + // Checksum not incremented for footer if (eeprom_Arr_list[j] != eeprom_footer_Arr) crc = incrementChecksum(crc, (int*)addrIterator, eeprom_Arr_list[j][i].size); addrIterator += eeprom_Arr_list[j][i].size; } + // Halt iterator j @2 (profile) for 3 iterations (3 profiles) + if ((j == 2) && (profile_counter < 2)) + { + profile_counter++; + j--; // We do this to keep j = 2 for three iterations (looping through all profiles) + } } -// /* Read the Header */ -// for (int i = 0; i < EEPROM_HEADER_COUNT; i ++) -// { -// readEepromExtension(addrIterator, eeprom_header_Arr, i); -// crc = incrementChecksum(crc, addrIterator, eeprom_header_Arr[i].size); -// addrIterator += eeprom_header_Arr[i].size; -// } -// -// /* Read the system array */ -// for (int i = 0; i < EEPROM_SYS_COUNT; i ++) -// { -// crc = incrementChecksum(crc, addrIterator, eeprom_sys_Arr[i].size); -// addrIterator += eeprom_sys_Arr[i].size; -// } -// -// /* Read the profile array */ -// for (int i = 0; i < EEPROM_PROFILE_COUNT; i ++) -// { -// crc = incrementChecksum(crc, addrIterator, eeprom_profile_Arr[i].size); -// addrIterator += eeprom_profile_Arr[i].size; -// } -// -// /* Read the Footer */ -// for (int i = 0; i < EEPROM_FOOTER_COUNT; i ++) -// { -// readEepromExtension(addrIterator, eeprom_footer_Arr, i); -// addrIterator += eeprom_footer_Arr[i].size; -// } - HAL_FLASH_Lock(); - /* Check to see if CRC matches */ if ( (crc == calculated_crc) && (stored_eeprom_identifier == (uint8_t)EEPROM_SYS_VERSION) ) { @@ -331,10 +294,13 @@ bool scanEEPROM(void) } else { + // ERROR!!! CORRUPT EEPROM, RESETTING + calculated_crc = old_crc; - //Error_Handler(); - // Reinitialize eeprom with default values. - return false; + Error_Handler(); + resetEEPROM(); // Reinitialize eeprom with default values. + + return true /* false */; } } @@ -342,52 +308,108 @@ bool scanEEPROM(void) * BRIEF: Reads EEPROM data from FLASH * * INFORMATION: passes all data directly to where they are defined * ***********************************************************************/ -void readEEPROM() +bool readEEPROM() { + bool success; + uint8_t profile_counter = 0; + uint8_t buff_counter = 0; uint32_t addrIterator = EEPROM_BASE_ADDR; /* This check has to be done as not to overwrite memory with bad data */ - if (scanEEPROM()) + if ((success = scanEEPROM())) { HAL_FLASH_Unlock(); HAL_Delay(100); for (int j = 0; j < 3; j++) // 3 excludes the footer { - for (int i = 0; i < eeprom_blocksize_Arr[j]; i++) + + + // We exclude reading of profiles not active to memory + if (!((j == 2) && (profile_counter != (uint8_t)(active_profile - 1)))) { - readEepromExtension(addrIterator, eeprom_Arr_list[j], i); - addrIterator += eeprom_Arr_list[j][i].size; + for (int i = 0; i < eeprom_blocksize_Arr[j]; i++) + { + readEepromExtension(addrIterator, eeprom_Arr_list[j], i); + addrIterator += eeprom_Arr_list[j][i].size; + } + } + // The two not loaded profiles are stored to buffers + else + { + if (buff_counter >= 2) + Error_Handler(); + + // Import the data from eeprom to buffer + readEepromBuff((uint32_t*)addrIterator, (uint8_t*)eeprom_profile_tmp_Arr[buff_counter], EEPROM_PROFILE_SIZE); + buff_counter++; + + // Adding size of eeprom array + addrIterator += eepromArrSize(eeprom_profile_Arr); + } + // Halt iterator j @2 (profile) for 3 iterations (3 profiles) + if ((j == 2) && (profile_counter < 2)) + { + profile_counter++; + j--; // We do this to keep j = 2 for three iterations (looping through all profiles) } } - -// /* Read the Header */ -// for (int i = 0; i < EEPROM_HEADER_COUNT; i ++) -// { -// readEepromExtension(addrIterator, eeprom_header_Arr, i); -// addrIterator += eeprom_header_Arr[i].size; -// } -// -// /* Read the system array */ -// for (int i = 0; i < EEPROM_SYS_COUNT; i ++) -// { -// readEepromExtension(addrIterator, eeprom_sys_Arr, i); -// addrIterator += eeprom_sys_Arr[i].size; -// } -// -// /* Read the profile array */ -// for (int i = 0; i < EEPROM_PROFILE_COUNT; i ++) -// { -// readEepromExtension(addrIterator, eeprom_profile_Arr, i); -// addrIterator += eeprom_profile_Arr[i].size; -// } - - // Now there is no need to read the footer - HAL_FLASH_Lock(); } else { - + Error_Handler(); } + return success; +} + +/*********************************************************************** +* BRIEF: Choose a profile between 1 .. 3 * +* INFORMATION: The current changes will be saved * +***********************************************************************/ +void setActiveProfile(ACTIVE_PROFILE new_profile) +{ + // Error handler + if (new_profile < 1 || new_profile > 3) + Error_Handler(); + + writeEEPROM(new_profile); + + readEEPROM(); +} + +/*********************************************************************** +* BRIEF: Writes current profile values to all EEPROM profiles * +* INFORMATION: used when EEPROM is corrupt or there is a version * +* mismatch * +***********************************************************************/ +void resetEEPROM(void) +{ + /* check so that the profile buffer sizes are not smaller than the size of one profile */ + if (EEPROM_PROFILE_SIZE < eepromArrSize(eeprom_profile_Arr) ) + Error_Handler(); // We need to increment the EEPROM_PROFILE_SIZE + + uint32_t bufferIterator = 0; + //Loop through all the values in the EEPROM profile + for (int j = 0; j < EEPROM_PROFILE_COUNT; j++) + { + //for each value loop on its byte size and then write byte by byte to the buffers + for (int i = 0; i < eeprom_profile_Arr[i].size; i++) + { + *(uint8_t*)(eeprom_profile_tmp_Arr[0] + bufferIterator) = *(uint8_t*)(eeprom_profile_Arr[j].dataPtr + i); + *(uint8_t*)(eeprom_profile_tmp_Arr[1] + bufferIterator) = *(uint8_t*)(eeprom_profile_Arr[j].dataPtr + i); + bufferIterator++; + } + } + writeEEPROM(active_profile); +} + +/*********************************************************************** +* BRIEF: Writes EEPROM data to FLASH without the need of setting next * +* active profile * +* INFORMATION: Keeps the current profile active * +***********************************************************************/ +void saveEEPROM() +{ + writeEEPROM(active_profile); } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index 59acb38..90cc40a 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -20,7 +20,7 @@ #include "drivers/uart1_inverter.h" // for test -uint8_t pid_pitch_pk = 0; +uint8_t pid_pitch_pk = 10; int main(void) @@ -32,22 +32,39 @@ int main(void) // Configure the system clock to 100 MHz system_clock_config(); - adcScaleStruct_t.vcc_scale = 20; - adcScaleStruct_t.i_scale_right = 10; - adcScaleStruct_t.i_scale_left = 30; + pid_pitch_pk = 10; + setActiveProfile(1); + resetEEPROM(); for(;;) { - writeEEPROM(); - adcScaleStruct_t.vcc_scale = 0; - adcScaleStruct_t.i_scale_right = 0; - adcScaleStruct_t.i_scale_left = 0; + + setActiveProfile(3); + pid_pitch_pk = 0; uart1_rx_inverter = 0; + + pid_pitch_pk++; + + setActiveProfile(1); + pid_pitch_pk++; readEEPROM(); - adcScaleStruct_t.vcc_scale ++; - adcScaleStruct_t.i_scale_right ++; - adcScaleStruct_t.i_scale_left ++; - uart1_rx_inverter = !uart1_rx_inverter; + pid_pitch_pk = 200; + + + setActiveProfile(2); + pid_pitch_pk = 0; + readEEPROM(); + pid_pitch_pk = 250; + + resetEEPROM(); + + setActiveProfile(3); + int kalle = pid_pitch_pk; + setActiveProfile(2); + kalle = pid_pitch_pk; + setActiveProfile(3); + kalle = pid_pitch_pk; + }