Plane: added support for elevon and vtail servo functions

these make trimming and limiting servo movement easier
This commit is contained in:
Andrew Tridgell 2017-04-19 21:33:35 +10:00
parent 61fbaf0db7
commit b4ffcfa6bd
2 changed files with 28 additions and 0 deletions

View File

@ -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);

View File

@ -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);
}
/*