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

VTOL: Added position feedback to backward transition braking controller(#23731)

Browse Source 
Instead of tracking a fixed deceleration setpoint during the backtransition we added here position feedback,
such that the vehicle comes to a stop latest at the current position setpoint. This reduces the risk of 
overshooting the landing point.
If no position feedback/position setpoint is present the old logic still applies.
It further moves the braking controller to the FlightTaskTransition instad of doing it in
the VTOL attitude mode.

* vtol_type: added position feedback to backward transition

* FlgithTaskTransition: refactored backtransition deceleration/pitch setpoint computation

* FlightTaskTransition: minor improvements

* FlightTaskTransition: use .dot() consistently and remove unnecessary comments

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

---------

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Co-authored-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Alvaro Fernandez
2024-11-11 11:53:28 +01:00
committed by GitHub
co-authored by Silvan Fuhrer
parent f47719d397
commit 079b756f1b
6 changed files with 140 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp
+119 -32
View File
@@ -42,32 +42,10 @@ using namespace matrix;
FlightTaskTransition::FlightTaskTransition()
{
_param_handle_pitch_cruise_degrees = param_find("FW_PSP_OFF");
_param_handle_vt_b_dec_i = param_find("VT_B_DEC_I");
_param_handle_vt_b_dec_mss = param_find("VT_B_DEC_MSS");
if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) {
param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees);
}
}
bool FlightTaskTransition::updateInitialize()
{
updateParameters();
return FlightTask::updateInitialize();
}
void FlightTaskTransition::updateParameters()
{
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) {
param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees);
}
}
updateParametersFromStorage();
}
bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
@@ -76,6 +54,8 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
_vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const);
_decel_error_bt_int = 0.f;
if (PX4_ISFINITE(last_setpoint.velocity[2])) {
_vel_z_filter.reset(last_setpoint.velocity[2]);
@@ -85,12 +65,7 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
_velocity_setpoint(2) = _vel_z_filter.getState();
_sub_vehicle_status.update();
const bool is_vtol_front_transition = _sub_vehicle_status.get().in_transition_mode
&& _sub_vehicle_status.get().in_transition_to_fw;
if (is_vtol_front_transition) {
if (isVtolFrontTransition()) {
_gear.landing_gear = landing_gear_s::GEAR_UP;
} else {
@@ -100,6 +75,13 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
return ret;
}
bool FlightTaskTransition::updateInitialize()
{
updateParameters();
updateSubscribers();
return FlightTask::updateInitialize();
}
bool FlightTaskTransition::update()
{
// tailsitters will override attitude and thrust setpoint
@@ -111,7 +93,13 @@ bool FlightTaskTransition::update()
// calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_pitch_cruise_degrees
// and zero roll angle
const Vector2f tmp = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f);
_acceleration_setpoint.xy() = tmp * tanf(math::radians(_param_pitch_cruise_degrees)) * CONSTANTS_ONE_G;
float pitch_setpoint = math::radians(_param_pitch_cruise_degrees);
if (isVtolBackTransition()) {
pitch_setpoint = computeBackTranstionPitchSetpoint();
}
_acceleration_setpoint.xy() = tmp * tanf(pitch_setpoint) * CONSTANTS_ONE_G;
// slowly move vertical velocity setpoint to zero
_vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const);
@@ -120,3 +108,102 @@ bool FlightTaskTransition::update()
_yaw_setpoint = NAN;
return ret;
}
void FlightTaskTransition::updateParameters()
{
// check for parameter updates
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
updateParametersFromStorage();
}
}
void FlightTaskTransition::updateParametersFromStorage()
{
if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) {
param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees);
}
if (_param_handle_vt_b_dec_i != PARAM_INVALID) {
param_get(_param_handle_vt_b_dec_i, &_param_vt_b_dec_i);
}
if (_param_handle_vt_b_dec_mss != PARAM_INVALID) {
param_get(_param_handle_vt_b_dec_mss, &_param_vt_b_dec_mss);
}
}
void FlightTaskTransition::updateSubscribers()
{
_sub_vehicle_status.update();
_sub_position_sp_triplet.update();
_sub_vehicle_local_position.update();
}
bool FlightTaskTransition::isVtolFrontTransition() const
{
return _sub_vehicle_status.get().in_transition_mode
&& _sub_vehicle_status.get().in_transition_to_fw;
}
bool FlightTaskTransition::isVtolBackTransition() const
{
return _sub_vehicle_status.get().in_transition_mode
&& !_sub_vehicle_status.get().in_transition_to_fw;
}
float FlightTaskTransition::computeBackTranstionPitchSetpoint()
{
const vehicle_local_position_s &local_pos = _sub_vehicle_local_position.get();
const position_setpoint_s &current_pos_sp = _sub_position_sp_triplet.get().current;
// Retrieve default decelaration setpoint from param
const float default_deceleration_sp = _param_vt_b_dec_mss;
float deceleration_sp = default_deceleration_sp;
const Vector2f position_local{local_pos.x, local_pos.y};
const Vector2f velocity_local{local_pos.vx, local_pos.vy};
const Vector2f acceleration_local{local_pos.ax, local_pos.ay};
const float accel_in_flight_direction = acceleration_local.dot(velocity_local.unit_or_zero());
if (current_pos_sp.valid && local_pos.xy_valid && local_pos.xy_global) {
Vector2f position_setpoint_local {0.f, 0.f};
_geo_projection.project(current_pos_sp.lat, current_pos_sp.lon, position_setpoint_local(0),
position_setpoint_local(1));
const Vector2f pos_sp_dist = position_setpoint_local - position_local; // backtransition end-point w.r.t. vehicle
const float dist_body_forward = pos_sp_dist.dot(velocity_local.unit_or_zero());
// Compute the deceleration setpoint if the backtransition end-point is ahead of the vehicle
if (dist_body_forward > FLT_EPSILON && PX4_ISFINITE(velocity_local.norm())) {
// Note this is deceleration (i.e. negative acceleration), and therefore the minus sign is skipped
deceleration_sp = velocity_local.norm_squared() / (2.f * dist_body_forward);
// Limit the deceleration setpoint to maximal twice the default deceleration setpoint (param)
deceleration_sp = math::constrain(deceleration_sp, 0.f, 2.f * default_deceleration_sp);
}
}
// positive means decelerating too slow, need to pitch up (must reverse accel_body_forward, as it is a positive number)
const float deceleration_error = deceleration_sp - (-accel_in_flight_direction);
updateBackTransitioDecelerationErrorIntegrator(deceleration_error);
return _decel_error_bt_int;
}
void FlightTaskTransition::updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error)
{
const float integrator_input = _param_vt_b_dec_i * deceleration_error;
_decel_error_bt_int += integrator_input * math::constrain(_deltatime, 0.001f, 0.1f);
_decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, _pitch_limit_bt);
}
src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp
+18
View File
@@ -44,6 +44,7 @@
#include <uORB/SubscriptionInterval.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/position_setpoint_triplet.h>
#include <drivers/drv_hrt.h>
using namespace time_literals;
@@ -66,12 +67,29 @@ private:
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
uORB::SubscriptionData<vehicle_status_s> _sub_vehicle_status{ORB_ID(vehicle_status)};
uORB::SubscriptionData<position_setpoint_triplet_s> _sub_position_sp_triplet{ORB_ID(position_setpoint_triplet)};
param_t _param_handle_pitch_cruise_degrees{PARAM_INVALID};
float _param_pitch_cruise_degrees{0.f};
param_t _param_handle_vt_b_dec_i{PARAM_INVALID};
float _param_vt_b_dec_i{0.f};
param_t _param_handle_vt_b_dec_mss{PARAM_INVALID};
float _param_vt_b_dec_mss{0.f};
AlphaFilter<float> _vel_z_filter;
float _decel_error_bt_int{0.f}; ///< Backtransition deceleration error integrator value
static constexpr float _pitch_limit_bt = .3f; ///< Bactransition pitch limit
void updateParameters();
void updateParametersFromStorage();
void updateSubscribers();
bool isVtolFrontTransition() const;
bool isVtolBackTransition() const;
float computeBackTranstionPitchSetpoint();
void updateBackTransitioDecelerationErrorIntegrator(const float deceleration_error);
};