void AP_MotorsHeli_Dual::output_to_motors()
{
if (!_flags.initialised_ok) {
return;
}
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
for (uint8_t i=0; i<AP_MOTORS_HELI_DUAL_NUM_SWASHPLATE_SERVOS; i++) {
rc_write_swash(i, _servo_out[i]);
}
switch (_spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
update_motor_control(ROTOR_CONTROL_STOP);
break;
case GROUND_IDLE:
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
update_motor_control(ROTOR_CONTROL_IDLE);
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
// set motor output based on thrust requests
update_motor_control(ROTOR_CONTROL_ACTIVE);
break;
case SPOOL_DOWN:
// sends idle output to motors and wait for rotor to stop
update_motor_control(ROTOR_CONTROL_IDLE);
break;
}
}
void AP_MotorsHeli_Single::output_to_motors()
{
if (!_flags.initialised_ok) {
return;
}
// actually move the servos. PWM is sent based on nominal 1500 center. servo output shifts center based on trim value.
rc_write_swash(AP_MOTORS_MOT_1, _servo1_out);
rc_write_swash(AP_MOTORS_MOT_2, _servo2_out);
rc_write_swash(AP_MOTORS_MOT_3, _servo3_out);
if (_tail_type != AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
}
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_SERVO_EXTGYRO) {
// output gain to exernal gyro
if (_acro_tail && _ext_gyro_gain_acro > 0) {
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_acro);
} else {
rc_write(AP_MOTORS_HELI_SINGLE_EXTGYRO, 1000 + _ext_gyro_gain_std);
}
}
switch (_spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
update_motor_control(ROTOR_CONTROL_STOP);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
}
break;
case GROUND_IDLE:
// sends idle output to motors when armed. rotor could be static or turning (autorotation)
update_motor_control(ROTOR_CONTROL_IDLE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
}
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
// set motor output based on thrust requests
update_motor_control(ROTOR_CONTROL_ACTIVE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
// constrain output so that motor never fully stops
_servo4_out = constrain_float(_servo4_out, -0.9f, 1.0f);
// output yaw servo to tail rsc
rc_write_angle(AP_MOTORS_MOT_4, _servo4_out * YAW_SERVO_MAX_ANGLE);
}
break;
case SPOOL_DOWN:
// sends idle output to motors and wait for rotor to stop
update_motor_control(ROTOR_CONTROL_IDLE);
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_FIXEDPITCH){
rc_write_angle(AP_MOTORS_MOT_4, -YAW_SERVO_MAX_ANGLE);
}
break;
}
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
if (_servo_test_cycle_counter > 0){
// perform boot-up servo test cycle if enabled
servo_test();
} else {
// manual override (i.e. when setting up swash)
switch (_servo_mode) {
case SERVO_CONTROL_MODE_MANUAL_PASSTHROUGH:
// pass pilot commands straight through to swash
_roll_control_input = _roll_radio_passthrough;
_pitch_control_input = _pitch_radio_passthrough;
_throttle_control_input = _throttle_radio_passthrough;
_yaw_control_input = _yaw_radio_passthrough;
break;
case SERVO_CONTROL_MODE_MANUAL_CENTER:
// fixate mid collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = _collective_mid_pwm;
_yaw_control_input = 0;
break;
case SERVO_CONTROL_MODE_MANUAL_MAX:
// fixate max collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = 1000;
_yaw_control_input = 4500;
break;
case SERVO_CONTROL_MODE_MANUAL_MIN:
// fixate min collective
_roll_control_input = 0;
_pitch_control_input = 0;
_throttle_control_input = 0;
_yaw_control_input = -4500;
break;
case SERVO_CONTROL_MODE_MANUAL_OSCILLATE:
// use servo_test function from child classes
servo_test();
break;
default:
// no manual override
break;
}
}
// ensure swash servo endpoints haven't been moved
init_outputs();
// continuously recalculate scalars to allow setup
calculate_scalars();
// helicopters always run stabilizing flight controls
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_STOP);
}
// output_armed_zero_throttle - sends commands to the motors
void AP_MotorsHeli::output_armed_zero_throttle()
{
// if manual override active after arming, deactivate it and reinitialize servos
if (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED) {
reset_flight_controls();
}
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_IDLE);
}
// sends commands to the motors
void AP_MotorsHeli::output_armed_stabilizing()
{
// if manual override active after arming, deactivate it and reinitialize servos
if (_servo_manual == 1) {
reset_flight_controls();
}
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_ACTIVE);
}
void AP_MotorsHeli::output_armed_not_stabilizing()
{
// if manual override active after arming, deactivate it and reinitialize servos
if (_servo_mode != SERVO_CONTROL_MODE_AUTOMATED) {
reset_flight_controls();
}
// helicopters always run stabilizing flight controls
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_ACTIVE);
}
// output_min - sets servos to neutral point with motors stopped
void AP_MotorsHeli::output_min()
{
// move swash to mid
move_actuators(0,0,500,0);
update_motor_control(ROTOR_CONTROL_STOP);
// override limits flags
limit.roll_pitch = true;
limit.yaw = true;
limit.throttle_lower = true;
limit.throttle_upper = false;
}
// output_disarmed - sends commands to the motors
void AP_MotorsHeli::output_disarmed()
{
// 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;
}
// ensure swash servo endpoints haven't been moved
init_outputs();
// continuously recalculate scalars to allow setup
calculate_scalars();
// helicopters always run stabilizing flight controls
move_actuators(_roll_control_input, _pitch_control_input, _throttle_control_input, _yaw_control_input);
update_motor_control(ROTOR_CONTROL_STOP);
}