@ -15,10 +15,7 @@
@@ -15,10 +15,7 @@
# include <stdlib.h>
# include <AP_HAL/AP_HAL.h>
< < < < < < < HEAD
= = = = = = =
# include <GCS_MAVLink/GCS.h>
> > > > > > > 5f aab0ee08 . . . AP_Motors : tradheli support for turbine start
# include "AP_MotorsHeli_RSC.h"
# include <AP_RPM/AP_RPM.h>
@ -272,8 +269,8 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
@@ -272,8 +269,8 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
governor_reset ( ) ;
_autothrottle = false ;
_governor_fault = false ;
//turbine start flag on
_starting = true ;
//turbine start flag on
_starting = true ;
break ;
case ROTOR_CONTROL_IDLE :
@ -289,25 +286,24 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
@@ -289,25 +286,24 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
_control_output = constrain_float ( _rsc_arot_bailout_pct / 100.0f , 0.0f , 0.4f ) ;
} else {
// set rotor control speed to idle speed parameter, this happens instantly and ignores ramping
if ( _turbine_start & & _starting = = true ) {
_control_output + = 0.001f ;
if ( _control_output > = 1.0f ) {
_control_output = get_idle_output ( ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Turbine startup " ) ;
_starting = false ;
}
} else {
if ( _cooldown_time > 0 ) {
_control_output = get_idle_output ( ) * 1.5f ;
_fast_idle_timer + = dt ;
if ( _fast_idle_timer > ( float ) _cooldown_time ) {
_fast_idle_timer = 0.0f ;
if ( _turbine_start & & _starting = = true ) {
_control_output + = 0.001f ;
if ( _control_output > = 1.0f ) {
_control_output = get_idle_output ( ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " Turbine startup " ) ;
_starting = false ;
}
} else {
if ( _cooldown_time > 0 ) {
_control_output = get_idle_output ( ) * 1.5f ;
_fast_idle_timer + = dt ;
if ( _fast_idle_timer > ( float ) _cooldown_time ) {
_fast_idle_timer = 0.0f ;
}
} else {
_control_output = get_idle_output ( ) ;
}
} else {
_control_output = get_idle_output ( ) ;
}
}
}
break ;
@ -315,6 +311,9 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
@@ -315,6 +311,9 @@ void AP_MotorsHeli_RSC::output(RotorControlState state)
// set main rotor ramp to increase to full speed
update_rotor_ramp ( 1.0f , dt ) ;
// if turbine engine started without using start sequence, set starting flag just to be sure it can't be triggered when back in idle
_starting = false ;
if ( ( _control_mode = = ROTOR_CONTROL_MODE_PASSTHROUGH ) | | ( _control_mode = = ROTOR_CONTROL_MODE_SETPOINT ) ) {
// set control rotor speed to ramp slewed value between idle and desired speed
_control_output = get_idle_output ( ) + ( _rotor_ramp_output * ( _desired_speed - get_idle_output ( ) ) ) ;