mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-18 03:27:34 +08:00
Thomas Stastny
498937c56c
before there was a corner case where if in an auto mode that sets a reset command on the attitude setpoint message (e.g. auto takeoff), if the mode was then switched stabilized, this reset bool would never be changed back to false and the integrators would reset every cycle
872 lines
31 KiB
C++
872 lines
31 KiB
C++
/****************************************************************************
|
|
*
|
|
* Copyright (c) 2013-2019 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
|
|
* are met:
|
|
*
|
|
* 1. Redistributions of source code must retain the above copyright
|
|
* notice, this list of conditions and the following disclaimer.
|
|
* 2. Redistributions in binary form must reproduce the above copyright
|
|
* notice, this list of conditions and the following disclaimer in
|
|
* the documentation and/or other materials provided with the
|
|
* distribution.
|
|
* 3. Neither the name PX4 nor the names of its contributors may be
|
|
* used to endorse or promote products derived from this software
|
|
* without specific prior written permission.
|
|
*
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
|
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
|
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
|
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
|
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
|
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
|
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
|
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
|
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
|
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
|
* POSSIBILITY OF SUCH DAMAGE.
|
|
*
|
|
****************************************************************************/
|
|
|
|
#include "FixedwingAttitudeControl.hpp"
|
|
|
|
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()
|
|
{
|
|
perf_free(_loop_perf);
|
|
}
|
|
|
|
bool
|
|
FixedwingAttitudeControl::init()
|
|
{
|
|
if (!_att_sub.registerCallback()) {
|
|
PX4_ERR("callback registration failed");
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
int
|
|
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());
|
|
|
|
/* 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)) {
|
|
|
|
// 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_attitude_enabled) {
|
|
// STABILIZED mode generate the attitude setpoint from manual user inputs
|
|
|
|
_att_sp.roll_body = _manual_control_setpoint.y * radians(_param_fw_man_r_max.get());
|
|
|
|
_att_sp.pitch_body = -_manual_control_setpoint.x * 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] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
|
|
|
|
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
|
|
q.copyTo(_att_sp.q_d);
|
|
|
|
_att_sp.reset_rate_integrals = false;
|
|
|
|
_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.y * radians(_param_fw_acro_x_max.get());
|
|
_rates_sp.pitch = -_manual_control_setpoint.x * radians(_param_fw_acro_y_max.get());
|
|
_rates_sp.yaw = _manual_control_setpoint.r * radians(_param_fw_acro_z_max.get());
|
|
_rates_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f);
|
|
|
|
_rate_sp_pub.publish(_rates_sp);
|
|
|
|
} else {
|
|
/* manual/direct control */
|
|
_actuator_controls.control[actuator_controls_s::INDEX_ROLL] =
|
|
_manual_control_setpoint.y * _param_fw_man_r_sc.get() + _param_trim_roll.get();
|
|
_actuator_controls.control[actuator_controls_s::INDEX_PITCH] =
|
|
-_manual_control_setpoint.x * _param_fw_man_p_sc.get() + _param_trim_pitch.get();
|
|
_actuator_controls.control[actuator_controls_s::INDEX_YAW] =
|
|
_manual_control_setpoint.r * _param_fw_man_y_sc.get() + _param_trim_yaw.get();
|
|
_actuator_controls.control[actuator_controls_s::INDEX_THROTTLE] = math::constrain(_manual_control_setpoint.z, 0.0f,
|
|
1.0f);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
void
|
|
FixedwingAttitudeControl::vehicle_attitude_setpoint_poll()
|
|
{
|
|
if (_att_sp_sub.update(&_att_sp)) {
|
|
_rates_sp.thrust_body[0] = _att_sp.thrust_body[0];
|
|
_rates_sp.thrust_body[1] = _att_sp.thrust_body[1];
|
|
_rates_sp.thrust_body[2] = _att_sp.thrust_body[2];
|
|
}
|
|
}
|
|
|
|
void
|
|
FixedwingAttitudeControl::vehicle_land_detected_poll()
|
|
{
|
|
if (_vehicle_land_detected_sub.updated()) {
|
|
vehicle_land_detected_s vehicle_land_detected {};
|
|
|
|
if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
|
|
_landed = vehicle_land_detected.landed;
|
|
}
|
|
}
|
|
}
|
|
|
|
float FixedwingAttitudeControl::get_airspeed_and_update_scaling()
|
|
{
|
|
_airspeed_validated_sub.update();
|
|
const bool airspeed_valid = PX4_ISFINITE(_airspeed_validated_sub.get().calibrated_airspeed_m_s)
|
|
&& (hrt_elapsed_time(&_airspeed_validated_sub.get().timestamp) < 1_s);
|
|
|
|
// if no airspeed measurement is available out best guess is to use the trim airspeed
|
|
float airspeed = _param_fw_airspd_trim.get();
|
|
|
|
if ((_param_fw_arsp_mode.get() == 0) && airspeed_valid) {
|
|
/* prevent numerical drama by requiring 0.5 m/s minimal speed */
|
|
airspeed = math::max(0.5f, _airspeed_validated_sub.get().calibrated_airspeed_m_s);
|
|
|
|
} else {
|
|
// VTOL: if we have no airspeed available and we are in hover mode then assume the lowest airspeed possible
|
|
// this assumption is good as long as the vehicle is not hovering in a headwind which is much larger
|
|
// than the stall airspeed
|
|
if (_vehicle_status.is_vtol && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|
|
&& !_vehicle_status.in_transition_mode) {
|
|
airspeed = _param_fw_airspd_stall.get();
|
|
}
|
|
}
|
|
|
|
/*
|
|
* 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;
|
|
}
|
|
|
|
void FixedwingAttitudeControl::Run()
|
|
{
|
|
if (should_exit()) {
|
|
_att_sub.unregisterCallback();
|
|
exit_and_cleanup();
|
|
return;
|
|
}
|
|
|
|
perf_begin(_loop_perf);
|
|
|
|
// only run controller if attitude changed
|
|
if (_att_sub.updated() || (hrt_elapsed_time(&_last_run) > 20_ms)) {
|
|
|
|
// only update parameters if they changed
|
|
bool params_updated = _parameter_update_sub.updated();
|
|
|
|
// check for parameter updates
|
|
if (params_updated) {
|
|
// clear update
|
|
parameter_update_s pupdate;
|
|
_parameter_update_sub.copy(&pupdate);
|
|
|
|
// update parameters from storage
|
|
updateParams();
|
|
parameters_update();
|
|
}
|
|
|
|
float dt = 0.f;
|
|
|
|
static constexpr float DT_MIN = 0.002f;
|
|
static constexpr float DT_MAX = 0.04f;
|
|
|
|
vehicle_attitude_s att{};
|
|
|
|
if (_att_sub.copy(&att)) {
|
|
dt = math::constrain((att.timestamp_sample - _last_run) * 1e-6f, DT_MIN, DT_MAX);
|
|
_last_run = att.timestamp_sample;
|
|
|
|
// get current rotation matrix and euler angles from control state quaternions
|
|
_R = matrix::Quatf(att.q);
|
|
}
|
|
|
|
if (dt < DT_MIN || dt > DT_MAX) {
|
|
const hrt_abstime time_now_us = hrt_absolute_time();
|
|
dt = math::constrain((time_now_us - _last_run) * 1e-6f, DT_MIN, DT_MAX);
|
|
_last_run = time_now_us;
|
|
}
|
|
|
|
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);
|
|
|
|
vehicle_angular_acceleration_s angular_acceleration{};
|
|
_vehicle_angular_acceleration_sub.copy(&angular_acceleration);
|
|
const Vector3f angular_accel{angular_acceleration.xyz};
|
|
|
|
if (_vehicle_status.is_vtol_tailsitter) {
|
|
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
|
|
*
|
|
* Since the VTOL airframe is initialized as a multicopter we need to
|
|
* modify the estimated attitude for the fixed wing operation.
|
|
* Since the neutral position of the vehicle in fixed wing mode is -90 degrees rotated around
|
|
* the pitch axis compared to the neutral position of the vehicle in multicopter mode
|
|
* we need to swap the roll and the yaw axis (1st and 3rd column) in the rotation matrix.
|
|
* Additionally, in order to get the correct sign of the pitch, we need to multiply
|
|
* the new x axis of the rotation matrix with -1
|
|
*
|
|
* original: modified:
|
|
*
|
|
* Rxx Ryx Rzx -Rzx Ryx Rxx
|
|
* Rxy Ryy Rzy -Rzy Ryy Rxy
|
|
* Rxz Ryz Rzz -Rzz Ryz Rxz
|
|
* */
|
|
matrix::Dcmf R_adapted = _R; //modified rotation matrix
|
|
|
|
/* move z to x */
|
|
R_adapted(0, 0) = _R(0, 2);
|
|
R_adapted(1, 0) = _R(1, 2);
|
|
R_adapted(2, 0) = _R(2, 2);
|
|
|
|
/* move x to z */
|
|
R_adapted(0, 2) = _R(0, 0);
|
|
R_adapted(1, 2) = _R(1, 0);
|
|
R_adapted(2, 2) = _R(2, 0);
|
|
|
|
/* change direction of pitch (convert to right handed system) */
|
|
R_adapted(0, 0) = -R_adapted(0, 0);
|
|
R_adapted(1, 0) = -R_adapted(1, 0);
|
|
R_adapted(2, 0) = -R_adapted(2, 0);
|
|
|
|
/* fill in new attitude data */
|
|
_R = R_adapted;
|
|
|
|
/* lastly, roll- and yawspeed have to be swaped */
|
|
float helper = rollspeed;
|
|
rollspeed = -yawspeed;
|
|
yawspeed = helper;
|
|
}
|
|
|
|
const matrix::Eulerf euler_angles(_R);
|
|
|
|
vehicle_manual_poll(euler_angles.psi());
|
|
|
|
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_sub.update(&_vehicle_status);
|
|
|
|
vehicle_control_mode_poll();
|
|
vehicle_land_detected_poll();
|
|
|
|
// the position controller will not emit attitude setpoints in some modes
|
|
// we need to make sure that this flag is reset
|
|
_att_sp.fw_control_yaw = _att_sp.fw_control_yaw && _vcontrol_mode.flag_control_auto_enabled;
|
|
|
|
bool wheel_control = false;
|
|
|
|
// TODO: manual wheel_control on ground?
|
|
if (_param_fw_w_en.get() && _att_sp.fw_control_yaw) {
|
|
wheel_control = true;
|
|
}
|
|
|
|
/* 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 integrals where needed */
|
|
if (_att_sp.reset_rate_integrals) {
|
|
_rate_control.resetIntegral();
|
|
}
|
|
|
|
/* Reset integrators if the aircraft is on ground
|
|
* or a multicopter (but not transitioning VTOL or tailsitter)
|
|
*/
|
|
if (_landed
|
|
|| (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|
|
&& !_vehicle_status.in_transition_mode && !_vehicle_status.is_vtol_tailsitter)) {
|
|
|
|
_rate_control.resetIntegral();
|
|
_wheel_ctrl.reset_integrator();
|
|
}
|
|
|
|
// update saturation status from control allocation feedback
|
|
control_allocator_status_s control_allocator_status;
|
|
|
|
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 (!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;
|
|
}
|
|
}
|
|
}
|
|
|
|
// TODO: send the unallocated value directly for better anti-windup
|
|
_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
|
|
}
|
|
|
|
/* Prepare data for attitude controllers */
|
|
ECL_ControlData control_input{};
|
|
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_stall.get(), _param_fw_airspd_trim.get(),
|
|
_param_fw_dtrim_r_vmin.get(),
|
|
0.0f);
|
|
trim_pitch += interpolate(airspeed, _param_fw_airspd_stall.get(), _param_fw_airspd_trim.get(),
|
|
_param_fw_dtrim_p_vmin.get(),
|
|
0.0f);
|
|
trim_yaw += interpolate(airspeed, _param_fw_airspd_stall.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();
|
|
|
|
/* Run attitude controllers */
|
|
if (_vcontrol_mode.flag_control_attitude_enabled) {
|
|
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);
|
|
|
|
if (wheel_control) {
|
|
_wheel_ctrl.control_attitude(dt, control_input);
|
|
|
|
} else {
|
|
// runs last, because is depending on output of roll and pitch attitude
|
|
_yaw_ctrl.control_attitude(dt, control_input);
|
|
_wheel_ctrl.reset_integrator();
|
|
}
|
|
|
|
/* Update input data for rate controllers */
|
|
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;
|
|
|
|
if (_autotune_attitude_control_status_sub.copy(&pid_autotune)) {
|
|
if ((pid_autotune.state == autotune_attitude_control_status_s::STATE_ROLL
|
|
|| 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)) {
|
|
|
|
bodyrate_autotune_ff = matrix::Vector3f(pid_autotune.rate_sp);
|
|
body_rates_setpoint += bodyrate_autotune_ff;
|
|
}
|
|
}
|
|
|
|
/* add yaw rate setpoint from sticks in Stabilized mode */
|
|
if (_vcontrol_mode.flag_control_manual_enabled) {
|
|
_actuator_controls.control[actuator_controls_s::INDEX_YAW] += _manual_control_setpoint.r;
|
|
body_rates_setpoint(2) += math::constrain(_manual_control_setpoint.r * radians(_param_fw_y_rmax.get()),
|
|
-radians(_param_fw_y_rmax.get()), radians(_param_fw_y_rmax.get()));
|
|
}
|
|
|
|
/* Publish the rate setpoint for analysis once available */
|
|
_rates_sp.roll = body_rates_setpoint(0);
|
|
_rates_sp.pitch = body_rates_setpoint(1);
|
|
_rates_sp.yaw = (wheel_control) ? _wheel_ctrl.get_body_rate_setpoint() : body_rates_setpoint(2);
|
|
|
|
_rates_sp.timestamp = hrt_absolute_time();
|
|
|
|
_rate_sp_pub.publish(_rates_sp);
|
|
}
|
|
}
|
|
|
|
if (_vcontrol_mode.flag_control_rates_enabled) {
|
|
_rates_sp_sub.update(&_rates_sp);
|
|
|
|
const Vector3f body_rates_setpoint = Vector3f(_rates_sp.roll, _rates_sp.pitch, _rates_sp.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;
|
|
|
|
float yaw_u = 0.0f;
|
|
|
|
if (wheel_control) {
|
|
yaw_u = _wheel_ctrl.control_bodyrate(dt, control_input);
|
|
|
|
// XXX: this is an abuse -- used to ferry manual yaw inputs from position controller during auto modes
|
|
yaw_u += _att_sp.yaw_sp_move_rate * _param_fw_man_y_sc.get();
|
|
|
|
} else {
|
|
const float yaw_feedforward = _param_fw_yr_ff.get() * _airspeed_scaling * body_rates_setpoint(2);
|
|
yaw_u = angular_acceleration_setpoint(2) * _airspeed_scaling * _airspeed_scaling + yaw_feedforward;
|
|
}
|
|
|
|
_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)) {
|
|
_rate_control.resetIntegral();
|
|
}
|
|
|
|
/* 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;
|
|
}
|
|
}
|
|
|
|
// 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);
|
|
|
|
} 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);
|
|
}
|
|
}
|
|
|
|
updateActuatorControlsStatus(dt);
|
|
}
|
|
|
|
// backup schedule
|
|
ScheduleDelayed(20_ms);
|
|
|
|
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;
|
|
|
|
if (argc > 1) {
|
|
if (strcmp(argv[1], "vtol") == 0) {
|
|
vtol = true;
|
|
}
|
|
}
|
|
|
|
FixedwingAttitudeControl *instance = new FixedwingAttitudeControl(vtol);
|
|
|
|
if (instance) {
|
|
_object.store(instance);
|
|
_task_id = task_id_is_work_queue;
|
|
|
|
if (instance->init()) {
|
|
return PX4_OK;
|
|
}
|
|
|
|
} else {
|
|
PX4_ERR("alloc failed");
|
|
}
|
|
|
|
delete instance;
|
|
_object.store(nullptr);
|
|
_task_id = -1;
|
|
|
|
return PX4_ERROR;
|
|
}
|
|
|
|
int FixedwingAttitudeControl::custom_command(int argc, char *argv[])
|
|
{
|
|
return print_usage("unknown command");
|
|
}
|
|
|
|
int FixedwingAttitudeControl::print_usage(const char *reason)
|
|
{
|
|
if (reason) {
|
|
PX4_WARN("%s\n", reason);
|
|
}
|
|
|
|
PRINT_MODULE_DESCRIPTION(
|
|
R"DESCR_STR(
|
|
### Description
|
|
fw_att_control is the fixed wing attitude controller.
|
|
|
|
)DESCR_STR");
|
|
|
|
PRINT_MODULE_USAGE_NAME("fw_att_control", "controller");
|
|
PRINT_MODULE_USAGE_COMMAND("start");
|
|
PRINT_MODULE_USAGE_ARG("vtol", "VTOL mode", true);
|
|
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
|
|
|
return 0;
|
|
}
|
|
|
|
extern "C" __EXPORT int fw_att_control_main(int argc, char *argv[])
|
|
{
|
|
return FixedwingAttitudeControl::main(argc, argv);
|
|
}
|