Merge remote-tracking branch 'origin/eeprom' into eeprom
# 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
commit
05247d9039
@ -39,6 +39,7 @@
|
||||
<listOptionValue builtIn="false" value="STM32"/>
|
||||
<listOptionValue builtIn="false" value="DEBUG"/>
|
||||
<listOptionValue builtIn="false" value="USE_HAL_DRIVER"/>
|
||||
<listOptionValue builtIn="false" value="bool=int"/>
|
||||
<listOptionValue builtIn="false" value="STM32F405xx"/>
|
||||
<listOptionValue builtIn="false" value="STM32F405RGTx"/>
|
||||
<listOptionValue builtIn="false" value="bool=int"/>
|
||||
|
||||
227
UAV-ControlSystem/inc/Scheduler/scheduler.h
Normal file
227
UAV-ControlSystem/inc/Scheduler/scheduler.h
Normal 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
51
UAV-ControlSystem/inc/Scheduler/tasks.h
Normal 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_ */
|
||||
@ -119,30 +119,85 @@
|
||||
/* Defines for what type of value something is, system, profile etc */
|
||||
#define EEPROM_VALUE_TYPE_SYSTEM 1
|
||||
#define EEPROM_VALUE_TYPE_PROFILE 2
|
||||
#define EEPROM_VALUE_TYPE_HEADER 3
|
||||
#define EEPROM_VALUE_TYPE_FOOTER 4
|
||||
|
||||
/* The size in bytes of the profile buffers. The error handler will be called if this is too small */
|
||||
#define EEPROM_PROFILE_SIZE 200
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: List of all EEPROM values *
|
||||
* INFORMATION: This content of this struct will be used in EEPROM *
|
||||
***********************************************************************/
|
||||
/* The profiles one can choose from */
|
||||
typedef enum {
|
||||
EEPROM_ADC_SCALES = 0,
|
||||
PROFILE_1 = 1,
|
||||
PROFILE_2,
|
||||
PROFILE_3
|
||||
} ACTIVE_PROFILE;
|
||||
|
||||
/* List of all header EEPROM values */
|
||||
typedef enum {
|
||||
EEPROM_VERSION = 0,
|
||||
|
||||
/* Counts the amount of system settings */
|
||||
EEPROM_HEADER_COUNT
|
||||
} EEPROM_HEADER_ID_t;
|
||||
|
||||
/* List of all system EEPROM values */
|
||||
typedef enum {
|
||||
EEPROM_ACTIVE_PROFILE = 0,
|
||||
EEPROM_ADC_SCALES,
|
||||
EEPROM_UART1_RX_INV,
|
||||
|
||||
EEPROM_COUNT // Needs to be the last element as is used as counter
|
||||
} EEPROM_ID_t;
|
||||
/* Counts the amount of system settings */
|
||||
EEPROM_SYS_COUNT
|
||||
} EEPROM_SYS_ID_t;
|
||||
|
||||
/* List of all profile EEPROM values */
|
||||
typedef enum {
|
||||
EEPROM_PID_ROLL_KP = 0,
|
||||
|
||||
/* Counts the amount of settings in profile */
|
||||
EEPROM_PROFILE_COUNT
|
||||
} EEPROM_PROFILE_ID_t;
|
||||
|
||||
/* List of all footer EEPROM values */
|
||||
typedef enum {
|
||||
EEPROM_CRC = 0,
|
||||
|
||||
/* Counts the amount of system settings */
|
||||
EEPROM_FOOTER_COUNT
|
||||
} EEPROM_FOOTER_ID_t;
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Writes EEPROM data to FLASH *
|
||||
* BRIEF: Writes EEPROM data to FLASH. Requires the next active profile *
|
||||
* to be selected (current profile can be used as input) *
|
||||
* INFORMATION: passes all data directly from where they are defined *
|
||||
***********************************************************************/
|
||||
void writeEEPROM();
|
||||
void writeEEPROM(ACTIVE_PROFILE new_active_profile);
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Writes EEPROM data to FLASH without the need of setting next *
|
||||
* active profile *
|
||||
* INFORMATION: Keeps the current profile active *
|
||||
***********************************************************************/
|
||||
void saveEEPROM();
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Reads EEPROM data from FLASH *
|
||||
* INFORMATION: passes all data directly to where they are defined *
|
||||
***********************************************************************/
|
||||
void readEEPROM();
|
||||
bool readEEPROM();
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Choose a profile between 1 .. 3 *
|
||||
* INFORMATION: The current changes will be saved *
|
||||
***********************************************************************/
|
||||
void setActiveProfile(ACTIVE_PROFILE profile);
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Writes current profile values to all EEPROM profiles *
|
||||
* INFORMATION: used when EEPROM is corrupt or there is a version *
|
||||
* mismatch *
|
||||
***********************************************************************/
|
||||
void resetEEPROM(void);
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Gets the address of a value from an id *
|
||||
|
||||
111
UAV-ControlSystem/inc/drivers/I2C.h
Normal file
111
UAV-ControlSystem/inc/drivers/I2C.h
Normal 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
183
UAV-ControlSystem/inc/drivers/accel_gyro.h
Normal 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
112
UAV-ControlSystem/inc/drivers/led.h
Normal 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
213
UAV-ControlSystem/inc/drivers/usart.h
Normal 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);
|
||||
}
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
@ -18,10 +18,10 @@
|
||||
//Code here
|
||||
|
||||
//Leds? (taken from betaflight REVO h file)
|
||||
#define Led0_PIN GPIO_PIN_5 //Correct?
|
||||
#define Led0_GPIO_PORT GPIOB //Correct?
|
||||
#define Led1 GPIO_PIN_4 //Correct?
|
||||
#define Led1_GPIO_PORT GPIOB //Correct?
|
||||
#define Led0_PIN GPIO_PIN_5 //blue
|
||||
#define Led0_GPIO_PORT GPIOB
|
||||
#define Led1 GPIO_PIN_4 //yellow
|
||||
#define Led1_GPIO_PORT GPIOB
|
||||
|
||||
|
||||
//Servo out, To be used for motors?
|
||||
@ -79,10 +79,19 @@
|
||||
#define USART6_RX_PIN GPIO_PIN_7
|
||||
#define USART6_RX_PORT GPIOC
|
||||
#define USART6_TX_PIN GPIO_PIN_6
|
||||
|
||||
#define USART6_TX_PORT GPIOC
|
||||
|
||||
|
||||
/* I2C */
|
||||
#define I2C1_SDA_PIN GPIO_PIN_9
|
||||
#define I2C1_SCL_PIN GPIO_PIN_8
|
||||
#define I2C1_PORT GPIOB
|
||||
|
||||
#define I2C2_SCL_PIN GPIO_PIN_10
|
||||
#define I2C2_SDA_PIN GPIO_PIN_11
|
||||
#define I2C2_PORT GPIOB
|
||||
|
||||
|
||||
/* Gyro */
|
||||
#define GYRO
|
||||
#define MPU6000_CS_PIN GPIO_PIN_4
|
||||
@ -91,13 +100,36 @@
|
||||
#define MPU6000_SPI_INSTANCE SPI1 //Dont know if necessary for us to specify
|
||||
|
||||
|
||||
/* Led Warnings */
|
||||
#define USE_LEDS
|
||||
#define USE_LED_WARNINGS
|
||||
#define USE_LED_WARNINGS_MISSED_PERIOD
|
||||
//#define USE_LED_WARNINGS_SYSTEM_LOAD
|
||||
|
||||
/* Scheduler */
|
||||
#define USE_DEBUG_TASKS //Only to be used when testing scheduler, not when intending to run the whole system
|
||||
#define USE_TASK_AGE_CYCLE_STATISTICS
|
||||
|
||||
|
||||
/* Baro */
|
||||
//#define BARO
|
||||
|
||||
|
||||
/* Compass */
|
||||
//#define COMPASS
|
||||
|
||||
|
||||
/* GPS */
|
||||
//#define GPS
|
||||
|
||||
|
||||
/* Sonar */
|
||||
//#define SONAR
|
||||
|
||||
|
||||
/* Beeper */
|
||||
//#define BEEPER
|
||||
|
||||
|
||||
|
||||
/* Define all the moter of the system, servos + extra */
|
||||
|
||||
@ -14,13 +14,13 @@
|
||||
#ifndef SYSTEM_VARIABLES_H_
|
||||
#define SYSTEM_VARIABLES_H_
|
||||
|
||||
|
||||
#define EEPROM_SYS_VERSION 101
|
||||
|
||||
#define ADC_STATE
|
||||
#include "stm32f4xx.h"
|
||||
|
||||
|
||||
|
||||
extern uint8_t pid_pitch_pk;
|
||||
|
||||
|
||||
|
||||
|
||||
21
UAV-ControlSystem/inc/utilities/math_helpers.h
Normal file
21
UAV-ControlSystem/inc/utilities/math_helpers.h
Normal 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
548
UAV-ControlSystem/src/Scheduler/scheduler.c
Normal 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
183
UAV-ControlSystem/src/Scheduler/tasks.c
Normal 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
|
||||
|
||||
};
|
||||
@ -17,30 +17,69 @@
|
||||
#include "config/eeprom.h"
|
||||
#include "drivers/adc.h"
|
||||
#include "drivers/uart1_inverter.h"
|
||||
#include "system_variables.h"
|
||||
#include "utilities.h"
|
||||
|
||||
/* Reads the EEPROM version from EEPROM - Is compared to EEPROM_SYS_VERSION */
|
||||
uint8_t stored_eeprom_identifier;
|
||||
|
||||
/* Buffer where we temporarily store profiles we not use as we erase eeprom before write */
|
||||
uint8_t eeprom_profile_tmp_a[EEPROM_PROFILE_SIZE];
|
||||
uint8_t eeprom_profile_tmp_b[EEPROM_PROFILE_SIZE];
|
||||
|
||||
/* The two temporary buffers are addressed through this array */
|
||||
uint8_t * eeprom_profile_tmp_Arr[2] = {
|
||||
eeprom_profile_tmp_a,
|
||||
eeprom_profile_tmp_b
|
||||
};
|
||||
|
||||
|
||||
/* Created CRC */
|
||||
uint8_t calculated_crc;
|
||||
uint8_t read_crc;
|
||||
|
||||
/* Defines which profile is active. Should not be changed other than calling setActiveProfile() */
|
||||
ACTIVE_PROFILE active_profile = 1; // Between 1 .. 3
|
||||
|
||||
|
||||
|
||||
/* List of all EEPROM lists */
|
||||
uint8_t eeprom_blocksize_Arr[4] = {
|
||||
EEPROM_HEADER_COUNT,
|
||||
EEPROM_SYS_COUNT,
|
||||
EEPROM_PROFILE_COUNT,
|
||||
EEPROM_FOOTER_COUNT
|
||||
};
|
||||
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: General EEPROM data info *
|
||||
* INFORMATION: Each EEPROM parameter needs an instance of this struct *
|
||||
* to be added in eepromArr *
|
||||
***********************************************************************/
|
||||
|
||||
/* General EEPROM data info. All data addressed needs both size and pointer */
|
||||
typedef struct
|
||||
{
|
||||
uint32_t writeTypeId;
|
||||
uint32_t size; //Size of the data
|
||||
void * dataPtr; //pointer to the data, that should be saved to EEPROM
|
||||
uint32_t padding[];
|
||||
}EEPROM_DATA_t;
|
||||
} EEPROM_DATA_t;
|
||||
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Data info to be passed to the write function *
|
||||
* INFORMATION: passes size and data pointer *
|
||||
***********************************************************************/
|
||||
EEPROM_DATA_t eepromArr[EEPROM_COUNT] = {
|
||||
|
||||
|
||||
/* Data pointers and sizes for header content */
|
||||
EEPROM_DATA_t eeprom_header_Arr[EEPROM_HEADER_COUNT] = {
|
||||
[EEPROM_VERSION] =
|
||||
{
|
||||
.size = sizeof(stored_eeprom_identifier),
|
||||
.dataPtr = &stored_eeprom_identifier,
|
||||
}
|
||||
};
|
||||
|
||||
/* Data pointers and sizes for sys content */
|
||||
EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
|
||||
[EEPROM_ACTIVE_PROFILE] =
|
||||
{
|
||||
.size = sizeof(active_profile),
|
||||
.dataPtr = &active_profile,
|
||||
},
|
||||
[EEPROM_ADC_SCALES] =
|
||||
{
|
||||
.size = sizeof(adcScaleStruct_t),
|
||||
@ -53,22 +92,85 @@ EEPROM_DATA_t eepromArr[EEPROM_COUNT] = {
|
||||
}
|
||||
};
|
||||
|
||||
/* Data pointers and sizes for profile content */
|
||||
EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = {
|
||||
[EEPROM_PID_ROLL_KP] =
|
||||
{
|
||||
.size = sizeof(pid_pitch_pk),
|
||||
.dataPtr = &pid_pitch_pk,
|
||||
}
|
||||
};
|
||||
|
||||
/* Data pointers and sizes for footer content */
|
||||
EEPROM_DATA_t eeprom_footer_Arr[EEPROM_FOOTER_COUNT] = {
|
||||
[EEPROM_CRC] =
|
||||
{
|
||||
.size = sizeof(calculated_crc),
|
||||
.dataPtr = &calculated_crc,
|
||||
}
|
||||
};
|
||||
|
||||
/* List of all EEPROM content arrays */
|
||||
EEPROM_DATA_t * eeprom_Arr_list[4] = {
|
||||
eeprom_header_Arr,
|
||||
eeprom_sys_Arr,
|
||||
eeprom_profile_Arr,
|
||||
eeprom_footer_Arr
|
||||
};
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Reads Calculates the combined size of all elements *
|
||||
* in an EEPROM_DATA_t array *
|
||||
* INFORMATION: return value is in bytes *
|
||||
***********************************************************************/
|
||||
uint16_t eepromArrSize(EEPROM_DATA_t * data)
|
||||
{
|
||||
int size = 0;
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
if (data == eeprom_Arr_list[j])
|
||||
for (int i = 0; i < eeprom_blocksize_Arr[j]; i++)
|
||||
{
|
||||
size += data[i].size;
|
||||
}
|
||||
}
|
||||
if (size == 0)
|
||||
Error_Handler(); // We should never get here
|
||||
return size;
|
||||
}
|
||||
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Increments the checksum *
|
||||
* INFORMATION: XOR operator byte per byte *
|
||||
***********************************************************************/
|
||||
uint8_t incrementChecksum(uint8_t crc, const void *data, uint32_t length)
|
||||
{
|
||||
const uint8_t * addrIterator = (const uint8_t *)data;
|
||||
const uint8_t * addrEnd = addrIterator + length;
|
||||
|
||||
for (;addrIterator != addrEnd; addrIterator++)
|
||||
crc ^= *addrIterator;
|
||||
|
||||
return crc;
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Stores one data type at the time from eepromArr to EEPROM *
|
||||
* INFORMATION: Writes byte per byte until end of data *
|
||||
***********************************************************************/
|
||||
void writeEepromExtension(uint32_t addr, uint32_t id)
|
||||
void writeEepromExtension(uint32_t addr,EEPROM_DATA_t * data, uint32_t id)
|
||||
{
|
||||
for (int i = 0; i < eepromArr[id].size; i ++)
|
||||
HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, addr + i , *((uint8_t*)(eepromArr[id].dataPtr + i)));
|
||||
for (int i = 0; i < data[id].size; i ++)
|
||||
HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, addr + i , *((uint8_t*)(data[id].dataPtr + i)));
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Writes EEPROM data to FLASH *
|
||||
* BRIEF: Writes EEPROM data to FLASH. Requires the next active profile *
|
||||
* to be selected (current profile can be used as input) *
|
||||
* INFORMATION: passes all data directly from where they are defined *
|
||||
***********************************************************************/
|
||||
void writeEEPROM()
|
||||
void writeEEPROM(uint8_t new_profile)
|
||||
{
|
||||
/* Add size and a pointer to the data for each value
|
||||
* Each value when saved will store the data that it points to.
|
||||
@ -76,15 +178,59 @@ void writeEEPROM()
|
||||
* point to and it can read the data directly to that value which is pointed to*/
|
||||
|
||||
uint32_t addrIterator = EEPROM_BASE_ADDR;
|
||||
uint8_t crc = 0; // Initializing the checksum to 0
|
||||
uint8_t profile_counter = 0;
|
||||
uint8_t buff_counter = 0;
|
||||
ACTIVE_PROFILE old_profile = active_profile;
|
||||
|
||||
HAL_FLASH_Unlock();
|
||||
__HAL_FLASH_CLEAR_FLAG(FLASH_FLAG_EOP | FLASH_FLAG_OPERR | FLASH_FLAG_WRPERR | FLASH_FLAG_PGAERR | FLASH_FLAG_PGSERR );
|
||||
|
||||
//FLASH_Erase_Sector(((uint32_t)11U),VOLTAGE_RANGE_3);
|
||||
FLASH_Erase_Sector(EEPROM_SECTOR_ERASE, VOLTAGE_RANGE_3);
|
||||
|
||||
for (int i = 0; i < EEPROM_COUNT; i ++)
|
||||
|
||||
stored_eeprom_identifier = (uint8_t)EEPROM_SYS_VERSION;
|
||||
|
||||
for (int j = 0; j < 4; j++)
|
||||
{
|
||||
writeEepromExtension(addrIterator, i);
|
||||
addrIterator += eepromArr[i].size;
|
||||
// We exclude reading of profiles not active to memory
|
||||
if (!((j == 2) && (profile_counter != ((uint8_t)active_profile - 1) )))
|
||||
{
|
||||
if (j == 1) // Writing sys we store the NEW profile
|
||||
active_profile = new_profile;
|
||||
for (int i = 0; i < eeprom_blocksize_Arr[j]; i++)
|
||||
{
|
||||
if (eeprom_Arr_list[j] == eeprom_footer_Arr)
|
||||
calculated_crc = crc;
|
||||
writeEepromExtension(addrIterator, eeprom_Arr_list[j], i);
|
||||
addrIterator += eeprom_Arr_list[j][i].size;
|
||||
crc = incrementChecksum(crc, eeprom_Arr_list[j][i].dataPtr, eeprom_Arr_list[j][i].size);
|
||||
}
|
||||
if (j == 1)
|
||||
active_profile = old_profile; // restore the old profile
|
||||
}
|
||||
else
|
||||
{
|
||||
if (buff_counter >= 2)
|
||||
Error_Handler();
|
||||
|
||||
int eeprom_profile_arr_size = eepromArrSize(eeprom_profile_Arr);
|
||||
for (int i = 0; i < eeprom_profile_arr_size; i ++)
|
||||
HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, addrIterator + i , *((uint8_t*)(eeprom_profile_tmp_Arr[buff_counter] + i)));
|
||||
|
||||
crc = incrementChecksum(crc, eeprom_profile_tmp_Arr[buff_counter], eepromArrSize(eeprom_profile_Arr));
|
||||
buff_counter++; // move on to next buffer
|
||||
|
||||
// Adding size of eeprom array
|
||||
addrIterator += eeprom_profile_arr_size;
|
||||
}
|
||||
// Halt iterator j @2 (profile) for 3 iterations (3 profiles)
|
||||
if ((j == 2) && (profile_counter < 2))
|
||||
{
|
||||
profile_counter++;
|
||||
j--; // We do this to keep j = 2 for three iterations (looping through all profiles)
|
||||
}
|
||||
}
|
||||
HAL_FLASH_Lock();
|
||||
}
|
||||
@ -93,28 +239,183 @@ void writeEEPROM()
|
||||
* BRIEF: Restores one data type at the time from eepromArr from EEPROM *
|
||||
* INFORMATION: *
|
||||
***********************************************************************/
|
||||
void readEepromExtension(uint32_t addr, uint32_t id)
|
||||
void readEepromExtension(uint32_t addr, EEPROM_DATA_t * data, uint32_t id)
|
||||
{
|
||||
for (int i = 0; i < eepromArr[id].size; i++)
|
||||
*(uint8_t*)(eepromArr[id].dataPtr + i) = *( uint8_t*)(addr + i * sizeof(uint8_t));
|
||||
for (int i = 0; i < data[id].size; i++)
|
||||
*(uint8_t*)(data[id].dataPtr + i) = *( uint8_t*)(addr + i * sizeof(uint8_t));
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Copies a profile from EEPROM into a buffer *
|
||||
* INFORMATION: Run once for every buffer *
|
||||
***********************************************************************/
|
||||
void readEepromBuff(uint32_t addr, uint8_t * eeprom_profile_tmp, uint32_t length)
|
||||
{
|
||||
for (int i = 0; i < length; i++)
|
||||
*(uint8_t*)(eeprom_profile_tmp + i) = *(uint8_t*)(addr + i * sizeof(uint8_t));
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Verifies EEPROM only *
|
||||
* INFORMATION: compares EEPROM version and makes CRC checks *
|
||||
* Return value is true if okay *
|
||||
***********************************************************************/
|
||||
bool scanEEPROM(void)
|
||||
{
|
||||
uint8_t crc = 0;
|
||||
uint8_t old_crc = calculated_crc; // store to restore
|
||||
uint8_t profile_counter = 0; // To iterate through the profiles
|
||||
uint32_t addrIterator = EEPROM_BASE_ADDR;
|
||||
HAL_FLASH_Unlock();
|
||||
HAL_Delay(100);
|
||||
|
||||
for (int j = 0; j < 4; j++) // 3 to skip footer from CRC calculation
|
||||
{
|
||||
for (int i = 0; i < eeprom_blocksize_Arr[j]; i++)
|
||||
{
|
||||
// Store data only for Header (EEPROM version) and footer (CRC) while scanning
|
||||
if ((eeprom_Arr_list[j] == eeprom_header_Arr) || (eeprom_Arr_list[j] == eeprom_footer_Arr))
|
||||
readEepromExtension(addrIterator, eeprom_Arr_list[j], i);
|
||||
// Checksum not incremented for footer
|
||||
if (eeprom_Arr_list[j] != eeprom_footer_Arr)
|
||||
crc = incrementChecksum(crc, (int*)addrIterator, eeprom_Arr_list[j][i].size);
|
||||
addrIterator += eeprom_Arr_list[j][i].size;
|
||||
}
|
||||
// Halt iterator j @2 (profile) for 3 iterations (3 profiles)
|
||||
if ((j == 2) && (profile_counter < 2))
|
||||
{
|
||||
profile_counter++;
|
||||
j--; // We do this to keep j = 2 for three iterations (looping through all profiles)
|
||||
}
|
||||
}
|
||||
|
||||
HAL_FLASH_Lock();
|
||||
|
||||
/* Check to see if CRC matches */
|
||||
if ( (crc == calculated_crc) && (stored_eeprom_identifier == (uint8_t)EEPROM_SYS_VERSION) )
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// ERROR!!! CORRUPT EEPROM, RESETTING
|
||||
|
||||
calculated_crc = old_crc;
|
||||
Error_Handler();
|
||||
resetEEPROM(); // Reinitialize eeprom with default values.
|
||||
|
||||
return true /* false */;
|
||||
}
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Reads EEPROM data from FLASH *
|
||||
* INFORMATION: passes all data directly to where they are defined *
|
||||
***********************************************************************/
|
||||
void readEEPROM()
|
||||
bool readEEPROM()
|
||||
{
|
||||
bool success;
|
||||
uint8_t profile_counter = 0;
|
||||
uint8_t buff_counter = 0;
|
||||
uint32_t addrIterator = EEPROM_BASE_ADDR;
|
||||
HAL_FLASH_Unlock();
|
||||
HAL_Delay(100);
|
||||
for (int i = 0; i < EEPROM_COUNT; i ++)
|
||||
|
||||
/* This check has to be done as not to overwrite memory with bad data */
|
||||
if ((success = scanEEPROM()))
|
||||
{
|
||||
readEepromExtension(addrIterator, i);
|
||||
addrIterator += eepromArr[i].size;
|
||||
HAL_FLASH_Unlock();
|
||||
HAL_Delay(100);
|
||||
|
||||
for (int j = 0; j < 3; j++) // 3 excludes the footer
|
||||
{
|
||||
|
||||
|
||||
// We exclude reading of profiles not active to memory
|
||||
if (!((j == 2) && (profile_counter != (uint8_t)(active_profile - 1))))
|
||||
{
|
||||
for (int i = 0; i < eeprom_blocksize_Arr[j]; i++)
|
||||
{
|
||||
readEepromExtension(addrIterator, eeprom_Arr_list[j], i);
|
||||
addrIterator += eeprom_Arr_list[j][i].size;
|
||||
}
|
||||
}
|
||||
// The two not loaded profiles are stored to buffers
|
||||
else
|
||||
{
|
||||
if (buff_counter >= 2)
|
||||
Error_Handler();
|
||||
|
||||
// Import the data from eeprom to buffer
|
||||
readEepromBuff((uint32_t*)addrIterator, (uint8_t*)eeprom_profile_tmp_Arr[buff_counter], EEPROM_PROFILE_SIZE);
|
||||
buff_counter++;
|
||||
|
||||
// Adding size of eeprom array
|
||||
addrIterator += eepromArrSize(eeprom_profile_Arr);
|
||||
}
|
||||
// Halt iterator j @2 (profile) for 3 iterations (3 profiles)
|
||||
if ((j == 2) && (profile_counter < 2))
|
||||
{
|
||||
profile_counter++;
|
||||
j--; // We do this to keep j = 2 for three iterations (looping through all profiles)
|
||||
}
|
||||
}
|
||||
HAL_FLASH_Lock();
|
||||
}
|
||||
HAL_FLASH_Lock();
|
||||
else
|
||||
{
|
||||
Error_Handler();
|
||||
}
|
||||
return success;
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Choose a profile between 1 .. 3 *
|
||||
* INFORMATION: The current changes will be saved *
|
||||
***********************************************************************/
|
||||
void setActiveProfile(ACTIVE_PROFILE new_profile)
|
||||
{
|
||||
// Error handler
|
||||
if (new_profile < 1 || new_profile > 3)
|
||||
Error_Handler();
|
||||
|
||||
writeEEPROM(new_profile);
|
||||
|
||||
readEEPROM();
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Writes current profile values to all EEPROM profiles *
|
||||
* INFORMATION: used when EEPROM is corrupt or there is a version *
|
||||
* mismatch *
|
||||
***********************************************************************/
|
||||
void resetEEPROM(void)
|
||||
{
|
||||
/* check so that the profile buffer sizes are not smaller than the size of one profile */
|
||||
if (EEPROM_PROFILE_SIZE < eepromArrSize(eeprom_profile_Arr) )
|
||||
Error_Handler(); // We need to increment the EEPROM_PROFILE_SIZE
|
||||
|
||||
uint32_t bufferIterator = 0;
|
||||
//Loop through all the values in the EEPROM profile
|
||||
for (int j = 0; j < EEPROM_PROFILE_COUNT; j++)
|
||||
{
|
||||
//for each value loop on its byte size and then write byte by byte to the buffers
|
||||
for (int i = 0; i < eeprom_profile_Arr[i].size; i++)
|
||||
{
|
||||
*(uint8_t*)(eeprom_profile_tmp_Arr[0] + bufferIterator) = *(uint8_t*)(eeprom_profile_Arr[j].dataPtr + i);
|
||||
*(uint8_t*)(eeprom_profile_tmp_Arr[1] + bufferIterator) = *(uint8_t*)(eeprom_profile_Arr[j].dataPtr + i);
|
||||
bufferIterator++;
|
||||
}
|
||||
}
|
||||
writeEEPROM(active_profile);
|
||||
}
|
||||
|
||||
/***********************************************************************
|
||||
* BRIEF: Writes EEPROM data to FLASH without the need of setting next *
|
||||
* active profile *
|
||||
* INFORMATION: Keeps the current profile active *
|
||||
***********************************************************************/
|
||||
void saveEEPROM()
|
||||
{
|
||||
writeEEPROM(active_profile);
|
||||
}
|
||||
|
||||
void * getDataAddresFromID(uint16_t id, uint8_t dataType)
|
||||
@ -126,9 +427,16 @@ void * getDataAddresFromID(uint16_t id, uint8_t dataType)
|
||||
switch(dataType)
|
||||
{
|
||||
case EEPROM_VALUE_TYPE_SYSTEM:
|
||||
toReturn = eepromArr[id].dataPtr;
|
||||
toReturn = eeprom_sys_Arr[id].dataPtr;
|
||||
break;
|
||||
case EEPROM_VALUE_TYPE_PROFILE:
|
||||
toReturn = eeprom_profile_Arr[id].dataPtr;
|
||||
break;
|
||||
case EEPROM_VALUE_TYPE_HEADER:
|
||||
toReturn = eeprom_header_Arr[id].dataPtr;
|
||||
break;
|
||||
case EEPROM_VALUE_TYPE_FOOTER:
|
||||
toReturn = eeprom_footer_Arr[id].dataPtr;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
||||
128
UAV-ControlSystem/src/drivers/I2C.c
Normal file
128
UAV-ControlSystem/src/drivers/I2C.c
Normal 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
553
UAV-ControlSystem/src/drivers/accel_gyro.c
Normal 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
183
UAV-ControlSystem/src/drivers/led.c
Normal 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
399
UAV-ControlSystem/src/drivers/usart.c
Normal 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;
|
||||
}
|
||||
}
|
||||
@ -1,3 +1,4 @@
|
||||
|
||||
/**
|
||||
******************************************************************************
|
||||
* @file main.c
|
||||
@ -11,103 +12,95 @@
|
||||
|
||||
|
||||
#include "drivers/adc.h"
|
||||
#include "config/eeprom.h"
|
||||
#include "drivers/system_clock.h"
|
||||
#include "drivers/led.h"
|
||||
#include "Scheduler/scheduler.h"
|
||||
#include "stm32f4xx.h"
|
||||
#include "stm32f4xx_revo.h"
|
||||
#include "system_variables.h"
|
||||
#include "utilities.h"
|
||||
#include <string.h>
|
||||
#include "drivers/uart1_inverter.h"
|
||||
#include "config/cli.h"
|
||||
#include "config/eeprom.h"
|
||||
|
||||
|
||||
int main(void)
|
||||
/**************************************************************************
|
||||
* BRIEF: Should contain all the initializations of the system, needs to
|
||||
* run before the scheduler.
|
||||
*
|
||||
* INFORMATION: Everything that will run somewhere in the system at some
|
||||
* possible point and needs to be initialized to run properly, have to do it
|
||||
* inside this function.
|
||||
**************************************************************************/
|
||||
void init_system()
|
||||
{
|
||||
// Initialize the Hardware Abstraction Layer
|
||||
//Init the Hardware abstraction layer (HAL)
|
||||
HAL_Init();
|
||||
|
||||
// Configure the system clock to 100 MHz
|
||||
//Configure the clock
|
||||
system_clock_config();
|
||||
|
||||
|
||||
#ifdef USE_LEDS
|
||||
//Initialize the on board leds
|
||||
ledReavoEnable();
|
||||
ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
|
||||
ledOffInverted(Led1, Led1_GPIO_PORT);
|
||||
//ledEnable(GPIO_PIN_0, GPIOA);
|
||||
#endif
|
||||
|
||||
adcScaleStruct_t.vcc_scale = 20;
|
||||
adcScaleStruct_t.i_scale_right = 10;
|
||||
adcScaleStruct_t.i_scale_left = 30;
|
||||
#ifdef BARO
|
||||
|
||||
//test cli
|
||||
cliRun();
|
||||
#endif
|
||||
|
||||
for(;;)
|
||||
{
|
||||
writeEEPROM();
|
||||
readEEPROM();
|
||||
adcScaleStruct_t.vcc_scale ++;
|
||||
adcScaleStruct_t.i_scale_right ++;
|
||||
adcScaleStruct_t.i_scale_left ++;
|
||||
uart1_rx_inverter = !uart1_rx_inverter;
|
||||
}
|
||||
#ifdef COMPASS
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef GPS
|
||||
|
||||
while(true);
|
||||
#endif
|
||||
|
||||
int i = 1;
|
||||
#ifdef SONAR
|
||||
|
||||
#endif
|
||||
|
||||
//Add ADC Channels
|
||||
adc_pin_add(ADC_CHANNEL_0);
|
||||
adc_pin_add(ADC_CHANNEL_1);
|
||||
adc_pin_add(ADC_CHANNEL_12);
|
||||
#if defined(BARO) || defined(SONAR)
|
||||
|
||||
//Configure the ADCs
|
||||
adc_configure();
|
||||
#endif
|
||||
|
||||
/* This is done in system_clock_config for all GPIO clocks */
|
||||
//__GPIOB_CLK_ENABLE();
|
||||
#if BEEPER
|
||||
|
||||
GPIO_InitTypeDef gpinit;
|
||||
gpinit.Pin = GPIO_PIN_5;
|
||||
gpinit.Mode = GPIO_MODE_OUTPUT_PP;
|
||||
gpinit.Pull = GPIO_PULLUP;
|
||||
gpinit.Speed = GPIO_SPEED_HIGH;
|
||||
HAL_GPIO_Init(GPIOB, &gpinit);
|
||||
#endif
|
||||
|
||||
adc_start();
|
||||
}
|
||||
|
||||
int num = 2000;
|
||||
int j = 0;
|
||||
volatile uint32_t time_us[num];
|
||||
/**************************************************************************
|
||||
* BRIEF: Main function of the system, every thing originates from this
|
||||
* point.
|
||||
*
|
||||
* INFORMATION: The function is responsible for calling the Initialize
|
||||
* function that will make the system ready to run. It is also responsible
|
||||
* for starting and running the scheduler in a loop that never stops.
|
||||
**************************************************************************/
|
||||
int main(void)
|
||||
{
|
||||
//Init the system
|
||||
init_system();
|
||||
|
||||
//Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler
|
||||
initScheduler();
|
||||
|
||||
while (1)
|
||||
{
|
||||
i++;
|
||||
//Run the scheduler, responsible for distributing all the work of the running system
|
||||
scheduler();
|
||||
|
||||
//g_ADCValue = accumulate(g_ADCBuffer,ADC_BUFFER_LENGTH) / ADC_BUFFER_LENGTH;
|
||||
//HAL_Delay(100);
|
||||
int g_ADCValue = adc_read(ADC_CHANNEL_0);
|
||||
int g_ADCValue1 = adc_read(ADC_CHANNEL_1);
|
||||
int g_ADCValue12 = adc_read(ADC_CHANNEL_12);
|
||||
|
||||
int offTime = g_ADCValue;
|
||||
int onTime = 4096 - offTime;
|
||||
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5,GPIO_PIN_SET);
|
||||
for (int i = 0; i < onTime; i++)
|
||||
{
|
||||
asm("nop");
|
||||
}
|
||||
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5,GPIO_PIN_RESET);
|
||||
for (int i = 0; i < offTime; i++)
|
||||
{
|
||||
asm("nop");
|
||||
}
|
||||
|
||||
//Get time in microseconds
|
||||
if(j < num)
|
||||
time_us[j++] = clock_get_us();
|
||||
#ifdef USE_LED_WARNINGS
|
||||
ledIndicatorsUpdate();
|
||||
#endif
|
||||
}
|
||||
|
||||
for(;;);
|
||||
}
|
||||
|
||||
|
||||
|
||||
132
UAV-ControlSystem/src/tasks_main.c
Normal file
132
UAV-ControlSystem/src/tasks_main.c
Normal 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
10
UAV-ControlSystem/src/utilities/math_helpers.c
Normal file
@ -0,0 +1,10 @@
|
||||
/*
|
||||
* math_helpers.c
|
||||
*
|
||||
* Created on: 21 sep. 2016
|
||||
* Author: holmis
|
||||
*/
|
||||
|
||||
#include "utilities/math_helpers.h"
|
||||
|
||||
|
||||
@ -4763,7 +4763,8 @@ static HAL_StatusTypeDef I2C_WaitOnFlagUntilTimeout(I2C_HandleTypeDef *hi2c, uin
|
||||
/* Check for the Timeout */
|
||||
if(Timeout != HAL_MAX_DELAY)
|
||||
{
|
||||
if((Timeout == 0U)||((HAL_GetTick() - tickstart ) > Timeout))
|
||||
uint32_t time = HAL_GetTick();
|
||||
if((Timeout == 0U)||((time - tickstart ) > Timeout))
|
||||
{
|
||||
hi2c->PreviousState = I2C_STATE_NONE;
|
||||
hi2c->State= HAL_I2C_STATE_READY;
|
||||
|
||||
Reference in New Issue
Block a user