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

Merge pull request #17 from MDHSweden/PID

Browse Source 
Pid fixed conflicts approved by Lennart
This commit is contained in:
Lennart Eriksson 2016-10-18 13:02:51 +02:00 committed by GitHub
commit f5603ecfea
9 changed files with 427 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

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

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 *

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

@ -14,6 +14,21 @@
* **************************************************************************/
#include "Flight/pid.h"
#include "drivers/accel_gyro.h"
#include "drivers/sbus.h"
#include <math.h>
#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;
}
}

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

5
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"
/**************************************************************************
@ -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();

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