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

Merge remote-tracking branch 'refs/remotes/origin/PID-Improved' into baro2

Browse Source 
# Conflicts:
#	UAV-ControlSystem/src/drivers/accel_gyro.c
This commit is contained in:
Jonas Holmberg 2016-10-28 10:26:19 +02:00
commit e1b7a5c2ff
10 changed files with 189 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -75,4 +75,6 @@ void pidInit();
**************************************************************************/
void pidRun(uint8_t ID);
void pidEproom(void);
#endif /* FLIGHT_PID_H_ */

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

@ -101,7 +101,7 @@
#define MPU6000_REV_D9 0x59
#define MPU6000_REV_D10 0x5A
#define YAW_ROT_0
#define YAW_ROT_180
typedef struct gyro_t {
int16_t gyroX, gyroY, gyroZ; /* Gyroscope data converted into °/s */

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

@ -83,6 +83,6 @@ void Error_Handler(void);
uint8_t reverse(uint8_t byte);
uint16_t constrain(uint16_t value, uint16_t min, uint16_t max);
int16_t constrain(int16_t value, int16_t min, int16_t max);
#endif /* UTILITIES_H_ */

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

@ -18,6 +18,7 @@
#include "drivers/sbus.h"
#include "scheduler/scheduler.h"
#include <math.h>
#include "drivers/failsafe_toggles.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*/
@ -25,7 +26,7 @@
#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 90 /*Determines the range of the maximum angle (limits the rc input) & (Accelerometer takes int to max 16 G)*/
#define ACCELEROMETER_RANGE 35 /*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)*/
@ -55,6 +56,38 @@ 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*/
pt1Filter_t accelFilter[2] = {0};
/**************************************************************************
* BRIEF: Calculates angle from accelerometer *
* INFORMATION: *
**************************************************************************/
float calcAngle(const uint8_t axis, const float x_axis, const float y_axis, const float z_axis)
{
float angle;
float angle_offset = (z_axis < 0 )? 90: 0;
switch (axis)
{
case ROLL:
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( 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;
break;
}
return angle;
}
/**************************************************************************
* BRIEF: Scales data from input range to output range *
@ -62,7 +95,8 @@ gyro_t gyroProfile; /*Struct profile for input data from sensor*/
**************************************************************************/
float convertData(int inputRange, int outputRange, int offset, float value)
{
return (outputRange/inputRange)*(value-offset);
return ((float)outputRange/(float)inputRange)*(value-(float)offset);
//return 1.0;
}
/**************************************************************************
@ -79,12 +113,21 @@ float constrainf(float amt, int low, int high)
return amt;
}
float oldSensorValue[2] = {0};
float oldSensorValueRoll[50] = {0};
float oldSensorValuePitch[50] = {0};
int i = 0;
/**************************************************************************
* BRIEF: Update current sensor values *
* INFORMATION: *
**************************************************************************/
void getCurrentValues(float *sensorValues, uint8_t ID_profile)
void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
{
switch (ID_profile)
{
case PID_ID_GYRO:
@ -100,9 +143,47 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile)
mpu6000_read_accel(&accelProfile); /*Reads data from accelerometer*/
float alpha = 0.5;
/*May need Low pass filter since the accelerometer may drift*/
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;
float X_roll = calcAngle(ROLL, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
float X_pitch = calcAngle(PITCH, accelProfile.accelXconv, accelProfile.accelYconv, accelProfile.accelZconv);
oldSensorValueRoll[i] = X_roll;
oldSensorValuePitch[i] = X_pitch;
float RollValue = 0;
float PitchValue = 0;
for (int ii = 0; ii < 50; ii++)
{
RollValue = RollValue + oldSensorValueRoll[ii];
PitchValue = PitchValue + oldSensorValuePitch[ii];
}
i = (i < 49)? i + 1:0;
sensorValues[ROLL] = RollValue/50;
sensorValues[PITCH] = PitchValue/50;
sensorValues[ROLL] = alpha*RollValue/50 + (1-alpha)*oldSensorValue[0];
sensorValues[PITCH] = alpha*PitchValue/50 + (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:
@ -129,7 +210,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
{
case PID_ID_GYRO:
if (!PidProfile[PID_ID_ACCELEROMETER].pidEnabled)
if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
{
desiredCommand[ROLL] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL]);
desiredCommand[PITCH] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH]);
@ -184,7 +265,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)
@ -247,7 +328,7 @@ 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);
}
@ -301,7 +382,7 @@ void pidRun(uint8_t ID)
break;
case PID_ID_ACCELEROMETER:
if (!PidProfile[PID_ID_ACCELEROMETER].pidEnabled)
if (!(PidProfile[PID_ID_ACCELEROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_acceleromter_id)))
{
PidProfile[PID_ID_ACCELEROMETER].PID_Out[ROLL] = rc_input.Roll;
PidProfile[PID_ID_ACCELEROMETER].PID_Out[PITCH] = rc_input.Pitch;
@ -341,6 +422,8 @@ void pidRun(uint8_t ID)
}
}
/*--------------------------------------------Init Functions----------------------------------------------------------------------------------*/
/**************************************************************************
* BRIEF: Initializes a certain pidbuffer profile connected to *
* a PID controller *
@ -415,14 +498,19 @@ 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].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;
@ -430,18 +518,30 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
break;
case PID_ID_ACCELEROMETER:
PidProfile[ID].pidEnabled = false;
//PidProfile[ID].dterm_lpf = 90;
//PidProfile[ID].pid_out_limit = 3000;
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;
break;
case PID_ID_COMPASS:
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].PIDweight[ROLL] = 100;
PidProfile[ID].PIDweight[PITCH] = 100;
PidProfile[ID].PIDweight[YAW] = 100;
PidProfile[ID].pidEnabled = false;
break;
@ -471,3 +571,12 @@ 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;
}

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

@ -278,6 +278,10 @@ typedef enum
COMMAND_ID_PID_ACCEL_YAW_P_LIMIT,
COMMAND_ID_PID_ACCEL_OUT_LIMIT,
/* Enable the different pid loops */
COMMAND_ID_PID_GYRO_ISENABLED,
COMMAND_ID_PID_ACCEL_ISENABLED,
/* Counter for the amount of commands */
COMMAND_ID_COUNT,
@ -544,110 +548,122 @@ 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, EEPROM_VALUE_TYPE_PROFILE, 5, 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, 255}
},
[COMMAND_ID_PID_GYRO_P_PITCH] =
{
"pid_gyro_p_pitch", COMMAND_ID_PID_GYRO_P_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 6, 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, 255}
},
[COMMAND_ID_PID_GYRO_P_YAW] =
{
"pid_gyro_p_yaw", COMMAND_ID_PID_GYRO_P_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 7, 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, 255}
},
//GYRO I
[COMMAND_ID_PID_GYRO_I_ROLL] =
{
"pid_gyro_i_roll", COMMAND_ID_PID_GYRO_I_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 8, 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, 255}
},
[COMMAND_ID_PID_GYRO_I_PITCH] =
{
"pid_gyro_i_pitch", COMMAND_ID_PID_GYRO_I_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 9, 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, 255}
},
[COMMAND_ID_PID_GYRO_I_YAW] =
{
"pid_gyro_i_yaw", COMMAND_ID_PID_GYRO_I_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 10, 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, 255}
},
//GYRO D
[COMMAND_ID_PID_GYRO_D_ROLL] =
{
"pid_gyro_d_roll", COMMAND_ID_PID_GYRO_D_ROLL, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 11, 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, 255}
},
[COMMAND_ID_PID_GYRO_D_PITCH] =
{
"pid_gyro_d_pitch", COMMAND_ID_PID_GYRO_D_PITCH, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 12, 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, 255}
},
[COMMAND_ID_PID_GYRO_D_YAW] =
{
"pid_gyro_d_yaw", COMMAND_ID_PID_GYRO_D_YAW, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 13, 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, 255}
},
//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}
"pid_gyro_dterm_lpf", COMMAND_ID_PID_GYRO_DTERM_LPF, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 255}
},
[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}
"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, 255}
},
[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}
"pid_gyro_yaw_lpf", COMMAND_ID_PID_GYRO_YAW_P_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 255}
},
[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}
"pid_gyro_out_limit", COMMAND_ID_PID_GYRO_OUT_LIMIT, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 255}
},
//ACCEL P
[COMMAND_ID_PID_ACCEL_P_ROLL] =
{
"pid_accel_p_roll", COMMAND_ID_PID_ACCEL_P_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 5, 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, 255}
},
[COMMAND_ID_PID_ACCEL_P_PITCH] =
{
"pid_accel_p_pitch", COMMAND_ID_PID_ACCEL_P_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 6, 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, 255}
},
//ACCEL I
[COMMAND_ID_PID_ACCEL_I_ROLL] =
{
"pid_accel_i_roll", COMMAND_ID_PID_ACCEL_I_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 8, 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, 255}
},
[COMMAND_ID_PID_ACCEL_I_PITCH] =
{
"pid_accel_i_pitch", COMMAND_ID_PID_ACCEL_I_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 9, 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, 255}
},
//ACCEL D
[COMMAND_ID_PID_ACCEL_D_ROLL] =
{
"pid_accel_d_roll", COMMAND_ID_PID_ACCEL_D_ROLL, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, 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, 255}
},
[COMMAND_ID_PID_ACCEL_D_PITCH] =
{
"pid_accel_d_pitch", COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 12, 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, 255}
},
//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}
"pid_accel_dterm_lpf", COMMAND_ID_PID_ACCEL_DTERM_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 65000}
},
[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}
"pid_accel_pterm_yaw_lpf", COMMAND_ID_PID_ACCEL_PTERM_YAW_LPF, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 65000}
},
[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}
"pid_accel_yaw_p_limit", COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 65000}
},
[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}
"pid_accel_out_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000}
},
/* Enable pid loops */
[COMMAND_ID_PID_GYRO_ISENABLED] =
{
"pid_gyro_isenabled", COMMAND_ID_PID_GYRO_ISENABLED, EEPROM_PID_GYRO, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1}
},
[COMMAND_ID_PID_ACCEL_ISENABLED] =
{
"pid_accel_isenabled", COMMAND_ID_PID_ACCEL_ISENABLED, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1}
},
};
/***********************************************************************

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

@ -241,6 +241,9 @@ bool mpu6000_init(gyro_t* gyro, accel_t* accel)
mpu6000_read_gyro_offset(gyro);
mpu6000_read_acc_offset(accel);
HAL_Delay(60);
return true;
}

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

@ -47,7 +47,7 @@ uint16_t motor_output[MOTOR_COUNT];
mixerConfig_s mixerConfig = {
.minThrottle = 1050,
.maxThrottle = MAX_PULSE - 100,
.minCommand = 1000,
.minCommand = 990,
.maxCommand = MAX_PULSE,
.minCheck = 1010,
.pid_at_min_throttle = true,
@ -114,8 +114,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,15 +194,21 @@ 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);
}
/* Mixer Low Scale - Scaling output around low throttle */
if (flags_IsSet_ID(systemFlags_mixerlowscale_id))
{
for (int i = 0; i < MOTOR_COUNT; i++)
// Find the minimum and maximum motor output
if (RPY_Mix[i] < RPY_Mix_Min) RPY_Mix_Min = RPY_Mix[i];
uint16_t RPY_Mix_Min_Overshoot = (RPY_Mix_Min < mixerConfig.minThrottle) ? mixerConfig.minThrottle - RPY_Mix_Min : 0;
if (RPY_Mix_Min_Overshoot > 0 ) //|| RPY_Mix_Max_Overshoot > 0)
@ -217,8 +223,12 @@ void mix()
for (int i = 0; i < MOTOR_COUNT; i++)
RPY_Mix[i] = (((float)(RPY_Mix[i] - throttle)) * (1 - mix_scale_reduction)) + throttle;
}
/* Recalculating RPY_Mix_Min */
RPY_Mix_Min = throttleMid;
}
for (int i = 0; i < MOTOR_COUNT; i++)
{
// Find the minimum and maximum motor output

2
UAV-ControlSystem/src/drivers/motors.c
View File

@ -248,7 +248,7 @@ void calibrateMotors()
pwmActivateAllMotors();
//First set the motor output to the maximum allowed throttle
for (uint8_t i = 1; i < 11; i++ ) pwmAdjustSpeedOfMotor(i,MotorPWMPeriode);
for (uint8_t i = 1; i < 11; i++ ) pwmAdjustSpeedOfMotor(i,MAX_PULSE);
//wait for a little while
HAL_Delay(6200);

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

@ -53,6 +53,8 @@ void init_system()
/* 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);

2
UAV-ControlSystem/src/utilities.c
View File

@ -219,7 +219,7 @@ uint8_t reverse(uint8_t byte)
return byte;
}
uint16_t constrain(uint16_t value, uint16_t min, uint16_t max)
int16_t constrain(int16_t value, int16_t min, int16_t max)
{
if (value < min)
return min;