Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-12 19:08:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

ACM: Added fast descent with Bill P's equation for managed accerlation

Browse Source
This commit is contained in:
Jason Short 2012-11-11 18:23:45 -08:00
parent 27a41c294b
commit 1049381dbe
1 changed files with 22 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
ArduCopter/navigation.pde
View File

@ -519,17 +519,18 @@ static void reset_desired_speed()
max_speed_old = 0; max_speed_old = 0;
} }
#define MAX_CLIMB_RATE 200 #define MAX_CLIMB_RATE_UP 300
#define MAX_CLIMB_RATE_DOWN 200
#define MIN_CLIMB_RATE 50 #define MIN_CLIMB_RATE 50
#define DECEL_CLIMB_RATE 30 #define DECEL_CLIMB_RATE 30
static int16_t get_desired_climb_rate_new() static int16_t get_desired_climb_rate()
{ {
static int16_t climb_old = 0; //static int16_t climb_old = 0;
if(alt_change_flag == REACHED_ALT) { if(alt_change_flag == REACHED_ALT) {
climb_old = 0; //climb_old = 0;
return 0; return 0;
} }
@ -540,14 +541,25 @@ static int16_t get_desired_climb_rate_new()
dist -= 300; // give ourselves 3 meter buffer to the desired alt dist -= 300; // give ourselves 3 meter buffer to the desired alt
float temp = 2 * DECEL_CLIMB_RATE * dist + (MIN_CLIMB_RATE * MIN_CLIMB_RATE); // 50cm minium climb_rate; float temp = 2 * DECEL_CLIMB_RATE * dist + (MIN_CLIMB_RATE * MIN_CLIMB_RATE); // 50cm minium climb_rate;
climb = sqrt(temp); climb = sqrt(temp);
climb = min(climb, MAX_CLIMB_RATE); // don't go to fast
if(alt_change_flag == ASCENDING){
climb = constrain(climb, 200, MAX_CLIMB_RATE_UP);
}else{
// Descending
climb = constrain(climb, MIN_CLIMB_RATE, MAX_CLIMB_RATE_DOWN);
//climb = min(climb, MAX_CLIMB_RATE_DOWN); // don't go to fast
}
}else{ }else{
climb = MAX_CLIMB_RATE; // no need to calc speed, just go the max if(alt_change_flag == ASCENDING){
climb = MAX_CLIMB_RATE_UP; // don't go to fast
}else{
climb = MAX_CLIMB_RATE_DOWN; // don't go to fast
}
} }
//climb = min(climb, climb_old + (100 * .02));// limit going faster //climb = min(climb, climb_old + (100 * .02));// limit acceleration
climb = max(climb, MIN_CLIMB_RATE); // don't go too slow //climb_old = climb;
climb_old = climb;
if(alt_change_flag == DESCENDING){ if(alt_change_flag == DESCENDING){
climb = -climb; climb = -climb;
@ -555,7 +567,7 @@ static int16_t get_desired_climb_rate_new()
return climb; return climb;
} }
static int16_t get_desired_climb_rate() static int16_t get_desired_climb_rate_old()
{ {
if(alt_change_flag == ASCENDING) { if(alt_change_flag == ASCENDING) {
return constrain(altitude_error / 4, 100, 180); // 180cm /s up, minimum is 100cm/s return constrain(altitude_error / 4, 100, 180); // 180cm /s up, minimum is 100cm/s