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