From c791a9ea3bce410735f8d3efbbeed9f6b41fd123 Mon Sep 17 00:00:00 2001 From: johan9107 Date: Wed, 9 Nov 2016 10:34:03 +0100 Subject: [PATCH] PID added barometer settings to CLI, eeprom and PID --- UAV-ControlSystem/inc/config/eeprom.h | 2 ++ UAV-ControlSystem/src/Flight/pid.c | 24 ++++++++++--- UAV-ControlSystem/src/config/cli.c | 52 +++++++++++++++++++++++++++ UAV-ControlSystem/src/config/eeprom.c | 10 ++++++ 4 files changed, 84 insertions(+), 4 deletions(-) diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 1774a35..7c1530c 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -208,6 +208,8 @@ typedef enum { typedef enum { EEPROM_PID_GYRO, EEPROM_PID_ACCELEROMETER, + EEPROM_PID_COMPASS, + EEPROM_PID_BAROMETER, /* Counts the amount of settings in profile */ EEPROM_PROFILE_COUNT diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 3503841..162ceed 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -27,7 +27,7 @@ #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 BAROMETER_RANGE 3 /*Determines the range of the maximum height (limits the rc input)*/ #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)*/ @@ -60,6 +60,7 @@ gyro_t gyroProfile = {0}; /*Struct profile for input data from senso pt1Filter_t accelFilter[2] = {0}; +float throttleRate = 0; float accRollFineTune = 0; float accPitchFineTune = 0; @@ -97,9 +98,22 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile) sensorValues[ROLL] = accelProfile.rollAngle + accRollFineTune; sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune; + /*Checks the biggest angle */ + throttleRate = 2 - (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? cos(sensorValues[ROLL]*M_PI/180) : cos(sensorValues[PITCH]*M_PI/180); + break; case PID_ID_COMPASS: + + sensorValues[ROLL] = 0; + sensorValues[PITCH] = 0; + sensorValues[YAW] = 0; + + + break; case PID_ID_BAROMETER: + + + break; default: sensorValues[ROLL] = 0; @@ -157,7 +171,9 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) break; case PID_ID_BAROMETER: - desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle); + //desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate); + + desiredCommand[THROTTLE] = 1.9; break; default: @@ -324,9 +340,9 @@ void pidRun(uint8_t ID) break; case PID_ID_BAROMETER: - if (!PidProfile[PID_ID_BAROMETER].pidEnabled) + if (!PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) { - PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle; + PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle*throttleRate; } else { diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index 5f5bee6..d77f8a9 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -284,9 +284,22 @@ typedef enum COMMAND_ID_PID_ACCEL_YAW_P_LIMIT, COMMAND_ID_PID_ACCEL_OUT_LIMIT, + //Barometer + COMMAND_ID_PID_BARO_P_HEIGHT, + + COMMAND_ID_PID_BARO_I_HEIGHT, + + COMMAND_ID_PID_BARO_D_HEIGHT, + + COMMAND_ID_PID_BARO_DTERM_LPF, + COMMAND_ID_PID_BARO_PTERM_YAW_LPF, + COMMAND_ID_PID_BARO_YAW_P_LIMIT, + COMMAND_ID_PID_BARO_OUT_LIMIT, + /* Enable the different pid loops */ COMMAND_ID_PID_GYRO_ISENABLED, COMMAND_ID_PID_ACCEL_ISENABLED, + COMMAND_ID_PID_BARO_ISENABLED, /* Counter for the amount of commands */ COMMAND_ID_COUNT, @@ -659,6 +672,41 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { "pid_accel_out_limit", COMMAND_ID_PID_ACCEL_OUT_LIMIT, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000} }, + //BARO P + [COMMAND_ID_PID_BARO_P_HEIGHT] = + { + "pid_baro_p_height", COMMAND_ID_PID_BARO_P_HEIGHT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 5, VAL_UINT_8, .valueRange = {0, 255} + }, + + //BARO I + [COMMAND_ID_PID_ACCEL_I_PITCH] = + { + "pid_baro_i_height", COMMAND_ID_PID_BARO_I_HEIGHT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 8, VAL_UINT_8, .valueRange = {0, 255} + }, + //BARO D + [COMMAND_ID_PID_ACCEL_D_PITCH] = + { + "pid_baro_d_height", COMMAND_ID_PID_ACCEL_D_PITCH, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 11, VAL_UINT_8, .valueRange = {0, 255} + }, + + //BARO Filters and limits + [COMMAND_ID_PID_BARO_DTERM_LPF] = + { + "pid_baro_dterm_lpf", COMMAND_ID_PID_BARO_DTERM_LPF, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 14, VAL_UINT_16, .valueRange = {0, 65000} + }, + [COMMAND_ID_PID_BARO_PTERM_YAW_LPF] = + { + "pid_baro_pterm_yaw_lpf", COMMAND_ID_PID_BARO_PTERM_YAW_LPF, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 16, VAL_UINT_16, .valueRange = {0, 65000} + }, + [COMMAND_ID_PID_BARO_YAW_P_LIMIT] = + { + "pid_baro_yaw_p_limit", COMMAND_ID_PID_BARO_YAW_P_LIMIT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 18, VAL_UINT_16, .valueRange = {0, 65000} + }, + [COMMAND_ID_PID_BARO_OUT_LIMIT] = + { + "pid_baro_out_limit", COMMAND_ID_PID_BARO_OUT_LIMIT, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 20, VAL_UINT_16, .valueRange = {0, 65000} + }, + /* Enable pid loops */ [COMMAND_ID_PID_GYRO_ISENABLED] = { @@ -668,6 +716,10 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { { "pid_accel_isenabled", COMMAND_ID_PID_ACCEL_ISENABLED, EEPROM_PID_ACCELEROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1} }, + [COMMAND_ID_PID_BARO_ISENABLED] = + { + "pid_baro_isenabled", COMMAND_ID_PID_BARO_ISENABLED, EEPROM_PID_BAROMETER, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_BOOL, .valueRange = {0, 1} + }, }; diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index 7852cc1..d727865 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -291,6 +291,16 @@ EEPROM_DATA_t eeprom_profile_Arr[EEPROM_PROFILE_COUNT] = { .size = sizeof(pidProfile_t), .dataPtr = &(PidProfile[PID_ID_ACCELEROMETER]), }, + [EEPROM_PID_COMPASS] = + { + .size = sizeof(pidProfile_t), + .dataPtr = &(PidProfile[PID_ID_COMPASS]), + }, + [EEPROM_PID_BAROMETER] = + { + .size = sizeof(pidProfile_t), + .dataPtr = &(PidProfile[PID_ID_BAROMETER]), + }, }; /* Data pointers and sizes for footer content */