Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 00:04:02 -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; KI_QUAD_YAW = 0.15;
STABLE_MODE_KP_RATE_YAW = 2.4; STABLE_MODE_KP_RATE_YAW = 2.4;
STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW STABLE_MODE_KP_RATE = 0.2; // NOT USED NOW
KP_GPS_ROLL = 0.015; KP_GPS_ROLL = 0.013;
KI_GPS_ROLL = 0.005; KI_GPS_ROLL = 0.005;
KD_GPS_ROLL = 0.01; KD_GPS_ROLL = 0.012;
KP_GPS_PITCH = 0.015; KP_GPS_PITCH = 0.013;
KI_GPS_PITCH = 0.005; KI_GPS_PITCH = 0.005;
KD_GPS_PITCH = 0.01; KD_GPS_PITCH = 0.01;
GPS_MAX_ANGLE = 22; GPS_MAX_ANGLE = 22;
KP_ALTITUDE = 0.8; KP_ALTITUDE = 0.8;
KI_ALTITUDE = 0.2; KI_ALTITUDE = 0.5;
KD_ALTITUDE = 0.2; KD_ALTITUDE = 0.6;
acc_offset_x = 2048; acc_offset_x = 2048;
acc_offset_y = 2048; acc_offset_y = 2048;
acc_offset_z = 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 ch_throttle_altitude_hold = Altitude_control_baro(press_alt,target_baro_altitude); // Altitude control
Baro_new_data=0; 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 #endif
} }
else // First time we enter in GPS position hold we capture the target position as the actual position else // First time we enter in GPS position hold we capture the target position as the actual position

22
ArducopterNG/Motors.pde
View File

@ -38,7 +38,19 @@
// Send output commands to ESC´s // Send output commands to ESC´s
void motor_output() void motor_output()
{ {
if (ch_throttle < (MIN_THROTTLE + 100)) // If throttle is low we disable yaw (neccesary to arm/disarm motors safely) 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; control_yaw = 0;
// Quadcopter mix // Quadcopter mix
@ -48,10 +60,10 @@ void motor_output()
#endif #endif
// Quadcopter output mix // Quadcopter output mix
rightMotor = constrain(ch_throttle - control_roll + control_yaw, minThrottle, 2000); rightMotor = constrain(throttle - control_roll + control_yaw, minThrottle, 2000);
leftMotor = constrain(ch_throttle + control_roll + control_yaw, minThrottle, 2000); leftMotor = constrain(throttle + control_roll + control_yaw, minThrottle, 2000);
frontMotor = constrain(ch_throttle + control_pitch - control_yaw, minThrottle, 2000); frontMotor = constrain(throttle + control_pitch - control_yaw, minThrottle, 2000);
backMotor = constrain(ch_throttle - control_pitch - control_yaw, minThrottle, 2000); backMotor = constrain(throttle - control_pitch - control_yaw, minThrottle, 2000);
} else { // MOTORS DISARMED } else { // MOTORS DISARMED

5
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_MIN 40
#define ALTITUDE_CONTROL_BARO_OUTPUT_MAX 80 #define ALTITUDE_CONTROL_BARO_OUTPUT_MAX 80
// !!!!! REMOVE THIS !!!!!!!
#define KP_BARO_ALTITUDE 0.25 //0.3 #define KP_BARO_ALTITUDE 0.25 //0.3
#define KD_BARO_ALTITUDE 0.09 //0.09 #define KD_BARO_ALTITUDE 0.09 //0.09
#define KI_BARO_ALTITUDE 0.1 #define KI_BARO_ALTITUDE 0.1
@ -121,8 +122,8 @@ int Altitude_control_baro(int altitude, int target_altitude)
err_altitude = target_altitude - altitude; err_altitude = target_altitude - altitude;
altitude_D = (float)(err_altitude-err_altitude_old)/0.05; // 20Hz altitude_D = (float)(err_altitude-err_altitude_old)/0.05; // 20Hz
altitude_I += (float)err_altitude*0.05; altitude_I += (float)err_altitude*0.05;
altitude_I = constrain(altitude_I,-120,120); altitude_I = constrain(altitude_I,-140,140);
command_altitude = KP_BARO_ALTITUDE*err_altitude + KD_BARO_ALTITUDE*altitude_D + KI_BARO_ALTITUDE*altitude_I; 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); command_altitude = Initial_Throttle + constrain(command_altitude,-ALTITUDE_CONTROL_BARO_OUTPUT_MIN,ALTITUDE_CONTROL_BARO_OUTPUT_MAX);
return command_altitude; return command_altitude;
} }