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

FlightTaskTransition: clean up and simplify

Browse Source
This commit is contained in:
Matthias Grob 2024-12-18 15:40:46 +01:00 committed by Silvan Fuhrer
parent 839010eeab
commit 11d7dd41fd
2 changed files with 42 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

149
src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp
View File

@ -41,21 +41,15 @@ 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");
updateParametersFromStorage();
param_get(param_find("FW_PSP_OFF"), &_param_fw_psp_off);
param_get(param_find("VT_B_DEC_I"), &_param_vt_b_dec_i);
param_get(param_find("VT_B_DEC_MSS"), &_param_vt_b_dec_mss);
}
bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(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]);
@ -63,9 +57,7 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
_vel_z_filter.reset(_velocity(2));
}
_velocity_setpoint(2) = _vel_z_filter.getState();
if (isVtolFrontTransition()) {
if (_sub_vehicle_status.get().in_transition_to_fw) {
_gear.landing_gear = landing_gear_s::GEAR_UP;
} else {
@ -77,9 +69,10 @@ bool FlightTaskTransition::activate(const trajectory_setpoint_s &last_setpoint)
bool FlightTaskTransition::updateInitialize()
{
updateParameters();
updateSubscribers();
return FlightTask::updateInitialize();
bool ret = FlightTask::updateInitialize();
_sub_vehicle_status.update();
_sub_position_sp_triplet.update();
return ret;
}
bool FlightTaskTransition::update()
@ -88,122 +81,56 @@ bool FlightTaskTransition::update()
// tiltrotors and standard vtol will overrride roll and pitch setpoint but keep vertical thrust setpoint
bool ret = FlightTask::update();
_position_setpoint.setAll(NAN);
// slowly move vertical velocity setpoint to zero
_velocity_setpoint(2) = _vel_z_filter.update(0.0f, _deltatime);
// calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_pitch_cruise_degrees
// calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_fw_psp_off
// and zero roll angle
const Vector2f tmp = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f);
float pitch_setpoint = math::radians(_param_pitch_cruise_degrees);
float pitch_setpoint = math::radians(_param_fw_psp_off);
if (isVtolBackTransition()) {
if (!_sub_vehicle_status.get().in_transition_to_fw) {
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);
_velocity_setpoint(2) = _vel_z_filter.update(0.0f);
// Calculate horizontal acceleration components to follow a pitch setpoint with the current vehicle heading
const Vector2f horizontal_acceleration_direction = Dcm2f(_yaw) * Vector2f(-1.0f, 0.0f);
_acceleration_setpoint.xy() = tanf(pitch_setpoint) * CONSTANTS_ONE_G * horizontal_acceleration_direction;
_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;
const Vector2f position_xy{_position};
const Vector2f velocity_xy{_velocity};
const Vector2f velocity_xy_direction = velocity_xy.unit_or_zero();
// Retrieve default decelaration setpoint from param
const float default_deceleration_sp = _param_vt_b_dec_mss;
float deceleration_setpoint = _param_vt_b_dec_mss;
float deceleration_sp = default_deceleration_sp;
if (_sub_position_sp_triplet.get().current.valid && _sub_vehicle_local_position.get().xy_global
&& position_xy.isAllFinite() && velocity_xy.isAllFinite()) {
Vector2f position_setpoint_local;
_geo_projection.project(_sub_position_sp_triplet.get().current.lat, _sub_position_sp_triplet.get().current.lon,
position_setpoint_local(0), position_setpoint_local(1));
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 Vector2f pos_to_target = position_setpoint_local - position_xy; // backtransition end-point w.r.t. vehicle
const float dist_to_target_in_moving_direction = pos_to_target.dot(velocity_xy_direction);
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);
if (dist_to_target_in_moving_direction > FLT_EPSILON) {
// Backtransition target point is ahead of the vehicle, compute the desired deceleration
deceleration_setpoint = velocity_xy.norm_squared() / (2.f * dist_to_target_in_moving_direction);
deceleration_setpoint = math::min(deceleration_setpoint, 2.f * _param_vt_b_dec_mss);
}
}
// 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);
// Pitch up to reach a negative accel_in_flight_direction otherwise we decelerate too slow
const Vector2f acceleration_xy{_sub_vehicle_local_position.get().ax, _sub_vehicle_local_position.get().ay};
const float deceleration = -acceleration_xy.dot(velocity_xy_direction);
const float deceleration_error = deceleration_setpoint - deceleration;
// Update back-transition deceleration error integrator
_decel_error_bt_int += (_param_vt_b_dec_i * deceleration_error) * _deltatime;
_decel_error_bt_int = math::constrain(_decel_error_bt_int, 0.f, DECELERATION_INTEGRATOR_LIMIT);
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);
}

25
src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp
View File

@ -61,35 +61,18 @@ public:
bool update() override;
private:
static constexpr float _vel_z_filter_time_const = 2.0f;
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
static constexpr float VERTICAL_VELOCITY_TIME_CONSTANT = 2.0f;
static constexpr float DECELERATION_INTEGRATOR_LIMIT = .3f;
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_fw_psp_off{0.f};
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;
AlphaFilter<float> _vel_z_filter{VERTICAL_VELOCITY_TIME_CONSTANT};
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);
};