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

Sub: modes keep controllers initialized when disarmed

This commit is contained in:
Josh Henderson 2021-09-20 03:28:39 -04:00 committed by Peter Barker
parent b417aad807
commit 69997626d3
3 changed files with 7 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduSub/control_auto.cpp
View File

@ -116,6 +116,7 @@ void Sub::auto_wp_run()
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
wp_nav.wp_and_spline_init(); // Reset xy target
return;
}
@ -307,6 +308,7 @@ void Sub::auto_loiter_run()
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
wp_nav.wp_and_spline_init(); // Reset xy target
return;
}
@ -591,6 +593,9 @@ void Sub::auto_terrain_recover_run()
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
loiter_nav.init_target(); // Reset xy target
pos_control.relax_z_controller(motors.get_throttle_hover()); // Reset z axis controller
return;
}

1
ArduSub/control_circle.cpp
View File

@ -43,6 +43,7 @@ void Sub::circle_run()
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
circle_nav.init();
return;
}

4
ArduSub/control_guided.cpp
View File

@ -286,9 +286,7 @@ void Sub::guided_pos_control_run()
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers();
// initialise velocity controller
pos_control.init_z_controller();
pos_control.init_xy_controller();
wp_nav.wp_and_spline_init();
return;
}