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

PID new rc input

Browse Source 
added a new gloabal functions to get rc input from radio (rc_input)
This commit is contained in:
johan9107 2016-10-18 11:02:23 +02:00
parent 4bb2d6fdaf
commit 8031d48cb0
5 changed files with 54 additions and 36 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 #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*/ /*Struct that belongs to a certain PID controller*/
typedef struct pidProfile_s { typedef struct pidProfile_s {
@ -79,8 +71,7 @@ typedef struct pidProfile_s {
} pidProfile_t; } pidProfile_t;
extern control_data_t RawRcCommand; extern pidProfile_t PidGyroProfile, PidAccelerometerProfile; /*Global variables to certain PID profiles*/
extern pidProfile_t PidGyroProfile, PidAccelerometerProfile;
extern pidProfile_t PidCompassProfile, PidBarometerProfile; extern pidProfile_t PidCompassProfile, PidBarometerProfile;
/************************************************************************** /**************************************************************************

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

@ -18,6 +18,7 @@
**********************************************************************/ **********************************************************************/
#include <stdint.h> #include <stdint.h>
#ifndef DRIVERS_SBUS_H_ #ifndef DRIVERS_SBUS_H_
#define 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_MIN 750 // minimum PWM pulse considered valid input
#define PWM_PULSE_MAX 2250 // maximum 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 * * BRIEF: Failsafe scenarios of the RX *
@ -107,9 +109,22 @@ typedef struct sbusFrame_s {
} __attribute__ ((__packed__)) 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 */ /* This instance is read by the whole system and should contain actual RX data */
extern sbusFrame_s sbusChannelData; extern sbusFrame_s sbusChannelData;
/* these are scaled RC Inputs centered around 0 */
extern rc_input_t rc_input;
/*********************************************************************** /***********************************************************************
* BRIEF: Initializes the UART for sbus * * BRIEF: Initializes the UART for sbus *
* INFORMATION: A DMA Buffer starts storing the bytes received from RX * * 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 "Flight/pid.h"
#include "drivers/accel_gyro.h" #include "drivers/accel_gyro.h"
#include "drivers/sbus.h"
#include <math.h> #include <math.h>
#define BAROMETER_RANGE 2000 #define RADIO_RANGE 1000 /*Radio range input*/
#define RADIO_RANGE 1000 #define BAROMETER_RANGE 2000 /*Determines the range of the maximum height (limits the rc input)*/
#define ACCELEROMETER_RANGE 90 // Accelerometer takes int to max 16 G #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 2000 #define GYRO_RANGE 720 /*Determines the maximum rotational limit (limits the rc input)*/
#define COMPASS_RANGE 360 #define COMPASS_RANGE 360 /*Determines the maximum compass limit (limits the rc input)*/
control_data_t RawRcCommand = {0}; pidProfile_t PidGyroProfile = {0}, PidAccelerometerProfile = {0}; /*Struct profile for each PID controller*/
pidProfile_t PidGyroProfile = {0}, PidAccelerometerProfile = {0}; pidProfile_t PidCompassProfile = {0}, PidBarometerProfile = {0}; /*Struct profile for each PID controller*/
pidProfile_t PidCompassProfile = {0}, PidBarometerProfile = {0};
accel_t accelProfile; accel_t accelProfile; /*Struct profile for input data from sensor*/
gyro_t gyroProfile; gyro_t gyroProfile; /*Struct profile for input data from sensor*/
/************************************************************************** /**************************************************************************
* BRIEF: Constrain float values within a defined limit * * BRIEF: Constrain float values within a defined limit *
@ -114,14 +114,14 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
break; break;
case PID_ID_ACCELEROMETER: case PID_ID_ACCELEROMETER:
desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Roll); desiredCommand[ROLL] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, rc_input.Roll);
desiredCommand[PITCH] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, RawRcCommand.Pitch); desiredCommand[PITCH] = convertData(RADIO_RANGE, ACCELEROMETER_RANGE, 0, rc_input.Pitch);
break; break;
case PID_ID_COMPASS: 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; break;
case PID_ID_BAROMETER: 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; break;
default: default:
break; break;
@ -205,7 +205,7 @@ void pidUAVInit(pidProfile_t *pidProfile, uint8_t ID)
void pidInit() void pidInit()
{ {
mpu6000_init(&gyroProfile,&accelProfile); mpu6000_init(&gyroProfile,&accelProfile); /*Init gyro and accelerometer*/
pidUAVInit(&PidGyroProfile, PID_ID_GYRO); pidUAVInit(&PidGyroProfile, PID_ID_GYRO);
pidUAVInit(&PidAccelerometerProfile, PID_ID_ACCELEROMETER); pidUAVInit(&PidAccelerometerProfile, PID_ID_ACCELEROMETER);
@ -323,9 +323,9 @@ void pidRun(uint8_t ID)
if (!PidGyroProfile.pidEnabled) if (!PidGyroProfile.pidEnabled)
{ {
PidGyroProfile.PID_Out[ROLL] = RawRcCommand.Roll; PidGyroProfile.PID_Out[ROLL] = rc_input.Roll;
PidGyroProfile.PID_Out[PITCH] = RawRcCommand.Pitch; PidGyroProfile.PID_Out[PITCH] = rc_input.Pitch;
PidGyroProfile.PID_Out[YAW] = RawRcCommand.Yaw; PidGyroProfile.PID_Out[YAW] = rc_input.Yaw;
} }
else else
{ {
@ -337,8 +337,8 @@ void pidRun(uint8_t ID)
if (!PidAccelerometerProfile.pidEnabled) if (!PidAccelerometerProfile.pidEnabled)
{ {
PidAccelerometerProfile.PID_Out[ROLL] = RawRcCommand.Roll; PidAccelerometerProfile.PID_Out[ROLL] = rc_input.Roll;
PidAccelerometerProfile.PID_Out[PITCH] = RawRcCommand.Pitch; PidAccelerometerProfile.PID_Out[PITCH] = rc_input.Pitch;
} }
else else
{ {
@ -350,7 +350,7 @@ void pidRun(uint8_t ID)
if (!PidCompassProfile.pidEnabled) if (!PidCompassProfile.pidEnabled)
{ {
PidCompassProfile.PID_Out[0] = RawRcCommand.Yaw; PidCompassProfile.PID_Out[0] = rc_input.Yaw;
} }
else else
{ {
@ -362,7 +362,7 @@ void pidRun(uint8_t ID)
if (!PidBarometerProfile.pidEnabled) if (!PidBarometerProfile.pidEnabled)
{ {
PidBarometerProfile.PID_Out[0] = RawRcCommand.Throttle; PidBarometerProfile.PID_Out[0] = rc_input.Throttle;
} }
else else
{ {

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

@ -29,6 +29,8 @@
/* This instance is read by the whole system and should contain actual RX data */ /* This instance is read by the whole system and should contain actual RX data */
sbusFrame_s sbusChannelData = {0}; sbusFrame_s sbusChannelData = {0};
dma_usart_return raw_dma_data_t = {0}; dma_usart_return raw_dma_data_t = {0};
rc_input_t rc_input = {0};
float rc_rate = 1.0;
/* Create a DMA Handler */ /* Create a DMA Handler */
usart_dma_profile dmaHandler; usart_dma_profile dmaHandler;
@ -94,6 +96,11 @@ uint16_t rx_truncate(uint16_t rx_channel)
return rx_channel; return rx_channel;
} }
int16_t rc_input_mapping(float channel)
{
return (int16_t)((channel-1500)*rc_rate);
}
/*********************************************************************** /***********************************************************************
* BRIEF: Updates "sbusChannelData" * * BRIEF: Updates "sbusChannelData" *
* INFORMATION: Is called by the scheduler * * INFORMATION: Is called by the scheduler *
@ -158,6 +165,11 @@ void sbus_read()
sbusChannelData.chan6 = rx_truncate(sbusChannelData.chan6); sbusChannelData.chan6 = rx_truncate(sbusChannelData.chan6);
sbusChannelData.chan7 = rx_truncate(sbusChannelData.chan7); sbusChannelData.chan7 = rx_truncate(sbusChannelData.chan7);
sbusChannelData.chan8 = rx_truncate(sbusChannelData.chan8); 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;
} }
} }
// Copy next byte into the sbus_arr // Copy next byte into the sbus_arr

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

@ -91,10 +91,10 @@ void systemTaskRx(void)
} }
//temporary send data from the RC directly form the RC //temporary send data from the RC directly form the RC
RawRcCommand.Roll = frame.chan1; // RawRcCommand.Roll = frame.chan1;
RawRcCommand.Pitch = frame.chan2; // RawRcCommand.Pitch = frame.chan2;
RawRcCommand.Yaw = frame.chan4; // RawRcCommand.Yaw = frame.chan4;
RawRcCommand.Throttle = frame.chan4; // RawRcCommand.Throttle = frame.chan4;
} }