diff --git a/UAV-ControlSystem/.cproject b/UAV-ControlSystem/.cproject
index 58f0fe6..20915b0 100644
--- a/UAV-ControlSystem/.cproject
+++ b/UAV-ControlSystem/.cproject
@@ -39,6 +39,7 @@
+
diff --git a/UAV-ControlSystem/inc/Scheduler/scheduler.h b/UAV-ControlSystem/inc/Scheduler/scheduler.h
new file mode 100644
index 0000000..5c17864
--- /dev/null
+++ b/UAV-ControlSystem/inc/Scheduler/scheduler.h
@@ -0,0 +1,227 @@
+/**************************************************************************
+* 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_
+#define SCHEDULER_H_
+
+#include
+#include
+#include "stm32f4xx_revo.h"
+
+#define taskAgeCycleCounterSize 16
+
+/* Enum containing all the possible task priorities */
+typedef enum
+{
+ PRIORITY_IDLE = 0,
+ PRIORITY_LOW,
+ PRIORITY_MEDIUM,
+ PRIORITY_HIGH,
+ PRIORITY_REALTIME,
+ PRIORITY_MAX_PRIORITY = 255
+} 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
+{
+ /* 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 */
+
+ /* 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;
+
+
+/* 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
+
+ //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,
+
+ /* Service task IDs */
+ TASK_NONE = TASK_COUNT,
+ TASK_SELF
+}taskId_t;
+
+/* Variables -------------------------------------------------------------*/
+extern task_t SystemTasks[TASK_COUNT]; /* All the tasks that exist in the system */
+extern uint16_t averageSystemLoadPercent; /* The average load on the system Todo */
+
+
+/* Functions that operate can operate on the tasks -----------------------*/
+
+/**************************************************************************
+* 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: 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: 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: 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: 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: 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
new file mode 100644
index 0000000..9b5ee73
--- /dev/null
+++ b/UAV-ControlSystem/inc/Scheduler/tasks.h
@@ -0,0 +1,51 @@
+/**************************************************************************
+* 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 *
+* -------- ---- ----------- *
+***************************************************************************/
+
+#ifndef SCHEDULER_TASKS_H_
+#define SCHEDULER_TASKS_H_
+
+#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);
+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);
+
+//Tasks used only for testing purposes
+void systemTaskDebug_1(void);
+void systemTaskDebug_2(void);
+void systemTaskDebug_3(void);
+
+#endif /* SCHEDULER_TASKS_H_ */
diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h
index c34b9dc..1de5ba2 100644
--- a/UAV-ControlSystem/inc/config/eeprom.h
+++ b/UAV-ControlSystem/inc/config/eeprom.h
@@ -119,30 +119,85 @@
/* Defines for what type of value something is, system, profile etc */
#define EEPROM_VALUE_TYPE_SYSTEM 1
#define EEPROM_VALUE_TYPE_PROFILE 2
+#define EEPROM_VALUE_TYPE_HEADER 3
+#define EEPROM_VALUE_TYPE_FOOTER 4
+/* The size in bytes of the profile buffers. The error handler will be called if this is too small */
+#define EEPROM_PROFILE_SIZE 200
-/***********************************************************************
-* BRIEF: List of all EEPROM values *
-* INFORMATION: This content of this struct will be used in EEPROM *
-***********************************************************************/
+/* The profiles one can choose from */
typedef enum {
- EEPROM_ADC_SCALES = 0,
+ PROFILE_1 = 1,
+ PROFILE_2,
+ PROFILE_3
+} 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,
- 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;
+
+/* 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);
/***********************************************************************
* BRIEF: Gets the address of a value from an id *
diff --git a/UAV-ControlSystem/inc/drivers/I2C.h b/UAV-ControlSystem/inc/drivers/I2C.h
new file mode 100644
index 0000000..44fea89
--- /dev/null
+++ b/UAV-ControlSystem/inc/drivers/I2C.h
@@ -0,0 +1,111 @@
+/***************************************************************************
+* 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_
+
+#include "stm32f4xx.h"
+
+/******************************************************************************
+* BRIEF: Configure the I2C bus to be used *
+* INFORMATION: This function only implements I2C1 or I2C2 which are available *
+* on the REVO board *
+******************************************************************************/
+bool i2c_configure(I2C_TypeDef* i2c,
+ I2C_HandleTypeDef* out_profile,
+ uint32_t my_address);
+
+/******************************************************************************
+* 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(I2C_HandleTypeDef* profile,
+ 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(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();
+
+ init_system();
+
+ 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 };
+
+ // 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);
+
+ // 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/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/inc/drivers/led.h b/UAV-ControlSystem/inc/drivers/led.h
new file mode 100644
index 0000000..72795ed
--- /dev/null
+++ b/UAV-ControlSystem/inc/drivers/led.h
@@ -0,0 +1,112 @@
+/**************************************************************************
+* 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/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/inc/stm32f4xx_revo.h b/UAV-ControlSystem/inc/stm32f4xx_revo.h
index fa46876..a47dab6 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,10 +79,19 @@
#define USART6_RX_PIN GPIO_PIN_7
#define USART6_RX_PORT GPIOC
#define USART6_TX_PIN GPIO_PIN_6
-
#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
@@ -91,13 +100,36 @@
#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 */
+//#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/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/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
new file mode 100644
index 0000000..ff3b743
--- /dev/null
+++ b/UAV-ControlSystem/src/Scheduler/scheduler.c
@@ -0,0 +1,548 @@
+/**************************************************************************
+* 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"
+
+
+/* Variables ------------------------------------------- */
+
+static task_t *currentTask = NULL; //The current task that the is run
+
+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; //The current time in microseconds
+uint16_t averageSystmeLoad = 0;
+ //
+static int taskReadyQueueSize = 0; //The number of tasks in the ready queue
+
+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: 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));
+ taskReadyQueueSize = 0;
+}
+
+
+/**************************************************************************
+* 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)
+{
+ //Go through the taskReadyQueue to see if the current task is contained
+ for (int i = 0; i < taskReadyQueueSize; i++)
+ {
+ if (taskReadyQueue[i] == task)
+ {
+ return true;
+ }
+ }
+
+ return false;
+}
+
+
+/**************************************************************************
+* 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)
+{
+ 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 )
+ {
+ 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: 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)
+{
+ 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;
+}
+
+
+/**************************************************************************
+* 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)
+{
+ 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);
+ }
+ }
+}
+
+
+/**************************************************************************
+* 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)
+{
+ if (taskId == TASK_SELF || taskId < TASK_COUNT) {
+ task_t *task = taskId == TASK_SELF ? currentTask : &SystemTasks[taskId];
+ return task->taskLatestDeltaTime;
+ } else {
+ return 0;
+ }
+}
+
+
+/**************************************************************************
+* 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)
+{
+ 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: 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)
+{
+ //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: 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)
+{
+ //Clear the queue
+ taskReadyQueueClear();
+
+ //Init all the ToDo: add back when it is known that the scheduler works
+#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: 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)
+{
+ //Get the current time in Micro seconds
+ currentTime = clock_get_us();
+ task_t * taskToRun;
+ task_t * selectedTask = NULL;
+ 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 */
+ 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
+ //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
+ {
+ //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;
+
+#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;
+
+ //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 && 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
+ * meaning no other tasks than realtime will be able to enter the function */
+ 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
+
+ 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 */
+
+
+ //Used for meassuring load
+ 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 = clock_get_us();
+ selectedTask->taskFunction(); //Run the function associated with the current task
+ 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
+ selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
+ }
+
+}
+
+
+
+
+
+
+
+
+
+
+
diff --git a/UAV-ControlSystem/src/Scheduler/tasks.c b/UAV-ControlSystem/src/Scheduler/tasks.c
new file mode 100644
index 0000000..a08cda0
--- /dev/null
+++ b/UAV-ControlSystem/src/Scheduler/tasks.c
@@ -0,0 +1,183 @@
+/**************************************************************************
+* 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 *
+* -------- ---- ----------- *
+***************************************************************************/
+
+#include "Scheduler/tasks.h"
+#include "Scheduler/scheduler.h"
+
+
+/**************************************************************************
+* 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] =
+ {
+
+ [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), //50 hz update rate (20 ms)
+ .staticPriority = PRIORITY_MEDIUM,
+ },
+
+#ifdef BARO
+ [TASK_BARO] =
+ {
+ .taskName = "BAROMETER",
+ .taskFunction = systemTaskBaro,
+ .desiredPeriod = GetUpdateRateHz(20), //20 hz update rate (50 ms)
+ .staticPriority = PRIORITY_LOW,
+ },
+#endif
+
+#ifdef COMPASS
+ [TASK_COMPASS] =
+ {
+ .taskName = "COMPASS",
+ .taskFunction = systemTaskCompass,
+ .desiredPeriod = GetUpdateRateHz(10), //10 hz update rate (100 ms)
+ .staticPriority = PRIORITY_LOW,
+ },
+#endif
+
+#ifdef GPS
+ [TASK_GPS] =
+ {
+ .taskName = "GPS",
+ .taskFunction = systemTaskGps,
+ .desiredPeriod = GetUpdateRateHz(10), //10 hz update rate (100 ms)
+ .staticPriority = PRIORITY_MEDIUM,
+ },
+#endif
+
+#ifdef SONAR
+ [TASK_SONAR] =
+ {
+ .taskName = "SONAR",
+ .taskFunction = systemTaskSonar,
+ .desiredPeriod = GetUpdateRateHz(20), //20 hz update rate (50 ms)
+ .staticPriority = PRIORITY_LOW,
+ },
+#endif
+
+#if defined(BARO) || defined(SONAR)
+ [TASK_ALTITUDE] =
+ {
+ .taskName = "ALTITUDE",
+ .taskFunction = systemTaskAltitude,
+ .desiredPeriod = GetUpdateRateHz(40), //40 hz update rate (25 ms)
+ .staticPriority = PRIORITY_LOW,
+ },
+#endif
+
+#ifdef BEEPER
+ [TASK_BEEPER] =
+ {
+ .taskName = "BEEPER",
+ .taskFunction = systemTaskBeeper,
+ .desiredPeriod = GetUpdateRateHz(100), //100 hz update rate (10 ms)
+ .staticPriority = PRIORITY_LOW,
+ },
+#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_REALTIME,
+ },
+
+ [TASK_DEBUG_2] =
+ {
+ .taskName = "DEBUG_2",
+ .taskFunction = systemTaskDebug_2,
+ .desiredPeriod = GetUpdateRateHz(40),
+ .staticPriority = PRIORITY_MEDIUM,
+ },
+
+ [TASK_DEBUG_3] =
+ {
+ .taskName = "DEBUG_3",
+ .taskFunction = systemTaskDebug_3,
+ .desiredPeriod = GetUpdateRateHz(20),
+ .staticPriority = PRIORITY_LOW,
+ },
+#endif
+
+};
diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c
index 34d29f2..7d92a30 100644
--- a/UAV-ControlSystem/src/config/eeprom.c
+++ b/UAV-ControlSystem/src/config/eeprom.c
@@ -17,30 +17,69 @@
#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 */
+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;
+
+/* Defines which profile is active. Should not be changed other than calling setActiveProfile() */
+ACTIVE_PROFILE active_profile = 1; // Between 1 .. 3
+/* List of all EEPROM lists */
+uint8_t eeprom_blocksize_Arr[4] = {
+ EEPROM_HEADER_COUNT,
+ EEPROM_SYS_COUNT,
+ EEPROM_PROFILE_COUNT,
+ EEPROM_FOOTER_COUNT
+};
-/***********************************************************************
-* 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;
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] = {
+
+
+/* Data pointers and sizes for header content */
+EEPROM_DATA_t eeprom_header_Arr[EEPROM_HEADER_COUNT] = {
+ [EEPROM_VERSION] =
+ {
+ .size = sizeof(stored_eeprom_identifier),
+ .dataPtr = &stored_eeprom_identifier,
+ }
+};
+
+/* Data pointers and sizes for sys content */
+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),
@@ -53,22 +92,85 @@ EEPROM_DATA_t eepromArr[EEPROM_COUNT] = {
}
};
+/* Data pointers and sizes for profile content */
+EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = {
+ [EEPROM_PID_ROLL_KP] =
+ {
+ .size = sizeof(pid_pitch_pk),
+ .dataPtr = &pid_pitch_pk,
+ }
+};
+
+/* Data pointers and sizes for footer content */
+EEPROM_DATA_t eeprom_footer_Arr[EEPROM_FOOTER_COUNT] = {
+ [EEPROM_CRC] =
+ {
+ .size = sizeof(calculated_crc),
+ .dataPtr = &calculated_crc,
+ }
+};
+
+/* List of all EEPROM content arrays */
+EEPROM_DATA_t * eeprom_Arr_list[4] = {
+ eeprom_header_Arr,
+ eeprom_sys_Arr,
+ eeprom_profile_Arr,
+ 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;
+ 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: XOR operator byte per byte *
+***********************************************************************/
+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)));
}
/***********************************************************************
-* 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.
@@ -76,15 +178,59 @@ 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
+ 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 );
+
//FLASH_Erase_Sector(((uint32_t)11U),VOLTAGE_RANGE_3);
FLASH_Erase_Sector(EEPROM_SECTOR_ERASE, 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;
+ // We exclude reading of profiles not active to memory
+ if (!((j == 2) && (profile_counter != ((uint8_t)active_profile - 1) )))
+ {
+ 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)
+ }
}
HAL_FLASH_Lock();
}
@@ -93,28 +239,183 @@ 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));
}
+/***********************************************************************
+* 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++) // 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)
+ }
+ }
+
+ HAL_FLASH_Lock();
+
+ /* Check to see if CRC matches */
+ if ( (crc == calculated_crc) && (stored_eeprom_identifier == (uint8_t)EEPROM_SYS_VERSION) )
+ {
+ return true;
+ }
+ else
+ {
+ // ERROR!!! CORRUPT EEPROM, RESETTING
+
+ calculated_crc = old_crc;
+ Error_Handler();
+ resetEEPROM(); // Reinitialize eeprom with default values.
+
+ return true /* false */;
+ }
+}
/***********************************************************************
* 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;
- 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 ((success = 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
+ {
+
+
+ // We exclude reading of profiles not active to memory
+ if (!((j == 2) && (profile_counter != (uint8_t)(active_profile - 1))))
+ {
+ 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)
+ }
+ }
+ HAL_FLASH_Lock();
}
- 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);
}
void * getDataAddresFromID(uint16_t id, uint8_t dataType)
@@ -126,9 +427,16 @@ void * getDataAddresFromID(uint16_t id, uint8_t dataType)
switch(dataType)
{
case EEPROM_VALUE_TYPE_SYSTEM:
- toReturn = eepromArr[id].dataPtr;
+ toReturn = eeprom_sys_Arr[id].dataPtr;
break;
case EEPROM_VALUE_TYPE_PROFILE:
+ toReturn = eeprom_profile_Arr[id].dataPtr;
+ break;
+ case EEPROM_VALUE_TYPE_HEADER:
+ toReturn = eeprom_header_Arr[id].dataPtr;
+ break;
+ case EEPROM_VALUE_TYPE_FOOTER:
+ toReturn = eeprom_footer_Arr[id].dataPtr;
break;
default:
break;
diff --git a/UAV-ControlSystem/src/drivers/I2C.c b/UAV-ControlSystem/src/drivers/I2C.c
new file mode 100644
index 0000000..ed643b0
--- /dev/null
+++ b/UAV-ControlSystem/src/drivers/I2C.c
@@ -0,0 +1,128 @@
+/***************************************************************************
+* 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"
+#include "stm32f4xx_revo.h"
+
+/******************************************************************************
+* BRIEF: Configure the I2C bus to be used *
+* INFORMATION: *
+******************************************************************************/
+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 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;
+ 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
+ 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;
+ 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(i2c_port, &GPIO_InitStruct);
+
+ //Initialize I2C communication
+ 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;
+}
+
+
+/******************************************************************************
+* 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(I2C_HandleTypeDef* profile,
+ uint8_t slave_address,
+ uint8_t* buffer,
+ uint32_t length)
+{
+ uint8_t i = 0;
+ while(HAL_I2C_Master_Receive(profile, (slave_address << 1) | 1, buffer, length, 1000)!= HAL_OK && i++ < 10)
+ {}
+ while (HAL_I2C_GetState(profile) != 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(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(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(profile) != HAL_I2C_STATE_READY){}
+
+ return (i < 10);
+}
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;
+}
diff --git a/UAV-ControlSystem/src/drivers/led.c b/UAV-ControlSystem/src/drivers/led.c
new file mode 100644
index 0000000..cfda577
--- /dev/null
+++ b/UAV-ControlSystem/src/drivers/led.c
@@ -0,0 +1,183 @@
+/**************************************************************************
+* 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/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;
+ }
+}
diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c
index 59e4189..bbc3485 100644
--- a/UAV-ControlSystem/src/main.c
+++ b/UAV-ControlSystem/src/main.c
@@ -1,3 +1,4 @@
+
/**
******************************************************************************
* @file main.c
@@ -11,103 +12,95 @@
#include "drivers/adc.h"
-#include "config/eeprom.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
#include "drivers/uart1_inverter.h"
#include "config/cli.h"
+#include "config/eeprom.h"
-
-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();
- // Configure the system clock to 100 MHz
+ //Configure the clock
system_clock_config();
+#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
- adcScaleStruct_t.vcc_scale = 20;
- adcScaleStruct_t.i_scale_right = 10;
- adcScaleStruct_t.i_scale_left = 30;
+#ifdef BARO
- //test cli
- cliRun();
+#endif
- for(;;)
- {
- writeEEPROM();
- readEEPROM();
- adcScaleStruct_t.vcc_scale ++;
- adcScaleStruct_t.i_scale_right ++;
- adcScaleStruct_t.i_scale_left ++;
- uart1_rx_inverter = !uart1_rx_inverter;
- }
+#ifdef COMPASS
+#endif
+#ifdef GPS
- while(true);
+#endif
- int i = 1;
+#ifdef SONAR
+#endif
- //Add ADC Channels
- adc_pin_add(ADC_CHANNEL_0);
- adc_pin_add(ADC_CHANNEL_1);
- adc_pin_add(ADC_CHANNEL_12);
+#if defined(BARO) || defined(SONAR)
- //Configure the ADCs
- adc_configure();
+#endif
- /* This is done in system_clock_config for all GPIO clocks */
- //__GPIOB_CLK_ENABLE();
+#if BEEPER
- 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();
+}
- int num = 2000;
- int j = 0;
- volatile uint32_t time_us[num];
+/**************************************************************************
+* 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
new file mode 100644
index 0000000..78ba9fb
--- /dev/null
+++ b/UAV-ControlSystem/src/tasks_main.c
@@ -0,0 +1,132 @@
+/**************************************************************************
+* 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 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)
+{
+
+}
+
+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)
+{
+
+}
+
+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)
+{
+
+}
+
+/* TO BE USED ONLY WHEN TESTING/DEBUGIN TASK FUNCTIONALLITY, DONT USE WHEN RUNNING THE REAL SYSTEM!!!!!!!!!! */
+#ifdef USE_DEBUG_TASKS
+
+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"
+
+
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;