diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index 205ac18..81b633e 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -37,14 +37,6 @@ #define PID_ID_BAROMETER 13 -typedef struct -{ - int Roll; - int Pitch; - int Yaw; - int Throttle; -} control_data_t; - /*Struct that belongs to a certain PID controller*/ typedef struct pidProfile_s { @@ -79,8 +71,7 @@ typedef struct pidProfile_s { } pidProfile_t; -extern control_data_t RawRcCommand; -extern pidProfile_t PidGyroProfile, PidAccelerometerProfile; +extern pidProfile_t PidGyroProfile, PidAccelerometerProfile; /*Global variables to certain PID profiles*/ extern pidProfile_t PidCompassProfile, PidBarometerProfile; /************************************************************************** diff --git a/UAV-ControlSystem/inc/config/eeprom.h b/UAV-ControlSystem/inc/config/eeprom.h index 06536a4..ccd1e40 100644 --- a/UAV-ControlSystem/inc/config/eeprom.h +++ b/UAV-ControlSystem/inc/config/eeprom.h @@ -197,7 +197,22 @@ typedef enum { /* List of all profile EEPROM values */ typedef enum { - EEPROM_PID_ROLL_KP = 0, + 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, /* Counts the amount of settings in profile */ EEPROM_PROFILE_COUNT diff --git a/UAV-ControlSystem/inc/drivers/sbus.h b/UAV-ControlSystem/inc/drivers/sbus.h index 486268a..47630d7 100644 --- a/UAV-ControlSystem/inc/drivers/sbus.h +++ b/UAV-ControlSystem/inc/drivers/sbus.h @@ -18,6 +18,7 @@ **********************************************************************/ #include + #ifndef DRIVERS_SBUS_H_ #define DRIVERS_SBUS_H_ @@ -41,6 +42,7 @@ #define PWM_PULSE_MIN 750 // minimum PWM pulse considered valid input #define PWM_PULSE_MAX 2250 // maximum PWM pulse considered valid input +extern float rc_rate; /*********************************************************************** * BRIEF: Failsafe scenarios of the RX * @@ -107,9 +109,22 @@ typedef struct sbusFrame_s { } __attribute__ ((__packed__)) sbusFrame_s; +/* these are scaled RC Inputs centered around 0 */ +typedef struct rc_input_t { + int16_t Roll; + int16_t Pitch; + int16_t Yaw; + int16_t Throttle; +} rc_input_t; + + + /* This instance is read by the whole system and should contain actual RX data */ extern sbusFrame_s sbusChannelData; +/* these are scaled RC Inputs centered around 0 */ +extern rc_input_t rc_input; + /*********************************************************************** * BRIEF: Initializes the UART for sbus * * INFORMATION: A DMA Buffer starts storing the bytes received from RX * diff --git a/UAV-ControlSystem/src/Flight/pid.c b/UAV-ControlSystem/src/Flight/pid.c index 7143944..f5cccfc 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -15,20 +15,20 @@ #include "Flight/pid.h" #include "drivers/accel_gyro.h" +#include "drivers/sbus.h" #include -#define BAROMETER_RANGE 2000 -#define RADIO_RANGE 1000 -#define ACCELEROMETER_RANGE 90 // Accelerometer takes int to max 16 G -#define GYRO_RANGE 2000 -#define COMPASS_RANGE 360 +#define RADIO_RANGE 1000 /*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 GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/ +#define COMPASS_RANGE 360 /*Determines the maximum compass limit (limits the rc input)*/ -control_data_t RawRcCommand = {0}; -pidProfile_t PidGyroProfile = {0}, PidAccelerometerProfile = {0}; -pidProfile_t PidCompassProfile = {0}, PidBarometerProfile = {0}; +pidProfile_t PidGyroProfile = {0}, PidAccelerometerProfile = {0}; /*Struct profile for each PID controller*/ +pidProfile_t PidCompassProfile = {0}, PidBarometerProfile = {0}; /*Struct profile for each PID controller*/ -accel_t accelProfile; -gyro_t gyroProfile; +accel_t accelProfile; /*Struct profile for input data from sensor*/ +gyro_t gyroProfile; /*Struct profile for input data from sensor*/ /************************************************************************** * BRIEF: Constrain float values within a defined limit * @@ -114,14 +114,14 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile) break; case PID_ID_ACCELEROMETER: - desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Roll); - desiredCommand[PITCH] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Pitch); + desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, rc_input.Roll); + desiredCommand[PITCH] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, rc_input.Pitch); break; case PID_ID_COMPASS: - desiredCommand[0] = convertData(RADIO_RANGE, COMPASS_RANGE, 0, RawRcCommand.Yaw); /*desiredCommand must have an index of 0, since its only loop once*/ + desiredCommand[0] = convertData(RADIO_RANGE, COMPASS_RANGE, 0, rc_input.Yaw); /*desiredCommand must have an index of 0, since its only loop once*/ break; case PID_ID_BAROMETER: - desiredCommand[THROTTLE] = convertData(RADIO_RANGE,BAROMETER_RANGE,0, RawRcCommand.Throttle); + desiredCommand[THROTTLE] = convertData(RADIO_RANGE,BAROMETER_RANGE,0, rc_input.Throttle); break; default: break; @@ -205,7 +205,7 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) void pidInit() { - mpu6000_init(&gyroProfile,&accelProfile); + mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/ pidUAVInit(&PidGyroProfile, PID_ID_GYRO); pidUAVInit(&PidAccelerometerProfile, PID_ID_ACCELEROMETER); @@ -323,9 +323,9 @@ void pidRun(uint8_t ID) if (!PidGyroProfile.pidEnabled) { - PidGyroProfile.PID_Out[ROLL] = RawRcCommand.Roll; - PidGyroProfile.PID_Out[PITCH] = RawRcCommand.Pitch; - PidGyroProfile.PID_Out[YAW] = RawRcCommand.Yaw; + PidGyroProfile.PID_Out[ROLL] = rc_input.Roll; + PidGyroProfile.PID_Out[PITCH] = rc_input.Pitch; + PidGyroProfile.PID_Out[YAW] = rc_input.Yaw; } else { @@ -337,8 +337,8 @@ void pidRun(uint8_t ID) if (!PidAccelerometerProfile.pidEnabled) { - PidAccelerometerProfile.PID_Out[ROLL] = RawRcCommand.Roll; - PidAccelerometerProfile.PID_Out[PITCH] = RawRcCommand.Pitch; + PidAccelerometerProfile.PID_Out[ROLL] = rc_input.Roll; + PidAccelerometerProfile.PID_Out[PITCH] = rc_input.Pitch; } else { @@ -350,7 +350,7 @@ void pidRun(uint8_t ID) if (!PidCompassProfile.pidEnabled) { - PidCompassProfile.PID_Out[0] = RawRcCommand.Yaw; + PidCompassProfile.PID_Out[0] = rc_input.Yaw; } else { @@ -362,7 +362,7 @@ void pidRun(uint8_t ID) if (!PidBarometerProfile.pidEnabled) { - PidBarometerProfile.PID_Out[0] = RawRcCommand.Throttle; + PidBarometerProfile.PID_Out[0] = rc_input.Throttle; } else { diff --git a/UAV-ControlSystem/src/config/cli.c b/UAV-ControlSystem/src/config/cli.c index dcf7d2f..aa90ceb 100644 --- a/UAV-ControlSystem/src/config/cli.c +++ b/UAV-ControlSystem/src/config/cli.c @@ -230,6 +230,32 @@ typedef enum COMMAND_ID_FLAG_FLIGHTMODE_3_CHANNEL, + /* Profile values */ + + /* PID value */ + //GYRO + COMMAND_ID_PID_GYRO_P_ROLL, + COMMAND_ID_PID_GYRO_P_PITCH, + COMMAND_ID_PID_GYRO_P_YAW, + + COMMAND_ID_PID_GYRO_I_ROLL, + COMMAND_ID_PID_GYRO_I_PITCH, + COMMAND_ID_PID_GYRO_I_YAW, + + COMMAND_ID_PID_GYRO_D_ROLL, + COMMAND_ID_PID_GYRO_D_PITCH, + COMMAND_ID_PID_GYRO_D_YAW, + + //ACCEL + COMMAND_ID_PID_ACCEL_P_ROLL, + COMMAND_ID_PID_ACCEL_P_PITCH, + + COMMAND_ID_PID_ACCEL_I_ROLL, + COMMAND_ID_PID_ACCEL_I_PITCH, + + COMMAND_ID_PID_ACCEL_D_ROLL, + COMMAND_ID_PID_ACCEL_D_PITCH, + /* Counter for the amount of commands */ COMMAND_ID_COUNT, @@ -273,11 +299,6 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { { "adc_scales_right", COMMAND_ID_ADC_SCALES_RIGHT, EEPROM_ADC_SCALES, EEPROM_VALUE_TYPE_SYSTEM, 8, VAL_UINT_32, .valueRange = {0, 200} }, - [COMMAND_ID_PID_ROLL_KP] = - { - "pid_roll_kp", COMMAND_ID_PID_ROLL_KP, EEPROM_PID_ROLL_KP, EEPROM_VALUE_TYPE_PROFILE, 0, VAL_UINT_8, .valueRange = {0, 100} - }, - /* Calibrate motors */ [COMMAND_ID_MOTORCALIBRATE] = @@ -285,7 +306,6 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { "motor_calibration", COMMAND_ID_MOTORCALIBRATE, EEPROM_MOTORCALIBRATE, EEPROM_VALUE_TYPE_SYSTEM, 0, VAL_BOOL, .valueRange = {0, 1} }, - /* set period commands */ [COMMAND_ID_PERIOD_SYSTEM] = { @@ -497,6 +517,80 @@ const cliCommandConfig_t commandTable[COMMAND_ID_COUNT] = { "flag_flightmode_3_channel", COMMAND_ID_FLAG_FLIGHTMODE_3_CHANNEL, EEPROM_FLAG_FLIGHTMODE_3, EEPROM_VALUE_TYPE_SYSTEM, 4, VAL_UINT_8, .valueRange = {0, 18} }, + //PROFILE VALUES + //PID VALUES + //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} + }, + [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} + }, + [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} + }, + + //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} + }, + [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} + }, + [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} + }, + + //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} + }, + [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} + }, + [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} + }, + + //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} + }, + [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} + }, + + //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} + }, + [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} + }, + + //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} + }, + [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} + }, + }; diff --git a/UAV-ControlSystem/src/config/eeprom.c b/UAV-ControlSystem/src/config/eeprom.c index 45e73df..d378ef9 100644 --- a/UAV-ControlSystem/src/config/eeprom.c +++ b/UAV-ControlSystem/src/config/eeprom.c @@ -24,6 +24,7 @@ #include "drivers/failsafe_toggles.h" #include "drivers/motormix.h" #include "drivers/motors.h" +#include "Flight/pid.h" /* Reads the EEPROM version from EEPROM - Is compared to EEPROM_SYS_VERSION */ uint8_t stored_eeprom_identifier; @@ -254,11 +255,93 @@ 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] = { - [EEPROM_PID_ROLL_KP] = + //GYRO P VAL + [EEPROM_PID_GYRO_P_ROLL] = { - .size = sizeof(pid_pitch_pk), - .dataPtr = &pid_pitch_pk, - } + .size = sizeof(PidGyroProfile.P[0]), + .dataPtr = &PidGyroProfile.P[0], + }, + [EEPROM_PID_GYRO_P_PITCH] = + { + .size = sizeof(PidGyroProfile.P[1]), + .dataPtr = &PidGyroProfile.P[1], + }, + [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/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c index 8df2742..f61c0b0 100644 --- a/UAV-ControlSystem/src/drivers/sbus.c +++ b/UAV-ControlSystem/src/drivers/sbus.c @@ -29,6 +29,9 @@ /* This instance is read by the whole system and should contain actual RX data */ sbusFrame_s sbusChannelData = {0}; dma_usart_return raw_dma_data_t; +dma_usart_return raw_dma_data_t = {0}; +rc_input_t rc_input = {0}; +float rc_rate = 1.0; /* Create a DMA Handler */ usart_dma_profile dmaHandler; @@ -94,6 +97,11 @@ uint16_t rx_truncate(uint16_t rx_channel) return rx_channel; } +int16_t rc_input_mapping(float channel) +{ + return (int16_t)((channel-1500)*rc_rate); +} + /*********************************************************************** * BRIEF: Updates "sbusChannelData" * * INFORMATION: Is called by the scheduler * @@ -196,6 +204,11 @@ void sbus_read() sbusChannelData.chan6 = rx_truncate(sbusChannelData.chan6); sbusChannelData.chan7 = rx_truncate(sbusChannelData.chan7); sbusChannelData.chan8 = rx_truncate(sbusChannelData.chan8); + + rc_input.Roll = rc_input_mapping((float)sbusChannelData.chan1); + rc_input.Pitch = rc_input_mapping((float)sbusChannelData.chan2); + rc_input.Yaw = rc_input_mapping((float)sbusChannelData.chan4); + rc_input.Throttle = (int16_t)sbusChannelData.chan3; } else { @@ -218,7 +231,6 @@ void sbus_read() } } - } } diff --git a/UAV-ControlSystem/src/main.c b/UAV-ControlSystem/src/main.c index c91f172..b69cd1d 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -46,6 +46,8 @@ void init_system() //Configure the clock system_clock_config(); + pidInit(); + /* read saved variables from eeprom, in most cases eeprom should be read after a lot of the initializes */ readEEPROM(); @@ -59,7 +61,6 @@ void init_system() HAL_Delay(1000); pwmEnableAllMotors(Oneshot125); - pidInit(); #ifdef USE_LEDS //Initialize the on board leds diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 954a858..b40d9ac 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -94,10 +94,10 @@ void systemTaskRx(void) flags_ProcessRcChannel_Improved(STICK_CHANNEL_COUNT+1, STICK_CHANNEL_COUNT + AUX_CHANNEL_COUNT); //temporary send data from the RC directly form the RC - RawRcCommand.Roll = frame.chan1; - RawRcCommand.Pitch = frame.chan2; - RawRcCommand.Yaw = frame.chan4; - RawRcCommand.Throttle = frame.chan4; +// RawRcCommand.Roll = frame.chan1; +// RawRcCommand.Pitch = frame.chan2; +// RawRcCommand.Yaw = frame.chan4; +// RawRcCommand.Throttle = frame.chan4; }