mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
Aaron Marburg
6eb0174bb5
If the sub is disarmed in many modes, the output "throttle" (vertical control) is set to 0.0, corresponding to full downward thrust. While this does not affect manual control, it can affect subsequent mode changes (e.g. to ALT_HOLD). Set to a constant NEUTRAL_THROTTLE, set to 0.5, instead.
36 lines
1.3 KiB
C++
36 lines
1.3 KiB
C++
#include "Sub.h"
|
|
|
|
|
|
bool ModeManual::init(bool ignore_checks) {
|
|
// set target altitude to zero for reporting
|
|
position_control->set_pos_desired_U_cm(0);
|
|
|
|
// attitude hold inputs become thrust inputs in manual mode
|
|
// set to neutral to prevent chaotic behavior (esp. roll/pitch)
|
|
sub.set_neutral_controls();
|
|
|
|
return true;
|
|
}
|
|
|
|
// manual_run - runs the manual (passthrough) controller
|
|
// should be called at 100hz or more
|
|
void ModeManual::run()
|
|
{
|
|
// if not armed set throttle to zero and exit immediately
|
|
if (!sub.motors.armed()) {
|
|
sub.motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE);
|
|
attitude_control->set_throttle_out(NEUTRAL_THROTTLE,true,g.throttle_filt);
|
|
attitude_control->relax_attitude_controllers();
|
|
return;
|
|
}
|
|
|
|
sub.motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
|
|
|
sub.motors.set_roll(channel_roll->norm_input());
|
|
sub.motors.set_pitch(channel_pitch->norm_input());
|
|
sub.motors.set_yaw(channel_yaw->norm_input() * g.acro_yaw_p / ACRO_YAW_P);
|
|
sub.motors.set_throttle((channel_throttle->norm_input() + 1.0f) / 2.0f);
|
|
sub.motors.set_forward(channel_forward->norm_input());
|
|
sub.motors.set_lateral(channel_lateral->norm_input());
|
|
}
|