// write_aux - converts servo_out parameter value (0 to 1 range) to pwm and outputs to aux channel (ch7) void AP_MotorsHeli_Single::write_aux(float servo_out) { if (_servo_aux) { rc_write(AP_MOTORS_HELI_SINGLE_AUX, calc_pwm_output_0to1(servo_out, _servo_aux)); } }
// // move_actuators - moves swash plate and tail rotor // - expected ranges: // roll : -1 ~ +1 // pitch: -1 ~ +1 // collective: 0 ~ 1 // yaw: -1 ~ +1 // void AP_MotorsHeli_Single::move_actuators(float roll_out, float pitch_out, float coll_in, float yaw_out) { float yaw_offset = 0.0f; // initialize limits flag limit.roll_pitch = false; limit.yaw = false; limit.throttle_lower = false; limit.throttle_upper = false; // 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 cyclic_max // coming into this equation at 4500 or less float total_out = norm(pitch_out, roll_out); if (total_out > (_cyclic_max/4500.0f)) { float ratio = (float)(_cyclic_max/4500.0f) / total_out; roll_out *= ratio; pitch_out *= ratio; limit.roll_pitch = true; } // constrain collective input float collective_out = coll_in; if (collective_out <= 0.0f) { collective_out = 0.0f; limit.throttle_lower = true; } if (collective_out >= 1.0f) { collective_out = 1.0f; limit.throttle_upper = true; } // ensure not below landed/landing collective if (_heliflags.landing_collective && collective_out < (_land_collective_min/1000.0f)) { collective_out = (_land_collective_min/1000.0f); limit.throttle_lower = true; } // if servo output not in manual mode, process pre-compensation factors if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) { // rudder feed forward based on collective // the feed-forward is not required when the motor is stopped or at idle, and thus not creating torque // also not required if we are using external gyro if ((_main_rotor.get_control_output() > _main_rotor.get_idle_output()) && _tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) { // sanity check collective_yaw_effect _collective_yaw_effect = constrain_float(_collective_yaw_effect, -AP_MOTORS_HELI_SINGLE_COLYAW_RANGE, AP_MOTORS_HELI_SINGLE_COLYAW_RANGE); yaw_offset = _collective_yaw_effect * fabsf(collective_out - _collective_mid_pct); } } else { yaw_offset = 0.0f; } // feed power estimate into main rotor controller // ToDo: include tail rotor power? // ToDo: add main rotor cyclic power? _main_rotor.set_motor_load(fabsf(collective_out - _collective_mid_pct)); // swashplate servos float collective_scalar = ((float)(_collective_max-_collective_min))/1000.0f; float coll_out_scaled = collective_out * collective_scalar + (_collective_min - 1000)/1000.0f; float servo1_out = ((_rollFactor[CH_1] * roll_out) + (_pitchFactor[CH_1] * pitch_out))*0.45f + _collectiveFactor[CH_1] * coll_out_scaled; float servo2_out = ((_rollFactor[CH_2] * roll_out) + (_pitchFactor[CH_2] * pitch_out))*0.45f + _collectiveFactor[CH_2] * coll_out_scaled; if (_swash_type == AP_MOTORS_HELI_SINGLE_SWASH_H1) { servo1_out += 0.5f; servo2_out += 0.5f; } float servo3_out = ((_rollFactor[CH_3] * roll_out) + (_pitchFactor[CH_3] * pitch_out))*0.45f + _collectiveFactor[CH_3] * coll_out_scaled; hal.rcout->cork(); // actually move the servos rc_write(AP_MOTORS_MOT_1, calc_pwm_output_0to1(servo1_out, _swash_servo_1)); rc_write(AP_MOTORS_MOT_2, calc_pwm_output_0to1(servo2_out, _swash_servo_2)); rc_write(AP_MOTORS_MOT_3, calc_pwm_output_0to1(servo3_out, _swash_servo_3)); // update the yaw rate using the tail rotor/servo move_yaw(yaw_out + yaw_offset); hal.rcout->push(); }