// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- /* 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 . */ /* * AP_MotorsHeli.cpp - ArduCopter motors library * Code by RandyMackay. DIYDrones.com * */ #include #include #include "AP_MotorsHeli.h" extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_MotorsHeli::var_info[] PROGMEM = { // @Param: SV1_POS // @DisplayName: Servo 1 Position // @Description: Angular location of swash servo #1 // @Range: -180 180 // @Units: Degrees // @User: Standard // @Increment: 1 AP_GROUPINFO("SV1_POS", 1, AP_MotorsHeli, _servo1_pos, AP_MOTORS_HELI_SERVO1_POS), // @Param: SV2_POS // @DisplayName: Servo 2 Position // @Description: Angular location of swash servo #2 // @Range: -180 180 // @Units: Degrees // @User: Standard // @Increment: 1 AP_GROUPINFO("SV2_POS", 2, AP_MotorsHeli, _servo2_pos, AP_MOTORS_HELI_SERVO2_POS), // @Param: SV3_POS // @DisplayName: Servo 3 Position // @Description: Angular location of swash servo #3 // @Range: -180 180 // @Units: Degrees // @User: Standard // @Increment: 1 AP_GROUPINFO("SV3_POS", 3, AP_MotorsHeli, _servo3_pos, AP_MOTORS_HELI_SERVO3_POS), // @Param: ROL_MAX // @DisplayName: Swash Roll Angle Max // @Description: Maximum roll angle of the swash plate // @Range: 0 18000 // @Units: Centi-Degrees // @Increment: 100 // @User: Advanced AP_GROUPINFO("ROL_MAX", 4, AP_MotorsHeli, _roll_max, AP_MOTORS_HELI_SWASH_ROLL_MAX), // @Param: PIT_MAX // @DisplayName: Swash Pitch Angle Max // @Description: Maximum pitch angle of the swash plate // @Range: 0 18000 // @Units: Centi-Degrees // @Increment: 100 // @User: Advanced AP_GROUPINFO("PIT_MAX", 5, AP_MotorsHeli, _pitch_max, AP_MOTORS_HELI_SWASH_PITCH_MAX), // @Param: COL_MIN // @DisplayName: Collective Pitch Minimum // @Description: Lowest possible servo position for the swashplate // @Range: 1000 2000 // @Units: PWM // @Increment: 1 // @User: Standard AP_GROUPINFO("COL_MIN", 6, AP_MotorsHeli, _collective_min, AP_MOTORS_HELI_COLLECTIVE_MIN), // @Param: COL_MAX // @DisplayName: Collective Pitch Maximum // @Description: Highest possible servo position for the swashplate // @Range: 1000 2000 // @Units: PWM // @Increment: 1 // @User: Standard AP_GROUPINFO("COL_MAX", 7, AP_MotorsHeli, _collective_max, AP_MOTORS_HELI_COLLECTIVE_MAX), // @Param: COL_MID // @DisplayName: Collective Pitch Mid-Point // @Description: Swash servo position corresponding to zero collective pitch (or zero lift for Assymetrical blades) // @Range: 1000 2000 // @Units: PWM // @Increment: 1 // @User: Standard AP_GROUPINFO("COL_MID", 8, AP_MotorsHeli, _collective_mid, AP_MOTORS_HELI_COLLECTIVE_MID), // @Param: TAIL_TYPE // @DisplayName: Tail Type // @Description: Tail type selection. Simpler yaw controller used if external gyro is selected // @Values: 0:Servo only,1:Servo with ExtGyro,2:DirectDrive VarPitch,3:DirectDrive FixedPitch // @User: Standard AP_GROUPINFO("TAIL_TYPE",9, AP_MotorsHeli, _tail_type, AP_MOTORS_HELI_TAILTYPE_SERVO), // @Param: SWASH_TYPE // @DisplayName: Swash Type // @Description: Swash Type Setting - either 3-servo CCPM or H1 Mechanical Mixing // @Values: 0:3-Servo CCPM, 1:H1 Mechanical Mixing // @User: Standard AP_GROUPINFO("SWASH_TYPE",10, AP_MotorsHeli, _swash_type, AP_MOTORS_HELI_SWASH_CCPM), // @Param: GYR_GAIN // @DisplayName: External Gyro Gain // @Description: PWM sent to external gyro on ch7 when tail type is Servo w/ ExtGyro // @Range: 0 1000 // @Units: PWM // @Increment: 1 // @User: Standard AP_GROUPINFO("GYR_GAIN", 11, AP_MotorsHeli, _ext_gyro_gain, AP_MOTORS_HELI_EXT_GYRO_GAIN), // @Param: SV_MAN // @DisplayName: Manual Servo Mode // @Description: Pass radio inputs directly to servos for set-up. Do not set this manually! // @Values: 0:Disabled,1:Enabled // @User: Standard AP_GROUPINFO("SV_MAN", 12, AP_MotorsHeli, _servo_manual, 0), // @Param: PHANG // @DisplayName: Swashplate Phase Angle Compensation // @Description: Phase angle correction for rotor head. If pitching the swash forward induces a roll, this can be correct the problem // @Range: -90 90 // @Units: Degrees // @User: Advanced // @Increment: 1 AP_GROUPINFO("PHANG", 13, AP_MotorsHeli, _phase_angle, 0), // @Param: COLYAW // @DisplayName: Collective-Yaw Mixing // @Description: Feed-forward compensation to automatically add rudder input when collective pitch is increased. Can be positive or negative depending on mechanics. // @Range: -10 10 // @Increment: 0.1 AP_GROUPINFO("COLYAW", 14, AP_MotorsHeli, _collective_yaw_effect, 0), // @Param: GOV_SETPOINT // @DisplayName: External Motor Governor Setpoint // @Description: PWM passed to the external motor governor when external governor is enabled // @Range: 0 1000 // @Units: PWM // @Increment: 10 // @User: Standard AP_GROUPINFO("RSC_SETPOINT", 15, AP_MotorsHeli, _rsc_setpoint, AP_MOTORS_HELI_RSC_SETPOINT), // @Param: RSC_MODE // @DisplayName: Rotor Speed Control Mode // @Description: Controls the source of the desired rotor speed, either ch8 or RSC_SETPOINT // @Values: 0:None, 1:Ch8 Input, 2:SetPoint // @User: Standard AP_GROUPINFO("RSC_MODE", 16, AP_MotorsHeli, _rsc_mode, AP_MOTORS_HELI_RSC_MODE_CH8_PASSTHROUGH), // @Param: FLYBAR_MODE // @DisplayName: Flybar Mode Selector // @Description: Flybar present or not. Affects attitude controller used during ACRO flight mode // @Range: 0:NoFlybar 1:Flybar // @User: Standard AP_GROUPINFO("FLYBAR_MODE", 17, AP_MotorsHeli, _flybar_mode, AP_MOTORS_HELI_NOFLYBAR), // @Param: LAND_COL_MIN // @DisplayName: Landing Collective Minimum // @Description: Minimum collective position while landed or landing // @Range: 0 500 // @Units: pwm // @Increment: 1 // @User: Standard AP_GROUPINFO("LAND_COL_MIN", 18, AP_MotorsHeli, _land_collective_min, AP_MOTORS_HELI_LAND_COLLECTIVE_MIN), // @Param: RSC_RAMP_TIME // @DisplayName: RSC Ramp Time // @Description: Time in seconds for the output to the main rotor's ESC to reach full speed // @Range: 0 60 // @Units: Seconds // @User: Standard AP_GROUPINFO("RSC_RAMP_TIME", 19, AP_MotorsHeli,_rsc_ramp_time, AP_MOTORS_HELI_RSC_RAMP_TIME), // @Param: RSC_RUNUP_TIME // @DisplayName: RSC Runup Time // @Description: Time in seconds for the main rotor to reach full speed. Must be longer than RSC_RAMP_TIME // @Range: 0 60 // @Units: Seconds // @User: Standard AP_GROUPINFO("RSC_RUNUP_TIME", 20, AP_MotorsHeli,_rsc_runup_time, AP_MOTORS_HELI_RSC_RUNUP_TIME), // @Param: TAIL_SPEED // @DisplayName: Direct Drive VarPitch Tail ESC speed // @Description: Direct Drive VarPitch Tail ESC speed. Only used when TailType is DirectDrive VarPitch // @Range: 0 1000 // @Units: PWM // @Increment: 1 // @User: Standard AP_GROUPINFO("TAIL_SPEED", 21, AP_MotorsHeli, _direct_drive_tailspeed, AP_MOTOR_HELI_DDTAIL_DEFAULT), // @Param: RSC_CRITICAL // @DisplayName: Critical Rotor Speed // @Description: Rotor speed below which flight is not possible // @Range: 0 1000 // @Increment: 10 // @User: Standard AP_GROUPINFO("RSC_CRITICAL", 22, AP_MotorsHeli, _rsc_critical, AP_MOTORS_HELI_RSC_CRITICAL), // parameters 1 ~ 29 reserved for tradheli // parameters 30 ~ 39 reserved for tricopter // parameters 40 ~ 49 for single copter and coax copter (these have identical parameter files) AP_GROUPEND }; // // public methods // // init void AP_MotorsHeli::Init() { // set update rate set_update_rate(_speed_hz); // ensure inputs are not passed through to servos _servo_manual = 0; // initialise some scalers recalc_scalers(); // initialise swash plate init_swash(); // disable channels 7 and 8 from being used by RC_Channel_aux RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_AUX]); RC_Channel_aux::disable_aux_channel(_motor_to_channel_map[AP_MOTORS_HELI_RSC]); } // set update rate to motors - a value in hertz void AP_MotorsHeli::set_update_rate( uint16_t speed_hz ) { // record requested speed _speed_hz = speed_hz; // setup fast channels uint32_t mask = 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]) | 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]) | 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]) | 1U << pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]); hal.rcout->set_freq(mask, _speed_hz); } // enable - starts allowing signals to be sent to motors void AP_MotorsHeli::enable() { // enable output channels hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1])); // swash servo 1 hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2])); // swash servo 2 hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3])); // swash servo 3 hal.rcout->enable_ch(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4])); // yaw hal.rcout->enable_ch(AP_MOTORS_HELI_AUX); // output for gyro gain or direct drive variable pitch tail motor hal.rcout->enable_ch(AP_MOTORS_HELI_RSC); // output for main rotor esc } // output - sends commands to the servos void AP_MotorsHeli::output() { // update throttle filter update_throttle_filter(); if (_flags.armed) { if (!_flags.interlock) { output_armed_zero_throttle(); } else if (_flags.stabilizing) { output_armed_stabilizing(); } else { output_armed_not_stabilizing(); } } else { output_disarmed(); } }; // output_min - sets servos to neutral point void AP_MotorsHeli::output_min() { // move swash to mid move_swash(0,0,500,0); // override limits flags limit.roll_pitch = true; limit.yaw = true; limit.throttle_lower = true; limit.throttle_upper = false; } // output_test - spin a motor at the pwm value specified // motor_seq is the motor's sequence number from 1 to the number of motors on the frame // pwm value is an actual pwm value that will be output, normally in the range of 1000 ~ 2000 void AP_MotorsHeli::output_test(uint8_t motor_seq, int16_t pwm) { // exit immediately if not armed if (!armed()) { return; } // output to motors and servos switch (motor_seq) { case 1: // swash servo 1 hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), pwm); break; case 2: // swash servo 2 hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), pwm); break; case 3: // swash servo 3 hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), pwm); break; case 4: // external gyro & tail servo if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { write_aux(_ext_gyro_gain); } hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), pwm); break; case 5: // main rotor hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_HELI_RSC]), pwm); break; default: // do nothing break; } } // allow_arming - check if it's safe to arm bool AP_MotorsHeli::allow_arming() const { // returns false if main rotor speed is not zero if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE && _rotor_speed_estimate > 0) { return false; } // all other cases it is OK to arm return true; } // parameter_check - check if helicopter specific parameters are sensible bool AP_MotorsHeli::parameter_check() const { // returns false if _rsc_setpoint is not higher than _rsc_critical as this would not allow rotor_runup_complete to ever return true if (_rsc_critical >= _rsc_setpoint) { return false; } // all other cases parameters are OK return true; } // return true if the main rotor is up to speed bool AP_MotorsHeli::rotor_runup_complete() const { // if we have no control of motors, assume pilot has spun them up if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) { return true; } return _heliflags.rotor_runup_complete; } // recalc_scalers - recalculates various scalers used. Should be called at about 1hz to allow users to see effect of changing parameters void AP_MotorsHeli::recalc_scalers() { // recalculate rotor ramp up increment if (_rsc_ramp_time <= 0) { _rsc_ramp_time = 1; } _rsc_ramp_increment = 1000.0f / (_rsc_ramp_time * _loop_rate); // recalculate rotor runup increment if (_rsc_runup_time <= 0 ) { _rsc_runup_time = 1; } if (_rsc_runup_time < _rsc_ramp_time) { _rsc_runup_time = _rsc_ramp_time; } _rsc_runup_increment = 1000.0f / (_rsc_runup_time * _loop_rate); } // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict uint16_t AP_MotorsHeli::get_motor_mask() { // heli uses channels 1,2,3,4,7 and 8 return (1U << 0 | 1U << 1 | 1U << 2 | 1U << 3 | 1U << AP_MOTORS_HELI_AUX | 1U << AP_MOTORS_HELI_RSC); } void AP_MotorsHeli::output_armed_not_stabilizing() { // stabilizing servos always operate for helicopters output_armed_stabilizing(); } // sends commands to the motors void AP_MotorsHeli::output_armed_stabilizing() { // if manual override (i.e. when setting up swash), pass pilot commands straight through to swash if (_servo_manual == 1) { _roll_control_input = _roll_radio_passthrough; _pitch_control_input = _pitch_radio_passthrough; _throttle_control_input = _throttle_radio_passthrough; _yaw_control_input = _yaw_radio_passthrough; } move_swash(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input); // update rotor and direct drive esc speeds rsc_control(); } // output_armed_zero_throttle - sends commands to the motors void AP_MotorsHeli::output_armed_zero_throttle() { // stabilizing servos always operate for helicopters // ToDo: Bring RSC Master On/Off into this function output_armed_stabilizing(); } // output_disarmed - sends commands to the motors void AP_MotorsHeli::output_disarmed() { // stabilizing servos always operate for helicopters output_armed_stabilizing(); } // reset_swash - free up swash for maximum movements. Used for set-up void AP_MotorsHeli::reset_swash() { // free up servo ranges _servo_1.radio_min = 1000; _servo_1.radio_max = 2000; _servo_2.radio_min = 1000; _servo_2.radio_max = 2000; _servo_3.radio_min = 1000; _servo_3.radio_max = 2000; // calculate factors based on swash type and servo position calculate_roll_pitch_collective_factors(); // set roll, pitch and throttle scaling _roll_scaler = 1.0f; _pitch_scaler = 1.0f; _collective_scalar = ((float)(_throttle_radio_max - _throttle_radio_min))/1000.0f; _collective_scalar_manual = 1.0f; // we must be in set-up mode so mark swash as uninitialised _heliflags.swash_initialised = false; } // init_swash - initialise the swash plate void AP_MotorsHeli::init_swash() { // swash servo initialisation _servo_1.set_range(0,1000); _servo_2.set_range(0,1000); _servo_3.set_range(0,1000); _servo_4.set_angle(4500); // range check collective min, max and mid if( _collective_min >= _collective_max ) { _collective_min = 1000; _collective_max = 2000; } _collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max); // calculate collective mid point as a number from 0 to 1000 _collective_mid_pwm = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min))*1000.0f; // determine roll, pitch and collective input scaling _roll_scaler = (float)_roll_max/4500.0f; _pitch_scaler = (float)_pitch_max/4500.0f; _collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f; // calculate factors based on swash type and servo position calculate_roll_pitch_collective_factors(); // servo min/max values _servo_1.radio_min = 1000; _servo_1.radio_max = 2000; _servo_2.radio_min = 1000; _servo_2.radio_max = 2000; _servo_3.radio_min = 1000; _servo_3.radio_max = 2000; // mark swash as initialised _heliflags.swash_initialised = true; } // calculate_roll_pitch_collective_factors - calculate factors based on swash type and servo position void AP_MotorsHeli::calculate_roll_pitch_collective_factors() { if (_swash_type == AP_MOTORS_HELI_SWASH_CCPM) { //CCPM Swashplate, perform control mixing // roll factors _rollFactor[CH_1] = cosf(radians(_servo1_pos + 90 - (_phase_angle + _delta_phase_angle))); _rollFactor[CH_2] = cosf(radians(_servo2_pos + 90 - (_phase_angle + _delta_phase_angle))); _rollFactor[CH_3] = cosf(radians(_servo3_pos + 90 - (_phase_angle + _delta_phase_angle))); // pitch factors _pitchFactor[CH_1] = cosf(radians(_servo1_pos - (_phase_angle + _delta_phase_angle))); _pitchFactor[CH_2] = cosf(radians(_servo2_pos - (_phase_angle + _delta_phase_angle))); _pitchFactor[CH_3] = cosf(radians(_servo3_pos - (_phase_angle + _delta_phase_angle))); // collective factors _collectiveFactor[CH_1] = 1; _collectiveFactor[CH_2] = 1; _collectiveFactor[CH_3] = 1; }else{ //H1 Swashplate, keep servo outputs seperated // roll factors _rollFactor[CH_1] = 1; _rollFactor[CH_2] = 0; _rollFactor[CH_3] = 0; // pitch factors _pitchFactor[CH_1] = 0; _pitchFactor[CH_2] = 1; _pitchFactor[CH_3] = 0; // collective factors _collectiveFactor[CH_1] = 0; _collectiveFactor[CH_2] = 0; _collectiveFactor[CH_3] = 1; } } // // heli_move_swash - moves swash plate to attitude of parameters passed in // - expected ranges: // roll : -4500 ~ 4500 // pitch: -4500 ~ 4500 // collective: 0 ~ 1000 // yaw: -4500 ~ 4500 // void AP_MotorsHeli::move_swash(int16_t roll_out, int16_t pitch_out, int16_t coll_in, int16_t yaw_out) { int16_t yaw_offset = 0; int16_t coll_out_scaled; // initialize limits flag limit.roll_pitch = false; limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; if (_servo_manual == 1) { // are we in manual servo mode? (i.e. swash set-up mode)? // check if we need to free up the swash if (_heliflags.swash_initialised) { reset_swash(); } // To-Do: This equation seems to be wrong. It probably restricts swash movement so that swash setup doesn't work right. // _collective_scalar should probably not be used or set to 1? coll_out_scaled = coll_in * _collective_scalar + _throttle_radio_min - 1000; }else{ // regular flight mode // check if we need to reinitialise the swash if (!_heliflags.swash_initialised) { init_swash(); } // rescale roll_out and pitch-out into the min and max ranges to provide linear motion // across the input range instead of stopping when the input hits the constrain value // these calculations are based on an assumption of the user specified roll_max and pitch_max // coming into this equation at 4500 or less, and based on the original assumption of the // total _servo_x.servo_out range being -4500 to 4500. roll_out = roll_out * _roll_scaler; if (roll_out < -_roll_max) { roll_out = -_roll_max; limit.roll_pitch = true; } if (roll_out > _roll_max) { roll_out = _roll_max; limit.roll_pitch = true; } // scale pitch and update limits pitch_out = pitch_out * _pitch_scaler; if (pitch_out < -_pitch_max) { pitch_out = -_pitch_max; limit.roll_pitch = true; } if (pitch_out > _pitch_max) { pitch_out = _pitch_max; limit.roll_pitch = true; } // constrain collective input _collective_out = coll_in; if (_collective_out <= 0) { _collective_out = 0; limit.throttle_lower = true; } if (_collective_out >= 1000) { _collective_out = 1000; limit.throttle_upper = true; } // ensure not below landed/landing collective if (_heliflags.landing_collective && _collective_out < _land_collective_min) { _collective_out = _land_collective_min; limit.throttle_lower = true; } // scale collective pitch coll_out_scaled = _collective_out * _collective_scalar + _collective_min - 1000; // rudder feed forward based on collective // the feed-forward is not required when the motor is shut down and not creating torque // also not required if we are using external gyro if ((_desired_rotor_speed > 0) && _tail_type != AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { // sanity check collective_yaw_effect _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTOR_HELI_COLYAW_RANGE, AP_MOTOR_HELI_COLYAW_RANGE); yaw_offset = _collective_yaw_effect * abs(_collective_out - _collective_mid_pwm); } } // swashplate servos _servo_1.servo_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out)/10 + _collectiveFactor[CH_1] * coll_out_scaled + (_servo_1.radio_trim-1500); _servo_2.servo_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out)/10 + _collectiveFactor[CH_2] * coll_out_scaled + (_servo_2.radio_trim-1500); if (_swash_type == AP_MOTORS_HELI_SWASH_H1) { _servo_1.servo_out += 500; _servo_2.servo_out += 500; } _servo_3.servo_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out)/10 + _collectiveFactor[CH_3] * coll_out_scaled + (_servo_3.radio_trim-1500); _servo_4.servo_out = yaw_out + yaw_offset; // constrain yaw and update limits if (_servo_4.servo_out < -4500) { _servo_4.servo_out = -4500; limit.yaw = true; } if (_servo_4.servo_out > 4500) { _servo_4.servo_out = 4500; limit.yaw = true; } // use servo_out to calculate pwm_out and radio_out _servo_1.calc_pwm(); _servo_2.calc_pwm(); _servo_3.calc_pwm(); _servo_4.calc_pwm(); // actually move the servos hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_1]), _servo_1.radio_out); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_2]), _servo_2.radio_out); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_3]), _servo_3.radio_out); hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_MOT_4]), _servo_4.radio_out); // output gain to exernal gyro if (_tail_type == AP_MOTORS_HELI_TAILTYPE_SERVO_EXTGYRO) { write_aux(_ext_gyro_gain); } } // rsc_control - update value to send to tail and main rotor's ESC // desired_rotor_speed is a desired speed from 0 to 1000 void AP_MotorsHeli::rsc_control() { // if disarmed output minimums if (!armed()) { // shut down tail rotor if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) { _tail_direct_drive_out = 0; write_aux(_tail_direct_drive_out); } // shut down main rotor if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE) { _rotor_out = 0; _rotor_speed_estimate = 0; write_rsc(_rotor_out); } return; } // ramp up or down main rotor and tail if (_desired_rotor_speed > 0) { // ramp up tail rotor (this does nothing if not using direct drive variable pitch tail) tail_ramp(_direct_drive_tailspeed); // note: this always returns true if not using direct drive variable pitch tail if (tail_rotor_runup_complete()) { rotor_ramp(_desired_rotor_speed); } }else{ // shutting down main rotor rotor_ramp(0); // shut-down tail rotor. Note: this does nothing if not using direct drive vairable pitch tail tail_ramp(0); } // direct drive fixed pitch tail servo gets copy of yaw servo out (ch4) while main rotor is running if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) { // output fixed-pitch speed control if Ch8 is high if (_desired_rotor_speed > 0 || _rotor_speed_estimate > 0) { // copy yaw output to tail esc write_aux(_servo_4.servo_out); }else{ write_aux(0); } } } // rotor_ramp - ramps rotor towards target // result put in _rotor_out and sent to ESC void AP_MotorsHeli::rotor_ramp(int16_t rotor_target) { // return immediately if not ramping required if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) { _rotor_out = rotor_target; return; } // range check rotor_target rotor_target = constrain_int16(rotor_target,0,1000); // ramp rotor esc output towards target if (_rotor_out < rotor_target) { // allow rotor out to jump to rotor's current speed if (_rotor_out < _rotor_speed_estimate) { _rotor_out = _rotor_speed_estimate; } // ramp up slowly to target _rotor_out += _rsc_ramp_increment; if (_rotor_out > rotor_target) { _rotor_out = rotor_target; } }else{ // ramping down happens instantly _rotor_out = rotor_target; } // ramp rotor speed estimate towards rotor out if (_rotor_speed_estimate < _rotor_out) { _rotor_speed_estimate += _rsc_runup_increment; if (_rotor_speed_estimate > _rotor_out) { _rotor_speed_estimate = _rotor_out; } }else{ _rotor_speed_estimate -= _rsc_runup_increment; if (_rotor_speed_estimate < _rotor_out) { _rotor_speed_estimate = _rotor_out; } } // set runup complete flag if (!_heliflags.rotor_runup_complete && rotor_target > 0 && _rotor_speed_estimate >= rotor_target) { _heliflags.rotor_runup_complete = true; } if (_heliflags.rotor_runup_complete && _rotor_speed_estimate <= _rsc_critical) { _heliflags.rotor_runup_complete = false; } // output to rsc servo write_rsc(_rotor_out); } // tail_ramp - ramps tail motor towards target. Only used for direct drive variable pitch tails // results put into _tail_direct_drive_out and sent to ESC void AP_MotorsHeli::tail_ramp(int16_t tail_target) { // return immediately if not ramping required if (_tail_type != AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { _tail_direct_drive_out = tail_target; return; } // range check tail_target tail_target = constrain_int16(tail_target,0,1000); // ramp towards target if (_tail_direct_drive_out < tail_target) { _tail_direct_drive_out += AP_MOTORS_HELI_TAIL_RAMP_INCREMENT; if (_tail_direct_drive_out >= tail_target) { _tail_direct_drive_out = tail_target; } }else if(_tail_direct_drive_out > tail_target) { _tail_direct_drive_out -= AP_MOTORS_HELI_TAIL_RAMP_INCREMENT; if (_tail_direct_drive_out < tail_target) { _tail_direct_drive_out = tail_target; } } // output to tail servo write_aux(_tail_direct_drive_out); } // return true if the tail rotor is up to speed bool AP_MotorsHeli::tail_rotor_runup_complete() { // always return true if not using direct drive variable pitch tails if (_tail_type != AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH) { return true; } // check speed return (armed() && _tail_direct_drive_out >= _direct_drive_tailspeed); } // write_rsc - outputs pwm onto output rsc channel (ch8) // servo_out parameter is of the range 0 ~ 1000 void AP_MotorsHeli::write_rsc(int16_t servo_out) { _servo_rsc.servo_out = servo_out; _servo_rsc.calc_pwm(); hal.rcout->write(AP_MOTORS_HELI_RSC, _servo_rsc.radio_out); } // write_aux - outputs pwm onto output aux channel (ch7) // servo_out parameter is of the range 0 ~ 1000 void AP_MotorsHeli::write_aux(int16_t servo_out) { _servo_aux.servo_out = servo_out; _servo_aux.calc_pwm(); hal.rcout->write(AP_MOTORS_HELI_AUX, _servo_aux.radio_out); } // set_delta_phase_angle for setting variable phase angle compensation and force // recalculation of collective factors void AP_MotorsHeli::set_delta_phase_angle(int16_t angle) { angle = constrain_int16(angle, -90, 90); _delta_phase_angle = angle; calculate_roll_pitch_collective_factors(); } // update the throttle input filter void AP_MotorsHeli::update_throttle_filter() { _throttle_filter.apply(_throttle_in, 1.0f/_loop_rate); // constrain throttle signal to 0-1000 _throttle_control_input = constrain_float(_throttle_filter.get(),0.0f,1000.0f); } // set_radio_passthrough used to pass radio inputs directly to outputs void AP_MotorsHeli::set_radio_passthrough(int16_t radio_roll_input, int16_t radio_pitch_input, int16_t radio_throttle_input, int16_t radio_yaw_input) { _roll_radio_passthrough = radio_roll_input; _pitch_radio_passthrough = radio_pitch_input; _throttle_radio_passthrough = radio_throttle_input; _yaw_radio_passthrough = radio_yaw_input; } // reset_radio_passthrough used to reset all radio inputs to center void AP_MotorsHeli::reset_radio_passthrough() { _roll_radio_passthrough = 0; _pitch_radio_passthrough = 0; _throttle_radio_passthrough = 500; _yaw_radio_passthrough = 0; }