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

Copter: Add pause in guided mode

This commit is contained in:
Leonard Hall 2022-01-27 19:30:15 +10:30 committed by Randy Mackay
parent 289b1ca75a
commit 670af8d469
2 changed files with 36 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduCopter/mode.h
View File

@ -1008,6 +1008,7 @@ private:
void pos_control_run();
void accel_control_run();
void velaccel_control_run();
void pause_control_run();
void posvelaccel_control_run();
void set_yaw_state(bool use_yaw, float yaw_cd, bool use_yaw_rate, float yaw_rate_cds, bool relative_angle);

35
ArduCopter/mode_guided.cpp
View File

@ -47,6 +47,11 @@ bool ModeGuided::init(bool ignore_checks)
// should be called at 100hz or more
void ModeGuided::run()
{
// if (paused) {
// pause_control_run();
// return;
// }
// call the correct auto controller
switch (guided_mode) {
@ -849,6 +854,36 @@ void ModeGuided::velaccel_control_run()
}
}
// velaccel_control_run - runs the guided velocity and acceleration controller
// called from guided_run
void ModeGuided::pause_control_run()
{
// if not armed set throttle to zero and exit immediately
if (is_disarmed_or_landed()) {
// do not spool down tradheli when on the ground with motor interlock enabled
make_safe_ground_handling(copter.is_tradheli() && motors->get_interlock());
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
// set the horizontal velocity and acceleration targets to zero
Vector2f vel_xy, accel_xy;
pos_control->input_vel_accel_xy(vel_xy, accel_xy, false);
// set the vertical velocity and acceleration targets to zero
float vel_z = 0.0;
pos_control->input_vel_accel_z(vel_z, 0.0, false, false);
// call velocity controller which includes z axis controller
pos_control->update_xy_controller();
pos_control->update_z_controller();
// call attitude controller
attitude_control->input_thrust_vector_rate_heading(pos_control->get_thrust_vector(), 0.0);
}
// posvelaccel_control_run - runs the guided position, velocity and acceleration controller
// called from guided_run
void ModeGuided::posvelaccel_control_run()