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

Merge branch 'PID-Improved'

Browse Source 
# Conflicts:
#	UAV-ControlSystem/src/main.c
This commit is contained in:
philsson 2016-11-01 14:41:18 +01:00
commit 27362fcbb6
24 changed files with 1455 additions and 133 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
UAV-ControlSystem/inc/Flight/pid.h
View File

@ -18,6 +18,7 @@
#include<stdio.h>
#include<stdint.h>
#include "Flight/filter.h"
#include "drivers/accel_gyro.h"
#define XYZ_AXIS_COUNT 3 /*The maximum number of DOF that belongings to the PID*/
@ -60,8 +61,13 @@ typedef struct pidProfile_s {
/*Array of all pid profiles of the system*/
extern pidProfile_t PidProfile[PID_COUNT];
/*Is set in motor mix and used in pidUAVcore and mix */
bool motorLimitReached;
/* */
extern float accRollFineTune;
extern float accPitchFineTune;
extern accel_t accelProfile; /*Struct profile for input data from sensor*/
/**************************************************************************
* BRIEF: Initializes PID profiles *
@ -75,4 +81,6 @@ void pidInit();
**************************************************************************/
void pidRun(uint8_t ID);
void pidEproom(void);
#endif /* FLIGHT_PID_H_ */

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

@ -191,6 +191,15 @@ typedef enum {
EEPROM_FLAG_MIXERLOWSCALE,
EEPROM_FLAG_FLIGHTMODE_3,
/* accel calibration values */
EEPROM_FLAG_ACCELTUNE_OFFSET_X,
EEPROM_FLAG_ACCELTUNE_OFFSET_Y,
EEPROM_FLAG_ACCELTUNE_OFFSET_Z,
/* accel calibration fine tune values */
EEPROM_FLAG_ACCELTUNE_FINE_ROLL,
EEPROM_FLAG_ACCELTUNE_FINE_PITCH,
/* Counts the amount of system settings */
EEPROM_SYS_COUNT
} EEPROM_SYS_ID_t;

12
UAV-ControlSystem/inc/drivers/I2C.h
View File

@ -54,6 +54,18 @@ bool i2c_send(I2C_HandleTypeDef* profile,
uint32_t length);
/******************************************************************************
* BRIEF: Configure the I2C bus to be used *
* INFORMATION: This function only implements I2C1 or I2C2 DMA which are *
* available on the REVO board *
******************************************************************************/
bool i2c_configure_DMA(I2C_TypeDef *i2c,
I2C_HandleTypeDef *out_profile,
DMA_HandleTypeDef *out_rxDMA_profile,
DMA_HandleTypeDef *out_txDMA_profile,
uint32_t my_address);
#endif /* DRIVERS_I2C_H_ */

2
UAV-ControlSystem/inc/drivers/accel_gyro.h
View File

@ -27,7 +27,7 @@
#ifndef DRIVERS_ACCEL_GYRO_H_
#define DRIVERS_ACCEL_GYRO_H_
#include <stdbool.h>
//#include <stdbool.h>
#include "stm32f4xx.h"
#include "stm32f4xx_revo.h"

26
UAV-ControlSystem/inc/drivers/barometer.h Normal file
View File

@ -0,0 +1,26 @@
/*
* barometer.h
*
* Created on: 18 okt. 2016
* Author: holmis
*/
#ifndef DRIVERS_BAROMETER_H_
#define DRIVERS_BAROMETER_H_
bool barometer_init();
bool barometer_reset();
void barometer_CaclulateValues();
double barometer_GetCurrentPreassure();
double barometer_GetCurrentTemperature();
float barometer_GetCurrentAltitudeBasedOnSeaLevel();
#endif /* DRIVERS_BAROMETER_H_ */

66
UAV-ControlSystem/inc/drivers/failsafe_toggles.h
View File

@ -28,7 +28,7 @@
#define getFlagMaskValue(x) (1 << x)
#define failSafe_vals_offset 0 //offset for the fail safe values in the bitfield
#define boolean_vals_offset 3 //offset for the booleans values in the bitfield
#define boolean_vals_offset 3 //offset for the booleans values in the bitfield. Equals the amount of failsafe ids
/*If a new value is added to the bitfield these IDs must be reviewed and checkd so that they still are correct*/
//failsafe values
@ -45,6 +45,17 @@
#define systemFlags_mixerfullscale_id 5 + boolean_vals_offset
#define systemFlags_mixerlowscale_id 6 + boolean_vals_offset
#define systemFlags_flightMode_3_id 7 + boolean_vals_offset
#define systemFlags_barometerIsCalibrated_id 8 + boolean_vals_offset
#define systemFlags_AcceleromterIsCalibrated_id 9 + boolean_vals_offset
/* Stick booleans */
#define systemFlags_throttleMax_id 10 + boolean_vals_offset
#define systemFlags_stickLeft_id 11 + boolean_vals_offset
#define systemFlags_stickRight_id 12 + boolean_vals_offset
#define systemFlags_stickUp_id 13 + boolean_vals_offset
#define systemFlags_stickDown_id 14 + boolean_vals_offset
#define systemFlags_stickCenterH_id 15 + boolean_vals_offset
#define systemFlags_stickCenterV_id 16 + boolean_vals_offset
#define systemFlags_throttleLeft_id 17 + boolean_vals_offset
/*Mask values for each of the flag values*/
@ -62,6 +73,17 @@
#define systemFlags_mixerfullscale_mask getFlagMaskValue(systemFlags_mixerfullscale_id)
#define systemFlags_mixerlowscale_mask getFlagMaskValue(systemFlags_mixerlowscale_id)
#define systemFlags_flightMode_3_mask getFlagMaskValue(systemFlags_flightMode_3_id)
#define systemFlags_barometerIsCalibrated_mask getFlagMaskValue(systemFlags_barometerIsCalibrated_id)
#define systemFlags_AcceleromterIsCalibrated_mask getFlagMaskValue(systemFlags_AcceleromterIsCalibrated_id)
/* Stick booleans */
#define systemFlags_throttleMax_mask getFlagMaskValue(systemFlags_throttleMax_id)
#define systemFlags_stickLeft_mask getFlagMaskValue(systemFlags_stickLeft_id)
#define systemFlags_stickRight_mask getFlagMaskValue(systemFlags_stickRight_id
#define systemFlags_stickUp_mask getFlagMaskValue(systemFlags_stickUp_id)
#define systemFlags_stickDown_mask getFlagMaskValue(systemFlags_stickDown_id)
#define systemFlags_stickCenterH_mask getFlagMaskValue(systemFlags_stickCenterH_id)
#define systemFlags_stickCenterV_mask getFlagMaskValue(systemFlags_stickCenterV_id)
#define systemFlags_throttleLeft_mask getFlagMaskValue(systemFlags_throttleLeft_id)
@ -74,18 +96,30 @@ typedef union bitSetRegion
struct
{
//fail-safe booleans
booleanValue_t rcChannelInRange : 1;
booleanValue_t noRcReceived : 1;
booleanValue_t rcChannelInRange : 1;
booleanValue_t noRcReceived : 1;
//Flag boleans that are not fail-safe
booleanValue_t armed : 1;
booleanValue_t acceleromter : 1;
booleanValue_t barometer : 1;
booleanValue_t compass : 1;
booleanValue_t gps : 1;
booleanValue_t mixerfullscale : 1;
booleanValue_t mixerlowscale : 1;
booleanValue_t flightMode_3 : 1;
booleanValue_t armed : 1;
booleanValue_t acceleromter : 1;
booleanValue_t barometer : 1;
booleanValue_t compass : 1;
booleanValue_t gps : 1;
booleanValue_t mixerfullscale : 1;
booleanValue_t mixerlowscale : 1;
booleanValue_t flightMode_3 : 1;
booleanValue_t barometerIsCalibrated : 1;
booleanValue_t AcceleromterIsCalibrated : 1;
/* Stick booleans */
booleanValue_t throttleMax : 1;
booleanValue_t stickLeft : 1;
booleanValue_t stickRight : 1;
booleanValue_t stickUp : 1;
booleanValue_t stickDown : 1;
booleanValue_t stickCenterH : 1;
booleanValue_t stickCenterV : 1;
booleanValue_t throttleLeft : 1;
}bitField;
uint64_t intRepresentation;
}boolFlags_t;
@ -109,6 +143,16 @@ typedef enum
FLAG_CONFIGURATION_MIXERFULLSCALE,
FLAG_CONFIGURATION_MIXERLOWSCALE,
FLAG_CONFIGURATION_FLIGHTMODE_3,
/* Stick booleans */
FLAG_CONFIGURATION_THROTTLEMAX,
FLAG_CONFIGURATION_STICKLEFT,
FLAG_CONFIGURATION_STICKRIGHT,
FLAG_CONFIGURATION_STICKUP,
FLAG_CONFIGURATION_STICKDOWN,
FLAG_CONFIGURATION_STICKCENTERH,
FLAG_CONFIGURATION_STICKCENTERV,
FLAG_CONFIGURATION_THROTTLELEFT,
//Counter
FLAG_CONFIGURATION_COUNT

28
UAV-ControlSystem/inc/drivers/i2c_soft.h Normal file
View File

@ -0,0 +1,28 @@
/*
* i2c_soft.h
*
* Created on: 27 okt. 2016
* Author: holmis
*/
#ifndef DRIVERS_I2C_SOFT_H_
#define DRIVERS_I2C_SOFT_H_
#include "stm32f4xx.h"
typedef struct
{
GPIO_TypeDef * i2c_Port;
uint16_t i2c_scl_pin;
uint16_t i2c_sda_pin;
}I2C_SOFT_handle_t;
void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile);
bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data);
bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf);
#endif /* DRIVERS_I2C_SOFT_H_ */

3
UAV-ControlSystem/inc/drivers/motormix.h
View File

@ -46,6 +46,9 @@ typedef struct {
/* Global mixerConfig to bee available to EEPROM */
extern mixerConfig_s mixerConfig;
/*Is set in motor mix and used in pidUAVcore and mix */
extern bool motorLimitReached;
/***********************************************************************
* BRIEF: The motormixer *
* INFORMATION: Sums the output from all control loops and adapts the *

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

@ -137,6 +137,8 @@
/* Baro */
#define BARO
#define BARO_USE_I2C_SOFT
//#define BARO_USE_I2C_HARD //Dont work with DMA right now if fixed should be faster. Otherwise software is faster than hardware I2C
#define MPU6000_NSS_PIN GPIO_PIN_4
#define MPU6000_NSS_PORT GPIOA

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

@ -14,7 +14,7 @@
#ifndef SYSTEM_VARIABLES_H_
#define SYSTEM_VARIABLES_H_
#define EEPROM_SYS_VERSION 102
#define EEPROM_SYS_VERSION 107
#define ADC_STATE
#include "stm32f4xx.h"

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

@ -25,6 +25,8 @@
#define maxStringSize_CLI 100 //Max sting size used for the messages in the CLI
#define ABS_FLOAT(x) (((x) < 0)? -(x): (x))
typedef char typeString[maxStringSize_CLI];
typedef struct typeStringArr { char val[maxStringSize_CLI]; } typeStringArr;
@ -81,6 +83,8 @@ uint32_t accumulate(uint32_t list[], int length);
***********************************************************************/
void Error_Handler(void);
uint8_t reverse(uint8_t byte);
int16_t constrain(int16_t value, int16_t min, int16_t max);

211
UAV-ControlSystem/src/Flight/pid.c
View File

@ -1,6 +1,6 @@
/**************************************************************************
* NAME: pid.c *
* AUTHOR: Johan Gärtner *
* AUTHOR: Johan G<EFBFBD>rtner *
*
* PURPOSE: This file contains pid functions *
* INFORMATION: pidUAV is the final pid controller which only takes *
@ -14,19 +14,21 @@
* **************************************************************************/
#include "Flight/pid.h"
#include "drivers/accel_gyro.h"
#include "drivers/sbus.h"
#include "scheduler/scheduler.h"
#include <math.h>
#include "drivers/failsafe_toggles.h"
#include "drivers/motormix.h"
#include "utilities.h"
#define PTERM_SCALE 0.032029f /*P-term used as a scale value to the PID controller*/
#define ITERM_SCALE 0.244381f /*I-term used as a scale value to the PID controller*/
#define ITERM_SCALE 0.0012f /*I-term used as a scale value to the PID controller*/
#define DTERM_SCALE 0.000529f /*D-term used as a scale value to the PID controller*/
#define RADIO_RANGE 500 /*Radio range input*/
#define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/
#define ACCELEROMETER_RANGE 20 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/
#define ACCELEROMETER_RANGE 60 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/
#define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/
#define COMPASS_RANGE 180 /*Determines the maximum compass limit (limits the rc input)*/
@ -53,8 +55,14 @@ typedef struct pidProfileBuff_s {
pidProfile_t PidProfile[PID_COUNT] = {0}; /*Array of all pid profiles of the system*/
pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0};
accel_t accelProfile; /*Struct profile for input data from sensor*/
gyro_t gyroProfile; /*Struct profile for input data from sensor*/
accel_t accelProfile = {0}; /*Struct profile for input data from sensor*/
gyro_t gyroProfile = {0}; /*Struct profile for input data from sensor*/
pt1Filter_t accelFilter[2] = {0};
float accRollFineTune = 0;
float accPitchFineTune = 0;
/**************************************************************************
@ -68,11 +76,16 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons
switch (axis)
{
case ROLL:
angle = atan2(z_axis, sqrt(x_axis*x_axis + y_axis*y_axis))*180/M_PI - 90;
angle = (x_axis < 0) ? -angle : angle; /*CW (right, form behind) = pos angle*/
angle = atan2(x_axis, sqrt(y_axis*y_axis + z_axis*z_axis))*180/M_PI;
angle = -1*((angle > 0)? (z_axis < 0 )? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle);
break;
case PITCH:
angle = atan2(sqrt(x_axis*x_axis + z_axis*z_axis), y_axis)*180/M_PI - 90; /*down (the front down against ground) = pos angle*/
angle = atan2( y_axis, sqrt(z_axis*z_axis + x_axis*x_axis))*180/M_PI; /*down (the front down against ground) = pos angle*/
angle = (angle > 0)? ((z_axis < 0))? 180 - angle: angle : (z_axis < 0 )? - 180 - angle: angle;
break;
default:
angle = 0;
@ -82,6 +95,11 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons
return angle;
}
float calcGravity(accel_t profile ) //const float x_axis, const float y_axis, const float z_axis)
{
return sqrt(profile.accelXconv*profile.accelXconv + profile.accelYconv*profile.accelYconv + profile.accelZconv*profile.accelZconv);
}
/**************************************************************************
* BRIEF: Scales data from input range to output range *
* INFORMATION: *
@ -89,7 +107,6 @@ float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, cons
float convertData(int inputRange, int outputRange, int offset, float value)
{
return ((float)outputRange/(float)inputRange)*(value-(float)offset);
//return 1.0;
}
/**************************************************************************
@ -98,20 +115,29 @@ float convertData(int inputRange, int outputRange, int offset, float value)
**************************************************************************/
float constrainf(float amt, int low, int high)
{
if (amt < low)
return low;
else if (amt > high)
return high;
if (amt < (float)low)
return (float)low;
else if (amt > (float)high)
return (float)high;
else
return amt;
}
float oldSensorValue[2] = {0};
float oldSensorValueRoll[12] = {0};
float oldSensorValuePitch[12] = {0};
int i = 0;
/**************************************************************************
* BRIEF: Update current sensor values *
* INFORMATION: *
**************************************************************************/
void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
{
switch (ID_profile)
{
case PID_ID_GYRO:
@ -127,9 +153,64 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/
//
// if (calcGravity(accelProfile) > 1.15)
// {
//
// sensorValues[ROLL] = gyroProfile.gyroY*PidProfileBuff[ROLL].dT;
// sensorValues[PITCH] = gyroProfile.gyroX*PidProfileBuff[PITCH].dT;
//
// }
// else
// {
float alpha = 0.5;
/*May need Low pass filter since the accelerometer may drift*/
sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
/*TODO add finetune for roll and pitch*/
X_roll += accRollFineTune;
X_pitch += accPitchFineTune;
oldSensorValueRoll[i] = X_roll;
oldSensorValuePitch[i] = X_pitch;
float RollValue = 0;
float PitchValue = 0;
for (int ii = 0; ii < 12; ii++)
{
RollValue = RollValue + oldSensorValueRoll[ii];
PitchValue = PitchValue + oldSensorValuePitch[ii];
}
i = (i < 11)? i + 1:0;
sensorValues[ROLL] = RollValue/12;
sensorValues[PITCH] = PitchValue/12;
sensorValues[ROLL] = alpha*RollValue/12 + (1-alpha)*oldSensorValue[0];
sensorValues[PITCH] = alpha*PitchValue/12 + (1-alpha)*oldSensorValue[1];
//
oldSensorValue[0] = sensorValues[ROLL];
oldSensorValue[1] = sensorValues[PITCH];
// sensorValues[ROLL] = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
// sensorValues[PITCH] = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
//float sensorRoll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
//sensorValues[ROLL] = pt1FilterApply4(&accelFilter[0], sensorRoll, 90, PidProfileBuff[ROLL].dT);
//float sensorPitch = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
//sensorValues[PITCH] = pt1FilterApply4(&accelFilter[1], sensorPitch, 90, PidProfileBuff[PITCH].dT);
break;
case PID_ID_COMPASS:
@ -211,7 +292,7 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
/* -----calculate P component ---- */
float PTerm = PTERM_SCALE * rateError * pidProfile->P[axis] * pidProfile-> PIDweight[axis] / 100;
float PTerm = PTERM_SCALE * rateError * (float)pidProfile->P[axis] * (float)pidProfile-> PIDweight[axis] / 100.0;
// Constrain YAW by yaw_p_limit value
if (axis == YAW)
@ -230,11 +311,11 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
/* -----calculate I component ---- */
float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * pidProfile->I[axis];
float ITerm = pidProfileBuff->lastITerm[axis] + ITERM_SCALE * rateError * pidProfileBuff->dT * (float)pidProfile->I[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I) moved before integration to make limiting independent from PID settings
ITerm = constrainf(ITerm, -PID_MAX_I, PID_MAX_I);
ITerm = constrainf(ITerm, -(int)PID_MAX_I, (int)PID_MAX_I);
// Anti windup protection
if (motorLimitReached)
@ -243,9 +324,15 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
}
else
{
pidProfileBuff->ITermLimit[axis] = abs(ITerm);
pidProfileBuff->ITermLimit[axis] = ABS_FLOAT(ITerm);
}
// if (motorLimitReached)
// {
// ITerm = pidProfileBuff->lastITerm[axis];
// }
pidProfileBuff->lastITerm[axis] = ITerm;
@ -274,13 +361,19 @@ void pidUAVcore(pidProfile_t *pidProfile, pidProfileBuff_t *pidProfileBuff,
delta = pt1FilterApply4(&pidProfileBuff->deltaFilter[axis], delta, pidProfile->dterm_lpf, pidProfileBuff->dT);
}
DTerm = DTERM_SCALE * delta * pidProfile->D[axis] * pidProfile->PIDweight[axis] / 100;
DTerm = DTERM_SCALE * delta * (float)pidProfile->D[axis] * (float)pidProfile->PIDweight[axis] / 100.0;
DTerm = constrainf(DTerm, -PID_MAX_D, PID_MAX_D);
}
/*----PID OUT----*/
pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -pidProfile->pid_out_limit, pidProfile->pid_out_limit);
if(!flags_IsSet_ID(systemFlags_armed_id) || (rc_input.Throttle < mixerConfig.minCheck))
{
ITerm = 0;
pidProfileBuff->lastITerm[axis] = 0;
pidProfileBuff->ITermLimit[axis] = 0;
}
pidProfile->PID_Out[axis] = constrainf(PTerm + ITerm + DTerm, -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit);
}
/**************************************************************************
@ -368,6 +461,8 @@ void pidRun(uint8_t ID)
}
}
/*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/
/**************************************************************************
* BRIEF: Initializes a certain pidbuffer profile connected to *
* a PID controller *
@ -383,23 +478,23 @@ void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID)
case PID_ID_GYRO:
PidProfileBuff[ID].DOF = 3;
PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //ÄNDRA TILL SEKUNDER inte ms
PidProfileBuff[ID].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000000; //<2F>NDRA TILL SEKUNDER inte ms
break;
case PID_ID_ACCELEROMETER:
PidProfileBuff[ID].DOF = 2;
PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000;
PidProfileBuff[ID].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000000;
break;
case PID_ID_COMPASS:
PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000;
PidProfileBuff[ID].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000000;
break;
case PID_ID_BAROMETER:
PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000;
PidProfileBuff[ID].dT = SystemTasks[TASK_BARO].desiredPeriod/1000000;
break;
default:
@ -442,18 +537,25 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
PidProfile[ID].PID_Out[PITCH] = 0;
PidProfile[ID].PID_Out[YAW] = 0;
PidProfile[ID].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100;
PidProfile[ID].P[ROLL] = 10;
PidProfile[ID].P[PITCH] = 10;
PidProfile[ID].P[YAW] = 10;
switch (ID)
{
case PID_ID_GYRO:
PidProfile[ID].P[ROLL] = 150;
PidProfile[ID].P[PITCH] = 135;
PidProfile[ID].P[YAW] = 150;
PidProfile[ID].D[ROLL] = 75;
PidProfile[ID].D[PITCH] = 95;
PidProfile[ID].D[YAW] = 50;
PidProfile[ID].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100;
PidProfile[ID].pidEnabled = true;
PidProfile[ID].dterm_lpf = 90;
PidProfile[ID].pid_out_limit = 3000;
@ -461,6 +563,18 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
break;
case PID_ID_ACCELEROMETER:
PidProfile[ID].P[ROLL] = 90;
PidProfile[ID].P[PITCH] = 90;
PidProfile[ID].P[YAW] = 0;
PidProfile[ID].D[ROLL] = 40;
PidProfile[ID].D[PITCH] = 40;
PidProfile[ID].D[YAW] = 0;
PidProfile[ID].PIDweight[ROLL] = 2;
PidProfile[ID].PIDweight[PITCH] = 2;
PidProfile[ID].PIDweight[YAW] = 100;
PidProfile[ID].pidEnabled = true;
PidProfile[ID].dterm_lpf = 90;
PidProfile[ID].pid_out_limit = 1000;
@ -468,11 +582,27 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
break;
case PID_ID_COMPASS:
PidProfile[ID].P[ROLL] = 10;
PidProfile[ID].P[PITCH] = 10;
PidProfile[ID].P[YAW] = 10;
PidProfile[ID].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100;
PidProfile[ID].pidEnabled = false;
break;
case PID_ID_BAROMETER:
PidProfile[ID].P[ROLL] = 10;
PidProfile[ID].P[PITCH] = 10;
PidProfile[ID].P[YAW] = 10;
PidProfile[ID].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100;
PidProfile[ID].pidEnabled = false;
break;
@ -490,7 +620,6 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
void pidInit()
{
mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/
motorLimitReached = false;
pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO);
pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER);
@ -502,3 +631,19 @@ void pidInit()
pidUAVInit(&PidProfile[PID_ID_BAROMETER], PID_ID_BAROMETER);
pidUAVInit(&PidProfile[PID_ID_COMPASS], PID_ID_COMPASS);
}
void pidEproom(void)
{
PidProfile[PID_ID_ACCELEROMETER].PIDweight[ROLL] = 2;
PidProfile[PID_ID_ACCELEROMETER].PIDweight[PITCH] = 2;
PidProfile[PID_ID_ACCELEROMETER].PIDweight[YAW] = 100;
PidProfileBuff[PID_ID_GYRO].dT = SystemTasks[TASK_GYROPID].desiredPeriod/1000; //<2F>NDRA TILL SEKUNDER inte ms
PidProfileBuff[PID_ID_ACCELEROMETER].dT = SystemTasks[TASK_ACCELEROMETER].desiredPeriod/1000;
PidProfileBuff[PID_ID_COMPASS].dT = SystemTasks[TASK_COMPASS].desiredPeriod/1000;
PidProfileBuff[PID_ID_BAROMETER].dT = SystemTasks[TASK_BARO].desiredPeriod/1000;
PidProfile[PID_ID_GYRO].I[YAW] = 40;
}

4
UAV-ControlSystem/src/Scheduler/tasks.c
View File

@ -88,7 +88,7 @@ task_t SystemTasks[TASK_COUNT] =
{
.taskName = "SERIAL",
.taskFunction = systemTaskSerial,
.desiredPeriod = GetUpdateRateHz(100), //100 hz update rate (10 ms)
.desiredPeriod = GetUpdateRateHz(2), //100 hz update rate (10 ms)
.staticPriority = PRIORITY_LOW,
},
@ -105,7 +105,7 @@ task_t SystemTasks[TASK_COUNT] =
{
.taskName = "BAROMETER",
.taskFunction = systemTaskBaro,
.desiredPeriod = GetUpdateRateHz(20), //20 hz update rate (50 ms)
.desiredPeriod = GetUpdateRateHz(200), //200 hz update rate (5 ms)
.staticPriority = PRIORITY_LOW,
},
#endif

102
UAV-ControlSystem/src/config/cli.c
View File

@ -24,6 +24,8 @@
#include "utilities.h"
#include "Scheduler/scheduler.h"
#include "drivers/motors.h"
#include "drivers/accel_gyro.h"
#include "Flight/pid.h"
#define cliActivateCharacter 35
#define commandValueError 0xFFFFFFFFFFFFFFFF
@ -73,6 +75,10 @@ typedef enum
ACTION_RESET, //Resets the entire eeprom
ACTION_STATS, //gives statistics on the system
ACTION_MOTORCALIBRATE,
ACTION_GYROCALIBRATE,
ACTION_ACCELEROMTETERCALIBRATE,
ACTION_COMPASSCALIBRATE,
ACTION_CALIBRATIONINFOACCEL,
//The number of actions
ACTION_COUNT, //Count of the number of actions
@ -94,7 +100,11 @@ const typeString commandAction_Strings[ACTION_COUNT] = {
"reboot",
"reset",
"stats",
"motorcalibrate"
"calibrate_motors",
"calibrate_gyro",
"calibrate_accelerometer",
"calibrate_compass",
"calibration_info_accelerometer"
};
/* String values descrbing information of a certain action */
@ -110,8 +120,12 @@ const typeString commandActionInformation_Strings[ACTION_COUNT] = {
"| exit - Exits the CLI mode.\n\r",
"| reboot - Exit CLI and reboots the system.\n\r",
"| reset - Restore all the values to its default values.\n\r",
"| stats - Gives some current stats of the system and tasks.\n\r"
"| motorcalibrate - Calibrates all motors."
"| stats - Gives some current stats of the system and tasks.\n\r",
"| calibrate_motors - Calibrates all motors.",
"| calibrate_gyro - Calibrates the gyro.",
"| calibrate_accelerometer - Calibrates the accelerometer.",
"| calibrate_compass - Calibrates the compass.",
"| calibration_info_accelerometer - Provides info on the accelerometer calibration."
};
/* String array containing all the signature examples for each action */
@ -128,7 +142,11 @@ const typeString commandActionSignature_Strings[ACTION_COUNT] = {
" reboot",
" reset",
" stats",
" motorcalibrate"
" calibrate_motors",
" calibrate_gyro",
" calibrate_accelerometer",
" calibrate_compass",
" calibration_info_accelerometer"
};
/* Size values for the values a command will require */
@ -1414,17 +1432,17 @@ void cliRun()
break;
case commandMask(commandSize_1, ACTION_RESET):
//resets all the values in the eeprom to the default values
TransmitBack("- All values will be deleted and system rebooted. Continue? (y/n)... \n\n\r");
TransmitBack("- All values will be deleted and system rebooted. Continue? (y/n)... \n\n\r");
//read until a y or n is found
if (ReceiveBinaryDecision(121, 110))
{
defaultEEPROM();
}
else
{
TransmitBack("- Values unchanged...\n\n\r");
}
//read until a y or n is found
if (ReceiveBinaryDecision(121, 110))
{
defaultEEPROM();
}
else
{
TransmitBack("- Values unchanged...\n\n\r");
}
break;
case commandMask(commandSize_1, ACTION_STATS):
TransmitSystemStats();
@ -1438,13 +1456,67 @@ void cliRun()
{
TransmitBack("Starting calibration, BE CAREFUL!!! \n\n\r");
calibrateMotors();
TransmitBack("Wait until the beeping have ceased... \n\n\r");
}
else
{
TransmitBack("Exiting the calibration, BE CAREFUL!!! \n\n\r");
}
break;
break;
case commandMask(commandSize_1, ACTION_GYROCALIBRATE):
TransmitBack("Do you really want to calibrate the gyro? (y/n)\n\n\r");
if (ReceiveBinaryDecision(121,110))
{
TransmitBack("Starting calibration! \n\n\r");
TransmitBack("NOT IMPLEMENTED! \n\n\r");
TransmitBack("Calibration complete! \n\n\r");
}
else
{
TransmitBack("Exiting the calibration! \n\n\r");
}
break;
case commandMask(commandSize_1, ACTION_ACCELEROMTETERCALIBRATE):
TransmitBack("Do you really want to calibrate the accelerometer? (y/n)\n\n\r");
if (ReceiveBinaryDecision(121,110))
{
TransmitBack("Starting calibration! \n\n\r");
mpu6000_read_acc_offset(&accelProfile);
TransmitBack("Calibration complete! \n\n\r");
}
else
{
TransmitBack("Exiting the calibration! \n\n\r");
}
break;
case commandMask(commandSize_1, ACTION_COMPASSCALIBRATE):
TransmitBack("Do you really want to calibrate the compass? (y/n)\n\n\r");
if (ReceiveBinaryDecision(121,110))
{
TransmitBack("Starting calibration! \n\n\r");
TransmitBack("NOT IMPLEMENTED! \n\n\r");
TransmitBack("Calibration complete! \n\n\r");
}
else
{
TransmitBack("Exiting the calibration! \n\n\r");
}
break;
case commandMask(commandSize_1, ACTION_CALIBRATIONINFOACCEL):
{
char tempBuffer[100];
TransmitBack("- Accelerometer calibration information: \n\n\r");
sprintf(tempBuffer, "- Finetune values: \n\r- Pitch: %0.2f \n\r- Roll: %0.2f \n\n\r", accPitchFineTune, accRollFineTune);
TransmitBack(tempBuffer);
sprintf(tempBuffer, "- BaseTune: \n\r-OffsetX: %d \n\r-OffsetY: %d \n\r-OffsetZ: %d \n\n\r", accelProfile.offsetX, accelProfile.offsetY, accelProfile.offsetZ);
TransmitBack(tempBuffer);
break;
}
default:
if (actionId != ACTION_NOACTION)
TransmitCommandInstruction(actionId);

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

@ -248,9 +248,34 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
.dataPtr = &(flagConfigArr[FLAG_CONFIGURATION_FLIGHTMODE_3]),
},
/* accel calibration values */
[EEPROM_FLAG_ACCELTUNE_OFFSET_X] =
{
.size = sizeof(int16_t),
.dataPtr = &(accelProfile.offsetX),
},
[EEPROM_FLAG_ACCELTUNE_OFFSET_Y] =
{
.size = sizeof(int16_t),
.dataPtr = &(accelProfile.offsetY),
},
[EEPROM_FLAG_ACCELTUNE_OFFSET_Z] =
{
.size = sizeof(int16_t),
.dataPtr = &(accelProfile.offsetZ),
},
/* accel fine tune */
[EEPROM_FLAG_ACCELTUNE_FINE_ROLL] =
{
.size = sizeof(float),
.dataPtr = &(accRollFineTune),
},
[EEPROM_FLAG_ACCELTUNE_FINE_PITCH] =
{
.size = sizeof(float),
.dataPtr = &(accPitchFineTune),
},
};
/* Data pointers and sizes for profile content */

161
UAV-ControlSystem/src/drivers/I2C.c
View File

@ -34,8 +34,8 @@ bool i2c_configure(I2C_TypeDef *i2c,
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();
// if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
// __HAL_RCC_I2C1_CLK_ENABLE();
}
else if(i2c == I2C2)
{
@ -55,22 +55,26 @@ bool i2c_configure(I2C_TypeDef *i2c,
//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.Mode = GPIO_MODE_AF_OD;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
GPIO_InitStruct.Alternate = i2c_af;
HAL_GPIO_Init(i2c_port, &GPIO_InitStruct);
HAL_Delay(10);
if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
__HAL_RCC_I2C1_CLK_ENABLE();
//Initialize I2C communication
out_profile->Instance = i2c;
out_profile->Init.ClockSpeed = 100000;
out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2;
out_profile->Init.ClockSpeed = 400000;
out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2/*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;
out_profile->Init.NoStretchMode = I2C_NOSTRETCH_DISABLED;
if(HAL_I2C_Init(out_profile) != HAL_OK)
return false;
@ -115,14 +119,155 @@ bool i2c_send(I2C_HandleTypeDef* profile,
uint8_t* data,
uint32_t length)
{
//TODO: Fix this function
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)
{}
{I2C1->CR1 |= (1 << 9);}
// while(HAL_I2C_Master_Transmit(profile, slave_address, (uint8_t*)data, length, 5000) != 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);
}
uint8_t dma_baro_rx_buffer[3];
uint8_t dma_baro_tx_buffer[1];
bool i2c_configure_DMA(I2C_TypeDef *i2c,
I2C_HandleTypeDef *out_profile,
DMA_HandleTypeDef *out_rxDMA_profile,
DMA_HandleTypeDef *out_txDMA_profile,
uint32_t my_address)
{
uint8_t i2c_af;
uint16_t sda_pin, scl_pin;
GPIO_TypeDef *i2c_port;
DMA_Stream_TypeDef *dma_rx_instance, *dma_tx_instance;
uint32_t channel;
// 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;
dma_rx_instance = DMA1_Stream5;
dma_tx_instance = DMA1_Stream6;
channel = DMA_CHANNEL_1;
// 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_OD;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
GPIO_InitStruct.Alternate = i2c_af;
HAL_GPIO_Init(i2c_port, &GPIO_InitStruct);
HAL_Delay(10);
if(__HAL_RCC_I2C1_IS_CLK_DISABLED())
__HAL_RCC_I2C1_CLK_ENABLE();
if(__HAL_RCC_DMA1_IS_CLK_DISABLED())
__HAL_RCC_DMA1_CLK_ENABLE();
//Initialize I2C communication
out_profile->Instance = i2c;
out_profile->Init.ClockSpeed = 400000;
out_profile->Init.DutyCycle = I2C_DUTYCYCLE_2/*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_DISABLED;
if(HAL_I2C_Init(out_profile) != HAL_OK)
return false;
// Enable the DMA on the i2c register level
out_profile->Instance->CR2 |= (1 << 11);
/* Create the DMAs */
DMA_HandleTypeDef out_rxDMA_profile2;
out_rxDMA_profile2.Instance = dma_rx_instance;
out_rxDMA_profile2.Init.Channel = channel;
out_rxDMA_profile2.Init.Direction = DMA_PERIPH_TO_MEMORY;
out_rxDMA_profile2.Init.PeriphInc = DMA_PINC_DISABLE;
out_rxDMA_profile2.Init.MemInc = DMA_MINC_ENABLE;
out_rxDMA_profile2.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
out_rxDMA_profile2.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
out_rxDMA_profile2.Init.Mode = DMA_CIRCULAR;
out_rxDMA_profile2.Init.Priority = DMA_PRIORITY_VERY_HIGH;
if(HAL_DMA_Init(&out_rxDMA_profile2) != HAL_OK)
return false;
__HAL_LINKDMA(out_profile ,hdmarx, out_rxDMA_profile2);
DMA_HandleTypeDef out_txDMA_profile2;
out_txDMA_profile2.Instance = dma_tx_instance;
out_txDMA_profile2.Init.Channel = channel;
out_txDMA_profile2.Init.Direction = DMA_MEMORY_TO_PERIPH;
out_txDMA_profile2.Init.PeriphInc = DMA_PINC_DISABLE;
out_txDMA_profile2.Init.MemInc = DMA_MINC_ENABLE;
out_txDMA_profile2.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
out_txDMA_profile2.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
out_txDMA_profile2.Init.Mode = DMA_CIRCULAR;
out_txDMA_profile2.Init.Priority = DMA_PRIORITY_VERY_HIGH;
if(HAL_DMA_Init(&out_txDMA_profile2) != HAL_OK)
return false;
__HAL_LINKDMA(out_profile ,hdmatx, out_txDMA_profile2);
//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 = 3; // Set the buffer size
dma_rx_instance->PAR = (uint32_t)&i2c->DR; // Set the address to the USART data register
dma_rx_instance->M0AR = (uint32_t)dma_baro_rx_buffer; // Set the address to the first 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 = 1;
dma_tx_instance->PAR = (uint32_t)&i2c->DR;
dma_tx_instance->M0AR = (uint32_t)dma_baro_tx_buffer;
dma_tx_instance->CR &= ~(0xF << 11);
dma_tx_instance->CR |= DMA_SxCR_EN;
return true;
}

98
UAV-ControlSystem/src/drivers/accel_gyro.c
View File

@ -44,7 +44,7 @@ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32
/***********************************************************************
* BRIEF: mpu6000_read_offset reads and returns the offset of the *
* gyroscope and accelerometer *
* gyroscope *
* INFORMATION: *
* Is automatically called when mpu6000_init is called *
* The flight controller needs to be stationary when this function is *
@ -52,46 +52,104 @@ bool mpu6000_transmit_receive(uint8_t reg, uint8_t* data, uint8_t length, uint32
* 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(gyro_t* gyro, accel_t* accel)
HAL_StatusTypeDef mpu6000_read_gyro_offset(gyro_t* gyro)
{
uint8_t dataG[6]; /* Temporary data variable used to receive gyroscope data */
uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */
int16_t dataGCheck[6] = {0}; /* checkval */
uint8_t maxDrift = 5;
bool calibrate_go = true;
bool calibrate_ok = false;
int countCalibrate = 0;
if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10))
return HAL_ERROR;
//small delay so that we know the gyro should give some values
HAL_Delay(100);
for (int i = 0; i < 100; i++)
{
//assume that we should calibrate at the start of each loop
calibrate_go = true;
//get new values
if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10))
return HAL_ERROR;
//cheack the new values and see if they are within acceptable range compared to the previous values, so that it is realativelly still
for(int j = 0; j < 6; j ++)
{
int16_t currentDrift = (int16_t)abs(dataGCheck[j] - dataG[j]);
if (!(currentDrift < maxDrift))
calibrate_go = false;
dataGCheck[j] = dataG[j];
}
//if true, we have good values exit loop
if (calibrate_go)
{
countCalibrate++;
}
if (countCalibrate > 4)
{
calibrate_ok = true;
break;
}
//wait for a little bit so the checks are not to fast
HAL_Delay(200);
}
if(!mpu6000_transmit_receive(MPU_RA_GYRO_XOUT_H, dataG, 6, 10))
return HAL_ERROR;
#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]);
#endif
return HAL_OK;
}
/***********************************************************************
* BRIEF: mpu6000_read_offset reads and returns the offset of the *
* 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_acc_offset(accel_t* accel)
{
uint8_t dataA[6]; /* Temporary data variable used to receive accelerometer data */
if(!mpu6000_transmit_receive(MPU_RA_ACCEL_XOUT_H, dataA, 6, 10))
return HAL_ERROR;
#ifdef YAW_ROT_0
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)
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)
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)
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]);
@ -180,9 +238,11 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
if(!mpu6000_transmit(reg, 2))
return false;
mpu6000_read_gyro_offset(gyro);
//mpu6000_read_acc_offset(accel);
HAL_Delay(60);
mpu6000_read_offset(gyro, accel);
return true;
}

383
UAV-ControlSystem/src/drivers/barometer.c Normal file
View File

@ -0,0 +1,383 @@
/*
* barometer.c
*
* Created on: 18 okt. 2016
* Author: holmis
*/
#include "drivers/barometer.h"
#include "drivers/I2C.h"
#include "stm32f4xx_revo.h"
#include "drivers/system_clock.h"
#include "math.h"
#include "drivers/i2c_soft.h"
#include "drivers/failsafe_toggles.h"
#define Device_address_1 0x56
#define ADDR_WRITE 0xEE // Module address write mode
#define ADDR_READ 0xEF // Module address read mode
#define ADDRESS_BARO 0x77 //0x77
#define CMD_RESET 0x1E // ADC reset command
#define CMD_ADC_READ 0x00 // ADC read command
#define CMD_ADC_CONV 0x40 // ADC conversion command
#define CMD_ADC_D1 0x00 // ADC D1 conversion
#define CMD_ADC_D2 0x10 // ADC D2 conversion
#define CMD_ADC_256 0x00 // ADC OSR=256
#define CMD_ADC_512 0x02 // ADC OSR=512
#define CMD_ADC_1024 0x04 // ADC OSR=1024
#define CMD_ADC_2048 0x06 // ADC OSR=2048
#define CMD_ADC_4096 0x08 // ADC OSR=4096
#define CMD_PROM_RD 0xA0 // Prom read command
#define SEA_PRESS 1013.25 //default sea level pressure level in mb
#define FTMETERS 0.3048 //convert feet to meters
#define CALIBRATION_VAL_AMOUNT 30
I2C_HandleTypeDef baroI2C_handle;
DMA_HandleTypeDef baroI2C_Rx_DMA_handle;
DMA_HandleTypeDef baroI2C_Tx_DMA_handle;
I2C_SOFT_handle_t baroI2C_soft_handle;
uint8_t sampleAmount;
double baro_Preassure; // compensated pressure value (mB)
double baro_Temperature; // compensated temperature value (degC)
double baro_Altitude; // altitude (ft)
double baro_S; // sea level barometer (mB)
float altitudeCalibrationValue = 0; //Value used as calibration value
float calibrationSamples[CALIBRATION_VAL_AMOUNT]; //array of stored values to be used for calibration, only samples calibration values when machine is not armed
int calibrationSamplesCount = 0;
int calibrationSamplesIterator = 0;
//TODO: remove when not used for testing any more
uint32_t tempTestCounterStart = 0;
uint8_t cobuf[3] = {0};
/* address: 0 = factory data and the setup
* address: 1-6 = calibration coefficients
* address: 7 = serial code and CRC */
uint32_t coefficients_arr[8]; //coefficient storage
void barometer_addCalibrationSample()
{
//fisrt check if the amount of samples is greater than the array
if (!(calibrationSamplesCount >= CALIBRATION_VAL_AMOUNT))
calibrationSamplesCount++; //if not increase the counter
//Check if the iterator should restart from the beginning because of overflow
if (calibrationSamplesIterator >= CALIBRATION_VAL_AMOUNT)
calibrationSamplesIterator = 0; //if it is set it to zero
//Add the lates calculated altitude value to the samples
calibrationSamples[calibrationSamplesIterator] = baro_Altitude;
//increase the iterator
calibrationSamplesIterator ++;
}
bool barometer_Calibrate()
{
//Check if any calibration values exist
if (calibrationSamplesCount <= 0)
return false;
float sampled = 0;
//loop through all the calibration samples
for (int i = 0; i < calibrationSamplesCount; i++)
{
sampled += calibrationSamples[i];
}
//calculate the new calibration value based on the average of all the samples
altitudeCalibrationValue = sampled / calibrationSamplesCount;
//rest all the values associated with the calibration samples
calibrationSamplesCount = 0;
calibrationSamplesIterator = 0;
//Set the calibration flag to true
flags_Set_ID(systemFlags_barometerIsCalibrated_id);
//Calibration went ok
return true;
}
bool barometer_init()
{
//Set the sample rate of the data that will be calculated on the barometer peripheral
sampleAmount = CMD_ADC_2048;
//Initialize pins for SCL and SDA
#ifdef BARO_USE_I2C_SOFT
i2c_soft_Init(I2C1, &baroI2C_soft_handle);
#endif
#ifdef BARO_USE_I2C_HARD
#endif
return true;
}
bool barometer_reset()
{
/* Send a reset command to the baromter
* Afterwards read in all the coefficient values that are stored on the PROM of the barometer */
#ifdef BARO_USE_I2C_SOFT
i2c_soft_Write(&baroI2C_soft_handle, ADDRESS_BARO, CMD_RESET, 1);
HAL_Delay(2800);
for (int i = 0; i < 8; i++)
{
uint8_t rxbuf[2] = { 0, 0 };
i2c_soft_Read(&baroI2C_soft_handle, ADDRESS_BARO, CMD_PROM_RD + i * 2, 2, rxbuf); // send PROM READ command
coefficients_arr[i] = rxbuf[0] << 8 | rxbuf[1];
}
#endif
#ifdef BARO_USE_I2C_HARD
uint8_t cobuf2[3] = {0};
/* Change to hardware polling mode */
cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount);
HAL_GPIO_DeInit(I2C1_PORT, I2C1_SCL_PIN);
HAL_GPIO_DeInit(I2C1_PORT, I2C1_SDA_PIN);
i2c_configure(I2C1, &baroI2C_handle, 0x0);
bool isSent = i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
HAL_Delay(20);
cobuf2[0] = CMD_ADC_READ;
i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf2, 3);
/* Hardware DMA test */
// cobuf2[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount);
// HAL_GPIO_DeInit(I2C1_PORT, I2C1_SCL_PIN);
// HAL_GPIO_DeInit(I2C1_PORT, I2C1_SDA_PIN);
// i2c_configure_DMA(I2C1, &baroI2C_handle, &baroI2C_Rx_DMA_handle, &baroI2C_Tx_DMA_handle, 0x0);
// bool isSent2 = HAL_I2C_Master_Transmit_DMA(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
// //bool isSent2 = i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
// HAL_Delay(20);
//
//
// cobuf2[0] = CMD_ADC_READ;
// isSent2 = HAL_I2C_Master_Transmit_DMA(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
// //isSent2 = i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf2, 1);
//
// HAL_Delay(20);
// HAL_I2C_Master_Receive_DMA(&baroI2C_handle, ADDRESS_BARO, cobuf2, 3);
// //i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf2, 3);
// while(HAL_DMA_GetState(&baroI2C_handle) != HAL_DMA_STATE_READY )
// {
//
// }
// HAL_Delay(20);
#endif
/* Produce an initial calibration value */
/* Run loop 5 times since there are 5 state, also need delay to ensure values will be read */
for (int i = 0; i < 5; i ++)
{
barometer_CaclulateValues();
HAL_Delay(10);
}
/* Set the inital calibration value */
barometer_Calibrate();
//force bakc the iscalibrated status to false
flags_Clear_ID(systemFlags_barometerIsCalibrated_id);
tempTestCounterStart = clock_get_ms();
return true;
}
typedef enum {
CALCSTATE_D2_CALCULATION = 0, //Tell the sensor that we want to read D2
CALCSTATE_D2_READ, //Read D2 from the sensor
CALCSTATE_D1_CALCULATION, //Tell the sensor that we want to read D1
CALCSTATE_D1_READ, //Read D1 from the sensor
CALCSTATE_CALCULATE_PTA //preassure, temp, altidute calc
}calculationState;
void barometer_CalculatePTA(uint32_t D1, uint32_t D2)
{
/* calculate dT, difference between actual and reference temp: (D2 - C5 * 2^8) */
int64_t dT = D2 - ((uint64_t)coefficients_arr[5] << 8);
/* Calculate OFF, offset at actual temp: (C2 * 2^16 + (C4 * dT)/2^7) */
int64_t OFF = (((uint32_t)coefficients_arr[2]) << 16) + ((coefficients_arr[4] * dT) >> 7);
/* Calculate SENS, sensitivity at actual temperature: (C1 * 2^15 + (C3 * dT)/2^8) */
int64_t SENS = ((uint32_t)coefficients_arr[1] << 15) + ((coefficients_arr[3] * dT) >> 8 );
/* Calculate TEMP: Actual temperature -40 to 85 C: (2000 + dT * C6/2^23) */
int32_t TEMP = 2000 + (int64_t)dT * (int64_t)coefficients_arr[6] / (int64_t)(1 << 23);
baro_Temperature = (double)TEMP / 100.0; //Assign the calculated temp to the holding variable
/* Improvements for different temperatures */
if (TEMP < 2000) //if temp is lower than 20 Celsius
{
int64_t T1 = ((int64_t)TEMP - 2000) * ((int64_t)TEMP - 2000);
int64_t OFF1 = (5 * T1) >> 1;
int64_t SENS1 = (5 * T1) >> 2;
if(TEMP < -1500) //if temp is lower than -15
{
T1 = ((int64_t)TEMP + 1500) * ((int64_t)TEMP + 1500);
OFF1 += 7 * T1;
SENS1 += 11 * T1 >> 1;
}
OFF -= OFF1;
SENS -= SENS1;
}
/* Calculate pressure, temperature compensated pressure 10..1200mbar: ( (D1 * SENS/2^21 - OFF)2^15 ) */
baro_Preassure = (double)(((((int64_t)D1 * SENS ) >> 21) - OFF) / (double) (1 << 15)) / 100.0;
/* Calculate the altitude */
float feet = ((float)1 - (pow(((float)baro_Preassure / (float)SEA_PRESS), (float)0.190284))) * (float)145366.45;
baro_Altitude = (flags_IsSet_ID(systemFlags_barometerIsCalibrated_id)) ? (feet * FTMETERS) - altitudeCalibrationValue : (feet * FTMETERS);
}
void barometer_CaclulateValues()
{
/*the calculation is in need of different states. This is because the
* a wait time is needed when talking to the sensor. Because we cant
* use a delay wait we need to do parts of the calculation every time
* the function is called. The "delay" is handled by the period of
* the task that handles the calculation. It cant have a period faster
* that 10 ms, or the wait will not be enough in some cases according
* to the datasheet of the sensor http://www.amsys.info/sheets/amsys.en.ms5611_01ba03.pdf*/
static uint8_t currentCalculationState = CALCSTATE_D2_CALCULATION;
static uint32_t D1 = 0;
static uint32_t D2 = 0;
uint8_t cobuf[3] = {0};
uint32_t startTime;
uint32_t endTime;
//If the machine is armed and not calibrated we perform a calibraton
if (!flags_IsSet_ID(systemFlags_barometerIsCalibrated_id))
{
if (flags_IsSet_ID(systemFlags_armed_id))
{
barometer_Calibrate();
}
}
switch (currentCalculationState)
{
case CALCSTATE_D2_CALCULATION:
//Set the message to be sent to the barometer
cobuf[0] = CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount);
//Send the message to the barometer
#ifdef BARO_USE_I2C_SOFT
i2c_soft_Write(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D2 + sampleAmount),1); // send conversion command
#endif
#ifdef BARO_USE_I2C_HARD
i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
#endif
//change the state so we will go to D2 read next time function is called
currentCalculationState = CALCSTATE_D2_READ;
break;
case CALCSTATE_D2_READ:
//Set the message to be sent to the barometer
cobuf[0] = CMD_ADC_READ;
//Read the message from the barometer
#ifdef BARO_USE_I2C_SOFT
i2c_soft_Read(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command
#endif
#ifdef BARO_USE_I2C_HARD
i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3);
#endif
//Shift the values to the correct position for the 24 bit D2 value
D2 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2];
//change the state so we will go to D2 read next time function is called
currentCalculationState = CALCSTATE_D1_CALCULATION;
break;
case CALCSTATE_D1_CALCULATION:
//Set the message to be sent to the barometer
cobuf[0] = CMD_ADC_CONV + (CMD_ADC_D1 + sampleAmount);
//Send the message to the barometer
#ifdef BARO_USE_I2C_SOFT
i2c_soft_Write(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_CONV + (CMD_ADC_D1 + sampleAmount),1); // send conversion command
#endif
#ifdef BARO_USE_I2C_HARD
i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
#endif
//change the state so we will go to D1 read next time function is called
currentCalculationState = CALCSTATE_D1_READ;
break;
case CALCSTATE_D1_READ:
//Set the message to be sent to the barometer
cobuf[0] = CMD_ADC_READ;
//Read the message from the baromter
#ifdef BARO_USE_I2C_SOFT
i2c_soft_Read(&baroI2C_soft_handle, ADDRESS_BARO, CMD_ADC_READ, 3, cobuf); // send PROM READ command
#endif
#ifdef BARO_USE_I2C_HARD
i2c_send(&baroI2C_handle, ADDRESS_BARO, cobuf, 1);
i2c_receive(&baroI2C_handle, ADDRESS_BARO, cobuf, 3);
#endif
//Shift the values to the correct position for the 24 bit D2 value
D1 = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2];
//Change the state for the next time the function is called
currentCalculationState = CALCSTATE_CALCULATE_PTA;
break;
case CALCSTATE_CALCULATE_PTA:
startTime = clock_get_us();
//Calculate the Pressure, temperature and altitude
barometer_CalculatePTA(D1, D2);
//only calculate new calibration values if we are not armed
if (!flags_IsSet_ID(systemFlags_armed_id))
{
barometer_addCalibrationSample(); //add new calibration value
flags_Clear_ID(systemFlags_barometerIsCalibrated_id); //Clear the flag for barometer calibration
}
endTime = clock_get_us() - startTime;
//Change the state
currentCalculationState = CALCSTATE_D2_CALCULATION;
break;
}
}
double barometer_GetCurrentPreassure()
{
return baro_Preassure;
}
double barometer_GetCurrentTemperature()
{
return baro_Temperature;
}
float barometer_GetCurrentAltitudeBasedOnSeaLevel()
{
return baro_Altitude;
}

74
UAV-ControlSystem/src/drivers/failsafe_toggles.c
View File

@ -29,14 +29,14 @@ boolFlags_t systemFlags = {{0}};
flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
[FLAG_CONFIGURATION_ARM] = {
.minRange = 1500,
.maxRange = 2000,
.maxRange = 2100,
.channelNumber = 8,
.flagId = systemFlags_armed_id,
},
[FLAG_CONFIGURATION_FLIGHTMODE_ACCELEROMETER] = {
.minRange = 1600,
.maxRange = 2000,
.channelNumber = 5,
.minRange = 1400,
.maxRange = 2100,
.channelNumber = 6,
.flagId = systemFlags_flightmode_acceleromter_id,
},
[FLAG_CONFIGURATION_FLIGHTMODE_BAROMETER] = {
@ -58,15 +58,15 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
.flagId = systemFlags_flightmode_gps_id,
},
[FLAG_CONFIGURATION_MIXERFULLSCALE] = {
.minRange = 0,
.maxRange = 0,
.channelNumber = 0,
.minRange = 1900,
.maxRange = 2100,
.channelNumber = 5,
.flagId = systemFlags_mixerfullscale_id,
},
[FLAG_CONFIGURATION_MIXERLOWSCALE] = {
.minRange = 0,
.maxRange = 0,
.channelNumber = 0,
.minRange = 900,
.maxRange = 1100,
.channelNumber = 5,
.flagId = systemFlags_mixerlowscale_id,
},
[FLAG_CONFIGURATION_FLIGHTMODE_3] = {
@ -74,7 +74,59 @@ flags_Configuration_t flagConfigArr[FLAG_CONFIGURATION_COUNT] = {
.maxRange = 0,
.channelNumber = 0,
.flagId = systemFlags_flightMode_3_id,
},
/*Stick controls*/
[FLAG_CONFIGURATION_THROTTLEMAX] = {
.minRange = 1950,
.maxRange = 2000,
.channelNumber = 3,
.flagId = systemFlags_throttleMax_id,
},
[FLAG_CONFIGURATION_STICKLEFT] = { //negative
.minRange = 1000,
.maxRange = 1100,
.channelNumber = 1,
.flagId = systemFlags_stickLeft_id,
},
[FLAG_CONFIGURATION_STICKRIGHT] = { //positive
.minRange = 1900,
.maxRange = 2000,
.channelNumber = 1,
.flagId = systemFlags_stickRight_id,
},
[FLAG_CONFIGURATION_STICKUP] = { //negative
.minRange = 1000,
.maxRange = 1100,
.channelNumber = 2,
.flagId = systemFlags_stickUp_id,
},
[FLAG_CONFIGURATION_STICKDOWN] = { //positive
.minRange = 1900,
.maxRange = 2000,
.channelNumber = 2,
.flagId = systemFlags_stickDown_id,
},
[FLAG_CONFIGURATION_STICKCENTERH] = {
.minRange = 1400,
.maxRange = 1600,
.channelNumber = 1,
.flagId = systemFlags_stickCenterH_id,
},
[FLAG_CONFIGURATION_STICKCENTERV] = {
.minRange = 1400,
.maxRange = 1600,
.channelNumber = 2,
.flagId = systemFlags_stickCenterV_id,
},
[FLAG_CONFIGURATION_THROTTLELEFT] = {
.minRange = 2000,
.maxRange = 1900,
.channelNumber = 4,
.flagId = systemFlags_throttleLeft_id,
}
};
/***********************************************************************
@ -123,7 +175,7 @@ void flags_ProcessRcChannel_Improved(uint8_t minChannel, uint8_t maxChannel)
{
int currentChannelNumb = flagConfigArr[i].channelNumber;
//Check if the current Flag channel is within the set bounds
if(currentChannelNumb >= 5 && currentChannelNumb <= maxChannel)
if(currentChannelNumb >= minChannel && currentChannelNumb <= maxChannel)
{
//Get the value for the channel that the current flag is linked to
int value = getChannelValue(sbusChannelData, currentChannelNumb);

241
UAV-ControlSystem/src/drivers/i2c_soft.c Normal file
View File

@ -0,0 +1,241 @@
/*
* i2c_soft.c
*
* Created on: 27 okt. 2016
* Author: holmis
*/
#include "drivers/i2c_soft.h"
#include "stm32f4xx_revo.h"
#define WRITE_INDICATOR 0
#define READ_INDICATOR 1
static void IOHi(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
{
HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_SET);
}
static void IOLo(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
{
HAL_GPIO_WritePin(GPIOx, GPIO_Pin, GPIO_PIN_RESET);
}
static bool IORead(GPIO_TypeDef* GPIOx, uint16_t GPIO_Pin)
{
return !! (GPIOx->IDR & GPIO_Pin);
}
static void i2c_soft_delay(void)
{
volatile int i = 1;
while (i) {
i--;
}
}
void i2c_soft_Init(I2C_TypeDef *i2c, I2C_SOFT_handle_t *out_profile)
{
uint16_t sda_pin, scl_pin;
GPIO_TypeDef *i2c_port;
//Check what i2c should be used
if(i2c == I2C1)
{
i2c_port = I2C1_PORT;
sda_pin = I2C1_SDA_PIN;
scl_pin = I2C1_SCL_PIN;
}
else if(i2c == I2C2)
{
i2c_port = I2C2_PORT;
sda_pin = I2C2_SDA_PIN;
scl_pin = I2C2_SCL_PIN;
}
//Init the GPIO pins
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = scl_pin | sda_pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_HIGH;
HAL_GPIO_Init(i2c_port, &GPIO_InitStruct);
//Assign the values to the out struct
out_profile->i2c_Port = i2c_port;
out_profile->i2c_scl_pin = scl_pin;
out_profile->i2c_sda_pin = sda_pin;
}
static bool i2c_soft_Start(I2C_SOFT_handle_t *handle)
{
IOHi(handle->i2c_Port, handle->i2c_sda_pin);
IOHi(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
if (!IORead(handle->i2c_Port, handle->i2c_sda_pin)) {
return false;
}
IOLo(handle->i2c_Port, handle->i2c_sda_pin);
asm ("nop"); // i2c_soft_delay();
if (IORead(handle->i2c_Port, handle->i2c_sda_pin)) {
return false;
}
IOLo(handle->i2c_Port, handle->i2c_sda_pin);
asm ("nop"); // i2c_soft_delay();
return true;
}
static void i2c_soft_Stop(I2C_SOFT_handle_t *handle)
{
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
IOLo(handle->i2c_Port, handle->i2c_sda_pin);
asm ("nop"); // i2c_soft_delay();
IOHi(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
IOHi(handle->i2c_Port, handle->i2c_sda_pin);
asm ("nop"); // i2c_soft_delay();
}
static void i2c_soft_Ack(I2C_SOFT_handle_t *handle)
{
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
IOLo(handle->i2c_Port, handle->i2c_sda_pin);
asm ("nop"); // i2c_soft_delay();
IOHi(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
}
static void i2c_soft_NoAck(I2C_SOFT_handle_t *handle)
{
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
IOHi(handle->i2c_Port, handle->i2c_sda_pin);
asm ("nop"); // i2c_soft_delay();
IOHi(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
}
static bool i2c_soft_WaitAck(I2C_SOFT_handle_t *handle)
{
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
IOHi(handle->i2c_Port, handle->i2c_sda_pin);
asm ("nop"); // i2c_soft_delay();
IOHi(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
if (IORead(handle->i2c_Port, handle->i2c_sda_pin)) {
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
return false;
}
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
return true;
}
static void i2c_soft_SendByte(I2C_SOFT_handle_t *handle, uint8_t byte)
{
uint8_t i = 8;
while (i--) {
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
if (byte & 0x80) {
IOHi(handle->i2c_Port, handle->i2c_sda_pin);
}
else {
IOLo(handle->i2c_Port, handle->i2c_sda_pin);
}
byte <<= 1;
asm ("nop"); // i2c_soft_delay();
IOHi(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
}
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
}
static uint8_t i2c_soft_ReceiveByte(I2C_SOFT_handle_t *handle)
{
uint8_t i = 8;
uint8_t byte = 0;
IOHi(handle->i2c_Port, handle->i2c_sda_pin);
while (i--) {
byte <<= 1;
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
IOHi(handle->i2c_Port, handle->i2c_scl_pin);
asm ("nop"); // i2c_soft_delay();
if (IORead(handle->i2c_Port, handle->i2c_sda_pin)) {
byte |= 0x01;
}
}
IOLo(handle->i2c_Port, handle->i2c_scl_pin);
return byte;
}
bool i2c_soft_Read(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf)
{
//just send the addres 0x77
//write = 0, read = 1
if (!i2c_soft_Start(handle)) {
return false;
}
i2c_soft_SendByte(handle, addr << 1 | 0);
if (!i2c_soft_WaitAck(handle)) {
i2c_soft_Stop(handle);
//i2cErrorCount++;
return false;
}
i2c_soft_SendByte(handle, reg);
i2c_soft_WaitAck(handle);
i2c_soft_Start(handle);
i2c_soft_SendByte(handle, addr << 1 | 1);
i2c_soft_WaitAck(handle);
while (len) {
*buf = i2c_soft_ReceiveByte(handle);
if (len == 1) {
i2c_soft_NoAck(handle);
}
else {
i2c_soft_Ack(handle);
}
buf++;
len--;
}
i2c_soft_Stop(handle);
return true;
}
bool i2c_soft_Write(I2C_SOFT_handle_t *handle, uint8_t addr, uint8_t reg, uint8_t data)
{
//just send the addres 0x77
//write = 0, read = 1
//Start the i2c, if it cant return
if (!i2c_soft_Start(handle)) {
return false;
}
//Send the address
i2c_soft_SendByte(handle, addr << 1 | WRITE_INDICATOR);
if (!i2c_soft_WaitAck(handle)) {
i2c_soft_Stop(handle);
// i2cErrorCount++;
return false;
}
//send the data
i2c_soft_SendByte(handle, reg);
i2c_soft_WaitAck(handle);
i2c_soft_SendByte(handle, data);
i2c_soft_WaitAck(handle);
i2c_soft_Stop(handle);
return true;
}

17
UAV-ControlSystem/src/drivers/motormix.c
View File

@ -42,17 +42,20 @@
/* An array containing the calculated motor outputs */
uint16_t motor_output[MOTOR_COUNT];
/* Bool to see if motors are maxed out. Stops windup in PID implementation */
bool motorLimitReached = false;
/* Default values for the mixerConfig */
// TODO: Implement in EEPROM
mixerConfig_s mixerConfig = {
.minThrottle = 1050,
.minThrottle = 1040,
.maxThrottle = MAX_PULSE - 100,
.minCommand = 990,
.maxCommand = MAX_PULSE,
.minCheck = 1010,
.pid_at_min_throttle = true,
.motorstop = false,
.yaw_reverse_direction = false
.yaw_reverse_direction = true
};
/* Used in "mixerUAV" to create the dynamic model of the UAV */
@ -102,8 +105,6 @@ void mix()
int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor
int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]; // Import throttle value from remote
// Might be used for some debug if necessary
//static bool motorLimitReached;
/* Mixer Full Scale enabled */
if (flags_IsSet_ID(systemFlags_mixerfullscale_id))
@ -114,8 +115,8 @@ void mix()
* Calculation is: Output from control system * weight from model for each motor
*/
RPY_Mix[i] = \
PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
(int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
(int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1);
@ -194,8 +195,8 @@ void mix()
{
RPY_Mix[i] = \
throttle * mixerUAV[i].throttle + \
PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
(int)PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
(int)PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1);
}

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

@ -28,8 +28,7 @@
#include "drivers/motormix.h"
#include "drivers/motors.h"
#include "Flight/pid.h"
#include "drivers/arduino_com.h"
#include "drivers/barometer.h"#include "drivers/arduino_com.h"
/**************************************************************************
* BRIEF: Should contain all the initializations of the system, needs to
@ -47,20 +46,25 @@ void init_system()
//Configure the clock
system_clock_config();
//init motors to run with oneshot 125, small delay
HAL_Delay(1000);
pwmEnableAllMotors(Oneshot125);
//Initializes all the pids that are used in the system. This part will also init the gyro and accelerometer.
pidInit();
/* read saved variables from eeprom, in most cases eeprom should be read after a lot of the initializes */
readEEPROM();
pidEproom();
//initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble
cliInit(USART3);
//init sbus, using USART1
sbus_init();
//init motors to run with oneshot 125, small delay
HAL_Delay(1000);
pwmEnableAllMotors(Oneshot125);
#ifdef USE_LEDS
@ -72,7 +76,8 @@ void init_system()
#endif
#ifdef BARO
//barometer_init();
//barometer_reset();
#endif
#ifdef COMPASS
@ -95,6 +100,8 @@ void init_system()
#endif
}
/**************************************************************************
@ -110,6 +117,9 @@ int main(void)
//Init the system
init_system();
//Light the yellow led
ledOnInverted(Led1, Led1_GPIO_PORT);
//Initialize the scheduler, add all the tasks that should run to the ready queue of the scheduler
initScheduler();

78
UAV-ControlSystem/src/tasks_main.c
View File

@ -41,6 +41,7 @@
#include "drivers/accel_gyro.h"
#include "drivers/motormix.h"
#include "Flight/pid.h"
#include "drivers/barometer.h"
void systemTaskGyroPid(void)
{
@ -91,7 +92,10 @@ void systemTaskRx(void)
// }
/*Updated flag processRcChannel function, not yet tested. Should work as the entire loop above*/
flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
//flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
/* Includes the stick channel in the toggles checks */
flags_ProcessRcChannel_Improved(1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT);
//temporary send data from the RC directly form the RC
// RawRcCommand.Roll = frame.chan1;
@ -161,29 +165,68 @@ bool systemTaskRxCliCheck(uint32_t currentDeltaTime)
void systemTaskSerial(void)
{
// uint8_t c = 118;
// usart_transmit(&cliUsart, &c, 1, 1000000000);
if (flags_IsSet_ID(systemFlags_armed_id))
ledOnInverted(Led0_PIN, Led0_GPIO_PORT);
else
ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
static bool readyToCalibrate = true;
const float calibrationAmount = 0.5;
//Only run if the system is not armed
if (!flags_IsSet_ID(systemFlags_armed_id))
{
if (flags_IsSet_ID(systemFlags_throttleMax_id))
{
// if(flags_IsSet_ID(systemFlags_throttleLeft_id))
// {
// if(flags_IsSet_ID(systemFlags_stickDown_id))
// {
// mpu6000_read_acc_offset(&accelProfile);
// }
// }
if(readyToCalibrate)
{
if (flags_IsSet_ID(systemFlags_stickLeft_id))
{
accRollFineTune -= calibrationAmount;
}
else if (flags_IsSet_ID(systemFlags_stickRight_id))
{
accRollFineTune += calibrationAmount;
}
else if (flags_IsSet_ID(systemFlags_stickUp_id))
{
accPitchFineTune -= calibrationAmount;
}
else if (flags_IsSet_ID(systemFlags_stickDown_id))
{
accPitchFineTune += calibrationAmount;
}
}
else
{
//if the stick is centered set ready to calibrate to true
if(flags_IsSet_MASK(systemFlags_stickCenterH_mask | systemFlags_stickCenterV_mask))
{
readyToCalibrate = true;
}
}
}
}
}
void systemTaskBattery(void)
{
//Keep track of the battery level of the system
// uint8_t c = 98;
// usart_transmit(&cliUsart, &c, 1, 1000000000);
if (flags_IsSet_MASK((systemFlags_flightmode_acceleromter_mask | systemFlags_armed_mask)))
ledOnInverted(Led1, Led1_GPIO_PORT);
// uint8_t c = 118;
// usart_transmit(&cliUsart, &c, 1, 1000000000);
if (flags_IsSet_ID(systemFlags_armed_id))
ledOnInverted(Led0_PIN, Led0_GPIO_PORT);
else
ledOffInverted(Led1, Led1_GPIO_PORT);
ledOffInverted(Led0_PIN, Led0_GPIO_PORT);
}
void systemTaskBaro(void)
{
pidRun(PID_ID_BAROMETER);
//barometer_CaclulateValues();
}
void systemTaskCompass(void)
@ -204,6 +247,13 @@ void systemTaskSonar(void)
void systemTaskAltitude(void)
{
//Keep track of the vehicles current altitude, based on some sensor. In this case either barometer or sonar
//double temperature = barometer_GetCurrentTemperature();
//double pressure = barometer_GetCurrentPreassure();
//float altitute = barometer_GetCurrentAltitudeBasedOnSeaLevel();
//pid run, should probably be moved to systemTaskAltitude
pidRun(PID_ID_BAROMETER);
}
void systemTaskBeeper(void)

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

@ -376,6 +376,8 @@ HAL_StatusTypeDef HAL_I2C_Init(I2C_HandleTypeDef *hi2c)
/* Get PCLK1 frequency */
pclk1 = HAL_RCC_GetPCLK1Freq();
//this is temp test
// pclk1 = 50000000;
/* Calculate frequency range */
freqrange = I2C_FREQRANGE(pclk1);