// output - update value to send to ESC/Servo
void AP_MotorsHeli_RSC::output(RotorControlState state)
{
switch (state){
case ROTOR_CONTROL_STOP:
// set rotor ramp to decrease speed to zero, this happens instantly inside update_rotor_ramp()
update_rotor_ramp(0.0f);
// control output forced to zero
_control_output = 0;
break;
case ROTOR_CONTROL_IDLE:
// set rotor ramp to decrease speed to zero
update_rotor_ramp(0.0f);
// set rotor control speed to idle speed parameter, this happens instantly and ignore ramping
_control_output = _idle_output;
break;
case ROTOR_CONTROL_ACTIVE:
// set main rotor ramp to increase to full speed
update_rotor_ramp(1.0f);
if ((_control_mode == ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH) || (_control_mode == ROTOR_CONTROL_MODE_SPEED_SETPOINT)) {
// set control rotor speed to ramp slewed value between idle and desired speed
_control_output = _idle_output + (_rotor_ramp_output * (_desired_speed - _idle_output));
} else if (_control_mode == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT) {
// throttle output depending on estimated power demand. Output is ramped up from idle speed during rotor runup.
_control_output = _idle_output + (_rotor_ramp_output * ((_power_output_low - _idle_output) + (_power_output_range * _load_feedforward)));
}
break;
}
// update rotor speed run-up estimate
update_rotor_runup();
// output to rsc servo
write_rsc(_control_output);
}
// output - update value to send to ESC/Servo
void AP_MotorsHeli_RSC::output(RotorControlState state)
{
float dt;
uint64_t now = AP_HAL::micros64();
float last_control_output = _control_output;
if (_last_update_us == 0) {
_last_update_us = now;
dt = 0.001f;
} else {
dt = 1.0e-6f * (now - _last_update_us);
_last_update_us = now;
}
switch (state){
case ROTOR_CONTROL_STOP:
// set rotor ramp to decrease speed to zero, this happens instantly inside update_rotor_ramp()
update_rotor_ramp(0.0f, dt);
// control output forced to zero
_control_output = 0.0f;
break;
case ROTOR_CONTROL_IDLE:
// set rotor ramp to decrease speed to zero
update_rotor_ramp(0.0f, dt);
// set rotor control speed to idle speed parameter, this happens instantly and ignore ramping
_control_output = _idle_output;
break;
case ROTOR_CONTROL_ACTIVE:
// set main rotor ramp to increase to full speed
update_rotor_ramp(1.0f, dt);
if ((_control_mode == ROTOR_CONTROL_MODE_SPEED_PASSTHROUGH) || (_control_mode == ROTOR_CONTROL_MODE_SPEED_SETPOINT)) {
// set control rotor speed to ramp slewed value between idle and desired speed
_control_output = _idle_output + (_rotor_ramp_output * (_desired_speed - _idle_output));
} else if (_control_mode == ROTOR_CONTROL_MODE_OPEN_LOOP_POWER_OUTPUT) {
// throttle output depending on estimated power demand. Output is ramped up from idle speed during rotor runup. A negative load
// is for the left side of the V-curve (-ve collective) A positive load is for the right side (+ve collective)
if (_load_feedforward >= 0) {
float range = _power_output_high - _power_output_low;
_control_output = _idle_output + (_rotor_ramp_output * ((_power_output_low - _idle_output) + (range * _load_feedforward)));
} else {
float range = _power_output_negc - _power_output_low;
_control_output = _idle_output + (_rotor_ramp_output * ((_power_output_low - _idle_output) - (range * _load_feedforward)));
}
}
break;
}
// update rotor speed run-up estimate
update_rotor_runup(dt);
if (_power_slewrate > 0) {
// implement slew rate for throttle
float max_delta = dt * _power_slewrate * 0.01f;
_control_output = constrain_float(_control_output, last_control_output-max_delta, last_control_output+max_delta);
}
// output to rsc servo
write_rsc(_control_output);
}
