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

Merge remote-tracking branch 'origin/PID' into PID

Browse Source 
# Conflicts:
#	UAV-ControlSystem/src/config/cli.c
#	UAV-ControlSystem/src/config/eeprom.c
#	UAV-ControlSystem/src/drivers/sbus.c
This commit is contained in:
Lennart Eriksson 2016-10-18 12:20:07 +02:00
commit ea48b3560b
9 changed files with 260 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;
/**************************************************************************

17
UAV-ControlSystem/inc/config/eeprom.h
View File

@ -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

15
UAV-ControlSystem/inc/drivers/sbus.h
View File

@ -18,6 +18,7 @@
**********************************************************************/
#include <stdint.h>
#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 *

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

@ -15,20 +15,20 @@
#include "Flight/pid.h"
#include "drivers/accel_gyro.h"
#include "drivers/sbus.h"
#include <math.h>
#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
{

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

@ -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}
},
};

91
UAV-ControlSystem/src/config/eeprom.c
View File

@ -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 */

14
UAV-ControlSystem/src/drivers/sbus.c
View File

@ -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()
}
}
}
}

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

@ -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

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

@ -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;
}