diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index d6ec4a6179..0763d3ae3c 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -109,9 +109,19 @@ void Plane::stabilize_roll(float speed_scaler) if (control_mode == &mode_stabilize && channel_roll->get_control_in() != 0) { disable_integrator = true; } - SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, - speed_scaler, - disable_integrator)); + int32_t roll_out; + if (!quadplane.use_fw_attitude_controllers()) { + // use the VTOL rate for control, to ensure consistency + const auto &pid_info = quadplane.attitude_control->get_rate_roll_pid().get_pid_info(); + roll_out = rollController.get_rate_out(degrees(pid_info.target), speed_scaler); + /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent + opposing integrators balancing between the two controllers + */ + rollController.decay_I(); + } else { + roll_out = rollController.get_servo_out(nav_roll_cd - ahrs.roll_sensor, speed_scaler, disable_integrator); + } + SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, roll_out); } /* @@ -134,14 +144,28 @@ void Plane::stabilize_pitch(float speed_scaler) disable_integrator = true; } - // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_CD if flight option FORCE_FLARE_ATTITUDE is set - if (!quadplane.in_transition() && !control_mode->is_vtol_mode() && channel_throttle->in_trim_dz() && !control_mode->does_auto_throttle() && flare_mode == FlareMode::ENABLED_PITCH_TARGET) { - demanded_pitch = landing.get_pitch_cd(); - } + int32_t pitch_out; + if (!quadplane.use_fw_attitude_controllers()) { + // use the VTOL rate for control, to ensure consistency + const auto &pid_info = quadplane.attitude_control->get_rate_pitch_pid().get_pid_info(); + pitch_out = pitchController.get_rate_out(degrees(pid_info.target), speed_scaler); + /* when slaving fixed wing control to VTOL control we need to decay the integrator to prevent + opposing integrators balancing between the two controllers + */ + pitchController.decay_I(); + } else { + // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_CD if flight option FORCE_FLARE_ATTITUDE is set + if (!quadplane.in_transition() && + !control_mode->is_vtol_mode() && + channel_throttle->in_trim_dz() && + !control_mode->does_auto_throttle() && + flare_mode == FlareMode::ENABLED_PITCH_TARGET) { + demanded_pitch = landing.get_pitch_cd(); + } - SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, - speed_scaler, - disable_integrator)); + pitch_out = pitchController.get_servo_out(demanded_pitch - ahrs.pitch_sensor, speed_scaler, disable_integrator); + } + SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, pitch_out); } /* diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c19c7840fa..dd76e69f89 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -3741,4 +3741,23 @@ uint16_t QuadPlane::get_pilot_velocity_z_max_dn() const return abs(pilot_velocity_z_max_dn); } +/* + should we use the fixed wing attitude controllers for roll/pitch control + */ +bool QuadPlane::use_fw_attitude_controllers(void) const +{ +#if 1 + if (available() && + motors->armed() && + motors->get_desired_spool_state() >= AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED && + in_vtol_mode() && + !is_tailsitter()) { + // we want the desired rates for fixed wing slaved to the + // multicopter rates + return false; + } +#endif + return true; +} + QuadPlane *QuadPlane::_singleton = nullptr; diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h index 683ee2eee3..4874bc7109 100644 --- a/ArduPlane/quadplane.h +++ b/ArduPlane/quadplane.h @@ -675,6 +675,11 @@ private: // Q assist state, can be enabled, disabled or force. Default to enabled Q_ASSIST_STATE_ENUM q_assist_state = Q_ASSIST_STATE_ENUM::Q_ASSIST_ENABLED; + /* + return true if we should use the fixed wing attitude control loop + */ + bool use_fw_attitude_controllers(void) const; + public: void motor_test_output(); MAV_RESULT mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_seq, uint8_t throttle_type,