mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-24 15:33:34 +08:00
Add new fixed wing rate control module (fw_rate_control)
* Fixedwing rate control into a separate module * Start fw_rate_control for vtol * Move over airspeed related parameters to fw_rate_control Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
JaeyoungLim
co-authored by
Silvan Fuhrer
Silvan Fuhrer
parent
4c2c06060d
commit
21e88f64b4
@@ -1,6 +1,6 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
@@ -37,29 +37,16 @@ using namespace time_literals;
|
||||
using namespace matrix;
|
||||
|
||||
using math::constrain;
|
||||
using math::interpolate;
|
||||
using math::radians;
|
||||
|
||||
FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
|
||||
_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
|
||||
_actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)),
|
||||
_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
|
||||
{
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
|
||||
// set initial maximum body rate setpoints
|
||||
_roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
|
||||
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
|
||||
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
|
||||
_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()
|
||||
@@ -78,116 +65,55 @@ FixedwingAttitudeControl::init()
|
||||
return true;
|
||||
}
|
||||
|
||||
int
|
||||
void
|
||||
FixedwingAttitudeControl::parameters_update()
|
||||
{
|
||||
/* pitch control parameters */
|
||||
_pitch_ctrl.set_time_constant(_param_fw_p_tc.get());
|
||||
|
||||
/* roll control parameters */
|
||||
_roll_ctrl.set_time_constant(_param_fw_r_tc.get());
|
||||
_roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
|
||||
|
||||
_pitch_ctrl.set_time_constant(_param_fw_p_tc.get());
|
||||
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get()));
|
||||
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get()));
|
||||
|
||||
_yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
|
||||
|
||||
/* wheel control parameters */
|
||||
_wheel_ctrl.set_k_p(_param_fw_wr_p.get());
|
||||
_wheel_ctrl.set_k_i(_param_fw_wr_i.get());
|
||||
_wheel_ctrl.set_k_ff(_param_fw_wr_ff.get());
|
||||
_wheel_ctrl.set_integrator_max(_param_fw_wr_imax.get());
|
||||
_wheel_ctrl.set_max_rate(radians(_param_fw_w_rmax.get()));
|
||||
|
||||
const Vector3f rate_p = Vector3f(_param_fw_rr_p.get(), _param_fw_pr_p.get(), _param_fw_yr_p.get());
|
||||
const Vector3f rate_i = Vector3f(_param_fw_rr_i.get(), _param_fw_pr_i.get(), _param_fw_yr_i.get());
|
||||
const Vector3f rate_d = Vector3f(_param_fw_rr_d.get(), _param_fw_pr_d.get(), _param_fw_yr_d.get());
|
||||
|
||||
_rate_control.setGains(rate_p, rate_i, rate_d);
|
||||
|
||||
_rate_control.setIntegratorLimit(
|
||||
Vector3f(_param_fw_rr_imax.get(), _param_fw_pr_imax.get(), _param_fw_yr_imax.get()));
|
||||
|
||||
_rate_control.setFeedForwardGain(
|
||||
// set FF gains to 0 as we add the FF control outside of the rate controller
|
||||
Vector3f(0.f, 0.f, 0.f));
|
||||
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_control_mode_poll()
|
||||
{
|
||||
_vcontrol_mode_sub.update(&_vcontrol_mode);
|
||||
|
||||
if (_vehicle_status.is_vtol) {
|
||||
const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|
||||
&& !_vehicle_status.in_transition_mode;
|
||||
const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _vehicle_status.is_vtol_tailsitter;
|
||||
|
||||
if (is_hovering || is_tailsitter_transition) {
|
||||
_vcontrol_mode.flag_control_attitude_enabled = false;
|
||||
_vcontrol_mode.flag_control_manual_enabled = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body)
|
||||
{
|
||||
const bool is_tailsitter_transition = _vehicle_status.is_vtol_tailsitter && _vehicle_status.in_transition_mode;
|
||||
const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
|
||||
|
||||
if (_vcontrol_mode.flag_control_manual_enabled && (!is_tailsitter_transition || is_fixed_wing)) {
|
||||
if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
|
||||
|
||||
// Always copy the new manual setpoint, even if it wasn't updated, to fill the actuators with valid values
|
||||
if (_manual_control_setpoint_sub.copy(&_manual_control_setpoint)) {
|
||||
|
||||
if (!_vcontrol_mode.flag_control_climb_rate_enabled) {
|
||||
if (!_vcontrol_mode.flag_control_climb_rate_enabled & _vcontrol_mode.flag_control_attitude_enabled) {
|
||||
|
||||
const float throttle = (_manual_control_setpoint.throttle + 1.f) * .5f;
|
||||
// STABILIZED mode generate the attitude setpoint from manual user inputs
|
||||
|
||||
if (_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
// STABILIZED mode generate the attitude setpoint from manual user inputs
|
||||
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());
|
||||
|
||||
_att_sp.roll_body = _manual_control_setpoint.roll * radians(_param_fw_man_r_max.get());
|
||||
_att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get())
|
||||
+ radians(_param_fw_psp_off.get());
|
||||
_att_sp.pitch_body = constrain(_att_sp.pitch_body,
|
||||
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
|
||||
|
||||
_att_sp.pitch_body = -_manual_control_setpoint.pitch * radians(_param_fw_man_p_max.get())
|
||||
+ radians(_param_fw_psp_off.get());
|
||||
_att_sp.pitch_body = constrain(_att_sp.pitch_body,
|
||||
-radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get()));
|
||||
_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
|
||||
_att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f;
|
||||
|
||||
_att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw
|
||||
_att_sp.thrust_body[0] = throttle;
|
||||
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
|
||||
q.copyTo(_att_sp.q_d);
|
||||
|
||||
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
|
||||
q.copyTo(_att_sp.q_d);
|
||||
_att_sp.reset_integral = false;
|
||||
|
||||
_att_sp.reset_integral = false;
|
||||
_att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
_att_sp.timestamp = hrt_absolute_time();
|
||||
|
||||
_attitude_sp_pub.publish(_att_sp);
|
||||
|
||||
} else if (_vcontrol_mode.flag_control_rates_enabled &&
|
||||
!_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
|
||||
// RATE mode we need to generate the rate setpoint from manual user inputs
|
||||
_rates_sp.timestamp = hrt_absolute_time();
|
||||
_rates_sp.roll = _manual_control_setpoint.roll * radians(_param_fw_acro_x_max.get());
|
||||
_rates_sp.pitch = -_manual_control_setpoint.pitch * radians(_param_fw_acro_y_max.get());
|
||||
_rates_sp.yaw = _manual_control_setpoint.yaw * radians(_param_fw_acro_z_max.get());
|
||||
_rates_sp.thrust_body[0] = throttle;
|
||||
|
||||
_rate_sp_pub.publish(_rates_sp);
|
||||
|
||||
} else {
|
||||
/* manual/direct control */
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
|
||||
_manual_control_setpoint.roll * _param_fw_man_r_sc.get() + _param_trim_roll.get();
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
|
||||
-_manual_control_setpoint.pitch * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
|
||||
_manual_control_setpoint.yaw * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = throttle;
|
||||
_landing_gear_wheel.normalized_wheel_setpoint = _manual_control_setpoint.yaw;
|
||||
}
|
||||
_attitude_sp_pub.publish(_att_sp);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -215,7 +141,7 @@ FixedwingAttitudeControl::vehicle_land_detected_poll()
|
||||
}
|
||||
}
|
||||
|
||||
float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
|
||||
float FixedwingAttitudeControl::get_airspeed_constrained()
|
||||
{
|
||||
_airspeed_validated_sub.update();
|
||||
const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s)
|
||||
@@ -238,19 +164,7 @@ float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* For scaling our actuators using anything less than the stall
|
||||
* speed doesn't make any sense - its the strongest reasonable deflection we
|
||||
* want to do in flight and it's the baseline a human pilot would choose.
|
||||
*
|
||||
* Forcing the scaling to this value allows reasonable handheld tests.
|
||||
*/
|
||||
const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(),
|
||||
_param_fw_airspd_max.get()), 0.1f, 1000.0f);
|
||||
|
||||
_airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f;
|
||||
|
||||
return airspeed;
|
||||
return math::constrain(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_max.get());
|
||||
}
|
||||
|
||||
void FixedwingAttitudeControl::Run()
|
||||
@@ -267,7 +181,7 @@ void FixedwingAttitudeControl::Run()
|
||||
if (_att_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) {
|
||||
|
||||
// only update parameters if they changed
|
||||
bool params_updated = _parameter_update_sub.updated();
|
||||
const bool params_updated = _parameter_update_sub.updated();
|
||||
|
||||
// check for parameter updates
|
||||
if (params_updated) {
|
||||
@@ -303,11 +217,7 @@ void FixedwingAttitudeControl::Run()
|
||||
|
||||
vehicle_angular_velocity_s angular_velocity{};
|
||||
_vehicle_rates_sub.copy(&angular_velocity);
|
||||
float rollspeed = angular_velocity.xyz[0];
|
||||
float pitchspeed = angular_velocity.xyz[1];
|
||||
float yawspeed = angular_velocity.xyz[2];
|
||||
const Vector3f rates(rollspeed, pitchspeed, yawspeed);
|
||||
const Vector3f angular_accel{angular_velocity.xyz_derivative};
|
||||
float yawspeed = angular_velocity.xyz[2]; // only used for wheel controller
|
||||
|
||||
if (_vehicle_status.is_vtol_tailsitter) {
|
||||
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
|
||||
@@ -347,9 +257,7 @@ void FixedwingAttitudeControl::Run()
|
||||
_R = R_adapted;
|
||||
|
||||
/* lastly, roll- and yawspeed have to be swaped */
|
||||
float helper = rollspeed;
|
||||
rollspeed = -yawspeed;
|
||||
yawspeed = helper;
|
||||
yawspeed = angular_velocity.xyz[0];
|
||||
}
|
||||
|
||||
const matrix::Eulerf euler_angles(_R);
|
||||
@@ -358,10 +266,15 @@ void FixedwingAttitudeControl::Run()
|
||||
|
||||
vehicle_attitude_setpoint_poll();
|
||||
|
||||
// vehicle status update must be before the vehicle_control_mode_poll(), otherwise rate sp are not published during whole transition
|
||||
// vehicle status update must be before the vehicle_control_mode poll, otherwise rate sp are not published during whole transition
|
||||
_vehicle_status_sub.update(&_vehicle_status);
|
||||
const bool is_in_transition_except_tailsitter = _vehicle_status.in_transition_mode
|
||||
&& !_vehicle_status.is_vtol_tailsitter;
|
||||
const bool is_fixed_wing = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING;
|
||||
_in_fw_or_transition_wo_tailsitter_transition = is_fixed_wing || is_in_transition_except_tailsitter;
|
||||
|
||||
_vehicle_control_mode_sub.update(&_vcontrol_mode);
|
||||
|
||||
vehicle_control_mode_poll();
|
||||
vehicle_land_detected_poll();
|
||||
|
||||
// the position controller will not emit attitude setpoints in some modes
|
||||
@@ -376,26 +289,18 @@ 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;
|
||||
}
|
||||
|
||||
controlFlaps(dt);
|
||||
controlSpoilers(dt);
|
||||
|
||||
if (_vcontrol_mode.flag_control_rates_enabled) {
|
||||
|
||||
const float airspeed = get_airspeed_and_update_scaling();
|
||||
|
||||
/* Reset integrators if commanded by attitude setpoint, or the aircraft is on ground
|
||||
* or a multicopter (but not transitioning VTOL or tailsitter)
|
||||
*/
|
||||
if (_att_sp.reset_integral
|
||||
|| _landed
|
||||
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|
||||
&& !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) {
|
||||
|| !_in_fw_or_transition_wo_tailsitter_transition) {
|
||||
|
||||
_rates_sp.reset_integral = true;
|
||||
_wheel_ctrl.reset_integrator();
|
||||
@@ -404,26 +309,25 @@ void FixedwingAttitudeControl::Run()
|
||||
_rates_sp.reset_integral = false;
|
||||
}
|
||||
|
||||
// update saturation status from control allocation feedback
|
||||
control_allocator_status_s control_allocator_status;
|
||||
float groundspeed_scale = 1.f;
|
||||
|
||||
if (_control_allocator_status_subs[_vehicle_status.is_vtol ? 1 : 0].update(&control_allocator_status)) {
|
||||
Vector<bool, 3> saturation_positive;
|
||||
Vector<bool, 3> saturation_negative;
|
||||
if (wheel_control) {
|
||||
if (_local_pos_sub.updated()) {
|
||||
vehicle_local_position_s vehicle_local_position;
|
||||
|
||||
if (!control_allocator_status.torque_setpoint_achieved) {
|
||||
for (size_t i = 0; i < 3; i++) {
|
||||
if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
|
||||
saturation_positive(i) = true;
|
||||
|
||||
} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
|
||||
saturation_negative(i) = true;
|
||||
}
|
||||
if (_local_pos_sub.copy(&vehicle_local_position)) {
|
||||
_groundspeed = sqrtf(vehicle_local_position.vx * vehicle_local_position.vx + vehicle_local_position.vy *
|
||||
vehicle_local_position.vy);
|
||||
}
|
||||
}
|
||||
|
||||
// TODO: send the unallocated value directly for better anti-windup
|
||||
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
|
||||
// Use stall airspeed to calculate ground speed scaling region. Don't scale below gspd_scaling_trim
|
||||
float gspd_scaling_trim = (_param_fw_airspd_stall.get());
|
||||
|
||||
if (_groundspeed > gspd_scaling_trim) {
|
||||
groundspeed_scale = gspd_scaling_trim / _groundspeed;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
/* Prepare data for attitude controllers */
|
||||
@@ -431,91 +335,19 @@ void FixedwingAttitudeControl::Run()
|
||||
control_input.roll = euler_angles.phi();
|
||||
control_input.pitch = euler_angles.theta();
|
||||
control_input.yaw = euler_angles.psi();
|
||||
control_input.body_x_rate = rollspeed;
|
||||
control_input.body_y_rate = pitchspeed;
|
||||
control_input.body_z_rate = yawspeed;
|
||||
control_input.roll_setpoint = _att_sp.roll_body;
|
||||
control_input.pitch_setpoint = _att_sp.pitch_body;
|
||||
control_input.yaw_setpoint = _att_sp.yaw_body;
|
||||
control_input.euler_roll_rate_setpoint = _roll_ctrl.get_euler_rate_setpoint();
|
||||
control_input.euler_pitch_rate_setpoint = _pitch_ctrl.get_euler_rate_setpoint();
|
||||
control_input.euler_yaw_rate_setpoint = _yaw_ctrl.get_euler_rate_setpoint();
|
||||
control_input.airspeed_min = _param_fw_airspd_stall.get();
|
||||
control_input.airspeed_max = _param_fw_airspd_max.get();
|
||||
control_input.airspeed = airspeed;
|
||||
|
||||
if (wheel_control) {
|
||||
_local_pos_sub.update(&_local_pos);
|
||||
|
||||
/* Use stall airspeed to calculate ground speed scaling region.
|
||||
* Don't scale below gspd_scaling_trim
|
||||
*/
|
||||
float groundspeed = sqrtf(_local_pos.vx * _local_pos.vx + _local_pos.vy * _local_pos.vy);
|
||||
float gspd_scaling_trim = (_param_fw_airspd_stall.get());
|
||||
|
||||
control_input.groundspeed = groundspeed;
|
||||
|
||||
if (groundspeed > gspd_scaling_trim) {
|
||||
control_input.groundspeed_scaler = gspd_scaling_trim / groundspeed;
|
||||
|
||||
} else {
|
||||
control_input.groundspeed_scaler = 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
/* reset body angular rate limits on mode change */
|
||||
if ((_vcontrol_mode.flag_control_attitude_enabled != _flag_control_attitude_enabled_last) || params_updated) {
|
||||
if (_vcontrol_mode.flag_control_attitude_enabled
|
||||
|| _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
_roll_ctrl.set_max_rate(radians(_param_fw_r_rmax.get()));
|
||||
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_p_rmax_pos.get()));
|
||||
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_p_rmax_neg.get()));
|
||||
_yaw_ctrl.set_max_rate(radians(_param_fw_y_rmax.get()));
|
||||
|
||||
} else {
|
||||
_roll_ctrl.set_max_rate(radians(_param_fw_acro_x_max.get()));
|
||||
_pitch_ctrl.set_max_rate_pos(radians(_param_fw_acro_y_max.get()));
|
||||
_pitch_ctrl.set_max_rate_neg(radians(_param_fw_acro_y_max.get()));
|
||||
_yaw_ctrl.set_max_rate(radians(_param_fw_acro_z_max.get()));
|
||||
}
|
||||
}
|
||||
|
||||
_flag_control_attitude_enabled_last = _vcontrol_mode.flag_control_attitude_enabled;
|
||||
|
||||
/* bi-linear interpolation over airspeed for actuator trim scheduling */
|
||||
float trim_roll = _param_trim_roll.get();
|
||||
float trim_pitch = _param_trim_pitch.get();
|
||||
float trim_yaw = _param_trim_yaw.get();
|
||||
|
||||
if (airspeed < _param_fw_airspd_trim.get()) {
|
||||
trim_roll += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
|
||||
_param_fw_dtrim_r_vmin.get(),
|
||||
0.0f);
|
||||
trim_pitch += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
|
||||
_param_fw_dtrim_p_vmin.get(),
|
||||
0.0f);
|
||||
trim_yaw += interpolate(airspeed, _param_fw_airspd_min.get(), _param_fw_airspd_trim.get(),
|
||||
_param_fw_dtrim_y_vmin.get(),
|
||||
0.0f);
|
||||
|
||||
} else {
|
||||
trim_roll += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
|
||||
_param_fw_dtrim_r_vmax.get());
|
||||
trim_pitch += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
|
||||
_param_fw_dtrim_p_vmax.get());
|
||||
trim_yaw += interpolate(airspeed, _param_fw_airspd_trim.get(), _param_fw_airspd_max.get(), 0.0f,
|
||||
_param_fw_dtrim_y_vmax.get());
|
||||
}
|
||||
|
||||
/* add trim increment if flaps are deployed */
|
||||
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();
|
||||
control_input.airspeed_constrained = get_airspeed_constrained();
|
||||
control_input.groundspeed = _groundspeed;
|
||||
control_input.groundspeed_scaler = groundspeed_scale;
|
||||
|
||||
/* Run attitude controllers */
|
||||
if (_vcontrol_mode.flag_control_attitude_enabled) {
|
||||
|
||||
if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
|
||||
if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
|
||||
_roll_ctrl.control_attitude(dt, control_input);
|
||||
_pitch_ctrl.control_attitude(dt, control_input);
|
||||
@@ -532,7 +364,6 @@ void FixedwingAttitudeControl::Run()
|
||||
Vector3f body_rates_setpoint = Vector3f(_roll_ctrl.get_body_rate_setpoint(), _pitch_ctrl.get_body_rate_setpoint(),
|
||||
_yaw_ctrl.get_body_rate_setpoint());
|
||||
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
autotune_attitude_control_status_s pid_autotune;
|
||||
matrix::Vector3f bodyrate_autotune_ff;
|
||||
|
||||
@@ -541,7 +372,7 @@ void FixedwingAttitudeControl::Run()
|
||||
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_PITCH
|
||||
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_YAW
|
||||
|| pid_autotune.state == autotune_attitude_control_status_s::STATE_TEST)
|
||||
&& ((now - pid_autotune.timestamp) < 1_s)) {
|
||||
&& ((hrt_absolute_time() - pid_autotune.timestamp) < 1_s)) {
|
||||
|
||||
bodyrate_autotune_ff = matrix::Vector3f(pid_autotune.rate_sp);
|
||||
body_rates_setpoint += bodyrate_autotune_ff;
|
||||
@@ -550,7 +381,7 @@ void FixedwingAttitudeControl::Run()
|
||||
|
||||
/* add yaw rate setpoint from sticks in all attitude-controlled modes */
|
||||
if (_vcontrol_mode.flag_control_manual_enabled) {
|
||||
body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.yaw * radians(_param_fw_man_yr_max.get()),
|
||||
body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.yaw * radians(_param_fw_y_rmax.get()),
|
||||
-radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get()));
|
||||
}
|
||||
|
||||
@@ -565,124 +396,29 @@ void FixedwingAttitudeControl::Run()
|
||||
}
|
||||
}
|
||||
|
||||
if (_vcontrol_mode.flag_control_rates_enabled) {
|
||||
_rates_sp_sub.update(&_rates_sp);
|
||||
// wheel control
|
||||
float wheel_u = 0.f;
|
||||
|
||||
const Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.yaw);
|
||||
if (_vcontrol_mode.flag_control_manual_enabled) {
|
||||
// always direct control of steering wheel with yaw stick in manual modes
|
||||
wheel_u = _manual_control_setpoint.yaw;
|
||||
|
||||
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
|
||||
const Vector3f angular_acceleration_setpoint = _rate_control.update(rates, body_rates_setpoint, angular_accel, dt,
|
||||
_landed);
|
||||
|
||||
float roll_feedforward = _param_fw_rr_ff.get() * _airspeed_scaling * body_rates_setpoint(0);
|
||||
float roll_u = angular_acceleration_setpoint(0) * _airspeed_scaling * _airspeed_scaling + roll_feedforward;
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
|
||||
(PX4_ISFINITE(roll_u)) ? math::constrain(roll_u + trim_roll, -1.f, 1.f) : trim_roll;
|
||||
|
||||
float pitch_feedforward = _param_fw_pr_ff.get() * _airspeed_scaling * body_rates_setpoint(1);
|
||||
float pitch_u = angular_acceleration_setpoint(1) * _airspeed_scaling * _airspeed_scaling + pitch_feedforward;
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
|
||||
(PX4_ISFINITE(pitch_u)) ? math::constrain(pitch_u + trim_pitch, -1.f, 1.f) : trim_pitch;
|
||||
|
||||
const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2);
|
||||
const float yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward;
|
||||
|
||||
// wheel control
|
||||
float wheel_u = 0.f;
|
||||
|
||||
if (_vcontrol_mode.flag_control_manual_enabled) {
|
||||
// always direct control of steering wheel with yaw stick in manual modes
|
||||
wheel_u = _manual_control_setpoint.yaw;
|
||||
|
||||
} else {
|
||||
// XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from
|
||||
// position controller during auto modes _manual_control_setpoint.yaw gets passed
|
||||
// whenever nudging is enabled, otherwise zero
|
||||
wheel_u = wheel_control ? _wheel_ctrl.control_bodyrate(dt, control_input)
|
||||
+ _att_sp.yaw_sp_move_rate : 0.f;
|
||||
}
|
||||
|
||||
_landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f;
|
||||
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = (PX4_ISFINITE(yaw_u)) ? math::constrain(yaw_u + trim_yaw,
|
||||
-1.f, 1.f) : trim_yaw;
|
||||
|
||||
if (!PX4_ISFINITE(roll_u) || !PX4_ISFINITE(pitch_u) || !PX4_ISFINITE(yaw_u) || _rates_sp.reset_integral) {
|
||||
_rate_control.resetIntegral();
|
||||
}
|
||||
|
||||
if (!PX4_ISFINITE(wheel_u)) {
|
||||
_wheel_ctrl.reset_integrator();
|
||||
}
|
||||
|
||||
/* throttle passed through if it is finite */
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] =
|
||||
(PX4_ISFINITE(_rates_sp.thrust_body[0])) ? _rates_sp.thrust_body[0] : 0.0f;
|
||||
|
||||
/* scale effort by battery status */
|
||||
if (_param_fw_bat_scale_en.get() &&
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] > 0.1f) {
|
||||
|
||||
if (_battery_status_sub.updated()) {
|
||||
battery_status_s battery_status{};
|
||||
|
||||
if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
|
||||
_battery_scale = battery_status.scale;
|
||||
}
|
||||
}
|
||||
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] *= _battery_scale;
|
||||
}
|
||||
} else {
|
||||
// XXX: yaw_sp_move_rate here is an abuse -- used to ferry manual yaw inputs from
|
||||
// position controller during auto modes _manual_control_setpoint.r gets passed
|
||||
// whenever nudging is enabled, otherwise zero
|
||||
wheel_u = wheel_control ? _wheel_ctrl.control_bodyrate(dt, control_input)
|
||||
+ _att_sp.yaw_sp_move_rate : 0.f;
|
||||
}
|
||||
|
||||
// publish rate controller status
|
||||
rate_ctrl_status_s rate_ctrl_status{};
|
||||
_rate_control.getRateControlStatus(rate_ctrl_status);
|
||||
rate_ctrl_status.timestamp = hrt_absolute_time();
|
||||
|
||||
if (wheel_control) {
|
||||
rate_ctrl_status.wheel_rate_integ = _wheel_ctrl.get_integrator();
|
||||
}
|
||||
|
||||
_rate_ctrl_status_pub.publish(rate_ctrl_status);
|
||||
_landing_gear_wheel.normalized_wheel_setpoint = PX4_ISFINITE(wheel_u) ? wheel_u : 0.f;
|
||||
_landing_gear_wheel.timestamp = hrt_absolute_time();
|
||||
_landing_gear_wheel_pub.publish(_landing_gear_wheel);
|
||||
|
||||
} else {
|
||||
// full manual
|
||||
_rate_control.resetIntegral();
|
||||
_wheel_ctrl.reset_integrator();
|
||||
}
|
||||
|
||||
// Add feed-forward from roll control output to yaw control output
|
||||
// This can be used to counteract the adverse yaw effect when rolling the plane
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _param_fw_rll_to_yaw_ff.get()
|
||||
* constrain(_actuator_controls.control[actuator_controls_s::INDEX_ROLL], -1.0f, 1.0f);
|
||||
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_SPOILERS] = _spoiler_setpoint_with_slewrate.getState();
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_AIRBRAKES] = 0.f;
|
||||
// FIXME: this should use _vcontrol_mode.landing_gear_pos in the future
|
||||
_actuator_controls.control[actuator_controls_s::INDEX_LANDING_GEAR] = _manual_control_setpoint.aux3;
|
||||
|
||||
/* lazily publish the setpoint only once available */
|
||||
_actuator_controls.timestamp = hrt_absolute_time();
|
||||
_actuator_controls.timestamp_sample = att.timestamp;
|
||||
|
||||
/* Only publish if any of the proper modes are enabled */
|
||||
if (_vcontrol_mode.flag_control_rates_enabled ||
|
||||
_vcontrol_mode.flag_control_attitude_enabled ||
|
||||
_vcontrol_mode.flag_control_manual_enabled) {
|
||||
_actuator_controls_0_pub.publish(_actuator_controls);
|
||||
|
||||
if (!_vehicle_status.is_vtol) {
|
||||
publishTorqueSetpoint(angular_velocity.timestamp_sample);
|
||||
publishThrustSetpoint(angular_velocity.timestamp_sample);
|
||||
}
|
||||
}
|
||||
|
||||
_landing_gear_wheel.timestamp = hrt_absolute_time();
|
||||
_landing_gear_wheel_pub.publish(_landing_gear_wheel);
|
||||
|
||||
updateActuatorControlsStatus(dt);
|
||||
}
|
||||
|
||||
// backup schedule
|
||||
@@ -691,132 +427,6 @@ void FixedwingAttitudeControl::Run()
|
||||
perf_end(_loop_perf);
|
||||
}
|
||||
|
||||
void FixedwingAttitudeControl::publishTorqueSetpoint(const hrt_abstime ×tamp_sample)
|
||||
{
|
||||
vehicle_torque_setpoint_s v_torque_sp = {};
|
||||
v_torque_sp.timestamp = hrt_absolute_time();
|
||||
v_torque_sp.timestamp_sample = timestamp_sample;
|
||||
v_torque_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_ROLL];
|
||||
v_torque_sp.xyz[1] = _actuator_controls.control[actuator_controls_s::INDEX_PITCH];
|
||||
v_torque_sp.xyz[2] = _actuator_controls.control[actuator_controls_s::INDEX_YAW];
|
||||
|
||||
_vehicle_torque_setpoint_pub.publish(v_torque_sp);
|
||||
}
|
||||
|
||||
void FixedwingAttitudeControl::publishThrustSetpoint(const hrt_abstime ×tamp_sample)
|
||||
{
|
||||
vehicle_thrust_setpoint_s v_thrust_sp = {};
|
||||
v_thrust_sp.timestamp = hrt_absolute_time();
|
||||
v_thrust_sp.timestamp_sample = timestamp_sample;
|
||||
v_thrust_sp.xyz[0] = _actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
|
||||
v_thrust_sp.xyz[1] = 0.0f;
|
||||
v_thrust_sp.xyz[2] = 0.0f;
|
||||
|
||||
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
|
||||
}
|
||||
|
||||
void FixedwingAttitudeControl::controlFlaps(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) {
|
||||
flap_control = _manual_control_setpoint.flaps;
|
||||
|
||||
} else if (_vcontrol_mode.flag_control_auto_enabled) {
|
||||
|
||||
switch (_att_sp.apply_flaps) {
|
||||
case vehicle_attitude_setpoint_s::FLAPS_OFF:
|
||||
flap_control = 0.0f; // no flaps
|
||||
break;
|
||||
|
||||
case vehicle_attitude_setpoint_s::FLAPS_LAND:
|
||||
flap_control = _param_fw_flaps_lnd_scl.get();
|
||||
break;
|
||||
|
||||
case vehicle_attitude_setpoint_s::FLAPS_TAKEOFF:
|
||||
flap_control = _param_fw_flaps_to_scl.get();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// move the actual control value continuous with time, full flap travel in 1sec
|
||||
_flaps_setpoint_with_slewrate.update(math::constrain(flap_control, 0.f, 1.f), dt);
|
||||
}
|
||||
|
||||
void FixedwingAttitudeControl::controlSpoilers(const float dt)
|
||||
{
|
||||
float spoiler_control = 0.f;
|
||||
|
||||
if (_vcontrol_mode.flag_control_manual_enabled) {
|
||||
switch (_param_fw_spoilers_man.get()) {
|
||||
case 0:
|
||||
break;
|
||||
|
||||
case 1:
|
||||
spoiler_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? _manual_control_setpoint.flaps : 0.f;
|
||||
break;
|
||||
|
||||
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) {
|
||||
switch (_att_sp.apply_spoilers) {
|
||||
case vehicle_attitude_setpoint_s::SPOILERS_OFF:
|
||||
spoiler_control = 0.f;
|
||||
break;
|
||||
|
||||
case vehicle_attitude_setpoint_s::SPOILERS_LAND:
|
||||
spoiler_control = _param_fw_spoilers_lnd.get();
|
||||
break;
|
||||
|
||||
case vehicle_attitude_setpoint_s::SPOILERS_DESCEND:
|
||||
spoiler_control = _param_fw_spoilers_desc.get();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
_spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_control, 0.f, 1.f), dt);
|
||||
}
|
||||
|
||||
void FixedwingAttitudeControl::updateActuatorControlsStatus(float dt)
|
||||
{
|
||||
for (int i = 0; i < 4; i++) {
|
||||
float control_signal;
|
||||
|
||||
if (i <= actuator_controls_status_s::INDEX_YAW) {
|
||||
// We assume that the attitude is actuated by control surfaces
|
||||
// consuming power only when they move
|
||||
control_signal = _actuator_controls.control[i] - _control_prev[i];
|
||||
_control_prev[i] = _actuator_controls.control[i];
|
||||
|
||||
} else {
|
||||
control_signal = _actuator_controls.control[i];
|
||||
}
|
||||
|
||||
_control_energy[i] += control_signal * control_signal * dt;
|
||||
}
|
||||
|
||||
_energy_integration_time += dt;
|
||||
|
||||
if (_energy_integration_time > 500e-3f) {
|
||||
|
||||
actuator_controls_status_s status;
|
||||
status.timestamp = _actuator_controls.timestamp;
|
||||
|
||||
for (int i = 0; i < 4; i++) {
|
||||
status.control_power[i] = _control_energy[i] / _energy_integration_time;
|
||||
_control_energy[i] = 0.f;
|
||||
}
|
||||
|
||||
_actuator_controls_status_pub.publish(status);
|
||||
_energy_integration_time = 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
int FixedwingAttitudeControl::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
bool vtol = false;
|
||||
|
||||
Reference in New Issue
Block a user