@ -67,6 +67,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
@@ -67,6 +67,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
_yaw_ctrl . set_max_rate ( radians ( _param_fw_acro_z_max . get ( ) ) ) ;
_rate_ctrl_status_pub . advertise ( ) ;
_spoiler_setpoint_with_slewrate . setSlewRate ( kSpoilerSlewRate ) ;
_flaps_setpoint_with_slewrate . setSlewRate ( kFlapSlewRate ) ;
}
FixedwingAttitudeControl : : ~ FixedwingAttitudeControl ( )
@ -382,11 +384,14 @@ void FixedwingAttitudeControl::Run()
@@ -382,11 +384,14 @@ void FixedwingAttitudeControl::Run()
/* if we are in rotary wing mode, do nothing */
if ( _vehicle_status . vehicle_type = = vehicle_status_s : : VEHICLE_TYPE_ROTARY_WING & & ! _vehicle_status . is_vtol ) {
_spoiler_setpoint_with_slewrate . setForcedValue ( 0.f ) ;
_flaps_setpoint_with_slewrate . setForcedValue ( 0.f ) ;
perf_end ( _loop_perf ) ;
return ;
}
control_flaps ( dt ) ;
controlFlaps ( dt ) ;
controlSpoilers ( dt ) ;
/* decide if in stabilized or full manual control */
if ( _vcontrol_mode . flag_control_rates_enabled ) {
@ -498,8 +503,11 @@ void FixedwingAttitudeControl::Run()
@@ -498,8 +503,11 @@ void FixedwingAttitudeControl::Run()
}
/* add trim increment if flaps are deployed */
trim_roll + = _flaps_applied * _param_fw_dtrim_r_flps . get ( ) ;
trim_pitch + = _flaps_applied * _param_fw_dtrim_p_flps . get ( ) ;
trim_roll + = _flaps_setpoint_with_slewrate . getState ( ) * _param_fw_dtrim_r_flps . get ( ) ;
trim_pitch + = _flaps_setpoint_with_slewrate . getState ( ) * _param_fw_dtrim_p_flps . get ( ) ;
// add trim increment from spoilers (only pitch)
trim_pitch + = _spoiler_setpoint_with_slewrate . getState ( ) * _param_fw_dtrim_p_spoil . get ( ) ;
/* Run attitude controllers */
if ( _vcontrol_mode . flag_control_attitude_enabled ) {
@ -645,11 +653,11 @@ void FixedwingAttitudeControl::Run()
@@ -645,11 +653,11 @@ void FixedwingAttitudeControl::Run()
_actuators . control [ actuator_controls_s : : INDEX_YAW ] + = _param_fw_rll_to_yaw_ff . get ( )
* constrain ( _actuators . control [ actuator_controls_s : : INDEX_ROLL ] , - 1.0f , 1.0f ) ;
_actuators . control [ actuator_controls_s : : INDEX_FLAPS ] = _flaps_applied ;
_actuators . control [ 5 ] = _manual_control_setpoint . aux1 ;
_actuators . control [ actuator_controls_s : : INDEX_AIRBRAKES ] = _flaperons_applied ;
_actuators . control [ actuator_controls_s : : INDEX_FLAPS ] = _flaps_setpoint_with_slewrate . getState ( ) ;
_actuators . control [ actuator_controls_s : : INDEX_SPOILERS ] = _spoiler_setpoint_with_slewrate . getState ( ) ;
_actuators . control [ actuator_controls_s : : INDEX_AIRBRAKES ] = 0.f ;
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
_actuators . control [ 7 ] = _manual_control_setpoint . aux3 ;
_actuators . control [ actuator_controls_s : : INDEX_LANDING_GEAR ] = _manual_control_setpoint . aux3 ;
/* lazily publish the setpoint only once available */
_actuators . timestamp = hrt_absolute_time ( ) ;
@ -697,18 +705,16 @@ void FixedwingAttitudeControl::publishThrustSetpoint(const hrt_abstime ×tam
@@ -697,18 +705,16 @@ void FixedwingAttitudeControl::publishThrustSetpoint(const hrt_abstime ×tam
_vehicle_thrust_setpoint_pub . publish ( v_thrust_sp ) ;
}
void FixedwingAttitudeControl : : control_f laps ( const float dt )
void FixedwingAttitudeControl : : controlF laps ( const float dt )
{
/* default flaps to center */
float flap_control = 0.0f ;
/* map flaps by default to manual if valid */
if ( PX4_ISFINITE ( _manual_control_setpoint . flaps ) & & _vcontrol_mode . flag_control_manual_enabled
& & fabsf ( _param_fw_flaps_scl . get ( ) ) > 0.01f ) {
flap_control = _manual_control_setpoint . flaps * _param_fw_flaps_scl . get ( ) ;
if ( PX4_ISFINITE ( _manual_control_setpoint . flaps ) & & _vcontrol_mode . flag_control_manual_enabled ) {
flap_control = _manual_control_setpoint . flaps ;
} else if ( _vcontrol_mode . flag_control_auto_enabled
& & fabsf ( _param_fw_flaps_scl . get ( ) ) > 0.01f ) {
} else if ( _vcontrol_mode . flag_control_auto_enabled ) {
switch ( _att_sp . apply_flaps ) {
case vehicle_attitude_setpoint_s : : FLAPS_OFF :
@ -716,50 +722,54 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
@@ -716,50 +722,54 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
break ;
case vehicle_attitude_setpoint_s : : FLAPS_LAND :
flap_control = 1.0f * _param_fw_flaps_scl . get ( ) * _param_fw_flaps_lnd_scl . get ( ) ;
flap_control = _param_fw_flaps_lnd_scl . get ( ) ;
break ;
case vehicle_attitude_setpoint_s : : FLAPS_TAKEOFF :
flap_control = 1.0f * _param_fw_flaps_scl . get ( ) * _param_fw_flaps_to_scl . get ( ) ;
flap_control = _param_fw_flaps_to_scl . get ( ) ;
break ;
}
}
// move the actual control value continuous with time, full flap travel in 1sec
if ( fabsf ( _flaps_applied - flap_control ) > 0.01f ) {
_flaps_applied + = ( _flaps_applied - flap_control ) < 0 ? dt : - dt ;
_flaps_setpoint_with_slewrate . update ( flap_control , dt ) ;
}
} else {
_flaps_applied = flap_control ;
}
void FixedwingAttitudeControl : : controlSpoilers ( const float dt )
{
float spoiler_control = 0.f ;
/* default flaperon to center */
float flaperon_control = 0.0f ;
if ( _vcontrol_mode . flag_control_manual_enabled ) {
switch ( _param_fw_spoilers_man . get ( ) ) {
case 0 :
break ;
/* map flaperons by default to manual if valid */
if ( PX4_ISFINITE ( _manual_control_setpoint . aux2 ) & & _vcontrol_mode . flag_control_manual_enabled
& & fabsf ( _param_fw_flaperon_scl . get ( ) ) > 0.01f ) {
case 1 :
spoiler_control = PX4_ISFINITE ( _manual_control_setpoint . flaps ) ? _manual_control_setpoint . flaps : 0.f ;
break ;
flaperon_control = 0.5f * ( _manual_control_setpoint . aux2 + 1.0f ) * _param_fw_flaperon_scl . get ( ) ;
case 2 :
spoiler_control = PX4_ISFINITE ( _manual_control_setpoint . aux1 ) ? _manual_control_setpoint . aux1 : 0.f ;
break ;
}
} else if ( _vcontrol_mode . flag_control_auto_enabled
& & fabsf ( _param_fw_flaperon_scl . get ( ) ) > 0.01f ) {
} else if ( _vcontrol_mode . flag_control_auto_enabled ) {
switch ( _att_sp . apply_spoilers ) {
case vehicle_attitude_setpoint_s : : SPOILERS_OFF :
spoiler_control = 0.f ;
break ;
if ( _att_sp . apply_flaps = = vehicle_attitude_setpoint_s : : FLAPS_LAND ) {
flaperon_control = _param_fw_flaperon_scl . get ( ) ;
case vehicle_attitude_setpoint_s : : SPOILERS_LAND :
spoiler_control = _param_fw_spoilers_lnd . get ( ) ;
break ;
} else {
flaperon_control = 0.0f ;
case vehicle_attitude_setpoint_s : : SPOILERS_DESCEND :
spoiler_control = _param_fw_spoilers_desc . get ( ) ;
break ;
}
}
// move the actual control value continuous with time, full flap travel in 1sec
if ( fabsf ( _flaperons_applied - flaperon_control ) > 0.01f ) {
_flaperons_applied + = ( _flaperons_applied - flaperon_control ) < 0 ? dt : - dt ;
} else {
_flaperons_applied = flaperon_control ;
}
_spoiler_setpoint_with_slewrate . update ( spoiler_control , dt ) ;
}
void FixedwingAttitudeControl : : updateActuatorControlsStatus ( float dt )
Silvan Fuhrer
3 years ago