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

Connected cli and eeprom, and motormix to PID settings

Browse Source 
Connected cli and eeprom, and motormix to PID settings
This commit is contained in:
johan9107 2016-10-19 16:59:48 +02:00
parent 8bb27e91fa
commit 7763f73c2c
8 changed files with 103 additions and 142 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -25,11 +25,15 @@
#define PID_ID_ACCELEROMETER 1 #define PID_ID_ACCELEROMETER 1
#define PID_ID_COMPASS 2 #define PID_ID_COMPASS 2
#define PID_ID_BAROMETER 3 #define PID_ID_BAROMETER 3
#define PID_COUNT 4
#define THROTTLE 0 /*Index terms to the PID*/ #define THROTTLE 0 /*Index terms to the PID*/
#define ROLL 0 /*Index terms to the PID*/
#define PITCH 1 /*Index terms to the PID*/ /*Enum of index to different profiles*/
#define YAW 2 /*Index terms to the PID*/ typedef enum {
ROLL = 0,
PITCH,
YAW
} RollPitchYaw;
/*Struct that belongs to a certain PID controller*/ /*Struct that belongs to a certain PID controller*/
typedef struct pidProfile_s { typedef struct pidProfile_s {
@ -53,8 +57,11 @@ typedef struct pidProfile_s {
} pidProfile_t; } pidProfile_t;
/*Is set in motor mix and used in pidUAVcore */ /*Array of all pid profiles of the system*/
extern bool motorLimitReached; extern pidProfile_t PidProfile[PID_COUNT];
/*Is set in motor mix and used in pidUAVcore and mix */
bool motorLimitReached;
/************************************************************************** /**************************************************************************
* BRIEF: Initializes PID profiles * * BRIEF: Initializes PID profiles *

4
UAV-ControlSystem/inc/config/cli.h
View File

@ -64,10 +64,10 @@ bool cliShouldRun();
* to place within the correct enum container system or profile. * to place within the correct enum container system or profile.
* *
* 2: In eeprom.c add two values to the correct array for the value * 2: In eeprom.c add two values to the correct array for the value
* profile or system. THe two values should be the pointer to the * profile or system. The two values should be the pointer to the
* value to store in eeprom and the size of said value. * value to store in eeprom and the size of said value.
* *
* 3: Now the value should be storable int the EEPROM. * 3: Now the value should be storable in the EEPROM.
* *
* B: Create the actual command in the CLI. * B: Create the actual command in the CLI.
* *

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

@ -197,22 +197,8 @@ typedef enum {
/* List of all profile EEPROM values */ /* List of all profile EEPROM values */
typedef enum { typedef enum {
EEPROM_PID_GYRO_P_ROLL, EEPROM_PID_GYRO,
EEPROM_PID_GYRO_P_PITCH, EEPROM_PID_ACCELEROMETER,
EEPROM_PID_GYRO_P_YAW,
EEPROM_PID_GYRO_I_ROLL,
EEPROM_PID_GYRO_I_PITCH,
EEPROM_PID_GYRO_I_YAW,
EEPROM_PID_GYRO_D_ROLL,
EEPROM_PID_GYRO_D_PITCH,
EEPROM_PID_GYRO_D_YAW,
EEPROM_PID_ACCEL_P_ROLL,
EEPROM_PID_ACCEL_P_PITCH,
EEPROM_PID_ACCEL_I_ROLL,
EEPROM_PID_ACCEL_I_PITCH,
EEPROM_PID_ACCEL_D_ROLL,
EEPROM_PID_ACCEL_D_PITCH,
/* Counts the amount of settings in profile */ /* Counts the amount of settings in profile */
EEPROM_PROFILE_COUNT EEPROM_PROFILE_COUNT

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

@ -27,12 +27,6 @@
// TODO: These are only temporary before merge with PID part // TODO: These are only temporary before merge with PID part
int16_t PID_Out[3]; int16_t PID_Out[3];
// TODO: Temporary before PID
typedef enum {
ROLL = 0,
PITCH,
YAW
} RollPitchYaw;
// TODO: Implement in EEPROM // TODO: Implement in EEPROM

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

@ -31,7 +31,6 @@
#define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_I 256 /*Constrains ITerm*/
#define PID_MAX_D 512 /*Constrains DTerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/
#define PID_COUNT 4
/*Struct that belongs to a certain PID controller*/ /*Struct that belongs to a certain PID controller*/
typedef struct pidProfileBuff_s { typedef struct pidProfileBuff_s {
@ -49,9 +48,8 @@ typedef struct pidProfileBuff_s {
} pidProfileBuff_t; } pidProfileBuff_t;
bool motorLimitReached = false; /*Implement in mix*/
pidProfile_t PidProfile[PID_COUNT] = {0}; pidProfile_t PidProfile[PID_COUNT] = {0}; /*Array of all pid profiles of the system*/
pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0}; pidProfileBuff_t PidProfileBuff[PID_COUNT] = {0};
accel_t accelProfile; /*Struct profile for input data from sensor*/ accel_t accelProfile; /*Struct profile for input data from sensor*/
@ -103,7 +101,7 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile)
mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/ mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/
/*May need Low pass filter since the accelerometer may drift*/ /*May need Low pass filter since the accelerometer may drift*/
sensorValues[ROLL] = atan2(accelProfile.accelXconv,accelProfile.accelZconv)*180*M_PI;//acos(accelProfile.accelXconv/R)*180/M_PI; sensorValues[ROLL] = atan2(accelProfile.accelXconv,accelProfile.accelZconv)*180*M_PI;
sensorValues[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI; sensorValues[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI;
break; break;
@ -174,7 +172,6 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
} }
} }
/************************************************************************** /**************************************************************************
* BRIEF: The PID core * * BRIEF: The PID core *
* INFORMATION: The actual PID controller, calculates the final output of * * INFORMATION: The actual PID controller, calculates the final output of *
@ -345,7 +342,8 @@ void pidRun(uint8_t ID)
} }
/************************************************************************** /**************************************************************************
* BRIEF: Initializes a certain pid profile connected to a PID controller * * BRIEF: Initializes a certain pidbuffer profile connected to *
* a PID controller *
* INFORMATION: Recommended to use if unexpected values occur of profile * * INFORMATION: Recommended to use if unexpected values occur of profile *
**************************************************************************/ **************************************************************************/
void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID) void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID)
@ -403,6 +401,11 @@ void pidUAVInitBuff(pidProfileBuff_t *pidProfile, uint8_t ID)
PidProfileBuff[ID].yawFilter.RC = 0; PidProfileBuff[ID].yawFilter.RC = 0;
} }
/**************************************************************************
* BRIEF: Initializes a certain pid profile connected to a PID controller *
* INFORMATION: Recommended to use if unexpected values occur of profile *
**************************************************************************/
void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
{ {
/*This is recommended init settings*/ /*This is recommended init settings*/
@ -456,6 +459,7 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
void pidInit() void pidInit()
{ {
mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/ mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/
motorLimitReached = false;
pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO); pidUAVInitBuff(&PidProfileBuff[PID_ID_GYRO], PID_ID_GYRO);
pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER); pidUAVInitBuff(&PidProfileBuff[PID_ID_ACCELEROMETER], PID_ID_ACCELEROMETER);

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

@ -246,6 +246,11 @@ typedef enum
COMMAND_ID_PID_GYRO_D_PITCH, COMMAND_ID_PID_GYRO_D_PITCH,
COMMAND_ID_PID_GYRO_D_YAW, COMMAND_ID_PID_GYRO_D_YAW,
COMMAND_ID_PID_GYRO_DTERM_LPF,
COMMAND_ID_PID_GYRO_PTERM_YAW_LPF,
COMMAND_ID_PID_GYRO_YAW_P_LIMIT,
COMMAND_ID_PID_GYRO_OUT_LIMIT,
//ACCEL //ACCEL
COMMAND_ID_PID_ACCEL_P_ROLL, COMMAND_ID_PID_ACCEL_P_ROLL,
COMMAND_ID_PID_ACCEL_P_PITCH, COMMAND_ID_PID_ACCEL_P_PITCH,
@ -256,6 +261,11 @@ typedef enum
COMMAND_ID_PID_ACCEL_D_ROLL, COMMAND_ID_PID_ACCEL_D_ROLL,
COMMAND_ID_PID_ACCEL_D_PITCH, COMMAND_ID_PID_ACCEL_D_PITCH,
COMMAND_ID_PID_ACCEL_DTERM_LPF,
COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF,
COMMAND_ID_PID_ACCEL_YAW_P_LIMIT,
COMMAND_ID_PID_ACCEL_OUT_LIMIT,
/* Counter for the amount of commands */ /* Counter for the amount of commands */
COMMAND_ID_COUNT, COMMAND_ID_COUNT,
@ -522,76 +532,110 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = {
//GYRO P //GYRO P
[COMMAND_ID_PID_GYRO_P_ROLL] = [COMMAND_ID_PID_GYRO_P_ROLL] =
{ {
"pid_gyro_p_roll", COMMAND_ID_PID_GYRO_P_ROLL, EEPROM_PID_GYRO_P_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_gyro_p_roll", COMMAND_ID_PID_GYRO_P_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 100}
}, },
[COMMAND_ID_PID_GYRO_P_PITCH] = [COMMAND_ID_PID_GYRO_P_PITCH] =
{ {
"pid_gyro_p_pitch", COMMAND_ID_PID_GYRO_P_PITCH, EEPROM_PID_GYRO_P_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_gyro_p_pitch", COMMAND_ID_PID_GYRO_P_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 6, VAL_UINT_8, .valueRange = {0, 100}
}, },
[COMMAND_ID_PID_GYRO_P_YAW] = [COMMAND_ID_PID_GYRO_P_YAW] =
{ {
"pid_gyro_p_yaw", COMMAND_ID_PID_GYRO_P_YAW, EEPROM_PID_GYRO_P_YAW, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_gyro_p_yaw", COMMAND_ID_PID_GYRO_P_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 7, VAL_UINT_8, .valueRange = {0, 100}
}, },
//GYRO I //GYRO I
[COMMAND_ID_PID_GYRO_I_ROLL] = [COMMAND_ID_PID_GYRO_I_ROLL] =
{ {
"pid_gyro_i_roll", COMMAND_ID_PID_GYRO_I_ROLL, EEPROM_PID_GYRO_I_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_gyro_i_roll", COMMAND_ID_PID_GYRO_I_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 100}
}, },
[COMMAND_ID_PID_GYRO_I_PITCH] = [COMMAND_ID_PID_GYRO_I_PITCH] =
{ {
"pid_gyro_i_pitch", COMMAND_ID_PID_GYRO_I_PITCH, EEPROM_PID_GYRO_I_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_gyro_i_pitch", COMMAND_ID_PID_GYRO_I_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 9, VAL_UINT_8, .valueRange = {0, 100}
}, },
[COMMAND_ID_PID_GYRO_I_YAW] = [COMMAND_ID_PID_GYRO_I_YAW] =
{ {
"pid_gyro_i_yaw", COMMAND_ID_PID_GYRO_I_YAW, EEPROM_PID_GYRO_I_YAW, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_gyro_i_yaw", COMMAND_ID_PID_GYRO_I_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 10, VAL_UINT_8, .valueRange = {0, 100}
}, },
//GYRO D //GYRO D
[COMMAND_ID_PID_GYRO_D_ROLL] = [COMMAND_ID_PID_GYRO_D_ROLL] =
{ {
"pid_gyro_d_roll", COMMAND_ID_PID_GYRO_D_ROLL, EEPROM_PID_GYRO_D_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_gyro_d_roll", COMMAND_ID_PID_GYRO_D_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 100}
}, },
[COMMAND_ID_PID_GYRO_D_PITCH] = [COMMAND_ID_PID_GYRO_D_PITCH] =
{ {
"pid_gyro_d_pitch", COMMAND_ID_PID_GYRO_D_PITCH, EEPROM_PID_GYRO_D_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_gyro_d_pitch", COMMAND_ID_PID_GYRO_D_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 12, VAL_UINT_8, .valueRange = {0, 100}
}, },
[COMMAND_ID_PID_GYRO_D_YAW] = [COMMAND_ID_PID_GYRO_D_YAW] =
{ {
"pid_gyro_d_yaw", COMMAND_ID_PID_GYRO_D_YAW, EEPROM_PID_GYRO_D_YAW, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_gyro_d_yaw", COMMAND_ID_PID_GYRO_D_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 13, VAL_UINT_8, .valueRange = {0, 100}
},
//Gyro filter and limits
[COMMAND_ID_PID_GYRO_DTERM_LPF] =
{
"pid_gyro_dterm_lpf", COMMAND_ID_PID_GYRO_DTERM_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 100}
},
[COMMAND_ID_PID_GYRO_PTERM_YAW_LPF] =
{
"pid_gyro_pterm_yaw_lpf", COMMAND_ID_PID_GYRO_PTERM_YAW_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 100}
},
[COMMAND_ID_PID_GYRO_YAW_P_LIMIT] =
{
"pid_gyro_yaw_lpf", COMMAND_ID_PID_GYRO_YAW_P_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 100}
},
[COMMAND_ID_PID_GYRO_OUT_LIMIT] =
{
"pid_gyro_out_limit", COMMAND_ID_PID_GYRO_OUT_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 100}
}, },
//ACCEL P //ACCEL P
[COMMAND_ID_PID_ACCEL_P_ROLL] = [COMMAND_ID_PID_ACCEL_P_ROLL] =
{ {
"pid_accel_p_roll", COMMAND_ID_PID_ACCEL_P_ROLL, EEPROM_PID_ACCEL_P_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_accel_p_roll", COMMAND_ID_PID_ACCEL_P_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 100}
}, },
[COMMAND_ID_PID_ACCEL_P_PITCH] = [COMMAND_ID_PID_ACCEL_P_PITCH] =
{ {
"pid_accel_p_pitch", COMMAND_ID_PID_ACCEL_P_PITCH, EEPROM_PID_ACCEL_P_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_accel_p_pitch", COMMAND_ID_PID_ACCEL_P_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 6, VAL_UINT_8, .valueRange = {0, 100}
}, },
//ACCEL I //ACCEL I
[COMMAND_ID_PID_ACCEL_I_ROLL] = [COMMAND_ID_PID_ACCEL_I_ROLL] =
{ {
"pid_accel_i_roll", COMMAND_ID_PID_ACCEL_I_ROLL, EEPROM_PID_ACCEL_I_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_accel_i_roll", COMMAND_ID_PID_ACCEL_I_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 100}
}, },
[COMMAND_ID_PID_ACCEL_I_PITCH] = [COMMAND_ID_PID_ACCEL_I_PITCH] =
{ {
"pid_accel_i_pitch", COMMAND_ID_PID_ACCEL_I_PITCH, EEPROM_PID_ACCEL_I_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_accel_i_pitch", COMMAND_ID_PID_ACCEL_I_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 9, VAL_UINT_8, .valueRange = {0, 100}
}, },
//ACCEL D //ACCEL D
[COMMAND_ID_PID_ACCEL_D_ROLL] = [COMMAND_ID_PID_ACCEL_D_ROLL] =
{ {
"pid_accel_d_roll", COMMAND_ID_PID_ACCEL_D_ROLL, EEPROM_PID_ACCEL_D_ROLL, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_accel_d_roll", COMMAND_ID_PID_ACCEL_D_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 100}
}, },
[COMMAND_ID_PID_ACCEL_D_PITCH] = [COMMAND_ID_PID_ACCEL_D_PITCH] =
{ {
"pid_accel_d_pitch", COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_ACCEL_D_PITCH, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} "pid_accel_d_pitch", COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 12, VAL_UINT_8, .valueRange = {0, 100}
}, },
//ACCEL Filters and limits
[COMMAND_ID_PID_ACCEL_DTERM_LPF] =
{
"pid_accel_dterm_lpf", COMMAND_ID_PID_ACCEL_DTERM_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 100}
},
[COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF] =
{
"pid_accel_pterm_yaw_lpf", COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 15, VAL_UINT_16, .valueRange = {0, 100}
},
[COMMAND_ID_PID_ACCEL_YAW_P_LIMIT] =
{
"pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 17, VAL_UINT_16, .valueRange = {0, 100}
},
[COMMAND_ID_PID_ACCEL_OUT_LIMIT] =
{
"pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 100}
},
}; };
/*********************************************************************** /***********************************************************************

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

@ -256,92 +256,16 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = {
/* Data pointers and sizes for profile content */ /* Data pointers and sizes for profile content */
EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = { EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = {
//GYRO P VAL //GYRO P VAL
[EEPROM_PID_GYRO_P_ROLL] = [EEPROM_PID_GYRO] =
{ {
.size = sizeof(PidGyroProfile.P[0]), .size = sizeof(pidProfile_t),
.dataPtr = &PidProfile[PID_ID_GYRO].P[0], .dataPtr = &(PidProfile[PID_ID_GYRO]),
}, },
[EEPROM_PID_GYRO_P_PITCH] = [EEPROM_PID_ACCELEROMETER] =
{ {
.size = sizeof(PidGyroProfile.P[1]), .size = sizeof(pidProfile_t),
.dataPtr = &PidGyroProfile.P[1], .dataPtr = &(PidProfile[PID_ID_ACCELEROMETER]),
}, },
[EEPROM_PID_GYRO_P_YAW] =
{
.size = sizeof(PidGyroProfile.P[2]),
.dataPtr = &PidGyroProfile.P[2],
},
//GYRO I VAL
[EEPROM_PID_GYRO_I_ROLL] =
{
.size = sizeof(PidGyroProfile.I[0]),
.dataPtr = &PidGyroProfile.I[0],
},
[EEPROM_PID_GYRO_I_PITCH] =
{
.size = sizeof(PidGyroProfile.I[1]),
.dataPtr = &PidGyroProfile.I[1],
},
[EEPROM_PID_GYRO_I_YAW] =
{
.size = sizeof(PidGyroProfile.I[2]),
.dataPtr = &PidGyroProfile.I[2],
},
//GYRO D VAL
[EEPROM_PID_GYRO_D_ROLL] =
{
.size = sizeof(PidGyroProfile.D[0]),
.dataPtr = &PidGyroProfile.D[0],
},
[EEPROM_PID_GYRO_D_PITCH] =
{
.size = sizeof(PidGyroProfile.D[1]),
.dataPtr = &PidGyroProfile.D[1],
},
[EEPROM_PID_GYRO_D_YAW] =
{
.size = sizeof(PidGyroProfile.D[2]),
.dataPtr = &PidGyroProfile.D[2],
},
//ACCEL P VAL
[EEPROM_PID_ACCEL_P_ROLL] =
{
.size = sizeof(PidAccelerometerProfile.P[0]),
.dataPtr = &PidAccelerometerProfile.P[0],
},
[EEPROM_PID_ACCEL_P_PITCH] =
{
.size = sizeof(PidAccelerometerProfile.P[1]),
.dataPtr = &PidAccelerometerProfile.P[1],
},
//ACCEL I VAL
[EEPROM_PID_ACCEL_I_ROLL] =
{
.size = sizeof(PidAccelerometerProfile.I[0]),
.dataPtr = &PidAccelerometerProfile.I[0],
},
[EEPROM_PID_ACCEL_I_PITCH] =
{
.size = sizeof(PidAccelerometerProfile.I[1]),
.dataPtr = &PidAccelerometerProfile.I[1],
},
//ACCEL_D_VAL
[EEPROM_PID_ACCEL_D_ROLL] =
{
.size = sizeof(PidAccelerometerProfile.D[0]),
.dataPtr = &PidAccelerometerProfile.D[0],
},
[EEPROM_PID_ACCEL_D_PITCH] =
{
.size = sizeof(PidAccelerometerProfile.D[1]),
.dataPtr = &PidAccelerometerProfile.D[1],
},
}; };
/* Data pointers and sizes for footer content */ /* Data pointers and sizes for footer content */

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

@ -100,7 +100,7 @@ void mix()
int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array
int16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor int16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor
int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor
int16_t throttle = sbusChannelData.chan3; // Import throttle value from remote int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]; // Import throttle value from remote
// Might be used for some debug if necessary // Might be used for some debug if necessary
//static bool motorLimitReached; //static bool motorLimitReached;
@ -114,9 +114,11 @@ void mix()
* Calculation is: Output from control system * weight from model for each motor * Calculation is: Output from control system * weight from model for each motor
*/ */
RPY_Mix[i] = \ RPY_Mix[i] = \
PID_Out[PITCH] * mixerUAV[i].pitch + \ PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
PID_Out[ROLL] * mixerUAV[i].roll + \ PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1);
// Update min and max values // Update min and max values
if (RPY_Mix[i] > RPY_Mix_Max) RPY_Mix_Max = RPY_Mix[i]; if (RPY_Mix[i] > RPY_Mix_Max) RPY_Mix_Max = RPY_Mix[i];
@ -191,10 +193,10 @@ void mix()
for (int i = 0; i < MOTOR_COUNT; i++) // Without airmode this includes throttle for (int i = 0; i < MOTOR_COUNT; i++) // Without airmode this includes throttle
{ {
RPY_Mix[i] = \ RPY_Mix[i] = \
throttle * mixerUAV[i].throttle + \ throttle * mixerUAV[i].throttle + \
PID_Out[PITCH] * mixerUAV[i].pitch + \ PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \
PID_Out[ROLL] * mixerUAV[i].roll + \ PidProfile[PID_ID_GYRO].PID_Out[PITCH] * mixerUAV[i].pitch + \
PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); PidProfile[PID_ID_GYRO].PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1);
} }