Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Sub: Missing Loiter Init on accel

This commit is contained in:
Leonard Hall 2018-09-19 12:21:16 +09:30 committed by Andrew Tridgell
parent 9b0d2eca07
commit dc0c9dee0c
2 changed files with 4 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduSub/control_auto.cpp
View File

@ -646,6 +646,7 @@ bool Sub::auto_terrain_recover_start()
mission.stop(); mission.stop();
// Reset xy target // Reset xy target
loiter_nav.clear_pilot_desired_acceleration();
loiter_nav.init_target(); loiter_nav.init_target();
// Reset z axis controller // Reset z axis controller

3
ArduSub/control_poshold.cpp
View File

@ -24,6 +24,7 @@ bool Sub::poshold_init()
// set target to current position // set target to current position
// only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I // only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
loiter_nav.clear_pilot_desired_acceleration();
loiter_nav.init_target(); loiter_nav.init_target();
last_pilot_heading = ahrs.yaw_sensor; last_pilot_heading = ahrs.yaw_sensor;
@ -40,6 +41,7 @@ void Sub::poshold_run()
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED); motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
loiter_nav.clear_pilot_desired_acceleration();
loiter_nav.init_target(); loiter_nav.init_target();
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt); attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
pos_control.relax_alt_hold_controllers(motors.get_throttle_hover()); pos_control.relax_alt_hold_controllers(motors.get_throttle_hover());
@ -64,6 +66,7 @@ void Sub::poshold_run()
if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) { if (fabsf(pilot_lateral) > 0.1 || fabsf(pilot_forward) > 0.1) {
lateral_out = pilot_lateral; lateral_out = pilot_lateral;
forward_out = pilot_forward; forward_out = pilot_forward;
loiter_nav.clear_pilot_desired_acceleration();
loiter_nav.init_target(); // initialize target to current position after repositioning loiter_nav.init_target(); // initialize target to current position after repositioning
} else { } else {
translate_wpnav_rp(lateral_out, forward_out); translate_wpnav_rp(lateral_out, forward_out);