diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index 04cc152..81b633e 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -31,9 +31,16 @@ #define PITCH 1 /*Index terms to the PID*/ #define YAW 2 /*Index terms to the PID*/ +#define PID_ID_GYRO 10 +#define PID_ID_ACCELEROMETER 11 +#define PID_ID_COMPASS 12 +#define PID_ID_BAROMETER 13 + + /*Struct that belongs to a certain PID controller*/ typedef struct pidProfile_s { + bool pidEnabled; bool pidStabilisationEnabled; /*Enables/Dissables PID controller*/ uint8_t ID_profile; /*ID of a certain PID, shall be referenced to a certain sensor*/ @@ -63,6 +70,10 @@ typedef struct pidProfile_s { } pidProfile_t; + +extern pidProfile_t PidGyroProfile, PidAccelerometerProfile; /*Global variables to certain PID profiles*/ +extern pidProfile_t PidCompassProfile, PidBarometerProfile; + /************************************************************************** * BRIEF: Constrain float values within a defined limit * * INFORMATION: Used in PID loop to limit values * @@ -81,11 +92,17 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile); **************************************************************************/ void getPointRate(float *desiredCommand, uint8_t ID_profile); +/************************************************************************** +* BRIEF: Initializes PID profiles * +* INFORMATION: * +**************************************************************************/ +void pidInit(); + /************************************************************************** * BRIEF: Initializes the pid profile PID controller * * INFORMATION: Recommended to use if unexpected values occur of profile * **************************************************************************/ -void pidUAVInit(pidProfile_t *pidProfile); +void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID); /************************************************************************** * BRIEF: Dynamic PID controller, able to handle several PID controller * @@ -94,4 +111,10 @@ void pidUAVInit(pidProfile_t *pidProfile); **************************************************************************/ void pidUAV(pidProfile_t *pidProfile); +/************************************************************************** +* BRIEF: Runs a certain PID Controller * +* INFORMATION: * +**************************************************************************/ +void pidRun(uint8_t ID); + #endif /* FLIGHT_PID_H_ */ 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 494376f..f5cccfc 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -14,6 +14,21 @@ * **************************************************************************/ #include "Flight/pid.h" +#include "drivers/accel_gyro.h" +#include "drivers/sbus.h" +#include + +#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)*/ + +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; /*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 * @@ -35,29 +50,36 @@ float constrainf(float amt, int low, int high) **************************************************************************/ void getCurrentValues(float *sensorValues, uint8_t ID_profile) { - //*Do something smart*// - switch (ID_profile) { - case 1: - sensorValues[ROLL] = 0; - sensorValues[PITCH] = 0; - sensorValues[YAW] = 0; + case PID_ID_GYRO: + mpu6000_read_gyro(&gyroProfile); + + sensorValues[ROLL] = gyroProfile.gyroY; + sensorValues[PITCH] = gyroProfile.gyroX; + sensorValues[YAW] = gyroProfile.gyroZ; break; - case 2: + case PID_ID_ACCELEROMETER: + mpu6000_read_accel(&accelProfile); + + /*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[PITCH] = atan2(-accelProfile.accelXconv, sqrt(accelProfile.accelYconv*accelProfile.accelYconv + accelProfile.accelZconv*accelProfile.accelZconv))*180/M_PI; break; - case 3: + case PID_ID_COMPASS: break; - case 4: - break; - case 5: + case PID_ID_BAROMETER: break; default: break; } - }; +float convertData(int inputRange, int outputRange, int offset, float value) +{ + return (outputRange/inputRange)*(value-offset); +} + /************************************************************************** * BRIEF: Update desired values from rc command * * INFORMATION: * @@ -65,35 +87,58 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile) void getPointRate(float *desiredCommand, uint8_t ID_profile) { //*Do something smart*// - switch (ID_profile) + switch(ID_profile) { - case 1: - desiredCommand[ROLL] = 0; - desiredCommand[PITCH] = 0; - desiredCommand[YAW] = 0; + case PID_ID_GYRO: + + if (!PidAccelerometerProfile.pidEnabled) + { + desiredCommand[ROLL] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[ROLL]); + desiredCommand[PITCH] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[PITCH]); + } + else + { + desiredCommand[ROLL] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[ROLL]); + desiredCommand[PITCH] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidAccelerometerProfile.PID_Out[PITCH]); + } + + + if (!PidCompassProfile.pidEnabled) + { + desiredCommand[YAW] = convertData(RADIO_RANGE, GYRO_RANGE, 0, PidCompassProfile.PID_Out[0]); + } + else + { + desiredCommand[YAW] = convertData(ACCELEROMETER_RANGE, GYRO_RANGE, 0, PidCompassProfile.PID_Out[0]); + } + break; - case 2: + case PID_ID_ACCELEROMETER: + desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, rc_input.Roll); + desiredCommand[PITCH] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, rc_input.Pitch); break; - case 3: + case PID_ID_COMPASS: + 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 4: - break; - case 5: + case PID_ID_BAROMETER: + desiredCommand[THROTTLE] = convertData(RADIO_RANGE,BAROMETER_RANGE,0, rc_input.Throttle); break; default: break; } } + /************************************************************************** * BRIEF: Initializes the pid profile PID controller * * INFORMATION: Recommended to use if unexpected values occur of profile * **************************************************************************/ -void pidUAVInit(pidProfile_t *pidProfile) +void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID) { /*This is recommended init settings*/ - pidProfile->ID_profile = 0; + pidProfile->ID_profile = ID; + pidProfile->pidEnabled = true; pidProfile-> pidStabilisationEnabled = true; pidProfile->DOF = 0; @@ -153,6 +198,21 @@ void pidUAVInit(pidProfile_t *pidProfile) } +/************************************************************************** +* BRIEF: Initializes PID profiles * +* INFORMATION: * +**************************************************************************/ +void pidInit() +{ + + mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/ + + pidUAVInit(&PidGyroProfile, PID_ID_GYRO); + pidUAVInit(&PidAccelerometerProfile, PID_ID_ACCELEROMETER); + pidUAVInit(&PidCompassProfile, PID_ID_COMPASS); + pidUAVInit(&PidBarometerProfile, PID_ID_BAROMETER); +} + /************************************************************************** * BRIEF: Dynamic PID controller, able to handle several PID controller * * connected to different profiles. @@ -249,3 +309,68 @@ void pidUAV(pidProfile_t *pidProfile) pidProfile->PID_Out[axis] = PID_Out[axis]; } } + +/************************************************************************** +* BRIEF: Runs a certain PID Controller * +* INFORMATION: * +**************************************************************************/ +void pidRun(uint8_t ID) +{ + switch(ID) + { + + case PID_ID_GYRO: + + if (!PidGyroProfile.pidEnabled) + { + PidGyroProfile.PID_Out[ROLL] = rc_input.Roll; + PidGyroProfile.PID_Out[PITCH] = rc_input.Pitch; + PidGyroProfile.PID_Out[YAW] = rc_input.Yaw; + } + else + { + pidUAV(&PidGyroProfile); + } + + break; + case PID_ID_ACCELEROMETER: + + if (!PidAccelerometerProfile.pidEnabled) + { + PidAccelerometerProfile.PID_Out[ROLL] = rc_input.Roll; + PidAccelerometerProfile.PID_Out[PITCH] = rc_input.Pitch; + } + else + { + pidUAV(&PidAccelerometerProfile); + } + + break; + case PID_ID_COMPASS: + + if (!PidCompassProfile.pidEnabled) + { + PidCompassProfile.PID_Out[0] = rc_input.Yaw; + } + else + { + pidUAV(&PidCompassProfile); + } + + break; + case PID_ID_BAROMETER: + + if (!PidBarometerProfile.pidEnabled) + { + PidBarometerProfile.PID_Out[0] = rc_input.Throttle; + } + else + { + pidUAV(&PidBarometerProfile); + } + + break; + default: + break; + } +} 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 73b8a41..b69cd1d 100644 --- a/UAV-ControlSystem/src/main.c +++ b/UAV-ControlSystem/src/main.c @@ -27,6 +27,7 @@ #include "drivers/sbus.h" #include "drivers/motormix.h" #include "drivers/motors.h" +#include "Flight/pid.h" /************************************************************************** @@ -45,10 +46,11 @@ 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(); - //initialize the CLI NOTE: Cant use the same usart as anything else or there will be some big trouble cliInit(USART6); @@ -59,6 +61,7 @@ void init_system() HAL_Delay(1000); pwmEnableAllMotors(Oneshot125); + #ifdef USE_LEDS //Initialize the on board leds ledReavoEnable(); diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index beffa23..b40d9ac 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -40,17 +40,30 @@ #include "drivers/I2C.h" #include "drivers/accel_gyro.h" #include "drivers/motormix.h" +#include "Flight/pid.h" void systemTaskGyroPid(void) { //Read gyro and update PID and finally update the motors. The most important task in the system + //Update Gyro + + //Convert? + + //PID Gyro + pidRun(PID_ID_GYRO); + + //MIX GO + + //call the motor mix mix(); } void systemTaskAccelerometer(void) { + + pidRun(PID_ID_ACCELEROMETER); //update the accelerometer data // uint8_t c = 97; // usart_transmit(&cliUsart, &c, 1, 1000000000); @@ -81,9 +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 - PID_Out[0] = frame.chan1 - 1500; - PID_Out[1] = frame.chan2 - 1500; - PID_Out[2] = frame.chan4 - 1500; +// RawRcCommand.Roll = frame.chan1; +// RawRcCommand.Pitch = frame.chan2; +// RawRcCommand.Yaw = frame.chan4; +// RawRcCommand.Throttle = frame.chan4; } @@ -158,12 +172,12 @@ void systemTaskBattery(void) void systemTaskBaro(void) { - //Obtain the barometer data + pidRun(PID_ID_BAROMETER); } void systemTaskCompass(void) { - //Obtain compass data + pidRun(PID_ID_COMPASS); } void systemTaskGps(void)