Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Clean up Transition Flighttask #24126

Merged
merged 3 commits into from
Dec 19, 2024
Merged

Clean up Transition Flighttask #24126

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
0a02e9a
FlightTaskTransition: clean up and simplify
MaEtUgR Dec 18, 2024
395bc56
FlightTaskTransition: keep high decelration when overshooting the tra…
MaEtUgR Dec 18, 2024
bb31767
FlightTaskTransition: Comment why using invalid horizontal velocity w…
MaEtUgR Dec 19, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
152 changes: 42 additions & 110 deletions src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,31 +41,23 @@ 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]);

} else {
_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 {
Expand All @@ -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()
Expand All @@ -88,122 +81,61 @@ 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(); // Zero when velocity invalid Vector2f(NAN, NAN).unit_or_zero() == Vector2f(0.f, 0.f)

// 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());
MaEtUgR marked this conversation as resolved.
Show resolved Hide resolved
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);

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);
} else {
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);
deceleration_setpoint = math::min(deceleration_setpoint, 2.f * _param_vt_b_dec_mss);
}

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); // Zero when velocity invalid
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 changes: 4 additions & 21 deletions src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -61,35 +61,18 @@ class FlightTaskTransition : public FlightTask
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);

};
Loading