Esempio n. 1
0
void AP_MotorsMatrix::output_to_motors()
{
    int8_t i;

    switch (_spool_mode) {
        case SHUT_DOWN: {
            // no output
            for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motor_enabled[i]) {
                    _actuator[i] = 0.0f;
                }
            }
            break;
        }
        case GROUND_IDLE:
            // sends output to motors when armed but not flying
            for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motor_enabled[i]) {
                    set_actuator_with_slew(_actuator[i], actuator_spin_up_to_ground_idle());
                }
            }
            break;
        case SPOOL_UP:
        case THROTTLE_UNLIMITED:
        case SPOOL_DOWN:
            // set motor output based on thrust requests
            for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
                if (motor_enabled[i]) {
                    set_actuator_with_slew(_actuator[i], thrust_to_actuator(_thrust_rpyt_out[i]));
                }
            }
            break;
    }

    // convert output to PWM and send to each motor
    for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
        if (motor_enabled[i]) {
            rc_write(i, output_to_pwm(_actuator[i]));
        }
    }
}
Esempio n. 2
0
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;
    }
}