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

Implementation of inclination compensation on throttle

Browse Source
This commit is contained in:
philsson 2016-11-29 17:10:54 +01:00
parent c82e0e5bf8
commit ff5eca5fd7
2 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -113,7 +113,7 @@ void getCurrentValues(float sensorValues[3], uint8_t ID_profile)
sensorValues[PITCH] = accelProfile.pitchAngle + accPitchFineTune;
/*Checks the biggest angle */
throttleRate = (ABS_FLOAT(sensorValues[ROLL]) > ABS_FLOAT(sensorValues[PITCH]) )? 2 - cos(sensorValues[ROLL]*M_PI/180) : 2 - cos(sensorValues[PITCH]*M_PI/180);
throttleRate = cos(ABS_FLOAT(sensorValues[PITCH])*M_PI/180)*cos(ABS_FLOAT(sensorValues[ROLL])*M_PI/180);
break;
case PID_ID_COMPASS:

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

@ -103,10 +103,12 @@ 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];//*throttleRate;
int16_t throttle = PidProfile[PID_ID_BAROMETER].PID_Out[THROTTLE];
if (PidProfile[PID_ID_BAROMETER].pidEnabled && flags_IsSet_ID(systemFlags_flightmode_barometer_id)) throttle += HoverForce;
throttle = (throttle - 1000)/throttleRate + 1000;
/* Mixer Full Scale enabled */
if (flags_IsSet_ID(systemFlags_mixerfullscale_id))
{