aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: in fixed wing mode slave quadplane attitude rate controller

Browse Source 
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);