void AP_MotorsCoax::output_to_motors()
{
switch (_spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_roll_radio_passthrough, _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough, _servo4));
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
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_spin_up_ratio * _actuator_out[3], _servo4));
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
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_yt_ccw));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_yt_cw));
break;
}
}
void AP_MotorsTri::output_to_motors()
{
switch (_spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
rc_write(AP_MOTORS_MOT_1, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_2, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_4, get_pwm_output_min());
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
rc_write(AP_MOTORS_MOT_1, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_2, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_MOT_4, calc_spin_up_to_pwm());
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
rc_write(AP_MOTORS_MOT_1, calc_thrust_to_pwm(_thrust_right));
rc_write(AP_MOTORS_MOT_2, calc_thrust_to_pwm(_thrust_left));
rc_write(AP_MOTORS_MOT_4, calc_thrust_to_pwm(_thrust_rear));
rc_write(AP_MOTORS_CH_TRI_YAW, calc_yaw_radio_output(_pivot_angle, radians(_yaw_servo_angle_max_deg)));
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_MotorsMatrix::output_to_motors()
{
int8_t i;
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = get_pwm_output_min();
}
}
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle);
}
}
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]) {
motor_out[i] = calc_thrust_to_pwm(_thrust_rpyt_out[i]);
}
}
break;
}
// send output to each motor
hal.rcout->cork();
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, motor_out[i]); //第二处
}
}
hal.rcout->push();
}
// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
// also sets throttle channel minimum and maximum pwm
void AP_MotorsMulticopter::set_throttle_range(uint16_t min_throttle, int16_t radio_min, int16_t radio_max)
{
// sanity check
if ((radio_max > radio_min) && (min_throttle < (radio_max - radio_min))) {
_throttle_radio_min = radio_min;
_throttle_radio_max = radio_max;
}
// update _min_throttle
_min_throttle = (float)min_throttle * ((get_pwm_output_max() - get_pwm_output_min()) / 1000.0f);
}
// passes throttle directly to all motors for ESC calibration.
// throttle_input is in the range of 0 ~ 1 where 0 will send get_pwm_output_min() and 1 will send get_pwm_output_max()
void AP_MotorsMulticopter::set_throttle_passthrough_for_esc_calibration(float throttle_input)
{
if (armed()) {
uint16_t pwm_out = get_pwm_output_min() + constrain_float(throttle_input, 0.0f, 1.0f) * (get_pwm_output_max() - get_pwm_output_min());
// send the pilot's input directly to each enabled motor
for (uint16_t i=0; i < AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, pwm_out);
}
}
}
}
// set_throttle_range - sets the minimum throttle that will be sent to the engines when they're not off (i.e. to prevents issues with some motors spinning and some not at very low throttle)
// also sets throttle channel minimum and maximum pwm
void AP_MotorsMulticopter::set_throttle_range(int16_t radio_min, int16_t radio_max)
{
// sanity check
if (radio_max <= radio_min) {
return;
}
_throttle_radio_min = radio_min;
_throttle_radio_max = radio_max;
hal.rcout->set_esc_scaling(get_pwm_output_min(), get_pwm_output_max());
}
void AP_MotorsSingle::output_to_motors()
{
switch (_multicopter_flags.spool_mode) {
case SHUT_DOWN:
// sends minimum values out to the motors
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_roll_radio_passthrough - _yaw_radio_passthrough, _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_pitch_radio_passthrough - _yaw_radio_passthrough, _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(-_roll_radio_passthrough - _yaw_radio_passthrough, _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(-_pitch_radio_passthrough - _yaw_radio_passthrough, _servo4));
rc_write(AP_MOTORS_MOT_5, get_pwm_output_min());
rc_write(AP_MOTORS_MOT_6, get_pwm_output_min());
hal.rcout->push();
break;
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_throttle_low_end_pct * _actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
rc_write(AP_MOTORS_MOT_6, constrain_int16(get_pwm_output_min() + _throttle_low_end_pct * _min_throttle, get_pwm_output_min(), get_pwm_output_min() + _min_throttle));
hal.rcout->push();
break;
case SPOOL_UP:
case THROTTLE_UNLIMITED:
case SPOOL_DOWN:
// set motor output based on thrust requests
hal.rcout->cork();
rc_write(AP_MOTORS_MOT_1, calc_pwm_output_1to1(_actuator_out[0], _servo1));
rc_write(AP_MOTORS_MOT_2, calc_pwm_output_1to1(_actuator_out[1], _servo2));
rc_write(AP_MOTORS_MOT_3, calc_pwm_output_1to1(_actuator_out[2], _servo3));
rc_write(AP_MOTORS_MOT_4, calc_pwm_output_1to1(_actuator_out[3], _servo4));
rc_write(AP_MOTORS_MOT_5, calc_thrust_to_pwm(_thrust_out));
rc_write(AP_MOTORS_MOT_6, calc_thrust_to_pwm(_thrust_out));
hal.rcout->push();
break;
}
}
// output a thrust to all motors that match a given motor mask. This
// is used to control tiltrotor motors in forward flight. Thrust is in
// the range 0 to 1
void AP_MotorsMulticopter::output_motor_mask(float thrust, uint8_t mask)
{
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
int16_t motor_out;
if (mask & (1U<<i)) {
motor_out = calc_thrust_to_pwm(thrust);
} else {
motor_out = get_pwm_output_min();
}
rc_write(i, motor_out);
}
}
}
void AP_MotorsMatrix::output_to_motors()
{
int8_t i;
int16_t motor_out[AP_MOTORS_MAX_NUM_MOTORS]; // final pwm values sent to the motor
switch (_spool_mode) {
case SHUT_DOWN: {
// sends minimum values out to the motors
// set motor output based on thrust requests
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
if (_disarm_disable_pwm && _disarm_safety_timer == 0 && !armed()) {
motor_out[i] = 0;
} else {
motor_out[i] = get_pwm_output_min();
}
}
}
break;
}
case SPIN_WHEN_ARMED:
// sends output to motors when armed but not flying
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
motor_out[i] = calc_spin_up_to_pwm();
}
}
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]) {
motor_out[i] = calc_thrust_to_pwm(_thrust_rpyt_out[i]);
}
}
break;
}
// send output to each motor
hal.rcout->cork();
for (i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
rc_write(i, motor_out[i]);
}
}
hal.rcout->push();
}
// output a thrust to all motors that match a given motor mask. This
// is used to control motors enabled for forward flight. Thrust is in
// the range 0 to 1
void AP_MotorsMatrixTS::output_motor_mask(float thrust, uint8_t mask, float rudder_dt)
{
const int16_t pwm_min = get_pwm_output_min();
const int16_t pwm_range = get_pwm_output_max() - pwm_min;
for (uint8_t i=0; i<AP_MOTORS_MAX_NUM_MOTORS; i++) {
if (motor_enabled[i]) {
int16_t motor_out;
if (mask & (1U<<i)) {
/*
apply rudder mixing differential thrust
copter frame roll is plane frame yaw (this is only
used by tiltrotors and tailsitters)
*/
float diff_thrust = get_roll_factor(i) * rudder_dt * 0.5f;
motor_out = pwm_min + pwm_range * constrain_float(thrust + diff_thrust, 0.0f, 1.0f);
} else {
motor_out = pwm_min;
}
rc_write(i, motor_out);
}
}
}
int16_t AP_MotorsMulticopter::calc_spin_up_to_pwm() const
{
return get_pwm_output_min() + constrain_float(_spin_up_ratio, 0.0f, 1.0f) * _spin_min * (get_pwm_output_max()-get_pwm_output_min());
}
int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
{
thrust_in = constrain_float(thrust_in, 0.0f, 1.0f);
return get_pwm_output_min() + (get_pwm_output_max()-get_pwm_output_min()) * (_spin_min + (_spin_max-_spin_min)*apply_thrust_curve_and_volt_scaling(thrust_in));
}
int16_t AP_MotorsMulticopter::calc_thrust_to_pwm(float thrust_in) const
{
thrust_in = constrain_float(thrust_in, 0, 1);
return constrain_int16((get_pwm_output_min() + _min_throttle + apply_thrust_curve_and_volt_scaling(thrust_in) *
(get_pwm_output_max() - (get_pwm_output_min() + _min_throttle))), get_pwm_output_min() + _min_throttle, get_pwm_output_max());
}
