// calculate_scalars - recalculates various scalers used.
void AP_MotorsHeli_Single::calculate_scalars()
{
// range check collective min, max and mid
if( _collective_min >= _collective_max ) {
_collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN;
_collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX;
}
_collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
// calculate collective mid point as a number from 0 to 1
_collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
// calculate factors based on swash type and servo position
calculate_roll_pitch_collective_factors();
// send setpoints to main rotor controller and trigger recalculation of scalars
_main_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));
calculate_armed_scalars();
// send setpoints to tail rotor controller and trigger recalculation of scalars
if (_tail_type == AP_MOTORS_HELI_SINGLE_TAILTYPE_DIRECTDRIVE_VARPITCH) {
_tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_SPEED_SETPOINT);
_tail_rotor.set_ramp_time(AP_MOTORS_HELI_SINGLE_DDVPT_RAMP_TIME);
_tail_rotor.set_runup_time(AP_MOTORS_HELI_SINGLE_DDVPT_RUNUP_TIME);
_tail_rotor.set_critical_speed(_rsc_critical/1000.0f);
_tail_rotor.set_idle_output(_rsc_idle_output/1000.0f);
} else {
_tail_rotor.set_control_mode(ROTOR_CONTROL_MODE_DISABLED);
_tail_rotor.set_ramp_time(0);
_tail_rotor.set_runup_time(0);
_tail_rotor.set_critical_speed(0);
_tail_rotor.set_idle_output(0);
}
}
// calculate_scalars
void AP_MotorsHeli_Dual::calculate_scalars()
{
// range check collective min, max and mid
if( _collective_min >= _collective_max ) {
_collective_min = AP_MOTORS_HELI_COLLECTIVE_MIN;
_collective_max = AP_MOTORS_HELI_COLLECTIVE_MAX;
}
// range check collective min, max and mid for rear swashplate
if( _collective2_min >= _collective2_max ) {
_collective2_min = AP_MOTORS_HELI_DUAL_COLLECTIVE2_MIN;
_collective2_max = AP_MOTORS_HELI_DUAL_COLLECTIVE2_MAX;
}
_collective_mid = constrain_int16(_collective_mid, _collective_min, _collective_max);
_collective2_mid = constrain_int16(_collective2_mid, _collective2_min, _collective2_max);
// calculate collective mid point as a number from 0 to 1000
_collective_mid_pct = ((float)(_collective_mid-_collective_min))/((float)(_collective_max-_collective_min));
_collective2_mid_pct = ((float)(_collective2_mid-_collective2_min))/((float)(_collective2_max-_collective2_min));
// calculate factors based on swash type and servo position
calculate_roll_pitch_collective_factors();
// set mode of main rotor controller and trigger recalculation of scalars
_rotor.set_control_mode(static_cast<RotorControlMode>(_rsc_mode.get()));
calculate_armed_scalars();
}
// output - sends commands to the servos
void AP_MotorsHeli::output()
{
// update throttle filter
update_throttle_filter();
if (_flags.armed) {
calculate_armed_scalars();
if (!_flags.interlock) {
output_armed_zero_throttle();
} else {
output_armed_stabilizing();
}
} else {
output_disarmed();
}
};
