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

Fixed throttlerate, added constrain so that we cant lower altitude to fast with barometer flight.

Browse Source
This commit is contained in:
Jonas Holmberg 2016-11-10 13:30:24 +01:00
parent abe3af0f46
commit aa93a954d8
3 changed files with 12 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -65,7 +65,10 @@ extern pidProfile_t PidProfile[PID_COUNT];
extern float accRollFineTune;
extern float accPitchFineTune;
extern accel_t accelProfile; /*Struct profile for input data from sensor*/
extern accel_t accelProfile;
extern float throttleRate;
extern int HoverForce;/*Struct profile for input data from sensor*/

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

@ -62,10 +62,11 @@ gyro_t gyroProfile = {0}; /*Struct profile for input data from senso
pt1Filter_t accelFilter[2] = {0};
float throttleRate = 1;
float accRollFineTune = 0;
float accPitchFineTune = 0;
float throttleRate = 1;
int HoverForce = 1485;/*Struct profile for input data from sensor*/
/**************************************************************************
* BRIEF: Scales data from input range to output range *
@ -101,7 +102,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune;
/*Checks the biggest angle */
throttleRate = 2 - (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? cos(sensorValues[ROLL]*M_PI/180) : cos(sensorValues[PITCH]*M_PI/180);
throttleRate = (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? 2 - cos(sensorValues[ROLL]*M_PI/180) : 2 - cos(sensorValues[PITCH]*M_PI/180);
break;
case PID_ID_COMPASS:
@ -175,7 +176,7 @@ void getPointRate(float *desiredCommand, uint8_t ID_profile)
//desiredCommand[THROTTLE] = convertData(RADIO_RANGE, BAROMETER_RANGE, 0, rc_input.Throttle*throttleRate);
desiredCommand[THROTTLE] = 1*BAROMETER_SCALE;
desiredCommand[THROTTLE] = 5*BAROMETER_SCALE;
break;
default:
@ -345,12 +346,12 @@ void pidRun(uint8_t ID)
if (!(PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)))
{
PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle*throttleRate;
PidProfile[PID_ID_BAROMETER].PID_Out[0] = rc_input.Throttle;
}
else
{
pidUAV(&PidProfile[PID_ID_BAROMETER], &PidProfileBuff[PID_ID_BAROMETER]);
PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -(int)pidProfile->pid_out_limit, (int)pidProfile->pid_out_limit);
PidProfile[PID_ID_BAROMETER].PID_Out[0] = constrainf(PidProfile[PID_ID_BAROMETER].PID_Out[0], -1, (int)PidProfile[PID_ID_BAROMETER].pid_out_limit);
}

4
UAV-ControlSystem/src/drivers/motormix.c
View File

@ -103,9 +103,9 @@ void mix()
int16_t RPY_Mix[MOTOR_COUNT]; // Roll Pitch and Yaw variables array
int16_t RPY_Mix_Min = 0; // Stores the minimum desired command for any motor
int16_t RPY_Mix_Max = 0; // Maximum desired command for any motor
int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE];
int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE]*throttleRate;
if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += 1482;
if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += HoverForce;
/* Mixer Full Scale enabled */
if (flags_IsSet_ID(systemFlags_mixerfullscale_id))