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

PID functions

Browse Source 
Added new global variables and functions to the PID controller which is
supposed to sync with the main program
This commit is contained in:
johan9107 2016-10-14 10:27:46 +02:00
parent de4348c385
commit a9bde4513a
4 changed files with 151 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -31,9 +31,24 @@
#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
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 {
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 +78,11 @@ typedef struct pidProfile_s {
} pidProfile_t;
extern control_data_t RawRcCommand;
extern pidProfile_t PidGyroProfile, PidAccelerometerProfile;
extern pidProfile_t PidCompassProfile, PidBarometerProfile;
/**************************************************************************
* BRIEF: Constrain float values within a defined limit *
* INFORMATION: Used in PID loop to limit values *
@ -81,6 +101,12 @@ 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 *
@ -94,4 +120,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_ */

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

@ -15,6 +15,17 @@
#include "Flight/pid.h"
#define BAROMETER_RANGE 2000
#define RADIO_RANGE 1000
#define ACCELEROMETER_RANGE 4000
#define GYRO_RANGE 32
#define COMPASS_RANGE 360
control_data_t RawRcCommand = {0};
pidProfile_t PidGyroProfile = {0}, PidAccelerometerProfile = {0};
pidProfile_t PidCompassProfile = {0}, PidBarometerProfile = {0};
/**************************************************************************
* BRIEF: Constrain float values within a defined limit *
* INFORMATION: Used in PID loop to limit values *
@ -58,6 +69,11 @@ void getCurrentValues(float *sensorValues, uint8_t ID_profile)
};
float convertData(int inputRange, int outputRange, int offset, float value)
{
return (outputRange/inputRange)*(value-offset);
}
/**************************************************************************
* BRIEF: Update desired values from rc command *
* INFORMATION: *
@ -65,26 +81,29 @@ 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:
desiredCommand[ROLL] = PidAccelerometerProfile.PID_Out[ROLL];
desiredCommand[PITCH] = PidAccelerometerProfile.PID_Out[PITCH];
desiredCommand[YAW] = PidCompassProfile.PID_Out[0];
break;
case 2:
case PID_ID_ACCELEROMETER:
desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Roll);
desiredCommand[PITCH] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Pitch);
break;
case 3:
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*/
break;
case 4:
break;
case 5:
case PID_ID_BAROMETER:
desiredCommand[THROTTLE] = convertData(RADIO_RANGE,BAROMETER_RANGE,0, RawRcCommand.Throttle);
break;
default:
break;
}
}
/**************************************************************************
* BRIEF: Initializes the pid profile PID controller *
* INFORMATION: Recommended to use if unexpected values occur of profile *
@ -153,6 +172,16 @@ void pidUAVInit(pidProfile_t *pidProfile)
}
/**************************************************************************
* BRIEF: Initializes PID profiles *
* INFORMATION: *
**************************************************************************/
void pidInit()
{
int test;
}
/**************************************************************************
* BRIEF: Dynamic PID controller, able to handle several PID controller *
* connected to different profiles.
@ -249,3 +278,61 @@ 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:
pidUAV(&PidGyroProfile);
break;
case PID_ID_ACCELEROMETER:
if (!PidAccelerometerProfile.pidEnabled)
{
PidAccelerometerProfile.PID_Out[ROLL] = RawRcCommand.Roll;
PidAccelerometerProfile.PID_Out[PITCH] = RawRcCommand.Pitch;
}
else
{
pidUAV(&PidAccelerometerProfile);
}
break;
case PID_ID_COMPASS:
if (!PidCompassProfile.pidEnabled)
{
PidCompassProfile.PID_Out[0] = RawRcCommand.Yaw;
}
else
{
pidUAV(&PidCompassProfile);
}
break;
case PID_ID_BAROMETER:
if (!PidBarometerProfile.pidEnabled)
{
PidBarometerProfile.PID_Out[0] = RawRcCommand.Throttle;
}
else
{
//convertData(1000, 3000, 0, );
pidUAV(&PidBarometerProfile);
}
break;
default:
break;
}
}

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

@ -27,6 +27,7 @@
#include "drivers/sbus.h"
#include "drivers/motormix.h"
#include "drivers/motors.h"
#include "Flight/pid.h"
/**************************************************************************
* BRIEF: Should contain all the initializations of the system, needs to
@ -47,7 +48,6 @@ void init_system()
/* 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(USART3);
@ -57,6 +57,8 @@ void init_system()
//init motors to run with oneshot 125
pwmEnableAllMotors(Oneshot125);
pidInit();
#ifdef USE_LEDS
//Initialize the on board leds
ledReavoEnable();

24
UAV-ControlSystem/src/tasks_main.c
View File

@ -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);
@ -78,9 +91,10 @@ void systemTaskRx(void)
}
//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;
}
@ -155,12 +169,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)