Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-22 07:44:03 -04:00
Code Issues Packages Projects Releases Wiki Activity

new baro/throttle algorithms

Browse Source 
git-svn-id: https://arducopter.googlecode.com/svn/trunk@1099 f9c3cf11-9bcb-44bc-f272-b75c42450872
This commit is contained in:
jphelirc 2010-12-10 04:07:08 +00:00
parent c7a8291725
commit d8642aeaab
4 changed files with 34 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArducopterNG/Arducopter.h
View File

@ -556,16 +556,16 @@ void defaultUserConfig() {
KI_QUAD_YAW = 0.15;
STABLE_MODE_KP_RATE_YAW = 2.4;
STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW
KP_GPS_ROLL = 0.015;
KP_GPS_ROLL = 0.013;
KI_GPS_ROLL = 0.005;
KD_GPS_ROLL = 0.01;
KP_GPS_PITCH = 0.015;
KD_GPS_ROLL = 0.012;
KP_GPS_PITCH = 0.013;
KI_GPS_PITCH = 0.005;
KD_GPS_PITCH = 0.01;
GPS_MAX_ANGLE = 22;
KP_ALTITUDE = 0.8;
KI_ALTITUDE = 0.2;
KD_ALTITUDE = 0.2;
KI_ALTITUDE = 0.5;
KD_ALTITUDE = 0.6;
acc_offset_x = 2048;
acc_offset_y = 2048;
acc_offset_z = 2048;

9
ArducopterNG/ArducopterNG.pde
View File

@ -388,9 +388,14 @@ void loop()
{
ch_throttle_altitude_hold = Altitude_control_baro(press_alt,target_baro_altitude); // Altitude control
Baro_new_data=0;
Serial.println(ch_throttle_altitude_hold);
if (abs(ch_throttle-Initial_Throttle)>100) // Change in stick position => altitude ascend/descend rate control
target_baro_altitude += (ch_throttle-Initial_Throttle)/25;
//Serial.print(Initial_Throttle);
//Serial.print(" ");
//Serial.print(ch_throttle);
//Serial.print(" ");
//Serial.println(target_baro_altitude);
}
ch_throttle = ch_throttle_altitude_hold;
#endif
}
else // First time we enter in GPS position hold we capture the target position as the actual position

24
ArducopterNG/Motors.pde
View File

@ -38,8 +38,20 @@
// Send output commands to ESC´s
void motor_output()
{
if (ch_throttle < (MIN_THROTTLE + 100)) // If throttle is low we disable yaw (neccesary to arm/disarm motors safely)
control_yaw = 0;
int throttle;
byte throttle_mode=0;
throttle = ch_throttle;
#ifdef UseBMP
if (AP_mode == AP_AUTOMATIC_MODE)
{
throttle = ch_throttle_altitude_hold;
throttle_mode=1;
}
#endif
if ((throttle_mode==0)&&(ch_throttle < (MIN_THROTTLE + 100))) // If throttle is low we disable yaw (neccesary to arm/disarm motors safely)
control_yaw = 0;
// Quadcopter mix
if (motorArmed == 1) {
@ -48,10 +60,10 @@ void motor_output()
#endif
// Quadcopter output mix
rightMotor = constrain(ch_throttle - control_roll + control_yaw, minThrottle, 2000);
leftMotor = constrain(ch_throttle + control_roll + control_yaw, minThrottle, 2000);
frontMotor = constrain(ch_throttle + control_pitch - control_yaw, minThrottle, 2000);
backMotor = constrain(ch_throttle - control_pitch - control_yaw, minThrottle, 2000);
rightMotor = constrain(throttle - control_roll + control_yaw, minThrottle, 2000);
leftMotor = constrain(throttle + control_roll + control_yaw, minThrottle, 2000);
frontMotor = constrain(throttle + control_pitch - control_yaw, minThrottle, 2000);
backMotor = constrain(throttle - control_pitch - control_yaw, minThrottle, 2000);
} else { // MOTORS DISARMED

7
ArducopterNG/Navigation.pde
View File

@ -111,6 +111,7 @@ int Altitude_control_baro(int altitude, int target_altitude)
#define ALTITUDE_CONTROL_BARO_OUTPUT_MIN 40
#define ALTITUDE_CONTROL_BARO_OUTPUT_MAX 80
// !!!!! REMOVE THIS !!!!!!!
#define KP_BARO_ALTITUDE 0.25 //0.3
#define KD_BARO_ALTITUDE 0.09 //0.09
#define KI_BARO_ALTITUDE 0.1
@ -121,8 +122,8 @@ int Altitude_control_baro(int altitude, int target_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,-120,120);
command_altitude = KP_BARO_ALTITUDE*err_altitude + KD_BARO_ALTITUDE*altitude_D + KI_BARO_ALTITUDE*altitude_I;
altitude_I = constrain(altitude_I,-140,140);
command_altitude = KP_ALTITUDE*err_altitude + KD_ALTITUDE*altitude_D + KI_ALTITUDE*altitude_I;
command_altitude = Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_BARO_OUTPUT_MIN,ALTITUDE_CONTROL_BARO_OUTPUT_MAX);
return command_altitude;
}
@ -148,4 +149,4 @@ int Altitude_control_Sonar(int Sonar_altitude, int target_sonar_altitude)
return (Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_SONAR_OUTPUT_MIN,ALTITUDE_CONTROL_SONAR_OUTPUT_MAX));
}