// move_yaw
void AP_MotorsHeli_Single::move_yaw(float yaw_out)
{
// sanity check yaw_out
if (yaw_out < -1.0f) {
yaw_out = -1.0f;
limit.yaw = true;
}
if (yaw_out > 1.0f) {
yaw_out = 1.0f;
limit.yaw = true;
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
if (_main_rotor.get_desired_speed() > 0.0f && hal.util->get_soft_armed()) {
// constrain output so that motor never fully stops
yaw_out = constrain_float(yaw_out, -0.9f, 1.0f);
// output yaw servo to tail rsc
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(yaw_out, _yaw_servo));
} else {
// output zero speed to tail rsc
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-1.0f, _yaw_servo));
}
} else {
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(yaw_out, _yaw_servo));
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// output gain to exernal gyro
if (_acro_tail && _ext_gyro_gain_acro > 0) {
write_aux(_ext_gyro_gain_acro/1000.0f);
} else {
write_aux(_ext_gyro_gain_std/1000.0f);
}
}
}
// move_yaw
void AP_MotorsHeli_Single::move_yaw(float yaw_out)
{
// sanity check yaw_out
if (yaw_out < -1.0f) {
yaw_out = -1.0f;
limit.yaw = true;
}
if (yaw_out > 1.0f) {
yaw_out = 1.0f;
limit.yaw = true;
}
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(yaw_out, _yaw_servo));
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// output gain to exernal gyro
if (_acro_tail && _ext_gyro_gain_acro > 0) {
write_aux(_ext_gyro_gain_acro/1000.0f);
} else {
write_aux(_ext_gyro_gain_std/1000.0f);
}
} else if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH && _main_rotor.get_desired_speed() > 0.0f) {
// output yaw servo to tail rsc
// To-Do: fix this messy calculation
write_aux(yaw_out*0.5f+1.0f);
}
}
//
// 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;
if (_heliflags.inverted_flight) {
coll_in = 1 - coll_in;
}
// 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);
// the 4.5 scaling factor is to bring the values in line with previous releases
yaw_offset = _collective_yaw_effect * fabsf(collective_out - _collective_mid_pct) / 4.5f;
}
} else {
yaw_offset = 0.0f;
}
// feed power estimate into main rotor controller
// ToDo: include tail rotor power?
// ToDo: add main rotor cyclic power?
if (collective_out > _collective_mid_pct) {
// +ve motor load for +ve collective
_main_rotor.set_motor_load((collective_out - _collective_mid_pct) / (1.0f - _collective_mid_pct));
} else {
// -ve motor load for -ve collective
_main_rotor.set_motor_load((collective_out - _collective_mid_pct) / _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;
// rescale from -1..1, so we can use the pwm calc that includes trim
servo1_out = 2*servo1_out - 1;
servo2_out = 2*servo2_out - 1;
servo3_out = 2*servo3_out - 1;
// actually move the servos
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(servo1_out, _swash_servo_1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(servo2_out, _swash_servo_2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(servo3_out, _swash_servo_3));
// update the yaw rate using the tail rotor/servo
move_yaw(yaw_out + yaw_offset);
}
void AP_MotorsSingle::output_to_motors()
{
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_roll_radio_passthrough+_yaw_radio_passthrough, _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough+_yaw_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough+_yaw_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough+_yaw_radio_passthrough, _servo4));
rc_write(AP_MOTORS_MOT_5, _throttle_radio_min);
rc_write(AP_MOTORS_MOT_6, _throttle_radio_min);
hal.rcout->push();
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
rc_write(AP_MOTORS_MOT_6, constrain_int16(_throttle_radio_min + _throttle_low_end_pct * _min_throttle, _throttle_radio_min, _throttle_radio_min + _min_throttle));
hal.rcout->push();
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out));
hal.rcout->push();
break;
}
}
void AP_MotorsSingle::output_to_motors()
{
switch (_spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_roll_radio_passthrough - _yaw_radio_passthrough, _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough - _yaw_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough - _yaw_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough - _yaw_radio_passthrough, _servo4));
rc_write(AP_MOTORS_MOT_5, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_6, get_pwm_output_min());
hal.rcout->push();
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm());
hal.rcout->push();
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out));
hal.rcout->push();
break;
}
}
//
// move_actuators - moves swash plate to attitude of parameters passed in
// - expected ranges:
// roll : -1 ~ +1
// pitch: -1 ~ +1
// collective: 0 ~ 1
// yaw: -1 ~ +1
//
void AP_MotorsHeli_Dual::move_actuators(float roll_out, float pitch_out, float collective_in, float yaw_out)
{
// initialize limits flag
limit.roll_pitch = false;
limit.yaw = false;
limit.throttle_lower = false;
limit.throttle_upper = false;
if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) {
if (pitch_out < -_cyclic_max/4500.0f) {
pitch_out = -_cyclic_max/4500.0f;
limit.roll_pitch = true;
}
if (pitch_out > _cyclic_max/4500.0f) {
pitch_out = _cyclic_max/4500.0f;
limit.roll_pitch = true;
}
} else {
if (roll_out < -_cyclic_max/4500.0f) {
roll_out = -_cyclic_max/4500.0f;
limit.roll_pitch = true;
}
if (roll_out > _cyclic_max/4500.0f) {
roll_out = _cyclic_max/4500.0f;
limit.roll_pitch = true;
}
}
float yaw_compensation = 0.0f;
// if servo output not in manual mode, process pre-compensation factors
if (_servo_mode == SERVO_CONTROL_MODE_AUTOMATED) {
// add differential collective pitch yaw compensation
if (_dual_mode == AP_MOTORS_HELI_DUAL_MODE_TRANSVERSE) {
yaw_compensation = _dcp_yaw_effect * roll_out;
} else { // AP_MOTORS_HELI_DUAL_MODE_TANDEM
yaw_compensation = _dcp_yaw_effect * pitch_out;
}
yaw_out = yaw_out + yaw_compensation;
}
// scale yaw and update limits
if (yaw_out < -_cyclic_max/4500.0f) {
yaw_out = -_cyclic_max/4500.0f;
limit.yaw = true;
}
if (yaw_out > _cyclic_max/4500.0f) {
yaw_out = _cyclic_max/4500.0f;
limit.yaw = true;
}
// constrain collective input
float collective_out = collective_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;
}
// Set rear collective to midpoint if required
float collective2_out = collective_out;
if (_servo_mode == SERVO_CONTROL_MODE_MANUAL_CENTER) {
collective2_out = _collective2_mid_pct;
}
// 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;
}
// scale collective pitch for front swashplate (servos 1,2,3)
float collective_scaler = ((float)(_collective_max-_collective_min))/1000.0f;
float collective_out_scaled = collective_out * collective_scaler + (_collective_min - 1000)/1000.0f;
// scale collective pitch for rear swashplate (servos 4,5,6)
float collective2_scaler = ((float)(_collective2_max-_collective2_min))/1000.0f;
float collective2_out_scaled = collective2_out * collective2_scaler + (_collective2_min - 1000)/1000.0f;
// feed power estimate into main rotor controller
// ToDo: add main rotor cyclic power?
_rotor.set_motor_load(fabsf(collective_out - _collective_mid_pct));
// swashplate servos
float servo1_out = (_rollFactor[CH_1] * roll_out + _pitchFactor[CH_1] * pitch_out + _yawFactor[CH_1] * yaw_out)/0.45f + _collectiveFactor[CH_1] * collective_out_scaled;
float servo2_out = (_rollFactor[CH_2] * roll_out + _pitchFactor[CH_2] * pitch_out + _yawFactor[CH_2] * yaw_out)/0.45f + _collectiveFactor[CH_2] * collective_out_scaled;
float servo3_out = (_rollFactor[CH_3] * roll_out + _pitchFactor[CH_3] * pitch_out + _yawFactor[CH_3] * yaw_out)/0.45f + _collectiveFactor[CH_3] * collective_out_scaled;
float servo4_out = (_rollFactor[CH_4] * roll_out + _pitchFactor[CH_4] * pitch_out + _yawFactor[CH_4] * yaw_out)/0.45f + _collectiveFactor[CH_4] * collective2_out_scaled;
float servo5_out = (_rollFactor[CH_5] * roll_out + _pitchFactor[CH_5] * pitch_out + _yawFactor[CH_5] * yaw_out)/0.45f + _collectiveFactor[CH_5] * collective2_out_scaled;
float servo6_out = (_rollFactor[CH_6] * roll_out + _pitchFactor[CH_6] * pitch_out + _yawFactor[CH_6] * yaw_out)/0.45f + _collectiveFactor[CH_6] * collective2_out_scaled;
// rescale from -1..1, so we can use the pwm calc that includes trim
servo1_out = 2*servo1_out - 1;
servo2_out = 2*servo2_out - 1;
servo3_out = 2*servo3_out - 1;
servo4_out = 2*servo4_out - 1;
servo5_out = 2*servo5_out - 1;
servo6_out = 2*servo6_out - 1;
// actually move the servos
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(servo1_out, _swash_servo_1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(servo2_out, _swash_servo_2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(servo3_out, _swash_servo_3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(servo4_out, _swash_servo_4));
rc_write(AP_MOTORS_MOT_5, calc_pwm_output_1to1(servo5_out, _swash_servo_5));
rc_write(AP_MOTORS_MOT_6, calc_pwm_output_1to1(servo6_out, _swash_servo_6));
}
