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

Plane: Quadplane: only advance to QPOS_POSITION2 once tilts have finished slewing

This commit is contained in:
Iampete1 2021-11-26 22:53:24 +00:00 committed by Andrew Tridgell
parent eac52fe08f
commit 2154738421
3 changed files with 12 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
ArduPlane/quadplane.cpp
View File

@ -2365,7 +2365,8 @@ void QuadPlane::vtol_position_controller(void)
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
desired_auto_yaw_rate_cds() + get_weathervane_yaw_rate_cds());
if (plane.auto_state.wp_distance < position2_dist_threshold) {
if ((plane.auto_state.wp_distance < position2_dist_threshold) && tiltrotor.tilt_angle_achieved()) {
// if continuous tiltrotor only advance to position 2 once tilts have finished moving
poscontrol.set_state(QPOS_POSITION2);
poscontrol.pilot_correction_done = false;
gcs().send_text(MAV_SEVERITY_INFO,"VTOL position2 started v=%.1f d=%.1f",

2
ArduPlane/tiltrotor.cpp
View File

@ -186,6 +186,8 @@ void Tiltrotor::slew(float newtilt)
float max_change = tilt_max_change(newtilt<current_tilt, newtilt > get_fully_forward_tilt());
current_tilt = constrain_float(newtilt, current_tilt-max_change, current_tilt+max_change);
angle_achieved = is_equal(newtilt, current_tilt);
// translate to 0..1000 range and output
SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, 1000 * current_tilt);
}

8
ArduPlane/tiltrotor.h
View File

@ -63,6 +63,10 @@ public:
bool motors_active() const { return enabled() && _motors_active; }
// true if the tilts have completed slewing
// always return true if not enabled or not a continuous type
bool tilt_angle_achieved() const { return !enabled() || (type != TILT_TYPE_CONTINUOUS) || angle_achieved; }
AP_Int8 enable;
AP_Int16 tilt_mask;
AP_Int16 max_rate_up_dps;
@ -100,6 +104,10 @@ private:
// true if all motors tilt with no fixed VTOL motor
bool _have_vtol_motor;
// true if the current tilt angle is equal to the desired
// with slow tilt rates the tilt angle can lag
bool angle_achieved;
// refences for convenience
QuadPlane& quadplane;
AP_MotorsMulticopter*& motors;