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