Copter: bug fix for take-off so it never descends

If copter was already flying when take-off command was received it could
descend to the target altitude if it was below the current altitude.
This commit is contained in:
Randy Mackay 2013-04-18 22:30:24 +09:00
parent 0fc9c8739e
commit d65d7d95e2
1 changed files with 1 additions and 1 deletions

View File

@ -245,7 +245,7 @@ static void do_takeoff()
// Set wp navigation target to safe altitude above current position
Vector3f pos = inertial_nav.get_position();
pos.z = command_nav_queue.alt;
pos.z = max(pos.z, command_nav_queue.alt);
wp_nav.set_destination(pos);
// prevent flips