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