From 7763f73c2ceb2317e425e22ba9df9d100fd96ae8 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 19 Oct 2016 16:59:48 +0200 Subject: [PATCH] Connected cli and eeprom, and motormix to PID settings Connected cli and eeprom, and motormix to PID settings --- UAV-ControlSystem/inc/Flight/pid.h | 19 +++-- UAV-ControlSystem/inc/config/cli.h | 4 +- UAV-ControlSystem/inc/config/eeprom.h | 18 +---- UAV-ControlSystem/inc/drivers/motormix.h | 6 -- UAV-ControlSystem/src/Flight/pid.c | 16 +++-- UAV-ControlSystem/src/config/cli.c | 76 +++++++++++++++----- UAV-ControlSystem/src/config/eeprom.c | 88 ++---------------------- UAV-ControlSystem/src/drivers/motormix.c | 18 ++--- 8 files changed, 103 insertions(+), 142 deletions(-) diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index 970551f..dd9f80f 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -25,11 +25,15 @@ #define PID_ID_ACCELEROMETER 1 #define PID_ID_COMPASS 2 #define PID_ID_BAROMETER 3 - +#define PID_COUNT 4 #define THROTTLE 0 /*Index terms to the PID*/ -#define ROLL 0 /*Index terms to the PID*/ -#define PITCH 1 /*Index terms to the PID*/ -#define YAW 2 /*Index terms to the PID*/ + +/*Enum of index to different profiles*/ +typedef enum { + ROLL = 0, + PITCH, + YAW +} RollPitchYaw; /*Struct that belongs to a certain PID controller*/ typedef struct pidProfile_s { @@ -53,8 +57,11 @@ typedef struct pidProfile_s { } pidProfile_t; -/*Is set in motor mix and used in pidUAVcore */ -extern bool motorLimitReached; +/*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; /************************************************************************** * BRIEF: Initializes PID profiles * diff --git a/UAV-ControlSystem/inc/config/cli.h b/UAV-ControlSystem/inc/config/cli.h index 8492df7..163b2ba 100644 --- a/UAV-ControlSystem/inc/config/cli.h +++ b/UAV-ControlSystem/inc/config/cli.h @@ -64,10 +64,10 @@ bool cliShouldRun(); * to place within the correct enum container system or profile. * * 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. * - * 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. * diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index ccd1e40..282761d 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -197,22 +197,8 @@ typedef enum { /* List of all profile EEPROM values */ typedef enum { - EEPROM_PID_GYRO_P_ROLL, - EEPROM_PID_GYRO_P_PITCH, - 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, + EEPROM_PID_GYRO, + EEPROM_PID_ACCELEROMETER, /* Counts the amount of settings in profile */ EEPROM_PROFILE_COUNT diff --git a/UAV-ControlSystem/inc/drivers/motormix.h b/UAV-ControlSystem/inc/drivers/motormix.h index 066a92a..85522b9 100644 --- a/UAV-ControlSystem/inc/drivers/motormix.h +++ b/UAV-ControlSystem/inc/drivers/motormix.h @@ -27,12 +27,6 @@ // TODO: These are only temporary before merge with PID part int16_t PID_Out[3]; -// TODO: Temporary before PID -typedef enum { - ROLL = 0, - PITCH, - YAW -} RollPitchYaw; // TODO: Implement in EEPROM diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 55e5db1..da126f9 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -31,7 +31,6 @@ #define PID_MAX_I 256 /*Constrains ITerm*/ #define PID_MAX_D 512 /*Constrains DTerm*/ -#define PID_COUNT 4 /*Struct that belongs to a certain PID controller*/ typedef struct pidProfileBuff_s { @@ -49,9 +48,8 @@ typedef struct pidProfileBuff_s { } 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}; 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*/ /*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; break; @@ -174,7 +172,6 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) } } - /************************************************************************** * BRIEF: The PID core * * 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 * **************************************************************************/ 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; } + +/************************************************************************** +* 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) { /*This is recommended init settings*/ @@ -456,6 +459,7 @@ 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); diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index aa90ceb..7f96636 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -246,6 +246,11 @@ typedef enum COMMAND_ID_PID_GYRO_D_PITCH, 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 COMMAND_ID_PID_ACCEL_P_ROLL, COMMAND_ID_PID_ACCEL_P_PITCH, @@ -256,6 +261,11 @@ typedef enum COMMAND_ID_PID_ACCEL_D_ROLL, 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 */ COMMAND_ID_COUNT, @@ -522,76 +532,110 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { //GYRO P [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] = { - "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] = { - "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 [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] = { - "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] = { - "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 [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] = { - "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] = { - "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 [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] = { - "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 [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] = { - "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 [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] = { - "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} + }, }; /*********************************************************************** diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index 44273b1..c58f327 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -256,92 +256,16 @@ EEPROM_DATA_t eeprom_sys_Arr[EEPROM_SYS_COUNT] = { /* Data pointers and sizes for profile content */ EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = { //GYRO P VAL - [EEPROM_PID_GYRO_P_ROLL] = + [EEPROM_PID_GYRO] = { - .size = sizeof(PidGyroProfile.P[0]), - .dataPtr = &PidProfile[PID_ID_GYRO].P[0], + .size = sizeof(pidProfile_t), + .dataPtr = &(PidProfile[PID_ID_GYRO]), }, - [EEPROM_PID_GYRO_P_PITCH] = + [EEPROM_PID_ACCELEROMETER] = { - .size = sizeof(PidGyroProfile.P[1]), - .dataPtr = &PidGyroProfile.P[1], + .size = sizeof(pidProfile_t), + .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 */ diff --git a/UAV-ControlSystem/src/drivers/motormix.c b/UAV-ControlSystem/src/drivers/motormix.c index aee2454..4179c9a 100644 --- a/UAV-ControlSystem/src/drivers/motormix.c +++ b/UAV-ControlSystem/src/drivers/motormix.c @@ -100,7 +100,7 @@ void mix() 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_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 //static bool motorLimitReached; @@ -114,9 +114,11 @@ void mix() * Calculation is: Output from control system * weight from model for each motor */ RPY_Mix[i] = \ - PID_Out[PITCH] * mixerUAV[i].pitch + \ - PID_Out[ROLL] * mixerUAV[i].roll + \ - PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); + PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \ + 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); + + // Update min and max values 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 { RPY_Mix[i] = \ - throttle * mixerUAV[i].throttle + \ - PID_Out[PITCH] * mixerUAV[i].pitch + \ - PID_Out[ROLL] * mixerUAV[i].roll + \ - PID_Out[YAW] * mixerUAV[i].yaw * ((mixerConfig.yaw_reverse_direction) ? -1 : 1); + throttle * mixerUAV[i].throttle + \ + PidProfile[PID_ID_GYRO].PID_Out[ROLL] * mixerUAV[i].roll + \ + 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); }