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

Plane: in fixed wing mode slave quadplane attitude rate controller 

For quadplanes this uses the attitude controller from fixed wing
when using the multicopter controller to assist fixed wing
flight. This prevents a rate mismatch between the two controller
leading to oscillation
This commit is contained in:
Andrew Tridgell 2017-03-13 18:45:46 +11:00
parent 2db8589f49
commit 3f7e7d456f
2 changed files with 26 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
ArduPlane/quadplane.cpp
View File

@ -616,14 +616,32 @@ void QuadPlane::init_stabilize(void)
throttle_wait = false;
}
/*
ask the multicopter attitude control to match the roll and pitch rates being demanded by the
fixed wing controller if not in a pure VTOL mode
*/
void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds, float smooth_gain)
{
if (in_vtol_mode() || is_tailsitter()) {
// use euler angle attitude control
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
yaw_rate_cds,
smooth_gain);
} else {
// use the fixed wing desired rates
float roll_rate = plane.rollController.get_pid_info().desired;
float pitch_rate = plane.pitchController.get_pid_info().desired;
attitude_control->input_euler_rate_roll_pitch_yaw(roll_rate*100, pitch_rate*100, yaw_rate_cds);
}
}
// hold in stabilize with given throttle
void QuadPlane::hold_stabilize(float throttle_in)
{
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_desired_yaw_rate_cds(),
smoothing_gain);
multicopter_attitude_rate_update(get_desired_yaw_rate_cds(), smoothing_gain);
if (throttle_in <= 0) {
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
@ -709,10 +727,7 @@ void QuadPlane::hold_hover(float target_climb_rate)
pos_control->set_accel_z(pilot_accel_z);
// call attitude controller
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(plane.nav_roll_cd,
plane.nav_pitch_cd,
get_desired_yaw_rate_cds(),
smoothing_gain);
multicopter_attitude_rate_update(get_desired_yaw_rate_cds(), smoothing_gain);
// call position controller
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, plane.G_Dt, false);

3
ArduPlane/quadplane.h
View File

@ -156,6 +156,9 @@ private:
// initialise throttle_wait when entering mode
void init_throttle_wait();
// use multicopter rate controller
void multicopter_attitude_rate_update(float yaw_rate_cds, float smoothing_gain);
// main entry points for VTOL flight modes
void init_stabilize(void);
void control_stabilize(void);