From b4ffcfa6bd4a93c5823906e2498915793ac37022 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 19 Apr 2017 21:33:35 +1000 Subject: [PATCH] Plane: added support for elevon and vtail servo functions these make trimming and limiting servo movement easier --- ArduPlane/Plane.h | 2 ++ ArduPlane/servos.cpp | 26 ++++++++++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 7610fd7d1f..aa6c6a4471 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -1058,6 +1058,8 @@ private: bool suppress_throttle(void); void channel_output_mixer_pwm(uint8_t mixing_type, uint16_t & chan1, uint16_t & chan2)const; void channel_output_mixer(uint8_t mixing_type, SRV_Channel::Aux_servo_function_t servo1, SRV_Channel::Aux_servo_function_t servo2); + void channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in, + SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out); void flaperon_update(int8_t flap_percent); bool start_command(const AP_Mission::Mission_Command& cmd); bool verify_command(const AP_Mission::Mission_Command& cmd); diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp index 197713fea4..2dfc92d579 100644 --- a/ArduPlane/servos.cpp +++ b/ArduPlane/servos.cpp @@ -229,6 +229,27 @@ void Plane::channel_output_mixer(uint8_t mixing_type, SRV_Channel::Aux_servo_fun } +/* + mixer for elevon and vtail channels setup using designated servo + function values. This mixer operates purely on scaled values, + allowing the user to trim and limit individual servos using the + SERVOn_* parameters + */ +void Plane::channel_function_mixer(SRV_Channel::Aux_servo_function_t func1_in, SRV_Channel::Aux_servo_function_t func2_in, + SRV_Channel::Aux_servo_function_t func1_out, SRV_Channel::Aux_servo_function_t func2_out) +{ + // the order is setup so that non-reversed servos go "up", and + // func1 is the "left" channel. Users can adjust with channel + // reversal as needed + float in1 = SRV_Channels::get_output_scaled(func1_in); + float in2 = SRV_Channels::get_output_scaled(func2_in); + float out1 = constrain_float((in2 - in1) * g.mixing_gain, -4500, 4500); + float out2 = constrain_float((in2 + in1) * g.mixing_gain, -4500, 4500); + SRV_Channels::set_output_scaled(func1_out, out1); + SRV_Channels::set_output_scaled(func2_out, out2); +} + + /* setup flaperon output channels */ @@ -614,6 +635,11 @@ void Plane::servo_output_mixers(void) (int16_t(ch4)-1500) * (int16_t)(SERVO_MAX/300) / (int16_t)2); } } + + + // allow for extra elevon 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); } /*