mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-02-07 21:01:29 +08:00
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.
65 lines
2.6 KiB
C++
65 lines
2.6 KiB
C++
#include "Sub.h"
|
|
|
|
|
|
bool ModeSurface::init(bool ignore_checks)
|
|
{
|
|
nobaro_mode = !sub.control_check_barometer();
|
|
|
|
// initialize vertical speeds and acceleration
|
|
// All limits must be positive
|
|
position_control->D_set_max_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
|
position_control->D_set_correction_speed_accel_cm(sub.get_pilot_speed_dn(), g.pilot_speed_up, g.pilot_accel_z);
|
|
|
|
// initialise position and desired velocity
|
|
position_control->D_init_controller();
|
|
|
|
return true;
|
|
|
|
}
|
|
|
|
void ModeSurface::run()
|
|
{
|
|
float target_roll, target_pitch;
|
|
|
|
// if not armed set throttle to zero and exit immediately
|
|
if (!motors.armed()) {
|
|
motors.output_min();
|
|
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();
|
|
position_control->D_init_controller();
|
|
return;
|
|
}
|
|
|
|
// If no barometer is available, use the surface_nobaro_thrust parameter to set the throttle output
|
|
if (nobaro_mode) {
|
|
float thrust_output = 0.5f + g2.surface_nobaro_thrust * 0.005f; // map -100, 100 to 0, 1
|
|
attitude_control->set_throttle_out(thrust_output, true, g.throttle_filt);
|
|
} else {
|
|
// Already at surface, hold depth at surface
|
|
if (sub.ap.at_surface) {
|
|
set_mode(Mode::Number::ALT_HOLD, ModeReason::SURFACE_COMPLETE);
|
|
}
|
|
|
|
// convert pilot input to lean angles
|
|
// To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats
|
|
sub.get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->lean_angle_max_cd());
|
|
|
|
// get pilot's desired yaw rate
|
|
float target_yaw_rate = sub.get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
|
|
|
// call attitude controller
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
|
|
|
|
// set target climb rate
|
|
float cmb_rate_cms = constrain_float(fabsf(sub.wp_nav.get_default_speed_up_cms()), 1, position_control->get_max_speed_up_cms());
|
|
|
|
// update altitude target and call position controller
|
|
position_control->D_set_pos_target_from_climb_rate_cms(cmb_rate_cms);
|
|
position_control->D_update_controller();
|
|
}
|
|
// pilot has control for repositioning
|
|
motors.set_forward(channel_forward->norm_input());
|
|
motors.set_lateral(channel_lateral->norm_input());
|
|
}
|