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

ArduCopter: get_throttle_althold - add check to avoid divide by zero

Browse Source
This commit is contained in:
rmackay9 2012-12-26 21:18:16 +09:00
parent 1e50875dd1
commit 71f6a116be
1 changed files with 11 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/Attitude.pde
View File

@ -995,14 +995,18 @@ get_throttle_althold(int32_t target_alt, int16_t min_climb_rate, int16_t max_cli
// calculate altitude error
altitude_error = target_alt - current_loc.alt;
linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
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));
// check kP to avoid division by zero
if( g.pi_alt_hold.kP() != 0 ) {
linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
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);
}
}else{
desired_rate = g.pi_alt_hold.get_p(altitude_error);
desired_rate = 0;
}
desired_rate = constrain(desired_rate, min_climb_rate, max_climb_rate);