Description
Hi, guys, maybe there is a small bug in set_target_altitude_proportion() when we are doing path planning.
Here is the website of this function:
ardupilot/ArduPlane/altitude.cpp
Line 266 in 1a3ca43
when current altitude is 5 meters higher than target altitude, we do not want plane go down to the target altitude on the planned path, we want to plan another path to make the plane can fly to the target waypoint without going down, especially in auto takeoff. the above is part of purpose of this function.
The code in APM is:
void Plane::set_target_altitude_proportion(const Location &loc, float proportion)
{
// ******omit several lines **************
//
if(g.glide_slope_threshold > 0) {
if(target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.glide_slope_threshold) {
set_target_altitude_location(loc);
set_offset_altitude_location(loc);
change_target_altitude(-target_altitude.offset_cm*proportion);
//adjust the new target offset altitude to reflect that we are partially already done
if(proportion > 0.0f)
target_altitude.offset_cm = ((float)target_altitude.offset_cm)/proportion;
}
}
}
What I adjust is:
void Plane::set_target_altitude_proportion(const Location &loc, float proportion)
{
// ******omit several lines **************
//
if(g.glide_slope_threshold > 0) {
if(target_altitude.offset_cm > 0 && calc_altitude_error_cm() < -100 * g.glide_slope_threshold) {
set_target_altitude_location(loc);
set_offset_altitude_location(loc);
//adjust the new target offset altitude to reflect that we are partially already done
if(proportion > 0.0f)
target_altitude.offset_cm = ((float)target_altitude.offset_cm)/proportion;
change_target_altitude(-target_altitude.offset_cm*proportion);
}
}
}
To be more clearly, what I adjust is in the red circle:
But, if we donn't adjust the code order of this function, it will means that once the plane is 5m higher than the target altitude, the target altitude will become "wrong_target_altitude", this is not our purpose. So we should adjust the code order as showed in picture.
After adjusted, the target altitude will become "target_altitude_after_adjust" which is equal to current altitude, and the planned path will change to the green dashed line, and plane will not go down. this is what we want.
Activity