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

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

Browse Source
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 set_landing_gear(void);
void dspoiler_update(void); void dspoiler_update(void);
void airbrake_update(void); void airbrake_update(void);
void servo_output_mixers(void);
void landing_neutral_control_surface_servos(void); void landing_neutral_control_surface_servos(void);
void servos_output(void); void servos_output(void);
void servos_auto_trim(void); void servos_auto_trim(void);

23
ArduPlane/servos.cpp
View File

@ -702,19 +702,6 @@ void Plane::set_landing_gear(void)
} }
#endif // LANDING_GEAR_ENABLED #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 support for twin-engine planes
@ -970,6 +957,10 @@ void Plane::servos_output(void)
// support twin-engine aircraft // support twin-engine aircraft
servos_twin_engine_mix(); 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 #if HAL_QUADPLANE_ENABLED
// cope with tailsitters and bicopters // cope with tailsitters and bicopters
quadplane.tailsitter.output(); quadplane.tailsitter.output();
@ -978,9 +969,9 @@ void Plane::servos_output(void)
// support forced flare option // support forced flare option
force_flare(); force_flare();
// run vtail and elevon mixers // implement differential spoilers
servo_output_mixers(); dspoiler_update();
// set control surface servos to neutral // set control surface servos to neutral
landing_neutral_control_surface_servos(); 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 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; 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) { if (roll_lim) {
motors->limit.roll = true; motors->limit.roll = true;
} }
@ -680,7 +707,6 @@ void Tailsitter::speed_scaling(void)
} else { } else {
v *= spd_scaler; v *= spd_scaler;
} }
v = constrain_int32(v, -SERVO_MAX, SERVO_MAX);
SRV_Channels::set_output_scaled(functions[i], v); SRV_Channels::set_output_scaled(functions[i], v);
} }
} }