philip/MDH-UAV-Control-System
Archived
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge remote-tracking branch 'origin/eeprom' into eeprom

Browse Source 
# Conflicts:
#	UAV-ControlSystem/inc/config/eeprom.h
#	UAV-ControlSystem/src/config/eeprom.c
#	UAV-ControlSystem/src/main.c
This commit is contained in:
Jonas Holmberg 2016-10-04 14:03:36 +02:00
commit 05247d9039
22 changed files with 3556 additions and 112 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
UAV-ControlSystem/.cproject
View File

@ -39,6 +39,7 @@
<listOptionValue builtIn="false" value="STM32"/> <listOptionValue builtIn="false" value="STM32"/>
<listOptionValue builtIn="false" value="DEBUG"/> <listOptionValue builtIn="false" value="DEBUG"/>
<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/> <listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
<listOptionValue builtIn="false" value="bool=int"/>
<listOptionValue builtIn="false" value="STM32F405xx"/> <listOptionValue builtIn="false" value="STM32F405xx"/>
<listOptionValue builtIn="false" value="STM32F405RGTx"/> <listOptionValue builtIn="false" value="STM32F405RGTx"/>
<listOptionValue builtIn="false" value="bool=int"/> <listOptionValue builtIn="false" value="bool=int"/>

227
UAV-ControlSystem/inc/Scheduler/scheduler.h Normal file
View File

@ -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 <stdint.h>
#include <stdbool.h>
#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_ */

51
UAV-ControlSystem/inc/Scheduler/tasks.h Normal file
View File

@ -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 <stdint.h>
#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_ */

75
UAV-ControlSystem/inc/config/eeprom.h
View File

@ -119,30 +119,85 @@
/* Defines for what type of value something is, system, profile etc */ /* Defines for what type of value something is, system, profile etc */
#define EEPROM_VALUE_TYPE_SYSTEM 1 #define EEPROM_VALUE_TYPE_SYSTEM 1
#define EEPROM_VALUE_TYPE_PROFILE 2 #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
/*********************************************************************** /* The profiles one can choose from */
* BRIEF: List of all EEPROM values *
* INFORMATION: This content of this struct will be used in EEPROM *
***********************************************************************/
typedef enum { 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_UART1_RX_INV,
EEPROM_COUNT // Needs to be the last element as is used as counter /* Counts the amount of system settings */
} EEPROM_ID_t; 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 * * 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 * * BRIEF: Reads EEPROM data from FLASH *
* INFORMATION: passes all data directly to where they are defined * * 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 * * BRIEF: Gets the address of a value from an id *

111
UAV-ControlSystem/inc/drivers/I2C.h Normal file
View File

@ -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;
}
}
*/

183
UAV-ControlSystem/inc/drivers/accel_gyro.h Normal file
View File

@ -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 <stdbool.h>
#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_ */

112
UAV-ControlSystem/inc/drivers/led.h Normal file
View File

@ -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_ */

213
UAV-ControlSystem/inc/drivers/usart.h Normal file
View File

@ -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);
}
}
}
*/

42
UAV-ControlSystem/inc/stm32f4xx_revo.h
View File

@ -18,10 +18,10 @@
//Code here //Code here
//Leds? (taken from betaflight REVO h file) //Leds? (taken from betaflight REVO h file)
#define Led0_PIN GPIO_PIN_5 //Correct? #define Led0_PIN GPIO_PIN_5 //blue
#define Led0_GPIO_PORT GPIOB //Correct? #define Led0_GPIO_PORT GPIOB
#define Led1 GPIO_PIN_4 //Correct? #define Led1 GPIO_PIN_4 //yellow
#define Led1_GPIO_PORT GPIOB //Correct? #define Led1_GPIO_PORT GPIOB
//Servo out, To be used for motors? //Servo out, To be used for motors?
@ -79,10 +79,19 @@
#define USART6_RX_PIN GPIO_PIN_7 #define USART6_RX_PIN GPIO_PIN_7
#define USART6_RX_PORT GPIOC #define USART6_RX_PORT GPIOC
#define USART6_TX_PIN GPIO_PIN_6 #define USART6_TX_PIN GPIO_PIN_6
#define USART6_TX_PORT GPIOC #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 */ /* Gyro */
#define GYRO #define GYRO
#define MPU6000_CS_PIN GPIO_PIN_4 #define MPU6000_CS_PIN GPIO_PIN_4
@ -91,13 +100,36 @@
#define MPU6000_SPI_INSTANCE SPI1 //Dont know if necessary for us to specify #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 */ /* Baro */
//#define BARO
/* Compass */ /* Compass */
//#define COMPASS
/* GPS */ /* GPS */
//#define GPS
/* Sonar */
//#define SONAR
/* Beeper */
//#define BEEPER
/* Define all the moter of the system, servos + extra */ /* Define all the moter of the system, servos + extra */

4
UAV-ControlSystem/inc/system_variables.h
View File

@ -14,13 +14,13 @@
#ifndef SYSTEM_VARIABLES_H_ #ifndef SYSTEM_VARIABLES_H_
#define SYSTEM_VARIABLES_H_ #define SYSTEM_VARIABLES_H_
#define EEPROM_SYS_VERSION 101
#define ADC_STATE #define ADC_STATE
#include "stm32f4xx.h" #include "stm32f4xx.h"
extern uint8_t pid_pitch_pk;

21
UAV-ControlSystem/inc/utilities/math_helpers.h Normal file
View File

@ -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_ */

548
UAV-ControlSystem/src/Scheduler/scheduler.c Normal file
View File

@ -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 <string.h>
#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);
}
}

183
UAV-ControlSystem/src/Scheduler/tasks.c Normal file
View File

@ -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
};

370
UAV-ControlSystem/src/config/eeprom.c
View File

@ -17,30 +17,69 @@
#include "config/eeprom.h" #include "config/eeprom.h"
#include "drivers/adc.h" #include "drivers/adc.h"
#include "drivers/uart1_inverter.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 * /* General EEPROM data info. All data addressed needs both size and pointer */
* INFORMATION: Each EEPROM parameter needs an instance of this struct *
* to be added in eepromArr *
***********************************************************************/
typedef struct typedef struct
{ {
uint32_t writeTypeId; uint32_t writeTypeId;
uint32_t size; //Size of the data uint32_t size; //Size of the data
void * dataPtr; //pointer to the data, that should be saved to EEPROM 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 * /* Data pointers and sizes for header content */
***********************************************************************/ EEPROM_DATA_t eeprom_header_Arr[EEPROM_HEADER_COUNT] = {
EEPROM_DATA_t eepromArr[EEPROM_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] = [EEPROM_ADC_SCALES] =
{ {
.size = sizeof(adcScaleStruct_t), .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 * * BRIEF: Stores one data type at the time from eepromArr to EEPROM *
* INFORMATION: Writes byte per byte until end of data * * 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 ++) for (int i = 0; i < data[id].size; i ++)
HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, addr + i , *((uint8_t*)(eepromArr[id].dataPtr + 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 * * 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 /* Add size and a pointer to the data for each value
* Each value when saved will store the data that it points to. * 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*/ * point to and it can read the data directly to that value which is pointed to*/
uint32_t addrIterator = EEPROM_BASE_ADDR; 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_Unlock();
__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR ); __HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR );
//FLASH_Erase_Sector(((uint32_t)11U),VOLTAGE_RANGE_3); //FLASH_Erase_Sector(((uint32_t)11U),VOLTAGE_RANGE_3);
FLASH_Erase_Sector(EEPROM_SECTOR_ERASE, 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); // We exclude reading of profiles not active to memory
addrIterator += eepromArr[i].size; 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(); HAL_FLASH_Lock();
} }
@ -93,28 +239,183 @@ void writeEEPROM()
* BRIEF: Restores one data type at the time from eepromArr from EEPROM * * BRIEF: Restores one data type at the time from eepromArr from EEPROM *
* INFORMATION: * * 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++) for (int i = 0; i < data[id].size; i++)
*(uint8_t*)(eepromArr[id].dataPtr + i) = *( uint8_t*)(addr + i * sizeof(uint8_t)); *(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 * * BRIEF: Reads EEPROM data from FLASH *
* INFORMATION: passes all data directly to where they are defined * * 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; uint32_t addrIterator = EEPROM_BASE_ADDR;
HAL_FLASH_Unlock();
HAL_Delay(100); /* This check has to be done as not to overwrite memory with bad data */
for (int i = 0; i < EEPROM_COUNT; i ++) if ((success = scanEEPROM()))
{ {
readEepromExtension(addrIterator, i); HAL_FLASH_Unlock();
addrIterator += eepromArr[i].size; 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) void * getDataAddresFromID(uint16_t id, uint8_t dataType)
@ -126,9 +427,16 @@ void * getDataAddresFromID(uint16_t id, uint8_t dataType)
switch(dataType) switch(dataType)
{ {
case EEPROM_VALUE_TYPE_SYSTEM: case EEPROM_VALUE_TYPE_SYSTEM:
toReturn = eepromArr[id].dataPtr; toReturn = eeprom_sys_Arr[id].dataPtr;
break; break;
case EEPROM_VALUE_TYPE_PROFILE: 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; break;
default: default:
break; break;

128
UAV-ControlSystem/src/drivers/I2C.c Normal file
View File

@ -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);
}

553
UAV-ControlSystem/src/drivers/accel_gyro.c Normal file
View File

@ -0,0 +1,553 @@
/*
* gyro.c
*
* Created on: 16 sep. 2016
* Author: rsd12002
*/
#include <drivers/accel_gyro.h>
/***********************************************************************
* 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;
}

183
UAV-ControlSystem/src/drivers/led.c Normal file
View File

@ -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();
}

399
UAV-ControlSystem/src/drivers/usart.c Normal file
View File

@ -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;
}
}

119
UAV-ControlSystem/src/main.c
View File

@ -1,3 +1,4 @@
/** /**
****************************************************************************** ******************************************************************************
* @file main.c * @file main.c
@ -11,103 +12,95 @@
#include "drivers/adc.h" #include "drivers/adc.h"
#include "config/eeprom.h"
#include "drivers/system_clock.h" #include "drivers/system_clock.h"
#include "drivers/led.h"
#include "Scheduler/scheduler.h"
#include "stm32f4xx.h" #include "stm32f4xx.h"
#include "stm32f4xx_revo.h"
#include "system_variables.h" #include "system_variables.h"
#include "utilities.h" #include "utilities.h"
#include <string.h> #include <string.h>
#include "drivers/uart1_inverter.h" #include "drivers/uart1_inverter.h"
#include "config/cli.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(); HAL_Init();
// Configure the system clock to 100 MHz //Configure the clock
system_clock_config(); 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; #ifdef BARO
adcScaleStruct_t.i_scale_right = 10;
adcScaleStruct_t.i_scale_left = 30;
//test cli #endif
cliRun();
for(;;) #ifdef COMPASS
{
writeEEPROM();
readEEPROM();
adcScaleStruct_t.vcc_scale ++;
adcScaleStruct_t.i_scale_right ++;
adcScaleStruct_t.i_scale_left ++;
uart1_rx_inverter = !uart1_rx_inverter;
}
#endif
#ifdef GPS
while(true); #endif
int i = 1; #ifdef SONAR
#endif
//Add ADC Channels #if defined(BARO) || defined(SONAR)
adc_pin_add(ADC_CHANNEL_0);
adc_pin_add(ADC_CHANNEL_1);
adc_pin_add(ADC_CHANNEL_12);
//Configure the ADCs #endif
adc_configure();
/* This is done in system_clock_config for all GPIO clocks */ #if BEEPER
//__GPIOB_CLK_ENABLE();
GPIO_InitTypeDef gpinit; #endif
gpinit.Pin = GPIO_PIN_5;
gpinit.Mode = GPIO_MODE_OUTPUT_PP;
gpinit.Pull = GPIO_PULLUP;
gpinit.Speed = GPIO_SPEED_HIGH;
HAL_GPIO_Init(GPIOB, &gpinit);
adc_start(); }
int num = 2000; /**************************************************************************
int j = 0; * BRIEF: Main function of the system, every thing originates from this
volatile uint32_t time_us[num]; * 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) 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; #ifdef USE_LED_WARNINGS
//HAL_Delay(100); ledIndicatorsUpdate();
int g_ADCValue = adc_read(ADC_CHANNEL_0); #endif
int g_ADCValue1 = adc_read(ADC_CHANNEL_1);
int g_ADCValue12 = adc_read(ADC_CHANNEL_12);
int offTime = g_ADCValue;
int onTime = 4096 - offTime;
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5,GPIO_PIN_SET);
for (int i = 0; i < onTime; i++)
{
asm("nop");
}
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5,GPIO_PIN_RESET);
for (int i = 0; i < offTime; i++)
{
asm("nop");
}
//Get time in microseconds
if(j < num)
time_us[j++] = clock_get_us();
} }
for(;;); for(;;);
} }

132
UAV-ControlSystem/src/tasks_main.c Normal file
View File

@ -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 <stdint.h>
#include <stdbool.h>
#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 */

10
UAV-ControlSystem/src/utilities/math_helpers.c Normal file
View File

@ -0,0 +1,10 @@
/*
* math_helpers.c
*
* Created on: 21 sep. 2016
* Author: holmis
*/
#include "utilities/math_helpers.h"

3
revolution_hal_lib/HAL_Driver/Src/stm32f4xx_hal_i2c.c
View File

@ -4763,7 +4763,8 @@ static HAL_StatusTypeDef I2C_WaitOnFlagUntilTimeout(I2C_HandleTypeDef *hi2c, uin
/* Check for the Timeout */ /* Check for the Timeout */
if(Timeout != HAL_MAX_DELAY) 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->PreviousState = I2C_STATE_NONE;
hi2c->State= HAL_I2C_STATE_READY; hi2c->State= HAL_I2C_STATE_READY;