/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see <http://www.gnu.org/licenses/>. */ /* mixer for failsafe operation when FMU is dead */ #include <AP_HAL/AP_HAL.h> #include <AP_Math/AP_Math.h> #include <SRV_Channel/SRV_Channel.h> #include "iofirmware.h" #define ANGLE_SCALE ((int32_t)4500) #define RANGE_SCALE ((int32_t)1000) /* return a RC input value scaled from -4500 to 4500 */ int16_t AP_IOMCU_FW::mix_input_angle(uint8_t channel, uint16_t radio_in) const { const uint16_t &rc_min = mixing.rc_min[channel]; const uint16_t &rc_max = mixing.rc_max[channel]; const uint16_t &rc_trim = mixing.rc_trim[channel]; const uint16_t &reversed = mixing.rc_reversed[channel]; int16_t ret = 0; if (radio_in > rc_trim && rc_max != rc_trim) { ret = (ANGLE_SCALE * (int32_t)(radio_in - rc_trim)) / (int32_t)(rc_max - rc_trim); } else if (radio_in < rc_trim && rc_trim != rc_min) { ret = (ANGLE_SCALE * (int32_t)(radio_in - rc_trim)) / (int32_t)(rc_trim - rc_min); } if (reversed) { ret = -ret; } return ret; } /* return a RC input value scaled from 0 to 1000 */ int16_t AP_IOMCU_FW::mix_input_range(uint8_t channel, uint16_t radio_in) const { const uint16_t &rc_min = mixing.rc_min[channel]; const uint16_t &rc_max = mixing.rc_max[channel]; const uint16_t &reversed = mixing.rc_reversed[channel]; int16_t ret = 0; if (radio_in > rc_max) { ret = RANGE_SCALE; } else if (radio_in < rc_min) { ret = -RANGE_SCALE; } else { ret = (RANGE_SCALE * (int32_t)(radio_in - rc_min)) / (int32_t)(rc_max - rc_min); } if (reversed) { ret = -ret; } return ret; } /* return an output pwm giving an angle for a servo channel */ uint16_t AP_IOMCU_FW::mix_output_angle(uint8_t channel, int16_t angle) const { const uint16_t &srv_min = mixing.servo_min[channel]; const uint16_t &srv_max = mixing.servo_max[channel]; const uint16_t &srv_trim = mixing.servo_trim[channel]; const uint16_t &reversed = mixing.servo_reversed[channel]; if (reversed) { angle = -angle; } angle = constrain_int16(angle, -ANGLE_SCALE, ANGLE_SCALE); if (angle > 0) { return srv_trim + ((int32_t)angle * (int32_t)(srv_max - srv_trim)) / ANGLE_SCALE; } return srv_trim - (-(int32_t)angle * (int32_t)(srv_trim - srv_min)) / ANGLE_SCALE; } /* return an output pwm giving an range for a servo channel */ uint16_t AP_IOMCU_FW::mix_output_range(uint8_t channel, int16_t value) const { const uint16_t &srv_min = mixing.servo_min[channel]; const uint16_t &srv_max = mixing.servo_max[channel]; const uint16_t &reversed = mixing.servo_reversed[channel]; value = constrain_int16(value, 0, RANGE_SCALE); if (reversed) { value = RANGE_SCALE - value; } return srv_min + ((int32_t)value * (int32_t)(srv_max - srv_min)) / RANGE_SCALE; } /* elevon and vtail mixer */ int16_t AP_IOMCU_FW::mix_elevon_vtail(int16_t angle1, int16_t angle2, bool first_output) const { if (first_output) { return (angle2 - angle1) * mixing.mixing_gain / 1000; } return (angle1 + angle2) * mixing.mixing_gain / 1000; } /* run mixer. This is used when FMU is not providing inputs, or when the OVERRIDE_CHAN is high. It allows for manual fixed wing flight */ void AP_IOMCU_FW::run_mixer(void) { int16_t rcin[4] = {0, 0, 0, 0}; int16_t &roll = rcin[0]; int16_t &pitch = rcin[1]; int16_t &throttle = rcin[2]; int16_t &rudder = rcin[3]; // get RC input angles if (rc_input.flags_rc_ok) { for (uint8_t i=0;i<4; i++) { if (mixing.rc_channel[i] > 0 && mixing.rc_channel[i] <= IOMCU_MAX_CHANNELS) { uint8_t chan = mixing.rc_channel[i]-1; if (i == 2 && !mixing.throttle_is_angle) { rcin[i] = mix_input_range(i, rc_input.pwm[chan]); } else { rcin[i] = mix_input_angle(i, rc_input.pwm[chan]); } } } } for (uint8_t i=0; i<IOMCU_MAX_CHANNELS; i++) { SRV_Channel::Aux_servo_function_t function = (SRV_Channel::Aux_servo_function_t)mixing.servo_function[i]; uint16_t &pwm = reg_direct_pwm.pwm[i]; if (mixing.manual_rc_mask & (1U<<i)) { // treat as pass-thru if this channel is set in MANUAL_RC_MASK function = SRV_Channel::k_manual; } switch (function) { case SRV_Channel::k_manual: pwm = rc_input.pwm[i]; break; case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: pwm = rc_input.pwm[(uint8_t)(function - SRV_Channel::k_rcin1)]; break; case SRV_Channel::k_aileron: case SRV_Channel::k_aileron_with_input: case SRV_Channel::k_flaperon_left: case SRV_Channel::k_flaperon_right: pwm = mix_output_angle(i, roll); break; case SRV_Channel::k_elevator: case SRV_Channel::k_elevator_with_input: pwm = mix_output_angle(i, pitch); break; case SRV_Channel::k_rudder: case SRV_Channel::k_steering: pwm = mix_output_angle(i, rudder); break; case SRV_Channel::k_throttle: case SRV_Channel::k_throttleLeft: case SRV_Channel::k_throttleRight: if (mixing.throttle_is_angle) { pwm = mix_output_angle(i, throttle); } else { pwm = mix_output_range(i, throttle); } break; case SRV_Channel::k_flap: case SRV_Channel::k_flap_auto: // zero flaps pwm = mix_output_range(i, 0); break; case SRV_Channel::k_elevon_left: case SRV_Channel::k_dspoilerLeft1: case SRV_Channel::k_dspoilerLeft2: // treat differential spoilers as elevons pwm = mix_output_angle(i, mix_elevon_vtail(roll, pitch, true)); break; case SRV_Channel::k_elevon_right: case SRV_Channel::k_dspoilerRight1: case SRV_Channel::k_dspoilerRight2: // treat differential spoilers as elevons pwm = mix_output_angle(i, mix_elevon_vtail(roll, pitch, false)); break; case SRV_Channel::k_vtail_left: pwm = mix_output_angle(i, mix_elevon_vtail(rudder, pitch, false)); break; case SRV_Channel::k_vtail_right: pwm = mix_output_angle(i, mix_elevon_vtail(rudder, pitch, true)); break; default: break; } } }