From 5d91e29ceadc18fd98f92931c053b2fa33083514 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 6 Jul 2017 14:15:47 +1000 Subject: [PATCH] Plane: fixed px4_mixer for new function based mixing this fixes the OVERRIDE_CHAN functionality to work correctly with the new function based mixing code It isn't a perfect match for ArduPilot mixing, but it is very close for the key control surfaces, including mixed surfaces --- ArduPlane/Plane.h | 4 + ArduPlane/px4_mixer.cpp | 387 ++++++++++++++++++++++------------------ 2 files changed, 213 insertions(+), 178 deletions(-) diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index f1bef136ea..804103d4a9 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -937,6 +937,10 @@ private: bool reached_loiter_target(void); bool print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...); uint16_t create_mixer(char *buf, uint16_t buf_size, const char *filename); + bool mix_one_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan); + bool mix_two_channels(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan1, uint8_t in_chan2, bool left_channel); + bool mix_passthrough(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan); + bool mix_trim_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan); bool setup_failsafe_mixing(void); void set_control_channels(void); void init_rc_in(); diff --git a/ArduPlane/px4_mixer.cpp b/ArduPlane/px4_mixer.cpp index 4cf0b2ec34..5683073416 100644 --- a/ArduPlane/px4_mixer.cpp +++ b/ArduPlane/px4_mixer.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #define PX4_LIM_RC_MIN 900 #define PX4_LIM_RC_MAX 2100 @@ -42,6 +43,165 @@ bool Plane::print_buffer(char *&buf, uint16_t &buf_size, const char *fmt, ...) return true; } +/* + create a mixer for a normal angle channel +*/ +bool Plane::mix_one_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan) +{ + const float limit = 10000; + const RC_Channel *inch = RC_Channels::rc_channel(in_chan); + const SRV_Channel *outch = SRV_Channels::srv_channel(out_chan); + const float scale_out = limit*(outch->get_output_max() - outch->get_output_min()) / (PX4_LIM_RC_MAX - PX4_LIM_RC_MIN); + + bool is_throttle = in_chan==rcmap.throttle()-1; + int16_t outch_trim = is_throttle?1500:outch->get_trim(); + + outch_trim = constrain_int16(outch_trim, outch->get_output_min()+1, outch->get_output_max()-1); + + if (!print_buffer(buf, buf_size, "M: 1\n")) { + return false; + } + + int32_t out_min = scale_out*(1500 - outch->get_output_min()) / (1500 - PX4_LIM_RC_MIN); + int32_t out_max = scale_out*(outch->get_output_max() - 1500) / (PX4_LIM_RC_MAX - 1500); + int32_t out_trim = scale_out*(outch_trim - 1500) / ((PX4_LIM_RC_MAX - PX4_LIM_RC_MIN) / 2); + int32_t in_mul = inch->get_reverse() == outch->get_reversed()?1:-1; + + // note that RC trim is taken care of in rc_config + float scale_low = limit*(1500 - PX4_LIM_RC_MIN)/(1500 - inch->get_radio_min()); + float scale_high = limit*(PX4_LIM_RC_MAX - 1500)/(inch->get_radio_max() - 1500); + + if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n", + int(out_min), + int(out_max), + int(out_trim), + int(-limit), int(limit))) { + return false; + } + if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", + in_chan, + int(in_mul*scale_low), int(in_mul*scale_high), + 0, + int(-limit), int(limit))) { + return false; + } + return true; +} + +/* + mix two channels using elevon style mixer +*/ +bool Plane::mix_two_channels(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan1, uint8_t in_chan2, bool left_channel) +{ + const float limit = 10000; + const RC_Channel *inch1 = RC_Channels::rc_channel(in_chan1); + const RC_Channel *inch2 = RC_Channels::rc_channel(in_chan2); + const SRV_Channel *outch = SRV_Channels::srv_channel(out_chan); + const float scale_out = limit*(outch->get_output_max() - outch->get_output_min()) / (PX4_LIM_RC_MAX - PX4_LIM_RC_MIN); + int16_t outch_trim = outch->get_trim(); + + outch_trim = constrain_int16(outch_trim, outch->get_output_min()+1, outch->get_output_max()-1); + + if (!print_buffer(buf, buf_size, "M: 2\n")) { + return false; + } + + int32_t out_min = scale_out*(1500 - outch->get_output_min()) / (1500 - PX4_LIM_RC_MIN); + int32_t out_max = scale_out*(outch->get_output_max() - 1500) / (PX4_LIM_RC_MAX - 1500); + int32_t out_trim = scale_out*(outch_trim - 1500) / ((PX4_LIM_RC_MAX - PX4_LIM_RC_MIN) / 2); + int32_t in_mul1 = inch1->get_reverse() == outch->get_reversed()?1:-1; + int32_t in_mul2 = inch2->get_reverse() == outch->get_reversed()?1:-1; + float in_gain = g.mixing_gain; + + if (left_channel) { + in_mul2 = -in_mul2; + } + + // note that RC trim is taken care of in rc_config + float scale_low1 = limit*(1500 - PX4_LIM_RC_MIN)/(1500 - inch1->get_radio_min()); + float scale_high1 = limit*(PX4_LIM_RC_MAX - 1500)/(inch1->get_radio_max() - 1500); + float scale_low2 = limit*(1500 - PX4_LIM_RC_MIN)/(1500 - inch2->get_radio_min()); + float scale_high2 = limit*(PX4_LIM_RC_MAX - 1500)/(inch2->get_radio_max() - 1500); + + if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n", + int(out_min), + int(out_max), + int(out_trim), + int(-limit*2), int(limit*2))) { + return false; + } + if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", + in_chan1, + int(in_mul1*in_gain*scale_low1), int(in_mul1*in_gain*scale_high1), + 0, + int(-limit*2), int(limit*2))) { + return false; + } + if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", + in_chan2, + int(in_mul2*in_gain*scale_low2), int(in_mul2*in_gain*scale_high2), + 0, + int(-limit*2), int(limit*2))) { + return false; + } + return true; +} + +/* + create a mixer for k_manual and k_rcin* +*/ +bool Plane::mix_passthrough(char *&buf, uint16_t &buf_size, uint8_t out_chan, uint8_t in_chan) +{ + const float limit = 10000; + + if (!print_buffer(buf, buf_size, "M: 1\n")) { + return false; + } + + if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n", + int(limit), + int(limit), + 0, + int(-limit), int(limit))) { + return false; + } + if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", + in_chan, + int(limit), int(limit), + 0, + int(-limit), int(limit))) { + return false; + } + return true; +} + +/* + create a mixer for outputting trim only +*/ +bool Plane::mix_trim_channel(char *&buf, uint16_t &buf_size, uint8_t out_chan) +{ + const float limit = 10000; + const SRV_Channel *outch = SRV_Channels::srv_channel(out_chan); + + int16_t outch_trim = outch->get_trim(); + outch_trim = constrain_int16(outch_trim, outch->get_output_min()+1, outch->get_output_max()-1); + + if (!print_buffer(buf, buf_size, "M: 0\n")) { + return false; + } + + int32_t out_trim = limit*(outch_trim - 1500) / ((PX4_LIM_RC_MAX - PX4_LIM_RC_MIN) / 2); + + if (!print_buffer(buf, buf_size, "O: %d %d %d %d %d\n", + int(limit), + int(limit), + int(out_trim), + int(-limit), int(limit))) { + return false; + } + return true; +} + /* create a PX4 mixer buffer given the current fixed wing parameters, returns the size of the buffer used */ @@ -50,165 +210,49 @@ uint16_t Plane::create_mixer(char *buf, uint16_t buf_size, const char *filename) char *buf0 = buf; uint16_t buf_size0 = buf_size; - /* - this is the equivalent of channel_output_mixer() - */ - const int8_t mixmul[5][2] = { { 0, 0 }, { 1, 1 }, { 1, -1 }, { -1, 1 }, { -1, -1 }}; - // these are the internal clipping limits. Use scale_max1 when - // clipping to user specified min/max is wanted. Use scale_max2 - // when no clipping is wanted (simulated by setting a very large - // clipping value) - const float scale_max1 = 10000; - const float scale_max2 = 1000000; - // range for mixers - const uint16_t mix_max = scale_max1 * g.mixing_gain; - // scaling factors used by PX4IO between pwm and internal values, - // as configured in setup_failsafe_mixing() below - const float pwm_min = PX4_LIM_RC_MIN; - const float pwm_max = PX4_LIM_RC_MAX; - const float pwm_scale = 2*scale_max1/(pwm_max - pwm_min); - for (uint8_t i=0; i<8; i++) { - int32_t c1, c2, mix=0; - bool rev = false; SRV_Channel::Aux_servo_function_t function = SRV_Channels::channel_function(i); - if (i == rcmap.pitch()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) { - // first channel of VTAIL mix - c1 = rcmap.yaw()-1; - c2 = i; - rev = false; - mix = -mix_max*mixmul[g.vtail_output][0]; - } else if (i == rcmap.yaw()-1 && g.vtail_output > MIXING_DISABLED && g.vtail_output <= MIXING_DNDN) { - // second channel of VTAIL mix - c1 = rcmap.pitch()-1; - c2 = i; - rev = true; - mix = mix_max*mixmul[g.vtail_output][1]; - } else if (i == rcmap.roll()-1 && g.elevon_output > MIXING_DISABLED && - g.elevon_output <= MIXING_DNDN && g.vtail_output == 0) { - // first channel of ELEVON mix - c1 = i; - c2 = rcmap.pitch()-1; - rev = true; - mix = mix_max*mixmul[g.elevon_output][1]; - } else if (i == rcmap.pitch()-1 && g.elevon_output > MIXING_DISABLED && - g.elevon_output <= MIXING_DNDN && g.vtail_output == 0) { - // second channel of ELEVON mix - c1 = i; - c2 = rcmap.roll()-1; - rev = false; - mix = mix_max*mixmul[g.elevon_output][0]; - } else if (function == SRV_Channel::k_aileron || - function == SRV_Channel::k_flaperon_left || - function == SRV_Channel::k_flaperon_right || - function == SRV_Channel::k_aileron_with_input) { - // a secondary aileron. We don't mix flap input in yet for flaperons - c1 = rcmap.roll()-1; - } else if (function == SRV_Channel::k_elevator || - function == SRV_Channel::k_elevator_with_input) { - // a secondary elevator - c1 = rcmap.pitch()-1; - } else if (function == SRV_Channel::k_rudder || - function == SRV_Channel::k_steering) { - // a secondary rudder or wheel - c1 = rcmap.yaw()-1; - } else if (g.flapin_channel > 0 && - (function == SRV_Channel::k_flap || - function == SRV_Channel::k_flap_auto)) { - // a flap output channel, and we have a manual flap input channel - c1 = g.flapin_channel-1; - } else if (i < 4 || - function == SRV_Channel::k_manual) { - // a pass-thru channel - c1 = i; - } else { - // a empty output - if (!print_buffer(buf, buf_size, "Z:\n")) { - return 0; - } - continue; - } - if (mix == 0) { - // pass through channel, possibly with reversal. We also - // adjust the gain based on the range of input and output - // channels and adjust for trims - const RC_Channel *chan1 = RC_Channels::rc_channel(i); - const RC_Channel *chan2 = RC_Channels::rc_channel(c1); - int16_t chan1_trim = (i==rcmap.throttle()-1?1500:chan1->get_radio_trim()); - int16_t chan2_trim = (c1==rcmap.throttle()-1?1500:chan2->get_radio_trim()); - chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); - chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); - // if the input and output channels are the same then we - // apply clipping. This allows for direct pass-thru - int32_t limit = (c1==i?scale_max2:scale_max1); - int32_t in_scale_low; - if (chan2_trim <= chan2->get_radio_min()) { - in_scale_low = scale_max1; - } else { - in_scale_low = scale_max1*(chan2_trim - pwm_min)/(float)(chan2_trim - chan2->get_radio_min()); - } - int32_t in_scale_high; - if (chan2->get_radio_max() <= chan2_trim) { - in_scale_high = scale_max1; - } else { - in_scale_high = scale_max1*(pwm_max - chan2_trim)/(float)(chan2->get_radio_max() - chan2_trim); - } - if (chan1->get_reverse() != chan2->get_reverse()) { - in_scale_low = -in_scale_low; - in_scale_high = -in_scale_high; - } - if (!print_buffer(buf, buf_size, "M: 1\n") || - !print_buffer(buf, buf_size, "O: %d %d %d %d %d\n", - (int)(pwm_scale*(chan1_trim - chan1->get_radio_min())), - (int)(pwm_scale*(chan1->get_radio_max() - chan1_trim)), - (int)(pwm_scale*(chan1_trim - 1500)), - (int)-scale_max2, (int)scale_max2) || - !print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", c1, - in_scale_low, - in_scale_high, - 0, - -limit, limit)) { - return 0; - } - } else { - const RC_Channel *chan1 = RC_Channels::rc_channel(c1); - const RC_Channel *chan2 = RC_Channels::rc_channel(c2); - int16_t chan1_trim = (c1==rcmap.throttle()-1?1500:chan1->get_radio_trim()); - int16_t chan2_trim = (c2==rcmap.throttle()-1?1500:chan2->get_radio_trim()); - chan1_trim = constrain_int16(chan1_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); - chan2_trim = constrain_int16(chan2_trim, PX4_LIM_RC_MIN+1, PX4_LIM_RC_MAX-1); - // mix of two input channels to give an output channel. To - // make the mixer match the behaviour of APM we need to - // scale and offset the input channels to undo the affects - // of the PX4IO input processing - if (!print_buffer(buf, buf_size, "M: 2\n") || - !print_buffer(buf, buf_size, "O: %d %d 0 %d %d\n", mix, mix, (int)-scale_max1, (int)scale_max1)) { - return 0; - } - int32_t in_scale_low = pwm_scale*(chan1_trim - pwm_min); - int32_t in_scale_high = pwm_scale*(pwm_max - chan1_trim); - int32_t offset = pwm_scale*(chan1_trim - 1500); - if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", - c1, in_scale_low, in_scale_high, offset, - (int)-scale_max2, (int)scale_max2)) { - return 0; - } - in_scale_low = pwm_scale*(chan2_trim - pwm_min); - in_scale_high = pwm_scale*(pwm_max - chan2_trim); - offset = pwm_scale*(chan2_trim - 1500); - if (rev) { - if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", - c2, in_scale_low, in_scale_high, offset, - (int)-scale_max2, (int)scale_max2)) { - return 0; - } - } else { - if (!print_buffer(buf, buf_size, "S: 0 %u %d %d %d %d %d\n", - c2, -in_scale_low, -in_scale_high, -offset, - (int)-scale_max2, (int)scale_max2)) { - return 0; - } - } + switch (function) { + case SRV_Channel::k_aileron: + case SRV_Channel::k_flaperon_left: + case SRV_Channel::k_flaperon_right: + mix_one_channel(buf, buf_size, i, rcmap.roll()-1); + break; + case SRV_Channel::k_elevator: + mix_one_channel(buf, buf_size, i, rcmap.pitch()-1); + break; + case SRV_Channel::k_throttle: + mix_one_channel(buf, buf_size, i, rcmap.throttle()-1); + break; + case SRV_Channel::k_rudder: + case SRV_Channel::k_steering: + mix_one_channel(buf, buf_size, i, rcmap.yaw()-1); + break; + case SRV_Channel::k_elevon_left: + case SRV_Channel::k_dspoilerLeft1: + case SRV_Channel::k_dspoilerLeft2: + mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.roll()-1, true); + break; + case SRV_Channel::k_elevon_right: + case SRV_Channel::k_dspoilerRight1: + case SRV_Channel::k_dspoilerRight2: + mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.roll()-1, false); + break; + case SRV_Channel::k_vtail_left: + mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.yaw()-1, true); + break; + case SRV_Channel::k_vtail_right: + mix_two_channels(buf, buf_size, i, rcmap.pitch()-1, rcmap.yaw()-1, false); + break; + case SRV_Channel::k_manual: + mix_passthrough(buf, buf_size, i, i); + break; + case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: + mix_passthrough(buf, buf_size, i, uint8_t(function - SRV_Channel::k_rcin1)); + break; + default: + mix_trim_channel(buf, buf_size, i); + break; } } @@ -299,16 +343,11 @@ bool Plane::setup_failsafe_mixing(void) continue; } struct pwm_output_rc_config config; - /* - we use a min/max of 900/2100 to allow for pass-thru of - larger values than the RC min/max range. This mimics the APM - behaviour of pass-thru in manual, which allows for dual-rate - transmitter setups in manual mode to go beyond the ranges - used in stabilised modes - */ config.channel = i; - config.rc_min = 900; - config.rc_max = 2100; + // use high rc limits to allow for correct pass-thru channels + // without limits + config.rc_min = PX4_LIM_RC_MIN; + config.rc_max = PX4_LIM_RC_MAX; if (rcmap.throttle()-1 == i) { // throttle uses a trim of 1500, so we don't get division // by small numbers near RC3_MIN @@ -318,16 +357,8 @@ bool Plane::setup_failsafe_mixing(void) } config.rc_dz = 0; // zero for the purposes of manual takeover - // we set reverse as false, as users of ArduPilot will have - // input reversed on transmitter, so from the point of view of - // the mixer the input is never reversed. The one exception is - // the 2nd channel, which is reversed inside the PX4IO code, - // so needs to be unreversed here to give sane behaviour. - if (i == 1) { - config.rc_reverse = true; - } else { - config.rc_reverse = false; - } + // undo the reversal of channel2 in px4io + config.rc_reverse = i==1?true:false; if (i+1 == g.override_channel.get()) { /* @@ -357,7 +388,7 @@ bool Plane::setup_failsafe_mixing(void) if (SRV_Channel::is_motor(SRV_Channels::channel_function(i))) { pwm_values.values[i] = quadplane.thr_min_pwm; } else { - pwm_values.values[i] = 900; + pwm_values.values[i] = PX4_LIM_RC_MIN; } } if (ioctl(px4io_fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values) != 0) { @@ -370,7 +401,7 @@ bool Plane::setup_failsafe_mixing(void) hal.rcout->write(i, quadplane.thr_min_pwm); pwm_values.values[i] = quadplane.thr_min_pwm; } else { - pwm_values.values[i] = 2100; + pwm_values.values[i] = PX4_LIM_RC_MAX; } } if (ioctl(px4io_fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values) != 0) {