Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-28 19:48:31 -04:00
Code Issues Packages Projects Releases Wiki Activity

Plane: slave fixed wing desired rate to multicopter desired rate

Browse Source 
this ensures that the two rate controllers don't fight each other when
in a VTOL mode
This commit is contained in:
Andrew Tridgell 2021-05-18 13:44:57 +10:00
parent d77d258442
commit f1f7f01300
3 changed files with 58 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

44
ArduPlane/Attitude.cpp
View File

@ -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);
}
/*

19
ArduPlane/quadplane.cpp
View File

@ -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;

5
ArduPlane/quadplane.h
View File

@ -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,