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

Plane : Improved LAND_THEN_NEUTRL to work on all control surfaces 

This fixes the issue #18593

Co-Authored-By: Peter Hall <33176108+IamPete1@users.noreply.github.com>
This commit is contained in:
Shiv Tyagi 2021-09-16 22:53:06 +05:30 committed by Tom Pittenger
parent 9eb3a063d8
commit cdb4ec8ad0
4 changed files with 68 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
ArduPlane/Plane.h
View File

@ -1070,6 +1070,7 @@ private:
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);
void servos_twin_engine_mix();

52
ArduPlane/servos.cpp
View File

@ -897,24 +897,6 @@ void Plane::set_servos(void)
}
}
if (landing.get_then_servos_neutral() > 0 &&
control_mode == &mode_auto &&
landing.get_disarm_delay() > 0 &&
landing.is_complete() &&
!arming.is_armed()) {
// after an auto land and auto disarm, set the servos to be neutral just
// in case we're upside down or some crazy angle and straining the servos.
if (landing.get_then_servos_neutral() == 1) {
SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, 0);
SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, 0);
} else if (landing.get_then_servos_neutral() == 2) {
SRV_Channels::set_output_limit(SRV_Channel::k_aileron, SRV_Channel::Limit::ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_elevator, SRV_Channel::Limit::ZERO_PWM);
SRV_Channels::set_output_limit(SRV_Channel::k_rudder, SRV_Channel::Limit::ZERO_PWM);
}
}
uint8_t override_pct;
if (g2.ice_control.throttle_override(override_pct)) {
// the ICE controller wants to override the throttle for starting
@ -925,6 +907,37 @@ void Plane::set_servos(void)
servos_output();
}
/*
This sets servos to neutral if it is a control surface servo in auto mode
*/
void Plane::landing_neutral_control_surface_servos(void)
{
if (!(landing.get_then_servos_neutral() > 0 &&
control_mode == &mode_auto &&
landing.get_disarm_delay() > 0 &&
landing.is_complete() &&
!arming.is_armed())) {
return;
}
// after an auto land and auto disarm, set the servos to be neutral just
// in case we're upside down or some crazy angle and straining the servos.
for (uint8_t i = 0; i < NUM_SERVO_CHANNELS ; i++) {
SRV_Channel *chan = SRV_Channels::srv_channel(i);
if (chan == nullptr || !SRV_Channel::is_control_surface(chan->get_function())) {
continue;
}
if (landing.get_then_servos_neutral() == 1) {
SRV_Channels::set_output_scaled(chan->get_function(), 0);
} else if (landing.get_then_servos_neutral() == 2) {
SRV_Channels::set_output_limit(chan->get_function(), SRV_Channel::Limit::ZERO_PWM);
}
}
}
/*
run configured output mixer. This takes calculated servo_out values
for each channel and calculates PWM values, then pushes them to
@ -949,6 +962,9 @@ void Plane::servos_output(void)
// run vtail and elevon mixers
servo_output_mixers();
// set control surface servos to neutral
landing_neutral_control_surface_servos();
// support MANUAL_RCMASK
if (g2.manual_rc_mask.get() != 0 && control_mode == &mode_manual) {
SRV_Channels::copy_radio_in_out_mask(uint16_t(g2.manual_rc_mask.get()));

30
libraries/SRV_Channel/SRV_Channel.cpp
View File

@ -221,3 +221,33 @@ bool SRV_Channel::should_e_stop(SRV_Channel::Aux_servo_function_t function)
(function >= SRV_Channel::k_boost_throttle && function <= SRV_Channel::k_motor12) ||
function == k_engine_run_enable);
}
// return true if function is for a control surface
bool SRV_Channel::is_control_surface(SRV_Channel::Aux_servo_function_t function)
{
switch (function)
{
case SRV_Channel::Aux_servo_function_t::k_flap:
case SRV_Channel::Aux_servo_function_t::k_flap_auto:
case SRV_Channel::Aux_servo_function_t::k_aileron:
case SRV_Channel::Aux_servo_function_t::k_dspoilerLeft1:
case SRV_Channel::Aux_servo_function_t::k_dspoilerLeft2:
case SRV_Channel::Aux_servo_function_t::k_dspoilerRight1:
case SRV_Channel::Aux_servo_function_t::k_dspoilerRight2:
case SRV_Channel::Aux_servo_function_t::k_elevator:
case SRV_Channel::Aux_servo_function_t::k_rudder:
case SRV_Channel::Aux_servo_function_t::k_flaperon_left:
case SRV_Channel::Aux_servo_function_t::k_flaperon_right:
case SRV_Channel::Aux_servo_function_t::k_elevon_left:
case SRV_Channel::Aux_servo_function_t::k_elevon_right:
case SRV_Channel::Aux_servo_function_t::k_vtail_left:
case SRV_Channel::Aux_servo_function_t::k_vtail_right:
case SRV_Channel::Aux_servo_function_t::k_airbrake:
return true;
default:
return false;
}
return false;
}

3
libraries/SRV_Channel/SRV_Channel.h
View File

@ -232,6 +232,9 @@ public:
// return true if function is for anything that should be stopped in a e-stop situation, ie is dangerous
static bool should_e_stop(SRV_Channel::Aux_servo_function_t function);
// return true if function is for a control surface
static bool is_control_surface(SRV_Channel::Aux_servo_function_t function);
// return the function of a channel
SRV_Channel::Aux_servo_function_t get_function(void) const {
return (SRV_Channel::Aux_servo_function_t)function.get();