From d8642aeaab6e03f0c90363bd2788977dbcf4695a Mon Sep 17 00:00:00 2001 From: jphelirc Date: Fri, 10 Dec 2010 04:07:08 +0000 Subject: [PATCH] new baro/throttle algorithms git-svn-id: https://arducopter.googlecode.com/svn/trunk@1099 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- ArducopterNG/Arducopter.h | 10 +++++----- ArducopterNG/ArducopterNG.pde | 9 +++++++-- ArducopterNG/Motors.pde | 24 ++++++++++++++++++------ ArducopterNG/Navigation.pde | 7 ++++--- 4 files changed, 34 insertions(+), 16 deletions(-) diff --git a/ArducopterNG/Arducopter.h b/ArducopterNG/Arducopter.h index c01e773097..086cfa45b5 100644 --- a/ArducopterNG/Arducopter.h +++ b/ArducopterNG/Arducopter.h @@ -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; diff --git a/ArducopterNG/ArducopterNG.pde b/ArducopterNG/ArducopterNG.pde index df2181a38e..0679f3eb8a 100644 --- a/ArducopterNG/ArducopterNG.pde +++ b/ArducopterNG/ArducopterNG.pde @@ -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 diff --git a/ArducopterNG/Motors.pde b/ArducopterNG/Motors.pde index 65384c9eb1..aaede5940c 100644 --- a/ArducopterNG/Motors.pde +++ b/ArducopterNG/Motors.pde @@ -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 diff --git a/ArducopterNG/Navigation.pde b/ArducopterNG/Navigation.pde index 505867e9b7..08a7476fe0 100644 --- a/ArducopterNG/Navigation.pde +++ b/ArducopterNG/Navigation.pde @@ -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)); } - +