Skip to content

Commit

Permalink
FlightTaskTransition: clean up and simplify
Browse files Browse the repository at this point in the history
  • Loading branch information
@MaEtUgR
MaEtUgR committed Dec 18, 2024
1 parent 6b10f1c commit 1ef3107
Show file tree
Hide file tree
Showing 2 changed files with 41 additions and 132 deletions.
148 changes: 37 additions & 111 deletions src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp
View file
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,55 @@ 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 && 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 changes: 4 additions & 21 deletions src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp
View file
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);

};

0 comments on commit 1ef3107

Please sign in to comment.