diff --git a/UAV-ControlSystem/inc/Flight/pid.h b/UAV-ControlSystem/inc/Flight/pid.h index 205ac18..81b633e 100644 --- a/UAV-ControlSystem/inc/Flight/pid.h +++ b/UAV-ControlSystem/inc/Flight/pid.h @@ -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; /************************************************************************** diff --git a/UAV-ControlSystem/inc/drivers/sbus.h b/UAV-ControlSystem/inc/drivers/sbus.h index 8076b61..0e3d7e9 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 7143944..f5cccfc 100644 --- a/UAV-ControlSystem/src/Flight/pid.c +++ b/UAV-ControlSystem/src/Flight/pid.c @@ -15,20 +15,20 @@ #include "Flight/pid.h" #include "drivers/accel_gyro.h" +#include "drivers/sbus.h" #include -#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 { diff --git a/UAV-ControlSystem/src/drivers/sbus.c b/UAV-ControlSystem/src/drivers/sbus.c index e518afd..4eb3cb0 100644 --- a/UAV-ControlSystem/src/drivers/sbus.c +++ b/UAV-ControlSystem/src/drivers/sbus.c @@ -29,6 +29,8 @@ /* 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 = {0}; +rc_input_t rc_input = {0}; +float rc_rate = 1.0; /* Create a DMA Handler */ usart_dma_profile dmaHandler; @@ -94,6 +96,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 * @@ -158,6 +165,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; } } // Copy next byte into the sbus_arr diff --git a/UAV-ControlSystem/src/tasks_main.c b/UAV-ControlSystem/src/tasks_main.c index 0019ead..8366324 100644 --- a/UAV-ControlSystem/src/tasks_main.c +++ b/UAV-ControlSystem/src/tasks_main.c @@ -91,10 +91,10 @@ void systemTaskRx(void) } //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; }