mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-02-07 00:18:14 +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.
70 lines
3.0 KiB
C++
70 lines
3.0 KiB
C++
#include "Sub.h"
|
|
|
|
|
|
bool ModeStabilize::init(bool ignore_checks) {
|
|
// set target altitude to zero for reporting
|
|
position_control->set_pos_desired_U_cm(0);
|
|
sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
|
|
|
|
return true;
|
|
return true;
|
|
}
|
|
|
|
void ModeStabilize::run()
|
|
{
|
|
uint32_t tnow = AP_HAL::millis();
|
|
float target_roll, target_pitch;
|
|
|
|
// if not armed set throttle to zero and exit immediately
|
|
if (!motors.armed()) {
|
|
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();
|
|
sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
|
|
return;
|
|
}
|
|
|
|
motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
|
|
|
|
// convert pilot input to lean angles
|
|
// To-Do: convert sub.get_pilot_desired_lean_angles to return angles as floats
|
|
// TODO2: move into mode.h
|
|
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 yaw_input = channel_yaw->pwm_to_angle_dz_trim(channel_yaw->get_dead_zone() * sub.gain, channel_yaw->get_radio_trim());
|
|
float target_yaw_rate = sub.get_pilot_desired_yaw_rate(yaw_input);
|
|
|
|
// call attitude controller
|
|
// update attitude controller targets
|
|
|
|
if (!is_zero(target_yaw_rate)) { // call attitude controller with rate yaw determined by pilot input
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
|
|
sub.last_pilot_heading_rad = ahrs.get_yaw_rad();
|
|
sub.last_pilot_yaw_input_ms = tnow; // time when pilot last changed heading
|
|
|
|
} else { // hold current heading
|
|
|
|
// this check is required to prevent bounce back after very fast yaw maneuvers
|
|
// the inertia of the vehicle causes the heading to move slightly past the point when pilot input actually stopped
|
|
if (tnow < sub.last_pilot_yaw_input_ms + 250) { // give 250ms to slow down, then set target heading
|
|
target_yaw_rate = 0; // Stop rotation on yaw axis
|
|
|
|
// call attitude controller with target yaw rate = 0 to decelerate on yaw axis
|
|
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw_cd(target_roll, target_pitch, target_yaw_rate);
|
|
sub.last_pilot_heading_rad = ahrs.get_yaw_rad(); // update heading to hold
|
|
|
|
} else { // call attitude controller holding absolute absolute bearing
|
|
attitude_control->input_euler_angle_roll_pitch_yaw_cd(target_roll, target_pitch, rad_to_cd(sub.last_pilot_heading_rad), true);
|
|
}
|
|
}
|
|
|
|
// output pilot's throttle
|
|
attitude_control->set_throttle_out((channel_throttle->norm_input() + 1.0f) / 2.0f, false, g.throttle_filt);
|
|
|
|
//control_in is range -1000-1000
|
|
//radio_in is raw pwm value
|
|
motors.set_forward(channel_forward->norm_input());
|
|
motors.set_lateral(channel_lateral->norm_input());
|
|
}
|