Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-08 17:03:57 -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 committed by Andrew Tridgell
parent af4d998697
commit daac540e08
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 // calculate altitude error
altitude_error = target_alt - current_loc.alt; altitude_error = target_alt - current_loc.alt;
linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP()); // check kP to avoid division by zero
if( g.pi_alt_hold.kP() != 0 ) {
if( altitude_error > 2*linear_distance ) { linear_distance = 250/(2*g.pi_alt_hold.kP()*g.pi_alt_hold.kP());
desired_rate = sqrt(2*250*(altitude_error-linear_distance)); if( altitude_error > 2*linear_distance ) {
}else if( altitude_error < -2*linear_distance ) { desired_rate = sqrt(2*250*(altitude_error-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{ }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); desired_rate = constrain(desired_rate, min_climb_rate, max_climb_rate);