Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduCopter: bug fix to althold desired rate calculation. Fix from Jonathan.

This commit is contained in:
rmackay9 2012-12-23 11:17:26 +09:00 committed by Andrew Tridgell
parent 02bf3ea027
commit 576af6d487
2 changed files with 6 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
ArduCopter/Attitude.pde
View File

@ -997,10 +997,12 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli
linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
if( altitude_error < linear_distance*2 ) {
desired_rate = g.pi_alt_hold.get_p(altitude_error);
}else{
if( altitude_error > 2*linear_distance ) {
desired_rate = sqrt(2*250*(altitude_error-linear_distance));
}else if( altitude_error < -2*linear_distance ) {
desired_rate = -sqrt(2*250*(-altitude_error-linear_distance));
}else{
desired_rate = g.pi_alt_hold.get_p(altitude_error);
}
desired_rate = constrain(desired_rate, min_climb_rate, max_climb_rate);

1
ArduCopter/navigation.pde
View File

@ -612,6 +612,7 @@ static int16_t get_desired_speed(int16_t max_speed)
int32_t temp = 2 * 100 * (int32_t)(wp_distance - g.waypoint_radius * 100);
int32_t s_min = WAYPOINT_SPEED_MIN;
temp += s_min * s_min;
if( temp < 0 ) temp = 0; // check to ensure we don't try to take the sqrt of a negative number
max_speed = sqrt((float)temp);
max_speed = min(max_speed, g.waypoint_speed_max);
}