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; } }
void AP_MotorsSingle::output_to_motors() { if (!_flags.initialised_ok) { return; } switch (_spool_mode) { case SHUT_DOWN: // sends minimum values out to the motors rc_write_angle(AP_MOTORS_MOT_1, _roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_3, -_roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_4, -_pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write(AP_MOTORS_MOT_5, get_pwm_output_min()); rc_write(AP_MOTORS_MOT_6, get_pwm_output_min()); break; case SPIN_WHEN_ARMED: // sends output to motors when armed but not flying for (uint8_t i=0; i<NUM_ACTUATORS; i++) { rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); } rc_write(AP_MOTORS_MOT_5, calc_spin_up_to_pwm()); rc_write(AP_MOTORS_MOT_6, calc_spin_up_to_pwm()); break; case SPOOL_UP: case THROTTLE_UNLIMITED: case SPOOL_DOWN: // set motor output based on thrust requests for (uint8_t i=0; i<NUM_ACTUATORS; i++) { rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); } rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out)); rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out)); break; } }
void AP_MotorsSingle::output_to_motors() { if (!_flags.initialised_ok) { return; } switch (_spool_state) { case SpoolState::SHUT_DOWN: // sends minimum values out to the motors rc_write_angle(AP_MOTORS_MOT_1, _roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_2, _pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_3, -_roll_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write_angle(AP_MOTORS_MOT_4, -_pitch_radio_passthrough * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); rc_write(AP_MOTORS_MOT_5, output_to_pwm(0)); rc_write(AP_MOTORS_MOT_6, output_to_pwm(0)); break; case SpoolState::GROUND_IDLE: // sends output to motors when armed but not flying for (uint8_t i=0; i<NUM_ACTUATORS; i++) { rc_write_angle(AP_MOTORS_MOT_1+i, _spin_up_ratio * _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); } set_actuator_with_slew(_actuator[5], actuator_spin_up_to_ground_idle()); set_actuator_with_slew(_actuator[6], actuator_spin_up_to_ground_idle()); rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5])); rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6])); break; case SpoolState::SPOOLING_UP: case SpoolState::THROTTLE_UNLIMITED: case SpoolState::SPOOLING_DOWN: // set motor output based on thrust requests for (uint8_t i=0; i<NUM_ACTUATORS; i++) { rc_write_angle(AP_MOTORS_MOT_1+i, _actuator_out[i] * AP_MOTORS_SINGLE_SERVO_INPUT_RANGE); } set_actuator_with_slew(_actuator[5], thrust_to_actuator(_thrust_out)); set_actuator_with_slew(_actuator[6], thrust_to_actuator(_thrust_out)); rc_write(AP_MOTORS_MOT_5, output_to_pwm(_actuator[5])); rc_write(AP_MOTORS_MOT_6, output_to_pwm(_actuator[6])); break; } }