// rotor_ramp - ramps rotor towards target
// result put in _rotor_out and sent to ESC
void AP_MotorsHeli::rotor_ramp(int16_t rotor_target)
{
// return immediately if not ramping required
if (_rsc_mode == AP_MOTORS_HELI_RSC_MODE_NONE) {
_rotor_out = rotor_target;
return;
}
// range check rotor_target
rotor_target = constrain_int16(rotor_target,0,1000);
// ramp rotor esc output towards target
if (_rotor_out < rotor_target) {
// allow rotor out to jump to rotor's current speed
if (_rotor_out < _rotor_speed_estimate) {
_rotor_out = _rotor_speed_estimate;
}
// ramp up slowly to target
_rotor_out += _rsc_ramp_increment;
if (_rotor_out > rotor_target) {
_rotor_out = rotor_target;
}
}else{
// ramping down happens instantly
_rotor_out = rotor_target;
}
// ramp rotor speed estimate towards rotor out
if (_rotor_speed_estimate < _rotor_out) {
_rotor_speed_estimate += _rsc_runup_increment;
if (_rotor_speed_estimate > _rotor_out) {
_rotor_speed_estimate = _rotor_out;
}
}else{
_rotor_speed_estimate -= _rsc_runup_increment;
if (_rotor_speed_estimate < _rotor_out) {
_rotor_speed_estimate = _rotor_out;
}
}
// set runup complete flag
if (!_heliflags.rotor_runup_complete && rotor_target > 0 && _rotor_speed_estimate >= rotor_target) {
_heliflags.rotor_runup_complete = true;
}
if (_heliflags.rotor_runup_complete && _rotor_speed_estimate <= _rsc_critical) {
_heliflags.rotor_runup_complete = false;
}
// output to rsc servo
write_rsc(_rotor_out);
}
// rsc_control - update value to send to tail and main rotor's ESC
// desired_rotor_speed is a desired speed from 0 to 1000
void AP_MotorsHeli::rsc_control()
{
// if disarmed output minimums
if (!armed()) {
// shut down tail rotor
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_VARPITCH || _tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) {
_tail_direct_drive_out = 0;
write_aux(_tail_direct_drive_out);
}
// shut down main rotor
if (_rsc_mode != AP_MOTORS_HELI_RSC_MODE_NONE) {
_rotor_out = 0;
_rotor_speed_estimate = 0;
write_rsc(_rotor_out);
}
return;
}
// ramp up or down main rotor and tail
if (_rotor_desired > 0) {
// ramp up tail rotor (this does nothing if not using direct drive variable pitch tail)
tail_ramp(_direct_drive_tailspeed);
// note: this always returns true if not using direct drive variable pitch tail
if (tail_rotor_runup_complete()) {
rotor_ramp(_rotor_desired);
}
}else{
// shutting down main rotor
rotor_ramp(0);
// if completed shutting down main motor then shut-down tail rotor. Note: this does nothing if not using direct drive vairable pitch tail
if (_rotor_speed_estimate <= 0) {
tail_ramp(0);
}
}
// direct drive fixed pitch tail servo gets copy of yaw servo out (ch4) while main rotor is running
if (_tail_type == AP_MOTORS_HELI_TAILTYPE_DIRECTDRIVE_FIXEDPITCH) {
// output fixed-pitch speed control if Ch8 is high
if (_rotor_desired > 0 || _rotor_speed_estimate > 0) {
// copy yaw output to tail esc
write_aux(_servo_4->servo_out);
}else{
write_aux(0);
}
}
}
// 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);
}
