Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-11 17:13:56 -03:00
Code Issues Packages Projects Releases Wiki Activity

Plane: moved assign tilt to the run() function

Browse Source 
this prevents double calling and fixes qhover
This commit is contained in:
Andrew Tridgell 2023-09-26 18:56:28 +10:00
parent 2f0bbf14a4
commit babdb3625a
3 changed files with 8 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/mode_qhover.cpp
View File

@ -37,6 +37,7 @@ void ModeQHover::run()
quadplane.relax_attitude_control(); quadplane.relax_attitude_control();
pos_control->relax_z_controller(0); pos_control->relax_z_controller(0);
} else { } else {
plane.quadplane.assign_tilt_to_fwd_thr();
quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms()); quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms());
} }

7
ArduPlane/mode_qstabilize.cpp
View File

@ -35,11 +35,6 @@ void ModeQStabilize::update()
plane.nav_roll_cd = roll_input * plane.quadplane.aparm.angle_max; plane.nav_roll_cd = roll_input * plane.quadplane.aparm.angle_max;
plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max; plane.nav_pitch_cd = pitch_input * plane.quadplane.aparm.angle_max;
} }
if (plane.control_mode->mode_number() == Mode::Number::QSTABILIZE) {
// protect against this function running twice
plane.quadplane.assign_tilt_to_fwd_thr();
}
} }
// quadplane stabilize mode // quadplane stabilize mode
@ -52,6 +47,8 @@ void ModeQStabilize::run()
return; return;
} }
plane.quadplane.assign_tilt_to_fwd_thr();
// special check for ESC calibration in QSTABILIZE // special check for ESC calibration in QSTABILIZE
if (quadplane.esc_calibration != 0) { if (quadplane.esc_calibration != 0) {
quadplane.run_esc_calibration(); quadplane.run_esc_calibration();

6
ArduPlane/quadplane.cpp
View File

@ -2972,6 +2972,9 @@ QuadPlane::ActiveFwdThr QuadPlane::get_vfwd_method(void) const
return ActiveFwdThr::NONE; return ActiveFwdThr::NONE;
} }
/*
map from pitch tilt to fwd throttle when enabled
*/
void QuadPlane::assign_tilt_to_fwd_thr(void) { void QuadPlane::assign_tilt_to_fwd_thr(void) {
const auto fwd_thr_active = get_vfwd_method(); const auto fwd_thr_active = get_vfwd_method();
@ -2991,7 +2994,8 @@ void QuadPlane::assign_tilt_to_fwd_thr(void) {
if (is_positive(fwd_tilt_range_cd)) { if (is_positive(fwd_tilt_range_cd)) {
// rate limit the forward tilt change to slew between the motor good and motor failed // rate limit the forward tilt change to slew between the motor good and motor failed
// value over 10 seconds // value over 10 seconds
const float fwd_pitch_lim_cd_tgt = plane.quadplane.pos_control->get_fwd_pitch_is_limited() ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim; const bool fwd_limited = plane.quadplane.pos_control->is_active_xy() and plane.quadplane.pos_control->get_fwd_pitch_is_limited();
const float fwd_pitch_lim_cd_tgt = fwd_limited ? (float)aparm.angle_max : 100.0f * q_fwd_pitch_lim;
const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt; const float delta_max = 0.1f * fwd_tilt_range_cd * plane.G_Dt;
q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max); q_fwd_pitch_lim_cd += constrain_float((fwd_pitch_lim_cd_tgt - q_fwd_pitch_lim_cd), -delta_max, delta_max);
// Don't let the forward pitch limit be more than the forward pitch demand before limiting to // Don't let the forward pitch limit be more than the forward pitch demand before limiting to