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