aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Added altitude control function based on baro (not activated yet)

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@834 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jjulio1234 2010-11-13 17:48:46 +00:00
parent c0c28f8eae
commit 069422c086
1 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

24
ArducopterNG/Navigation.pde
View File

@ -102,4 +102,28 @@ void Reset_I_terms_navigation()
gps_pitch_I = 0;
}
/* ************************************************************ */
/* Altitude control... (based on barometer) */
int Altitude_control_baro(int altitude, int target_altitude)
{
#define ALTITUDE_CONTROL_BARO_OUTPUT_MIN 50
#define ALTITUDE_CONTROL_BARO_OUTPUT_MAX 90
#define KP_BARO_ALTITUDE 0.65
#define KD_BARO_ALTITUDE 0.05
#define KI_BARO_ALTITUDE 0.1
int command_altitude;
err_altitude_old = err_altitude;
err_altitude = target_altitude - altitude;
altitude_D = (float)(err_altitude-err_altitude_old)/0.05; // 20Hz
altitude_I += (float)err_altitude*0.05;
altitude_I = constrain(altitude_I,-100,100);
command_altitude = KP_BARO_ALTITUDE*err_altitude + KD_BARO_ALTITUDE*altitude_D + KI_BARO_ALTITUDE*altitude_I;
command_altitude = Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_BARO_OUTPUT_MIN,ALTITUDE_CONTROL_BARO_OUTPUT_MAX);
return command_altitude;
}