mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
Copter: bug fix for take-off in loiter
This commit is contained in:
parent
c3daf78340
commit
05248738e2
@ -1752,8 +1752,8 @@ void update_roll_pitch_mode(void)
|
||||
|
||||
// if landed simply keep the copter level
|
||||
if (ap.land_complete) {
|
||||
nav_roll = 0;
|
||||
nav_pitch = 0;
|
||||
nav_roll = ahrs.roll_sensor;
|
||||
nav_pitch = ahrs.pitch_sensor;
|
||||
}else{
|
||||
// update loiter target from user controls
|
||||
wp_nav.move_loiter_target(control_roll, control_pitch,0.01f);
|
||||
|
@ -162,9 +162,10 @@ static void update_nav_mode()
|
||||
// reset target if we are still on the ground
|
||||
if (ap.land_complete) {
|
||||
wp_nav.init_loiter_target(inertial_nav.get_position(),inertial_nav.get_velocity());
|
||||
}else{
|
||||
// call loiter controller
|
||||
wp_nav.update_loiter();
|
||||
}
|
||||
// call loiter controller
|
||||
wp_nav.update_loiter();
|
||||
break;
|
||||
|
||||
case NAV_WP:
|
||||
|
Loading…
Reference in New Issue
Block a user