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

Plane: tailsitter: always give pitch prority elevon mixing in VTOL modes

This commit is contained in:
Iampete1 2021-11-13 23:57:05 +00:00 committed by Andrew Tridgell
parent 135c88d474
commit 0fa1ddf7c2
3 changed files with 34 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -1086,7 +1086,6 @@ private:
void set_landing_gear(void);
void dspoiler_update(void);
void airbrake_update(void);
void servo_output_mixers(void);
void landing_neutral_control_surface_servos(void);
void servos_output(void);
void servos_auto_trim(void);

21
ArduPlane/servos.cpp
View File

@ -702,19 +702,6 @@ void Plane::set_landing_gear(void)
}
#endif // LANDING_GEAR_ENABLED
/*
apply vtail and elevon mixers
the rewrites radio_out for the corresponding channels
*/
void Plane::servo_output_mixers(void)
{
// mix elevons and vtail channels
channel_function_mixer(SRV_Channel::k_aileron, SRV_Channel::k_elevator, SRV_Channel::k_elevon_left, SRV_Channel::k_elevon_right);
channel_function_mixer(SRV_Channel::k_rudder, SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left);
// implement differential spoilers
dspoiler_update();
}
/*
support for twin-engine planes
@ -970,6 +957,10 @@ void Plane::servos_output(void)
// support twin-engine aircraft
servos_twin_engine_mix();
// run vtail and elevon mixers
channel_function_mixer(SRV_Channel::k_aileron, SRV_Channel::k_elevator, SRV_Channel::k_elevon_left, SRV_Channel::k_elevon_right);
channel_function_mixer(SRV_Channel::k_rudder, SRV_Channel::k_elevator, SRV_Channel::k_vtail_right, SRV_Channel::k_vtail_left);
#if HAL_QUADPLANE_ENABLED
// cope with tailsitters and bicopters
quadplane.tailsitter.output();
@ -979,8 +970,8 @@ void Plane::servos_output(void)
// support forced flare option
force_flare();
// run vtail and elevon mixers
servo_output_mixers();
// implement differential spoilers
dspoiler_update();
// set control surface servos to neutral
landing_neutral_control_surface_servos();

28
ArduPlane/tailsitter.cpp
View File

@ -421,6 +421,33 @@ void Tailsitter::output(void)
bool pitch_lim = fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_elevator)) >= SERVO_MAX;
bool yaw_lim = fabsf(SRV_Channels::get_output_scaled(SRV_Channel::Aux_servo_function_t::k_aileron)) >= SERVO_MAX;
// Mix elevons and V-tail, always giving full priority to pitch
float elevator_mix = SRV_Channels::get_output_scaled(SRV_Channel::k_elevator) * (100.0 - plane.g.mixing_offset) * 0.01 * plane.g.mixing_gain;
float aileron_mix = SRV_Channels::get_output_scaled(SRV_Channel::k_aileron) * (100.0 + plane.g.mixing_offset) * 0.01 * plane.g.mixing_gain;
float rudder_mix = SRV_Channels::get_output_scaled(SRV_Channel::k_rudder) * (100.0 + plane.g.mixing_offset) * 0.01 * plane.g.mixing_gain;
const float headroom = SERVO_MAX - fabsf(elevator_mix);
if (is_positive(headroom)) {
if (fabsf(aileron_mix) > headroom) {
aileron_mix *= headroom / fabsf(aileron_mix);
yaw_lim = true;
}
if (fabsf(rudder_mix) > headroom) {
rudder_mix *= headroom / fabsf(rudder_mix);
roll_lim = true;
}
} else {
aileron_mix = 0.0;
rudder_mix = 0.0;
roll_lim = true;
pitch_lim = true;
yaw_lim = true;
}
SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_left, elevator_mix - aileron_mix);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevon_right, elevator_mix + aileron_mix);
SRV_Channels::set_output_scaled(SRV_Channel::k_vtail_right, elevator_mix - rudder_mix);
SRV_Channels::set_output_scaled(SRV_Channel::k_vtail_left, elevator_mix + rudder_mix);
if (roll_lim) {
motors->limit.roll = true;
}
@ -680,7 +707,6 @@ void Tailsitter::speed_scaling(void)
} else {
v *= spd_scaler;
}
v = constrain_int32(v, -SERVO_MAX, SERVO_MAX);
SRV_Channels::set_output_scaled(functions[i], v);
}
}