fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-25 06:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Rework flaps/spoilers logic

Browse Source 
- remove deprecated actuator_controls[INDEX_FLAPS/SPOILERS/AIRBRAKES]
- use new topic normalized_unsigned_setpoint.msg (with instances flaps_setpoint
and spoilers_setpoint) to pass into control allocation
- remove flaps/spoiler related fields from attitude_setpoint topic
- CA: add possibility to map flaps/spoilers to any control surface
- move flaps/spoiler pitch trimming to CA (previously called DTRIM_FLAPS/SPOILER)
- move manual flaps/spoiler handling from rate to attitude controller

FW Position controller: change how negative switch readings are intepreted
for flaps/spoilers (considered negative as 0).

VTOL: Rework spoiler publishing in hover

- pushlish spoiler_setpoint.msg in the VTOL module if in hover
- also set spoilers to land configuration if in Descend mode

Allocation: add slew rate limit of 0.5 to flaps/spoilers configuration change

Instead of doing the flaps/spoilers slew rate limiting in the FW Position Controller
(which then only is applied in Auto flight), do it consistently over all flight
modes, so also for manual modes.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-03-06 22:43:41 +01:00
parent 16594bffa9
commit 1e56d9c219
37 changed files with 468 additions and 398 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_rate_control/FixedwingRateControl.cpp
+41 -91
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2023 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
@@ -51,8 +51,6 @@ FixedwingRateControl::FixedwingRateControl(bool vtol) :
parameters_update();
_rate_ctrl_status_pub.advertise();
_spoiler_setpoint_with_slewrate.setSlewRate(kSpoilerSlewRate);
_flaps_setpoint_with_slewrate.setSlewRate(kFlapSlewRate);
}
FixedwingRateControl::~FixedwingRateControl()
@@ -255,11 +253,6 @@ void FixedwingRateControl::Run()
angular_velocity.xyz_derivative[0]);
}
// this is only to pass through flaps/spoiler setpoints, can be removed once flaps/spoilers
// are handled outside of attitude/rate controller.
// TODO remove it
_att_sp_sub.update(&_att_sp);
// 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
@@ -276,15 +269,10 @@ void FixedwingRateControl::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();
@@ -347,13 +335,6 @@ void FixedwingRateControl::Run()
_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();
if (_vcontrol_mode.flag_control_rates_enabled) {
_rates_sp_sub.update(&_rates_sp);
@@ -434,10 +415,6 @@ void FixedwingRateControl::Run()
_actuator_controls.control[actuator_controls_s::INDEX_YAW] = -helper;
}
_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;
/* lazily publish the setpoint only once available */
_actuator_controls.timestamp = hrt_absolute_time();
_actuator_controls.timestamp_sample = vehicle_angular_velocity.timestamp;
@@ -455,6 +432,46 @@ void FixedwingRateControl::Run()
}
updateActuatorControlsStatus(dt);
// Manual flaps/spoilers control, also active in VTOL Hover. Is handled and published in FW Position controller/VTOL module if Auto.
if (_vcontrol_mode.flag_control_manual_enabled) {
// Flaps control
float flaps_control = 0.f; // default to no flaps
/* map flaps by default to manual if valid */
if (PX4_ISFINITE(_manual_control_setpoint.flaps)) {
flaps_control = math::max(_manual_control_setpoint.flaps, 0.f); // do not consider negative switch settings
}
normalized_unsigned_setpoint_s flaps_setpoint;
flaps_setpoint.timestamp = hrt_absolute_time();
flaps_setpoint.normalized_setpoint = flaps_control;
_flaps_setpoint_pub.publish(flaps_setpoint);
// Spoilers control
float spoilers_control = 0.f; // default to no spoilers
switch (_param_fw_spoilers_man.get()) {
case 0:
break;
case 1:
// do not consider negative switch settings
spoilers_control = PX4_ISFINITE(_manual_control_setpoint.flaps) ? math::max(_manual_control_setpoint.flaps, 0.f) : 0.f;
break;
case 2:
// do not consider negative switch settings
spoilers_control = PX4_ISFINITE(_manual_control_setpoint.aux1) ? math::max(_manual_control_setpoint.aux1, 0.f) : 0.f;
break;
}
normalized_unsigned_setpoint_s spoilers_setpoint;
spoilers_setpoint.timestamp = hrt_absolute_time();
spoilers_setpoint.normalized_setpoint = spoilers_control;
_spoilers_setpoint_pub.publish(spoilers_setpoint);
}
}
// backup schedule
@@ -487,73 +504,6 @@ void FixedwingRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sa
_vehicle_thrust_setpoint_pub.publish(v_thrust_sp);
}
void FixedwingRateControl::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 FixedwingRateControl::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 FixedwingRateControl::updateActuatorControlsStatus(float dt)
{
for (int i = 0; i < 4; i++) {
src/modules/fw_rate_control/FixedwingRateControl.hpp
+7 -34
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2023 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
@@ -59,10 +59,10 @@
#include <uORB/topics/battery_status.h>
#include <uORB/topics/control_allocator_status.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/normalized_unsigned_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/rate_ctrl_status.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
@@ -77,9 +77,6 @@ using uORB::SubscriptionData;
using namespace time_literals;
static constexpr float kFlapSlewRate = 1.f; //minimum time from none to full flap deflection [s]
static constexpr float kSpoilerSlewRate = 1.f; //minimum time from none to full spoiler deflection [s]
class FixedwingRateControl final : public ModuleBase<FixedwingRateControl>, public ModuleParams,
public px4::ScheduledWorkItem
{
@@ -108,7 +105,6 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::Subscription _att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};
@@ -127,10 +123,11 @@ private:
uORB::PublicationMulti<rate_ctrl_status_s> _rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
uORB::Publication<vehicle_thrust_setpoint_s> _vehicle_thrust_setpoint_pub{ORB_ID(vehicle_thrust_setpoint)};
uORB::Publication<vehicle_torque_setpoint_s> _vehicle_torque_setpoint_pub{ORB_ID(vehicle_torque_setpoint)};
uORB::Publication<normalized_unsigned_setpoint_s> _flaps_setpoint_pub{ORB_ID(flaps_setpoint)};
uORB::Publication<normalized_unsigned_setpoint_s> _spoilers_setpoint_pub{ORB_ID(spoilers_setpoint)};
actuator_controls_s _actuator_controls{};
manual_control_setpoint_s _manual_control_setpoint{};
vehicle_attitude_setpoint_s _att_sp{};
vehicle_control_mode_s _vcontrol_mode{};
vehicle_rates_setpoint_s _rates_sp{};
vehicle_status_s _vehicle_status{};
@@ -149,9 +146,6 @@ private:
float _control_energy[4] {};
float _control_prev[3] {};
SlewRate<float> _spoiler_setpoint_with_slewrate;
SlewRate<float> _flaps_setpoint_with_slewrate;
bool _in_fw_or_transition_wo_tailsitter_transition{false}; // only run the FW attitude controller in these states
DEFINE_PARAMETERS(
@@ -169,22 +163,13 @@ private:
(ParamBool<px4::params::FW_BAT_SCALE_EN>) _param_fw_bat_scale_en,
(ParamFloat<px4::params::FW_DTRIM_P_FLPS>) _param_fw_dtrim_p_flps,
(ParamFloat<px4::params::FW_DTRIM_P_SPOIL>) _param_fw_dtrim_p_spoil,
(ParamFloat<px4::params::FW_DTRIM_P_VMAX>) _param_fw_dtrim_p_vmax,
(ParamFloat<px4::params::FW_DTRIM_P_VMIN>) _param_fw_dtrim_p_vmin,
(ParamFloat<px4::params::FW_DTRIM_R_FLPS>) _param_fw_dtrim_r_flps,
(ParamFloat<px4::params::FW_DTRIM_R_VMAX>) _param_fw_dtrim_r_vmax,
(ParamFloat<px4::params::FW_DTRIM_R_VMIN>) _param_fw_dtrim_r_vmin,
(ParamFloat<px4::params::FW_DTRIM_Y_VMAX>) _param_fw_dtrim_y_vmax,
(ParamFloat<px4::params::FW_DTRIM_Y_VMIN>) _param_fw_dtrim_y_vmin,
(ParamFloat<px4::params::FW_FLAPS_LND_SCL>) _param_fw_flaps_lnd_scl,
(ParamFloat<px4::params::FW_FLAPS_TO_SCL>) _param_fw_flaps_to_scl,
(ParamFloat<px4::params::FW_SPOILERS_LND>) _param_fw_spoilers_lnd,
(ParamFloat<px4::params::FW_SPOILERS_DESC>) _param_fw_spoilers_desc,
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man,
(ParamFloat<px4::params::FW_MAN_P_MAX>) _param_fw_man_p_max,
(ParamFloat<px4::params::FW_MAN_P_SC>) _param_fw_man_p_sc,
(ParamFloat<px4::params::FW_MAN_R_MAX>) _param_fw_man_r_max,
@@ -212,25 +197,13 @@ private:
(ParamFloat<px4::params::TRIM_PITCH>) _param_trim_pitch,
(ParamFloat<px4::params::TRIM_ROLL>) _param_trim_roll,
(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw
(ParamFloat<px4::params::TRIM_YAW>) _param_trim_yaw,
(ParamInt<px4::params::FW_SPOILERS_MAN>) _param_fw_spoilers_man
)
RateControl _rate_control; ///< class for rate control calculations
/**
* @brief Update flap control setting
*
* @param dt Current time delta [s]
*/
void controlFlaps(const float dt);
/**
* @brief Update spoiler control setting
*
* @param dt Current time delta [s]
*/
void controlSpoilers(const float dt);
void updateActuatorControlsStatus(float dt);
/**
src/modules/fw_rate_control/fw_rate_control_params.c
+1 -94
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2023 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
@@ -469,75 +469,6 @@ PARAM_DEFINE_FLOAT(FW_DTRIM_P_VMAX, 0.0f);
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_Y_VMAX, 0.0f);
/**
* Roll trim increment for flaps configuration
*
* This increment is added to TRIM_ROLL whenever flaps are fully deployed.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_R_FLPS, 0.0f);
/**
* Pitch trim increment for flaps configuration
*
* This increment is added to the pitch trim whenever flaps are fully deployed.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_FLPS, 0.0f);
/**
* Pitch trim increment for spoiler configuration
*
* This increment is added to the pitch trim whenever spoilers are fully deployed.
*
* @group FW Rate Control
* @min -0.25
* @max 0.25
* @decimal 2
* @increment 0.01
*/
PARAM_DEFINE_FLOAT(FW_DTRIM_P_SPOIL, 0.f);
/**
* Flaps setting during take-off
*
* Sets a fraction of full flaps during take-off.
* Also applies to flaperons if enabled in the mixer/allocation.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_FLAPS_TO_SCL, 0.0f);
/**
* Flaps setting during landing
*
* Sets a fraction of full flaps during landing.
* Also applies to flaperons if enabled in the mixer/allocation.
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Rate Control
*/
PARAM_DEFINE_FLOAT(FW_FLAPS_LND_SCL, 1.0f);
/**
* Manual roll scale
*
@@ -623,30 +554,6 @@ PARAM_DEFINE_FLOAT(FW_MAN_Y_SC, 1.0f);
*/
PARAM_DEFINE_FLOAT(FW_RLL_TO_YAW_FF, 0.0f);
/**
* Spoiler landing setting
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_SPOILERS_LND, 0.f);
/**
* Spoiler descend setting
*
* @unit norm
* @min 0.0
* @max 1.0
* @decimal 2
* @increment 0.01
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_SPOILERS_DESC, 0.f);
/**
* Spoiler input in manual flight
*