forked from Archive/PX4-Autopilot
VTOL: move to cpp params API
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
parent
ff16131874
commit
7292ce483c
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2015-2022 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
|
||||
|
|
@ -60,41 +60,15 @@ Standard::Standard(VtolAttitudeControl *attc) :
|
|||
_mc_pitch_weight = 1.0f;
|
||||
_mc_yaw_weight = 1.0f;
|
||||
_mc_throttle_weight = 1.0f;
|
||||
|
||||
_params_handles_standard.pusher_ramp_dt = param_find("VT_PSHER_RMP_DT");
|
||||
_params_handles_standard.back_trans_ramp = param_find("VT_B_TRANS_RAMP");
|
||||
_params_handles_standard.pitch_setpoint_offset = param_find("FW_PSP_OFF");
|
||||
_params_handles_standard.reverse_output = param_find("VT_B_REV_OUT");
|
||||
_params_handles_standard.reverse_delay = param_find("VT_B_REV_DEL");
|
||||
}
|
||||
|
||||
void
|
||||
Standard::parameters_update()
|
||||
{
|
||||
float v;
|
||||
|
||||
/* duration of a forwards transition to fw mode */
|
||||
param_get(_params_handles_standard.pusher_ramp_dt, &v);
|
||||
_params_standard.pusher_ramp_dt = math::constrain(v, 0.0f, 20.0f);
|
||||
|
||||
/* MC ramp up during back transition to mc mode */
|
||||
param_get(_params_handles_standard.back_trans_ramp, &v);
|
||||
_params_standard.back_trans_ramp = math::constrain(v, 0.0f, _params->back_trans_duration);
|
||||
|
||||
_airspeed_trans_blend_margin = _params->transition_airspeed - _params->airspeed_blend;
|
||||
|
||||
/* pitch setpoint offset */
|
||||
param_get(_params_handles_standard.pitch_setpoint_offset, &v);
|
||||
_params_standard.pitch_setpoint_offset = math::radians(v);
|
||||
|
||||
/* reverse output */
|
||||
param_get(_params_handles_standard.reverse_output, &v);
|
||||
_params_standard.reverse_output = math::constrain(v, 0.0f, 1.0f);
|
||||
|
||||
/* reverse output */
|
||||
param_get(_params_handles_standard.reverse_delay, &v);
|
||||
_params_standard.reverse_delay = math::constrain(v, 0.0f, 10.0f);
|
||||
VtolType::updateParams();
|
||||
|
||||
// make sure that pusher ramp in backtransition is smaller than back transition (max) duration
|
||||
_param_vt_b_trans_ramp.set(math::min(_param_vt_b_trans_ramp.get(), _param_vt_b_trans_dur.get()));
|
||||
}
|
||||
|
||||
void Standard::update_vtol_state()
|
||||
|
|
@ -132,7 +106,7 @@ void Standard::update_vtol_state()
|
|||
// Regular backtransition
|
||||
_vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC;
|
||||
_vtol_schedule.transition_start = hrt_absolute_time();
|
||||
_reverse_output = _params_standard.reverse_output;
|
||||
_reverse_output = _param_vt_b_rev_out.get();
|
||||
|
||||
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
|
||||
// failsafe back to mc mode
|
||||
|
|
@ -148,13 +122,13 @@ void Standard::update_vtol_state()
|
|||
if (_local_pos->v_xy_valid) {
|
||||
const Dcmf R_to_body(Quatf(_v_att->q).inversed());
|
||||
const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
|
||||
exit_backtransition_speed_condition = vel(0) < _params->mpc_xy_cruise;
|
||||
exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get();
|
||||
|
||||
} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
|
||||
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _params->mpc_xy_cruise;
|
||||
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get();
|
||||
}
|
||||
|
||||
const bool exit_backtransition_time_condition = time_since_trans_start > _params->back_trans_duration;
|
||||
const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get();
|
||||
|
||||
if (can_transition_on_ground() || exit_backtransition_speed_condition || exit_backtransition_time_condition) {
|
||||
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
|
||||
|
|
@ -179,14 +153,14 @@ void Standard::update_vtol_state()
|
|||
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
|
||||
|
||||
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
|
||||
&& !_params->airspeed_disabled;
|
||||
&& !_param_fw_arsp_mode.get();
|
||||
const bool minimum_trans_time_elapsed = time_since_trans_start > getMinimumFrontTransitionTime();
|
||||
|
||||
bool transition_to_fw = false;
|
||||
|
||||
if (minimum_trans_time_elapsed) {
|
||||
if (airspeed_triggers_transition) {
|
||||
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
|
||||
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get();
|
||||
|
||||
} else {
|
||||
transition_to_fw = true;
|
||||
|
|
@ -248,41 +222,43 @@ void Standard::update_transition_state()
|
|||
}
|
||||
|
||||
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) {
|
||||
if (_params_standard.pusher_ramp_dt <= 0.0f) {
|
||||
if (_param_vt_psher_rmp_dt.get() <= 0.0f) {
|
||||
// just set the final target throttle value
|
||||
_pusher_throttle = _params->front_trans_throttle;
|
||||
_pusher_throttle = _param_vt_f_trans_thr.get();
|
||||
|
||||
} else if (_pusher_throttle <= _params->front_trans_throttle) {
|
||||
} else if (_pusher_throttle <= _param_vt_f_trans_thr.get()) {
|
||||
// ramp up throttle to the target throttle value
|
||||
_pusher_throttle = _params->front_trans_throttle * time_since_trans_start / _params_standard.pusher_ramp_dt;
|
||||
_pusher_throttle = _param_vt_f_trans_thr.get() * time_since_trans_start / _param_vt_psher_rmp_dt.get();
|
||||
}
|
||||
|
||||
_airspeed_trans_blend_margin = _param_vt_arsp_trans.get() - _param_vt_arsp_blend.get();
|
||||
|
||||
// do blending of mc and fw controls if a blending airspeed has been provided and the minimum transition time has passed
|
||||
if (_airspeed_trans_blend_margin > 0.0f &&
|
||||
PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
|
||||
_airspeed_validated->calibrated_airspeed_m_s > 0.0f &&
|
||||
_airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend &&
|
||||
_airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get() &&
|
||||
time_since_trans_start > getMinimumFrontTransitionTime()) {
|
||||
|
||||
mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) /
|
||||
mc_weight = 1.0f - fabsf(_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
|
||||
_airspeed_trans_blend_margin;
|
||||
// time based blending when no airspeed sensor is set
|
||||
|
||||
} else if (_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
|
||||
} else if (_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
|
||||
mc_weight = 1.0f - time_since_trans_start / getMinimumFrontTransitionTime();
|
||||
mc_weight = math::constrain(2.0f * mc_weight, 0.0f, 1.0f);
|
||||
|
||||
}
|
||||
|
||||
// ramp up FW_PSP_OFF
|
||||
_v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight);
|
||||
_v_att_sp->pitch_body = math::radians(_param_fw_psp_off.get()) * (1.0f - mc_weight);
|
||||
|
||||
const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));
|
||||
q_sp.copyTo(_v_att_sp->q_d);
|
||||
|
||||
// check front transition timeout
|
||||
if (_params->front_trans_timeout > FLT_EPSILON) {
|
||||
if (time_since_trans_start > _params->front_trans_timeout) {
|
||||
if (_param_vt_trans_timeout.get() > FLT_EPSILON) {
|
||||
if (time_since_trans_start > _param_vt_trans_timeout.get()) {
|
||||
// transition timeout occured, abort transition
|
||||
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
|
||||
}
|
||||
|
|
@ -304,17 +280,16 @@ void Standard::update_transition_state()
|
|||
|
||||
_pusher_throttle = 0.0f;
|
||||
|
||||
if (time_since_trans_start >= _params_standard.reverse_delay) {
|
||||
if (time_since_trans_start >= _param_vt_b_rev_del.get()) {
|
||||
// Handle throttle reversal for active breaking
|
||||
float thrscale = (time_since_trans_start - _params_standard.reverse_delay) / (_params_standard.pusher_ramp_dt);
|
||||
float thrscale = (time_since_trans_start - _param_vt_b_rev_del.get()) / (_param_vt_psher_rmp_dt.get());
|
||||
thrscale = math::constrain(thrscale, 0.0f, 1.0f);
|
||||
_pusher_throttle = thrscale * _params->back_trans_throttle;
|
||||
_pusher_throttle = thrscale * _param_vt_b_trans_thr.get();
|
||||
}
|
||||
|
||||
// continually increase mc attitude control as we transition back to mc mode
|
||||
if (_params_standard.back_trans_ramp > FLT_EPSILON) {
|
||||
mc_weight = time_since_trans_start / _params_standard.back_trans_ramp;
|
||||
|
||||
if (_param_vt_b_trans_ramp.get() > FLT_EPSILON) {
|
||||
mc_weight = time_since_trans_start / _param_vt_b_trans_ramp.get();
|
||||
}
|
||||
|
||||
set_all_motor_state(motor_state::ENABLED);
|
||||
|
|
@ -357,8 +332,6 @@ void Standard::fill_actuator_outputs()
|
|||
auto &mc_out = _actuators_out_0->control;
|
||||
auto &fw_out = _actuators_out_1->control;
|
||||
|
||||
const bool elevon_lock = (_params->elevons_mc_lock == 1);
|
||||
|
||||
switch (_vtol_schedule.flight_mode) {
|
||||
case vtol_mode::MC_MODE:
|
||||
|
||||
|
|
@ -370,8 +343,10 @@ void Standard::fill_actuator_outputs()
|
|||
mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
|
||||
|
||||
// FW out = 0, other than roll and pitch depending on elevon lock
|
||||
fw_out[actuator_controls_s::INDEX_ROLL] = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_ROLL];
|
||||
fw_out[actuator_controls_s::INDEX_PITCH] = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_PITCH];
|
||||
fw_out[actuator_controls_s::INDEX_ROLL] = _param_vt_elev_mc_lock.get() ? 0 :
|
||||
fw_in[actuator_controls_s::INDEX_ROLL];
|
||||
fw_out[actuator_controls_s::INDEX_PITCH] = _param_vt_elev_mc_lock.get() ? 0 :
|
||||
fw_in[actuator_controls_s::INDEX_PITCH];
|
||||
fw_out[actuator_controls_s::INDEX_YAW] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle;
|
||||
fw_out[actuator_controls_s::INDEX_FLAPS] = _flaps_setpoint_with_slewrate.getState();
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2015-2022 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
|
||||
|
|
@ -46,8 +46,6 @@
|
|||
#ifndef STANDARD_H
|
||||
#define STANDARD_H
|
||||
#include "vtol_type.h"
|
||||
#include <parameters/param.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
class Standard : public VtolType
|
||||
{
|
||||
|
|
@ -67,22 +65,6 @@ public:
|
|||
|
||||
private:
|
||||
|
||||
struct {
|
||||
float pusher_ramp_dt;
|
||||
float back_trans_ramp;
|
||||
float pitch_setpoint_offset;
|
||||
float reverse_output;
|
||||
float reverse_delay;
|
||||
} _params_standard;
|
||||
|
||||
struct {
|
||||
param_t pusher_ramp_dt;
|
||||
param_t back_trans_ramp;
|
||||
param_t pitch_setpoint_offset;
|
||||
param_t reverse_output;
|
||||
param_t reverse_delay;
|
||||
} _params_handles_standard;
|
||||
|
||||
enum class vtol_mode {
|
||||
MC_MODE = 0,
|
||||
TRANSITION_TO_FW,
|
||||
|
|
@ -100,5 +82,13 @@ private:
|
|||
float _airspeed_trans_blend_margin{0.0f};
|
||||
|
||||
void parameters_update() override;
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
|
||||
(ParamFloat<px4::params::VT_PSHER_RMP_DT>) _param_vt_psher_rmp_dt,
|
||||
(ParamFloat<px4::params::VT_B_TRANS_RAMP>) _param_vt_b_trans_ramp,
|
||||
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off,
|
||||
(ParamFloat<px4::params::VT_B_REV_OUT>) _param_vt_b_rev_out,
|
||||
(ParamFloat<px4::params::VT_B_REV_DEL>) _param_vt_b_rev_del
|
||||
)
|
||||
};
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2015-2022 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
|
||||
|
|
@ -60,16 +60,13 @@ Tailsitter::Tailsitter(VtolAttitudeControl *attc) :
|
|||
_mc_yaw_weight = 1.0f;
|
||||
|
||||
_flag_was_in_trans_mode = false;
|
||||
_params_handles_tailsitter.fw_pitch_sp_offset = param_find("FW_PSP_OFF");
|
||||
}
|
||||
|
||||
void
|
||||
Tailsitter::parameters_update()
|
||||
{
|
||||
float v;
|
||||
VtolType::updateParams();
|
||||
|
||||
param_get(_params_handles_tailsitter.fw_pitch_sp_offset, &v);
|
||||
_params_tailsitter.fw_pitch_sp_offset = math::radians(v);
|
||||
}
|
||||
|
||||
void Tailsitter::update_vtol_state()
|
||||
|
|
@ -111,7 +108,7 @@ void Tailsitter::update_vtol_state()
|
|||
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
|
||||
|
||||
// check if we have reached pitch angle to switch to MC mode
|
||||
if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _params->back_trans_duration) {
|
||||
if (pitch >= PITCH_TRANSITION_BACK || time_since_trans_start > _param_vt_b_trans_dur.get()) {
|
||||
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
|
||||
}
|
||||
|
||||
|
|
@ -136,13 +133,13 @@ void Tailsitter::update_vtol_state()
|
|||
|
||||
|
||||
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
|
||||
&& !_params->airspeed_disabled;
|
||||
&& !_param_fw_arsp_mode.get() ;
|
||||
|
||||
bool transition_to_fw = false;
|
||||
|
||||
if (pitch <= PITCH_TRANSITION_FRONT_P1) {
|
||||
if (airspeed_triggers_transition) {
|
||||
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
|
||||
transition_to_fw = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
|
||||
|
||||
} else {
|
||||
transition_to_fw = true;
|
||||
|
|
@ -156,8 +153,8 @@ void Tailsitter::update_vtol_state()
|
|||
}
|
||||
|
||||
// check front transition timeout
|
||||
if (_params->front_trans_timeout > FLT_EPSILON) {
|
||||
if (time_since_trans_start > _params->front_trans_timeout) {
|
||||
if (_param_vt_trans_timeout.get() > FLT_EPSILON) {
|
||||
if (time_since_trans_start > _param_vt_trans_timeout.get()) {
|
||||
// transition timeout occured, abort transition
|
||||
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
|
||||
}
|
||||
|
|
@ -245,16 +242,16 @@ void Tailsitter::update_transition_state()
|
|||
|
||||
if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P1) {
|
||||
|
||||
const float trans_pitch_rate = M_PI_2_F / _params->front_trans_duration;
|
||||
const float trans_pitch_rate = M_PI_2_F / _param_vt_f_trans_dur.get() ;
|
||||
|
||||
if (tilt < M_PI_2_F - _params_tailsitter.fw_pitch_sp_offset) {
|
||||
if (tilt < M_PI_2_F - math::radians(_param_fw_psp_off.get())) {
|
||||
_q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis,
|
||||
time_since_trans_start * trans_pitch_rate)) * _q_trans_start;
|
||||
}
|
||||
|
||||
// check front transition timeout
|
||||
if (_params->front_trans_timeout > FLT_EPSILON) {
|
||||
if (time_since_trans_start > _params->front_trans_timeout) {
|
||||
if (_param_vt_trans_timeout.get() > FLT_EPSILON) {
|
||||
if (time_since_trans_start > _param_vt_trans_timeout.get()) {
|
||||
// transition timeout occured, abort transition
|
||||
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
|
||||
}
|
||||
|
|
@ -262,7 +259,7 @@ void Tailsitter::update_transition_state()
|
|||
|
||||
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) {
|
||||
|
||||
const float trans_pitch_rate = M_PI_2_F / _params->back_trans_duration;
|
||||
const float trans_pitch_rate = M_PI_2_F / _param_vt_b_trans_dur.get() ;
|
||||
|
||||
if (!_flag_idle_mc) {
|
||||
_flag_idle_mc = set_idle_mc();
|
||||
|
|
@ -349,9 +346,9 @@ void Tailsitter::fill_actuator_outputs()
|
|||
_thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE];
|
||||
|
||||
/* allow differential thrust if enabled */
|
||||
if (_params->diff_thrust == 1) {
|
||||
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
|
||||
_torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
|
||||
if (_param_vt_fw_difthr_en.get()) {
|
||||
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
|
||||
_torque_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
@ -371,7 +368,7 @@ void Tailsitter::fill_actuator_outputs()
|
|||
mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP;
|
||||
}
|
||||
|
||||
if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
|
||||
if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
|
||||
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
|
||||
|
||||
|
|
|
|||
|
|
@ -62,15 +62,6 @@ public:
|
|||
void waiting_on_tecs() override;
|
||||
|
||||
private:
|
||||
|
||||
struct {
|
||||
float fw_pitch_sp_offset;
|
||||
} _params_tailsitter{};
|
||||
|
||||
struct {
|
||||
param_t fw_pitch_sp_offset;
|
||||
} _params_handles_tailsitter{};
|
||||
|
||||
enum class vtol_mode {
|
||||
MC_MODE = 0, /**< vtol is in multicopter mode */
|
||||
TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */
|
||||
|
|
@ -89,5 +80,10 @@ private:
|
|||
|
||||
void parameters_update() override;
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
|
||||
(ParamFloat<px4::params::FW_PSP_OFF>) _param_fw_psp_off
|
||||
)
|
||||
|
||||
|
||||
};
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2015-2022 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
|
||||
|
|
@ -63,38 +63,12 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
|
|||
_mc_yaw_weight = 1.0f;
|
||||
|
||||
_flag_was_in_trans_mode = false;
|
||||
|
||||
_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
|
||||
_params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS");
|
||||
_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW");
|
||||
_params_handles_tiltrotor.tilt_spinup = param_find("VT_TILT_SPINUP");
|
||||
_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
|
||||
}
|
||||
|
||||
void
|
||||
Tiltrotor::parameters_update()
|
||||
{
|
||||
float v;
|
||||
|
||||
/* vtol tilt mechanism position in mc mode */
|
||||
param_get(_params_handles_tiltrotor.tilt_mc, &v);
|
||||
_params_tiltrotor.tilt_mc = v;
|
||||
|
||||
/* vtol tilt mechanism position in transition mode */
|
||||
param_get(_params_handles_tiltrotor.tilt_transition, &v);
|
||||
_params_tiltrotor.tilt_transition = v;
|
||||
|
||||
/* vtol tilt mechanism position in fw mode */
|
||||
param_get(_params_handles_tiltrotor.tilt_fw, &v);
|
||||
_params_tiltrotor.tilt_fw = v;
|
||||
|
||||
/* vtol tilt mechanism position during motor spinup */
|
||||
param_get(_params_handles_tiltrotor.tilt_spinup, &v);
|
||||
_params_tiltrotor.tilt_spinup = v;
|
||||
|
||||
/* vtol front transition phase 2 duration */
|
||||
param_get(_params_handles_tiltrotor.front_trans_dur_p2, &v);
|
||||
_params_tiltrotor.front_trans_dur_p2 = v;
|
||||
VtolType::updateParams();
|
||||
}
|
||||
|
||||
void Tiltrotor::update_vtol_state()
|
||||
|
|
@ -137,7 +111,7 @@ void Tiltrotor::update_vtol_state()
|
|||
break;
|
||||
|
||||
case vtol_mode::TRANSITION_BACK:
|
||||
const bool exit_backtransition_tilt_condition = _tilt_control <= (_params_tiltrotor.tilt_mc + 0.01f);
|
||||
const bool exit_backtransition_tilt_condition = _tilt_control <= (_param_vt_tilt_mc.get() + 0.01f);
|
||||
|
||||
// speed exit condition: use ground if valid, otherwise airspeed
|
||||
bool exit_backtransition_speed_condition = false;
|
||||
|
|
@ -145,14 +119,14 @@ void Tiltrotor::update_vtol_state()
|
|||
if (_local_pos->v_xy_valid) {
|
||||
const Dcmf R_to_body(Quatf(_v_att->q).inversed());
|
||||
const Vector3f vel = R_to_body * Vector3f(_local_pos->vx, _local_pos->vy, _local_pos->vz);
|
||||
exit_backtransition_speed_condition = vel(0) < _params->mpc_xy_cruise;
|
||||
exit_backtransition_speed_condition = vel(0) < _param_mpc_xy_cruise.get() ;
|
||||
|
||||
} else if (PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) {
|
||||
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _params->mpc_xy_cruise;
|
||||
exit_backtransition_speed_condition = _airspeed_validated->calibrated_airspeed_m_s < _param_mpc_xy_cruise.get() ;
|
||||
}
|
||||
|
||||
const float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
|
||||
const bool exit_backtransition_time_condition = time_since_trans_start > _params->back_trans_duration;
|
||||
const bool exit_backtransition_time_condition = time_since_trans_start > _param_vt_b_trans_dur.get() ;
|
||||
|
||||
if (exit_backtransition_tilt_condition && (exit_backtransition_speed_condition || exit_backtransition_time_condition)) {
|
||||
_vtol_schedule.flight_mode = vtol_mode::MC_MODE;
|
||||
|
|
@ -178,16 +152,16 @@ void Tiltrotor::update_vtol_state()
|
|||
float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f;
|
||||
|
||||
const bool airspeed_triggers_transition = PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)
|
||||
&& !_params->airspeed_disabled;
|
||||
&& !_param_fw_arsp_mode.get() ;
|
||||
|
||||
bool transition_to_p2 = false;
|
||||
|
||||
if (time_since_trans_start > getMinimumFrontTransitionTime()) {
|
||||
if (airspeed_triggers_transition) {
|
||||
transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _params->transition_airspeed;
|
||||
transition_to_p2 = _airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_trans.get() ;
|
||||
|
||||
} else {
|
||||
transition_to_p2 = _tilt_control >= _params_tiltrotor.tilt_transition &&
|
||||
transition_to_p2 = _tilt_control >= _param_vt_tilt_trans.get() &&
|
||||
time_since_trans_start > getOpenLoopFrontTransitionTime();
|
||||
}
|
||||
}
|
||||
|
|
@ -200,8 +174,8 @@ void Tiltrotor::update_vtol_state()
|
|||
}
|
||||
|
||||
// check front transition timeout
|
||||
if (_params->front_trans_timeout > FLT_EPSILON) {
|
||||
if (time_since_trans_start > _params->front_trans_timeout) {
|
||||
if (_param_vt_trans_timeout.get() > FLT_EPSILON) {
|
||||
if (time_since_trans_start > _param_vt_trans_timeout.get()) {
|
||||
// transition timeout occured, abort transition
|
||||
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::TransitionTimeout);
|
||||
}
|
||||
|
|
@ -213,9 +187,9 @@ void Tiltrotor::update_vtol_state()
|
|||
case vtol_mode::TRANSITION_FRONT_P2:
|
||||
|
||||
// if the rotors have been tilted completely we switch to fw mode
|
||||
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
|
||||
if (_tilt_control >= _param_vt_tilt_fw.get()) {
|
||||
_vtol_schedule.flight_mode = vtol_mode::FW_MODE;
|
||||
_tilt_control = _params_tiltrotor.tilt_fw;
|
||||
_tilt_control = _param_vt_tilt_fw.get();
|
||||
}
|
||||
|
||||
break;
|
||||
|
|
@ -263,7 +237,7 @@ void Tiltrotor::update_mc_state()
|
|||
// reset this timestamp while disarmed
|
||||
if (!_v_control_mode->flag_armed) {
|
||||
_last_timestamp_disarmed = hrt_absolute_time();
|
||||
_tilt_motors_for_startup = _params_tiltrotor.tilt_spinup > 0.01f; // spinup phase only required if spinup tilt > 0
|
||||
_tilt_motors_for_startup = _param_vt_tilt_spinup.get() > 0.01f; // spinup phase only required if spinup tilt > 0
|
||||
|
||||
} else if (_tilt_motors_for_startup) {
|
||||
// leave motors tilted forward after arming to allow them to spin up easier
|
||||
|
|
@ -274,12 +248,12 @@ void Tiltrotor::update_mc_state()
|
|||
|
||||
if (_tilt_motors_for_startup) {
|
||||
if (hrt_absolute_time() - _last_timestamp_disarmed < spin_up_duration_p1) {
|
||||
_tilt_control = _params_tiltrotor.tilt_spinup;
|
||||
_tilt_control = _param_vt_tilt_spinup.get();
|
||||
|
||||
} else {
|
||||
// duration phase 2: begin to adapt tilt to multicopter tilt
|
||||
float delta_tilt = (_params_tiltrotor.tilt_mc - _params_tiltrotor.tilt_spinup);
|
||||
_tilt_control = _params_tiltrotor.tilt_spinup + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() -
|
||||
float delta_tilt = (_param_vt_tilt_mc.get() - _param_vt_tilt_spinup.get());
|
||||
_tilt_control = _param_vt_tilt_spinup.get() + delta_tilt / spin_up_duration_p2 * (hrt_absolute_time() -
|
||||
(_last_timestamp_disarmed + spin_up_duration_p1));
|
||||
}
|
||||
|
||||
|
|
@ -287,11 +261,11 @@ void Tiltrotor::update_mc_state()
|
|||
|
||||
} else {
|
||||
// normal operation
|
||||
_tilt_control = VtolType::pusher_assist() + _params_tiltrotor.tilt_mc;
|
||||
_tilt_control = VtolType::pusher_assist() + _param_vt_tilt_mc.get();
|
||||
_mc_yaw_weight = 1.0f;
|
||||
|
||||
// do thrust compensation only for legacy (static) allocation
|
||||
if (_params->ctrl_alloc != 1) {
|
||||
if (!_param_sys_ctrl_alloc.get()) {
|
||||
_v_att_sp->thrust_body[2] = Tiltrotor::thrust_compensation_for_tilt();
|
||||
}
|
||||
}
|
||||
|
|
@ -307,7 +281,7 @@ void Tiltrotor::update_fw_state()
|
|||
_v_att_sp->thrust_body[2] = -_v_att_sp->thrust_body[0];
|
||||
|
||||
// make sure motors are tilted forward
|
||||
_tilt_control = _params_tiltrotor.tilt_fw;
|
||||
_tilt_control = _param_vt_tilt_fw.get();
|
||||
}
|
||||
|
||||
void Tiltrotor::update_transition_state()
|
||||
|
|
@ -338,10 +312,10 @@ void Tiltrotor::update_transition_state()
|
|||
set_all_motor_state(motor_state::ENABLED);
|
||||
|
||||
// tilt rotors forward up to certain angle
|
||||
if (_tilt_control <= _params_tiltrotor.tilt_transition) {
|
||||
const float ramped_up_tilt = _params_tiltrotor.tilt_mc +
|
||||
fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc) *
|
||||
time_since_trans_start / _params->front_trans_duration;
|
||||
if (_tilt_control <= _param_vt_tilt_trans.get()) {
|
||||
const float ramped_up_tilt = _param_vt_tilt_mc.get() +
|
||||
fabsf(_param_vt_tilt_trans.get() - _param_vt_tilt_mc.get()) *
|
||||
time_since_trans_start / _param_vt_f_trans_dur.get() ;
|
||||
|
||||
// only allow increasing tilt (tilt in hover can already be non-zero)
|
||||
_tilt_control = math::max(_tilt_control, ramped_up_tilt);
|
||||
|
|
@ -351,16 +325,16 @@ void Tiltrotor::update_transition_state()
|
|||
_mc_roll_weight = 1.0f;
|
||||
_mc_yaw_weight = 1.0f;
|
||||
|
||||
if (!_params->airspeed_disabled && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
|
||||
_airspeed_validated->calibrated_airspeed_m_s >= _params->airspeed_blend) {
|
||||
const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _params->airspeed_blend) /
|
||||
(_params->transition_airspeed - _params->airspeed_blend);
|
||||
if (!_param_fw_arsp_mode.get() && PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s) &&
|
||||
_airspeed_validated->calibrated_airspeed_m_s >= _param_vt_arsp_blend.get()) {
|
||||
const float weight = 1.0f - (_airspeed_validated->calibrated_airspeed_m_s - _param_vt_arsp_blend.get()) /
|
||||
(_param_vt_arsp_trans.get() - _param_vt_arsp_blend.get());
|
||||
_mc_roll_weight = weight;
|
||||
_mc_yaw_weight = weight;
|
||||
}
|
||||
|
||||
// without airspeed do timed weight changes
|
||||
if ((_params->airspeed_disabled || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
|
||||
if ((_param_fw_arsp_mode.get() || !PX4_ISFINITE(_airspeed_validated->calibrated_airspeed_m_s)) &&
|
||||
time_since_trans_start > getMinimumFrontTransitionTime()) {
|
||||
_mc_roll_weight = 1.0f - (time_since_trans_start - getMinimumFrontTransitionTime()) /
|
||||
(getOpenLoopFrontTransitionTime() - getMinimumFrontTransitionTime());
|
||||
|
|
@ -376,15 +350,15 @@ void Tiltrotor::update_transition_state()
|
|||
|
||||
} else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) {
|
||||
// the plane is ready to go into fixed wing mode, tilt the rotors forward completely
|
||||
_tilt_control = math::constrain(_params_tiltrotor.tilt_transition +
|
||||
fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition) * time_since_trans_start /
|
||||
_params_tiltrotor.front_trans_dur_p2, _params_tiltrotor.tilt_transition, _params_tiltrotor.tilt_fw);
|
||||
_tilt_control = math::constrain(_param_vt_tilt_trans.get() +
|
||||
fabsf(_param_vt_tilt_fw.get() - _param_vt_tilt_trans.get()) * time_since_trans_start /
|
||||
_param_vt_trans_p2_dur.get(), _param_vt_tilt_trans.get(), _param_vt_tilt_fw.get());
|
||||
|
||||
_mc_roll_weight = 0.0f;
|
||||
_mc_yaw_weight = 0.0f;
|
||||
|
||||
// ramp down motors not used in fixed-wing flight (setting MAX_PWM down scales the given output into the new range)
|
||||
int ramp_down_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) *
|
||||
int ramp_down_value = (1.0f - time_since_trans_start / _param_vt_trans_p2_dur.get()) *
|
||||
(PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN;
|
||||
|
||||
set_alternate_motor_state(motor_state::VALUE, ramp_down_value);
|
||||
|
|
@ -412,7 +386,7 @@ void Tiltrotor::update_transition_state()
|
|||
|
||||
float progress = (time_since_trans_start - BACKTRANS_THROTTLE_DOWNRAMP_DUR_S) / BACKTRANS_MOTORS_UPTILT_DUR_S;
|
||||
progress = math::constrain(progress, 0.0f, 1.0f);
|
||||
_tilt_control = moveLinear(_params_tiltrotor.tilt_fw, _params_tiltrotor.tilt_mc, progress);
|
||||
_tilt_control = moveLinear(_param_vt_tilt_fw.get(), _param_vt_tilt_mc.get(), progress);
|
||||
}
|
||||
|
||||
_mc_yaw_weight = 1.0f;
|
||||
|
|
@ -521,9 +495,9 @@ void Tiltrotor::fill_actuator_outputs()
|
|||
_thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE];
|
||||
|
||||
/* allow differential thrust if enabled */
|
||||
if (_params->diff_thrust == 1) {
|
||||
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
|
||||
_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
|
||||
if (_param_vt_fw_difthr_en.get()) {
|
||||
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
|
||||
_torque_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_sc.get() ;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
@ -544,10 +518,10 @@ void Tiltrotor::fill_actuator_outputs()
|
|||
// Fixed wing output
|
||||
fw_out[actuator_controls_s::INDEX_COLLECTIVE_TILT] = _tilt_control;
|
||||
|
||||
if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
|
||||
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_YAW] = 0;
|
||||
if (_param_vt_elev_mc_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
|
||||
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
|
||||
fw_out[actuator_controls_s::INDEX_YAW] = 0;
|
||||
|
||||
} else {
|
||||
fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL];
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2015-2022 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
|
||||
|
|
@ -62,23 +62,6 @@ public:
|
|||
void blendThrottleAfterFrontTransition(float scale) override;
|
||||
|
||||
private:
|
||||
|
||||
struct {
|
||||
float tilt_mc; /**< actuator value corresponding to mc tilt */
|
||||
float tilt_transition; /**< actuator value corresponding to transition tilt (e.g 45 degrees) */
|
||||
float tilt_fw; /**< actuator value corresponding to fw tilt */
|
||||
float tilt_spinup; /**< actuator value corresponding to spinup tilt */
|
||||
float front_trans_dur_p2;
|
||||
} _params_tiltrotor;
|
||||
|
||||
struct {
|
||||
param_t tilt_mc;
|
||||
param_t tilt_transition;
|
||||
param_t tilt_fw;
|
||||
param_t tilt_spinup;
|
||||
param_t front_trans_dur_p2;
|
||||
} _params_handles_tiltrotor;
|
||||
|
||||
enum class vtol_mode {
|
||||
MC_MODE = 0, /**< vtol is in multicopter mode */
|
||||
TRANSITION_FRONT_P1, /**< vtol is in front transition part 1 mode */
|
||||
|
|
@ -111,5 +94,13 @@ private:
|
|||
hrt_abstime _last_timestamp_disarmed{0}; /**< used for calculating time since arming */
|
||||
bool _tilt_motors_for_startup{false};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(VtolType,
|
||||
(ParamFloat<px4::params::VT_TILT_MC>) _param_vt_tilt_mc,
|
||||
(ParamFloat<px4::params::VT_TILT_TRANS>) _param_vt_tilt_trans,
|
||||
(ParamFloat<px4::params::VT_TILT_FW>) _param_vt_tilt_fw,
|
||||
(ParamFloat<px4::params::VT_TILT_SPINUP>) _param_vt_tilt_spinup,
|
||||
(ParamFloat<px4::params::VT_TRANS_P2_DUR>) _param_vt_trans_p2_dur
|
||||
)
|
||||
|
||||
};
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2022 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
|
||||
|
|
@ -55,71 +55,21 @@ using namespace matrix;
|
|||
using namespace time_literals;
|
||||
|
||||
VtolAttitudeControl::VtolAttitudeControl() :
|
||||
ModuleParams(nullptr),
|
||||
WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control: cycle"))
|
||||
{
|
||||
_vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/
|
||||
|
||||
_params.idle_pwm_mc = PWM_DEFAULT_MIN;
|
||||
_params.vtol_motor_id = 0;
|
||||
|
||||
_params_handles.sys_ctrl_alloc = param_find("SYS_CTRL_ALLOC");
|
||||
_params.ctrl_alloc = 0;
|
||||
param_get(_params_handles.sys_ctrl_alloc, &_params.ctrl_alloc);
|
||||
|
||||
if (_params.ctrl_alloc != 1) {
|
||||
// these are not used with dynamic control allocation
|
||||
_params_handles.idle_pwm_mc = param_find("VT_IDLE_PWM_MC");
|
||||
_params_handles.vtol_motor_id = param_find("VT_MOT_ID");
|
||||
_params_handles.vt_mc_on_fmu = param_find("VT_MC_ON_FMU");
|
||||
_params_handles.fw_motors_off = param_find("VT_FW_MOT_OFFID");
|
||||
}
|
||||
|
||||
_params_handles.vtol_fw_permanent_stab = param_find("VT_FW_PERM_STAB");
|
||||
_params_handles.vtol_type = param_find("VT_TYPE");
|
||||
_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
|
||||
_params_handles.fw_min_alt = param_find("VT_FW_MIN_ALT");
|
||||
_params_handles.fw_alt_err = param_find("VT_FW_ALT_ERR");
|
||||
_params_handles.fw_qc_max_pitch = param_find("VT_FW_QC_P");
|
||||
_params_handles.fw_qc_max_roll = param_find("VT_FW_QC_R");
|
||||
_params_handles.front_trans_time_openloop = param_find("VT_F_TR_OL_TM");
|
||||
_params_handles.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
|
||||
|
||||
_params_handles.front_trans_duration = param_find("VT_F_TRANS_DUR");
|
||||
_params_handles.back_trans_duration = param_find("VT_B_TRANS_DUR");
|
||||
_params_handles.transition_airspeed = param_find("VT_ARSP_TRANS");
|
||||
_params_handles.front_trans_throttle = param_find("VT_F_TRANS_THR");
|
||||
_params_handles.back_trans_throttle = param_find("VT_B_TRANS_THR");
|
||||
_params_handles.airspeed_blend = param_find("VT_ARSP_BLEND");
|
||||
_params_handles.airspeed_mode = param_find("FW_ARSP_MODE");
|
||||
_params_handles.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
|
||||
_params_handles.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
|
||||
_params_handles.diff_thrust = param_find("VT_FW_DIFTHR_EN");
|
||||
_params_handles.diff_thrust_scale = param_find("VT_FW_DIFTHR_SC");
|
||||
_params_handles.dec_to_pitch_ff = param_find("VT_B_DEC_FF");
|
||||
_params_handles.dec_to_pitch_i = param_find("VT_B_DEC_I");
|
||||
_params_handles.back_trans_dec_sp = param_find("VT_B_DEC_MSS");
|
||||
|
||||
_params_handles.pitch_min_rad = param_find("VT_PTCH_MIN");
|
||||
_params_handles.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
|
||||
|
||||
_params_handles.vt_forward_thrust_enable_mode = param_find("VT_FWD_THRUST_EN");
|
||||
_params_handles.mpc_land_alt1 = param_find("MPC_LAND_ALT1");
|
||||
_params_handles.mpc_land_alt2 = param_find("MPC_LAND_ALT2");
|
||||
|
||||
_params_handles.land_pitch_min_rad = param_find("VT_LND_PTCH_MIN");
|
||||
|
||||
_params_handles.vt_spoiler_mc_ld = param_find("VT_SPOILER_MC_LD");
|
||||
/* fetch initial parameter values */
|
||||
parameters_update();
|
||||
|
||||
if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TAILSITTER) {
|
||||
if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::TAILSITTER) {
|
||||
_vtol_type = new Tailsitter(this);
|
||||
|
||||
} else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::TILTROTOR) {
|
||||
} else if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::TILTROTOR) {
|
||||
_vtol_type = new Tiltrotor(this);
|
||||
|
||||
} else if (static_cast<vtol_type>(_params.vtol_type) == vtol_type::STANDARD) {
|
||||
} else if (static_cast<vtol_type>(_param_vt_type.get()) == vtol_type::STANDARD) {
|
||||
_vtol_type = new Standard(this);
|
||||
|
||||
} else {
|
||||
|
|
@ -269,118 +219,24 @@ VtolAttitudeControl::quadchute(QuadchuteReason reason)
|
|||
}
|
||||
}
|
||||
|
||||
int
|
||||
void
|
||||
VtolAttitudeControl::parameters_update()
|
||||
{
|
||||
float v;
|
||||
int32_t l;
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s param_update;
|
||||
_parameter_update_sub.copy(¶m_update);
|
||||
|
||||
if (_params.ctrl_alloc != 1) {
|
||||
/* idle pwm for mc mode */
|
||||
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
|
||||
param_get(_params_handles.vtol_motor_id, &_params.vtol_motor_id);
|
||||
param_get(_params_handles.vt_mc_on_fmu, &l);
|
||||
_params.vt_mc_on_fmu = l;
|
||||
// update parameters from storage
|
||||
updateParams();
|
||||
|
||||
/* vtol motor count */
|
||||
param_get(_params_handles.fw_motors_off, &_params.fw_motors_off);
|
||||
if (_vtol_type != nullptr) {
|
||||
_vtol_type->parameters_update();
|
||||
}
|
||||
|
||||
_vtol_vehicle_status.fw_permanent_stab = _param_vt_fw_perm_stab.get();
|
||||
}
|
||||
|
||||
/* vtol fw permanent stabilization */
|
||||
param_get(_params_handles.vtol_fw_permanent_stab, &l);
|
||||
_vtol_vehicle_status.fw_permanent_stab = (l == 1);
|
||||
|
||||
param_get(_params_handles.vtol_type, &l);
|
||||
_params.vtol_type = l;
|
||||
|
||||
/* vtol lock elevons in multicopter */
|
||||
param_get(_params_handles.elevons_mc_lock, &l);
|
||||
_params.elevons_mc_lock = (l == 1);
|
||||
|
||||
/* minimum relative altitude for FW mode (QuadChute) */
|
||||
param_get(_params_handles.fw_min_alt, &v);
|
||||
_params.fw_min_alt = v;
|
||||
|
||||
/* maximum negative altitude error for FW mode (Adaptive QuadChute) */
|
||||
param_get(_params_handles.fw_alt_err, &v);
|
||||
_params.fw_alt_err = v;
|
||||
|
||||
/* maximum pitch angle (QuadChute) */
|
||||
param_get(_params_handles.fw_qc_max_pitch, &l);
|
||||
_params.fw_qc_max_pitch = l;
|
||||
|
||||
/* maximum roll angle (QuadChute) */
|
||||
param_get(_params_handles.fw_qc_max_roll, &l);
|
||||
_params.fw_qc_max_roll = l;
|
||||
|
||||
param_get(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);
|
||||
|
||||
param_get(_params_handles.front_trans_time_min, &_params.front_trans_time_min);
|
||||
|
||||
/*
|
||||
* Open loop transition time needs to be larger than minimum transition time,
|
||||
* anything else makes no sense and can potentially lead to numerical problems.
|
||||
*/
|
||||
if (_params.front_trans_time_openloop < _params.front_trans_time_min * 1.1f) {
|
||||
_params.front_trans_time_openloop = _params.front_trans_time_min * 1.1f;
|
||||
param_set_no_notification(_params_handles.front_trans_time_openloop, &_params.front_trans_time_openloop);
|
||||
mavlink_log_critical(&_mavlink_log_pub, "OL transition time set larger than min transition time\t");
|
||||
/* EVENT
|
||||
* @description <param>VT_F_TR_OL_TM</param> set to {1:.1}.
|
||||
*/
|
||||
events::send<float>(events::ID("vtol_att_ctrl_ol_trans_too_large"), events::Log::Warning,
|
||||
"Open loop transition time set larger than minimum transition time", _params.front_trans_time_openloop);
|
||||
}
|
||||
|
||||
param_get(_params_handles.front_trans_duration, &_params.front_trans_duration);
|
||||
param_get(_params_handles.back_trans_duration, &_params.back_trans_duration);
|
||||
param_get(_params_handles.transition_airspeed, &_params.transition_airspeed);
|
||||
param_get(_params_handles.front_trans_throttle, &_params.front_trans_throttle);
|
||||
param_get(_params_handles.back_trans_throttle, &_params.back_trans_throttle);
|
||||
param_get(_params_handles.airspeed_blend, &_params.airspeed_blend);
|
||||
param_get(_params_handles.airspeed_mode, &l);
|
||||
_params.airspeed_disabled = l != 0;
|
||||
param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout);
|
||||
param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise);
|
||||
param_get(_params_handles.diff_thrust, &_params.diff_thrust);
|
||||
|
||||
param_get(_params_handles.diff_thrust_scale, &v);
|
||||
_params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f);
|
||||
|
||||
/* maximum down pitch allowed */
|
||||
param_get(_params_handles.pitch_min_rad, &v);
|
||||
_params.pitch_min_rad = math::radians(v);
|
||||
|
||||
/* maximum down pitch allowed during landing*/
|
||||
param_get(_params_handles.land_pitch_min_rad, &v);
|
||||
_params.land_pitch_min_rad = math::radians(v);
|
||||
|
||||
/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
|
||||
param_get(_params_handles.forward_thrust_scale, &_params.forward_thrust_scale);
|
||||
|
||||
// make sure parameters are feasible, require at least 1 m/s difference between transition and blend airspeed
|
||||
_params.airspeed_blend = math::min(_params.airspeed_blend, _params.transition_airspeed - 1.0f);
|
||||
|
||||
param_get(_params_handles.back_trans_dec_sp, &v);
|
||||
// increase the target deceleration setpoint provided to the controller by 20%
|
||||
// to make overshooting the transition waypoint less likely in the presence of tracking errors
|
||||
_params.back_trans_dec_sp = 1.2f * v;
|
||||
|
||||
param_get(_params_handles.dec_to_pitch_ff, &_params.dec_to_pitch_ff);
|
||||
param_get(_params_handles.dec_to_pitch_i, &_params.dec_to_pitch_i);
|
||||
|
||||
param_get(_params_handles.vt_forward_thrust_enable_mode, &_params.vt_forward_thrust_enable_mode);
|
||||
param_get(_params_handles.mpc_land_alt1, &_params.mpc_land_alt1);
|
||||
param_get(_params_handles.mpc_land_alt2, &_params.mpc_land_alt2);
|
||||
|
||||
param_get(_params_handles.vt_spoiler_mc_ld, &_params.vt_spoiler_mc_ld);
|
||||
|
||||
// update the parameters of the instances of base VtolType
|
||||
if (_vtol_type != nullptr) {
|
||||
_vtol_type->parameters_update();
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void
|
||||
|
|
@ -408,7 +264,6 @@ VtolAttitudeControl::Run()
|
|||
_last_run_timestamp = now;
|
||||
|
||||
if (!_initialized) {
|
||||
parameters_update(); // initialize parameter cache
|
||||
|
||||
if (_vtol_type->init()) {
|
||||
_initialized = true;
|
||||
|
|
@ -445,15 +300,7 @@ VtolAttitudeControl::Run()
|
|||
}
|
||||
|
||||
if (should_run) {
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
parameters_update();
|
||||
}
|
||||
parameters_update();
|
||||
|
||||
_v_control_mode_sub.update(&_v_control_mode);
|
||||
_v_att_sub.update(&_v_att);
|
||||
|
|
@ -473,8 +320,8 @@ VtolAttitudeControl::Run()
|
|||
}
|
||||
|
||||
// check if mc and fw sp were updated
|
||||
bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
|
||||
bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
|
||||
const bool mc_att_sp_updated = _mc_virtual_att_sp_sub.update(&_mc_virtual_att_sp);
|
||||
const bool fw_att_sp_updated = _fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);
|
||||
|
||||
// update the vtol state machine which decides which mode we are in
|
||||
_vtol_type->update_vtol_state();
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2013-2022 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
|
||||
|
|
@ -53,12 +53,12 @@
|
|||
#include <drivers/drv_pwm_output.h>
|
||||
#include <lib/geo/geo.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/parameters/param.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
#include <matrix/math.hpp>
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/defines.h>
|
||||
#include <px4_platform_common/module.h>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
#include <px4_platform_common/posix.h>
|
||||
#include <px4_platform_common/px4_work_queue/WorkItem.hpp>
|
||||
#include <uORB/Publication.hpp>
|
||||
|
|
@ -94,7 +94,7 @@ extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
|
|||
|
||||
static constexpr float kMaxVTOLAttitudeControlTimeStep = 0.1f; // max time step [s]
|
||||
|
||||
class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public px4::WorkItem
|
||||
class VtolAttitudeControl : public ModuleBase<VtolAttitudeControl>, public ModuleParams, public px4::WorkItem
|
||||
{
|
||||
public:
|
||||
|
||||
|
|
@ -152,8 +152,6 @@ public:
|
|||
struct vehicle_thrust_setpoint_s *get_thrust_setpoint_0() {return &_thrust_setpoint_0;}
|
||||
struct vehicle_thrust_setpoint_s *get_thrust_setpoint_1() {return &_thrust_setpoint_1;}
|
||||
|
||||
struct Params *get_params() {return &_params;}
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
|
|
@ -214,46 +212,6 @@ private:
|
|||
|
||||
float _air_density{CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C}; // [kg/m^3]
|
||||
|
||||
Params _params{}; // struct holding the parameters
|
||||
|
||||
struct {
|
||||
param_t idle_pwm_mc;
|
||||
param_t vtol_motor_id;
|
||||
param_t vtol_fw_permanent_stab;
|
||||
param_t vtol_type;
|
||||
param_t elevons_mc_lock;
|
||||
param_t fw_min_alt;
|
||||
param_t fw_alt_err;
|
||||
param_t fw_qc_max_pitch;
|
||||
param_t fw_qc_max_roll;
|
||||
param_t front_trans_time_openloop;
|
||||
param_t front_trans_time_min;
|
||||
param_t front_trans_duration;
|
||||
param_t back_trans_duration;
|
||||
param_t transition_airspeed;
|
||||
param_t front_trans_throttle;
|
||||
param_t back_trans_throttle;
|
||||
param_t airspeed_blend;
|
||||
param_t airspeed_mode;
|
||||
param_t front_trans_timeout;
|
||||
param_t mpc_xy_cruise;
|
||||
param_t fw_motors_off;
|
||||
param_t diff_thrust;
|
||||
param_t diff_thrust_scale;
|
||||
param_t pitch_min_rad;
|
||||
param_t land_pitch_min_rad;
|
||||
param_t forward_thrust_scale;
|
||||
param_t dec_to_pitch_ff;
|
||||
param_t dec_to_pitch_i;
|
||||
param_t back_trans_dec_sp;
|
||||
param_t vt_mc_on_fmu;
|
||||
param_t vt_forward_thrust_enable_mode;
|
||||
param_t mpc_land_alt1;
|
||||
param_t mpc_land_alt2;
|
||||
param_t sys_ctrl_alloc;
|
||||
param_t vt_spoiler_mc_ld;
|
||||
} _params_handles{};
|
||||
|
||||
hrt_abstime _last_run_timestamp{0};
|
||||
|
||||
/* for multicopters it is usual to have a non-zero idle speed of the engines
|
||||
|
|
@ -271,5 +229,10 @@ private:
|
|||
void action_request_poll();
|
||||
void vehicle_cmd_poll();
|
||||
|
||||
int parameters_update(); //Update local parameter cache
|
||||
void parameters_update();
|
||||
|
||||
DEFINE_PARAMETERS(
|
||||
(ParamInt<px4::params::VT_TYPE>) _param_vt_type,
|
||||
(ParamBool<px4::params::VT_FW_PERM_STAB>) _param_vt_fw_perm_stab
|
||||
)
|
||||
};
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2014-2022 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
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2015-2022 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
|
||||
|
|
@ -52,6 +52,7 @@ using namespace matrix;
|
|||
|
||||
|
||||
VtolType::VtolType(VtolAttitudeControl *att_controller) :
|
||||
ModuleParams(nullptr),
|
||||
_attc(att_controller),
|
||||
_vtol_mode(mode::ROTARY_WING)
|
||||
{
|
||||
|
|
@ -74,7 +75,6 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
|
|||
_airspeed_validated = _attc->get_airspeed();
|
||||
_tecs_status = _attc->get_tecs_status();
|
||||
_land_detected = _attc->get_land_detected();
|
||||
_params = _attc->get_params();
|
||||
|
||||
for (auto &pwm_max : _max_mc_pwm_values.values) {
|
||||
pwm_max = PWM_DEFAULT_MAX;
|
||||
|
|
@ -87,8 +87,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
|
|||
|
||||
bool VtolType::init()
|
||||
{
|
||||
if (_params->ctrl_alloc != 1) {
|
||||
const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
|
||||
if (!_param_sys_ctrl_alloc.get()) {
|
||||
const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
|
||||
|
||||
int fd = px4_open(dev, 0);
|
||||
|
||||
|
|
@ -124,8 +124,8 @@ bool VtolType::init()
|
|||
|
||||
px4_close(fd);
|
||||
|
||||
_main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->vtol_motor_id);
|
||||
_alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_params->fw_motors_off);
|
||||
_main_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_param_vt_mot_id.get());
|
||||
_alternate_motor_channel_bitmap = generate_bitmap_from_channel_numbers(_param_vt_fw_mot_offid.get());
|
||||
|
||||
|
||||
// in order to get the main motors we take all motors and clear the alternate motor bits
|
||||
|
|
@ -143,6 +143,16 @@ bool VtolType::init()
|
|||
|
||||
}
|
||||
|
||||
void VtolType::parameters_update()
|
||||
{
|
||||
updateParams();
|
||||
|
||||
// make sure that transition speed is above blending speed
|
||||
_param_vt_arsp_trans.set(math::max(_param_vt_arsp_trans.get(), _param_vt_arsp_blend.get()));
|
||||
// make sure that openloop transition time is above minimum time
|
||||
_param_vt_f_tr_ol_tm.set(math::max(_param_vt_f_tr_ol_tm.get(), _param_vt_trans_min_tm.get()));
|
||||
}
|
||||
|
||||
void VtolType::update_mc_state()
|
||||
{
|
||||
if (!_flag_idle_mc) {
|
||||
|
|
@ -165,7 +175,7 @@ void VtolType::update_mc_state()
|
|||
|
||||
if (_attc->get_pos_sp_triplet()->current.valid
|
||||
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
spoiler_setpoint_hover = _params->vt_spoiler_mc_ld;
|
||||
spoiler_setpoint_hover = _param_vt_spoiler_mc_ld.get();
|
||||
}
|
||||
|
||||
_spoiler_setpoint_with_slewrate.update(math::constrain(spoiler_setpoint_hover, 0.f, 1.f), _dt);
|
||||
|
|
@ -243,12 +253,16 @@ float VtolType::update_and_get_backtransition_pitch_sp()
|
|||
const float track = atan2f(_local_pos->vy, _local_pos->vx);
|
||||
const float accel_body_forward = cosf(track) * _local_pos->ax + sinf(track) * _local_pos->ay;
|
||||
|
||||
// increase the target deceleration setpoint provided to the controller by 20%
|
||||
// to make overshooting the transition waypoint less likely in the presence of tracking errors
|
||||
const float dec_sp = _param_vt_b_dec_mss.get() * 1.2f;
|
||||
|
||||
// get accel error, positive means decelerating too slow, need to pitch up (must reverse dec_max, as it is a positive number)
|
||||
const float accel_error_forward = _params->back_trans_dec_sp + accel_body_forward;
|
||||
const float accel_error_forward = dec_sp + accel_body_forward;
|
||||
|
||||
const float pitch_sp_new = _params->dec_to_pitch_ff * _params->back_trans_dec_sp + _accel_to_pitch_integ;
|
||||
const float pitch_sp_new = _param_vt_b_dec_ff.get() * dec_sp + _accel_to_pitch_integ;
|
||||
|
||||
float integrator_input = _params->dec_to_pitch_i * accel_error_forward;
|
||||
float integrator_input = _param_vt_b_dec_i.get() * accel_error_forward;
|
||||
|
||||
if ((pitch_sp_new >= pitch_lim && accel_error_forward > 0.0f) ||
|
||||
(pitch_sp_new <= 0.f && accel_error_forward < 0.0f)) {
|
||||
|
|
@ -288,15 +302,15 @@ void VtolType::check_quadchute_condition()
|
|||
Eulerf euler = Quatf(_v_att->q);
|
||||
|
||||
// fixed-wing minimum altitude
|
||||
if (_params->fw_min_alt > FLT_EPSILON) {
|
||||
if (_param_vt_fw_min_alt.get() > FLT_EPSILON) {
|
||||
|
||||
if (-(_local_pos->z) < _params->fw_min_alt) {
|
||||
if (-(_local_pos->z) < _param_vt_fw_min_alt.get()) {
|
||||
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MinimumAltBreached);
|
||||
}
|
||||
}
|
||||
|
||||
// adaptive quadchute
|
||||
if (_params->fw_alt_err > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) {
|
||||
if (_param_vt_fw_alt_err.get() > FLT_EPSILON && _v_control_mode->flag_control_altitude_enabled) {
|
||||
|
||||
// We use tecs for tracking in FW and local_pos_sp during transitions
|
||||
if (_tecs_running) {
|
||||
|
|
@ -305,7 +319,7 @@ void VtolType::check_quadchute_condition()
|
|||
_ra_hrate_sp = (49 * _ra_hrate_sp + _tecs_status->height_rate_setpoint) / 50;
|
||||
|
||||
// are we dropping while requesting significant ascend?
|
||||
if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _params->fw_alt_err) &&
|
||||
if (((_tecs_status->altitude_sp - _tecs_status->altitude_filtered) > _param_vt_fw_alt_err.get()) &&
|
||||
(_ra_hrate < -1.0f) &&
|
||||
(_ra_hrate_sp > 1.0f)) {
|
||||
|
||||
|
|
@ -313,7 +327,7 @@ void VtolType::check_quadchute_condition()
|
|||
}
|
||||
|
||||
} else {
|
||||
const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _params->fw_alt_err);
|
||||
const bool height_error = _local_pos->z_valid && ((-_local_pos_sp->z - -_local_pos->z) > _param_vt_fw_alt_err.get());
|
||||
const bool height_rate_error = _local_pos->v_z_valid && (_local_pos->vz > 1.0f) && (_local_pos->z_deriv > 1.0f);
|
||||
|
||||
if (height_error && height_rate_error) {
|
||||
|
|
@ -323,17 +337,17 @@ void VtolType::check_quadchute_condition()
|
|||
}
|
||||
|
||||
// fixed-wing maximum pitch angle
|
||||
if (_params->fw_qc_max_pitch > 0) {
|
||||
if (_param_vt_fw_qc_p.get() > 0) {
|
||||
|
||||
if (fabsf(euler.theta()) > fabsf(math::radians(_params->fw_qc_max_pitch))) {
|
||||
if (fabsf(euler.theta()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_p.get())))) {
|
||||
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumPitchExceeded);
|
||||
}
|
||||
}
|
||||
|
||||
// fixed-wing maximum roll angle
|
||||
if (_params->fw_qc_max_roll > 0) {
|
||||
if (_param_vt_fw_qc_r.get() > 0) {
|
||||
|
||||
if (fabsf(euler.phi()) > fabsf(math::radians(_params->fw_qc_max_roll))) {
|
||||
if (fabsf(euler.phi()) > fabsf(math::radians(static_cast<float>(_param_vt_fw_qc_r.get())))) {
|
||||
_attc->quadchute(VtolAttitudeControl::QuadchuteReason::MaximumRollExceeded);
|
||||
}
|
||||
}
|
||||
|
|
@ -342,15 +356,15 @@ void VtolType::check_quadchute_condition()
|
|||
|
||||
bool VtolType::set_idle_mc()
|
||||
{
|
||||
if (_params->ctrl_alloc == 1) {
|
||||
if (_param_sys_ctrl_alloc.get()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
unsigned pwm_value = _params->idle_pwm_mc;
|
||||
unsigned pwm_value = _param_vt_idle_pwm_mc.get();
|
||||
struct pwm_output_values pwm_values {};
|
||||
|
||||
for (int i = 0; i < num_outputs_max; i++) {
|
||||
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
|
||||
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()))) {
|
||||
pwm_values.values[i] = pwm_value;
|
||||
|
||||
} else {
|
||||
|
|
@ -365,14 +379,14 @@ bool VtolType::set_idle_mc()
|
|||
|
||||
bool VtolType::set_idle_fw()
|
||||
{
|
||||
if (_params->ctrl_alloc == 1) {
|
||||
if (_param_sys_ctrl_alloc.get()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
struct pwm_output_values pwm_values {};
|
||||
|
||||
for (int i = 0; i < num_outputs_max; i++) {
|
||||
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_params->vtol_motor_id))) {
|
||||
if (is_channel_set(i, generate_bitmap_from_channel_numbers(_param_vt_mot_id.get()))) {
|
||||
pwm_values.values[i] = PWM_DEFAULT_MIN;
|
||||
|
||||
} else {
|
||||
|
|
@ -387,7 +401,7 @@ bool VtolType::set_idle_fw()
|
|||
|
||||
bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_type type)
|
||||
{
|
||||
const char *dev = _params->vt_mc_on_fmu ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
|
||||
const char *dev = _param_vt_mc_on_fmu.get() ? PWM_OUTPUT1_DEVICE_PATH : PWM_OUTPUT0_DEVICE_PATH;
|
||||
|
||||
int fd = px4_open(dev, 0);
|
||||
|
||||
|
|
@ -418,7 +432,7 @@ bool VtolType::apply_pwm_limits(struct pwm_output_values &pwm_values, pwm_limit_
|
|||
|
||||
void VtolType::set_all_motor_state(const motor_state target_state, const int value)
|
||||
{
|
||||
if (_params->ctrl_alloc == 1) {
|
||||
if (_param_sys_ctrl_alloc.get()) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -428,7 +442,7 @@ void VtolType::set_all_motor_state(const motor_state target_state, const int val
|
|||
|
||||
void VtolType::set_main_motor_state(const motor_state target_state, const int value)
|
||||
{
|
||||
if (_params->ctrl_alloc == 1) {
|
||||
if (_param_sys_ctrl_alloc.get()) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -442,7 +456,7 @@ void VtolType::set_main_motor_state(const motor_state target_state, const int va
|
|||
|
||||
void VtolType::set_alternate_motor_state(const motor_state target_state, const int value)
|
||||
{
|
||||
if (_params->ctrl_alloc == 1) {
|
||||
if (_param_sys_ctrl_alloc.get()) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
|
@ -479,7 +493,7 @@ bool VtolType::set_motor_state(const motor_state target_state, const int32_t cha
|
|||
|
||||
for (int i = 0; i < num_outputs_max; i++) {
|
||||
if (is_channel_set(i, channel_bitmap)) {
|
||||
_current_max_pwm_values.values[i] = _params->idle_pwm_mc;
|
||||
_current_max_pwm_values.values[i] = _param_vt_idle_pwm_mc.get();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -537,7 +551,7 @@ float VtolType::pusher_assist()
|
|||
}
|
||||
|
||||
// disable pusher assist depending on setting of forward_thrust_enable_mode:
|
||||
switch (_params->vt_forward_thrust_enable_mode) {
|
||||
switch (_param_vt_fwd_thrust_en.get()) {
|
||||
case DISABLE: // disable in all modes
|
||||
return 0.0f;
|
||||
break;
|
||||
|
|
@ -552,14 +566,14 @@ float VtolType::pusher_assist()
|
|||
break;
|
||||
|
||||
case ENABLE_ABOVE_MPC_LAND_ALT1: // disable if below MPC_LAND_ALT1
|
||||
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1)) {
|
||||
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get())) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
break;
|
||||
|
||||
case ENABLE_ABOVE_MPC_LAND_ALT2: // disable if below MPC_LAND_ALT2
|
||||
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2)) {
|
||||
if (!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get())) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
|
|
@ -569,7 +583,7 @@ float VtolType::pusher_assist()
|
|||
if ((_attc->get_pos_sp_triplet()->current.valid
|
||||
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
|
||||
&& _v_control_mode->flag_control_auto_enabled) ||
|
||||
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt1))) {
|
||||
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt1.get()))) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
|
|
@ -579,7 +593,7 @@ float VtolType::pusher_assist()
|
|||
if ((_attc->get_pos_sp_triplet()->current.valid
|
||||
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND
|
||||
&& _v_control_mode->flag_control_auto_enabled) ||
|
||||
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _params->mpc_land_alt2))) {
|
||||
(!PX4_ISFINITE(dist_to_ground) || (dist_to_ground < _param_mpc_land_alt2.get()))) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
|
|
@ -588,7 +602,7 @@ float VtolType::pusher_assist()
|
|||
|
||||
// if the thrust scale param is zero or the drone is not in some position or altitude control mode,
|
||||
// then the pusher-for-pitch strategy is disabled and we can return
|
||||
if (_params->forward_thrust_scale < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled
|
||||
if (_param_vt_fwd_thrust_sc.get() < FLT_EPSILON || !(_v_control_mode->flag_control_position_enabled
|
||||
|| _v_control_mode->flag_control_altitude_enabled)) {
|
||||
return 0.0f;
|
||||
}
|
||||
|
|
@ -619,11 +633,12 @@ float VtolType::pusher_assist()
|
|||
// normalized pusher support throttle (standard VTOL) or tilt (tiltrotor), initialize to 0
|
||||
float forward_thrust = 0.0f;
|
||||
|
||||
float pitch_setpoint_min = _params->pitch_min_rad;
|
||||
float pitch_setpoint_min = math::radians(_param_vt_ptch_min.get());
|
||||
|
||||
if (_attc->get_pos_sp_triplet()->current.valid
|
||||
&& _attc->get_pos_sp_triplet()->current.type == position_setpoint_s::SETPOINT_TYPE_LAND) {
|
||||
pitch_setpoint_min = _params->land_pitch_min_rad; // set min pitch during LAND (usually lower to generate less lift)
|
||||
pitch_setpoint_min = math::radians(
|
||||
_param_vt_lnd_ptch_min.get()); // set min pitch during LAND (usually lower to generate less lift)
|
||||
}
|
||||
|
||||
// only allow pitching down up to threshold, the rest of the desired
|
||||
|
|
@ -633,7 +648,7 @@ float VtolType::pusher_assist()
|
|||
// desired roll angle in heading frame stays the same
|
||||
const float roll_new = -asinf(body_z_sp(1));
|
||||
|
||||
forward_thrust = (sinf(pitch_setpoint_min) - sinf(pitch_setpoint)) * _params->forward_thrust_scale;
|
||||
forward_thrust = (sinf(pitch_setpoint_min) - sinf(pitch_setpoint)) * _param_vt_fwd_thrust_sc.get();
|
||||
// limit forward actuation to [0, 0.9]
|
||||
forward_thrust = math::constrain(forward_thrust, 0.0f, 0.9f);
|
||||
|
||||
|
|
@ -684,10 +699,10 @@ float VtolType::getFrontTransitionTimeFactor() const
|
|||
|
||||
float VtolType::getMinimumFrontTransitionTime() const
|
||||
{
|
||||
return getFrontTransitionTimeFactor() * _params->front_trans_time_min;
|
||||
return getFrontTransitionTimeFactor() * _param_vt_trans_min_tm.get();
|
||||
}
|
||||
|
||||
float VtolType::getOpenLoopFrontTransitionTime() const
|
||||
{
|
||||
return getFrontTransitionTimeFactor() * _params->front_trans_time_openloop;
|
||||
return getFrontTransitionTimeFactor() * _param_vt_f_tr_ol_tm.get();
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015, 2021 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2015-2022 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
|
||||
|
|
@ -43,50 +43,16 @@
|
|||
#ifndef VTOL_TYPE_H
|
||||
#define VTOL_TYPE_H
|
||||
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
#include <lib/slew_rate/SlewRate.hpp>
|
||||
#include <px4_platform_common/module_params.h>
|
||||
|
||||
|
||||
static constexpr float kFlapSlewRateVtol = 1.f; // minimum time from none to full flap deflection [s]
|
||||
static constexpr float kSpoilerSlewRateVtol = 1.f; // minimum time from none to full spoiler deflection [s]
|
||||
|
||||
struct Params {
|
||||
int32_t ctrl_alloc;
|
||||
int32_t idle_pwm_mc; // pwm value for idle in mc mode
|
||||
int32_t vtol_motor_id;
|
||||
int32_t vtol_type;
|
||||
bool elevons_mc_lock; // lock elevons in multicopter mode
|
||||
float fw_min_alt; // minimum relative altitude for FW mode (QuadChute)
|
||||
float fw_alt_err; // maximum negative altitude error for FW mode (Adaptive QuadChute)
|
||||
float fw_qc_max_pitch; // maximum pitch angle FW mode (QuadChute)
|
||||
float fw_qc_max_roll; // maximum roll angle FW mode (QuadChute)
|
||||
float front_trans_time_openloop;
|
||||
float front_trans_time_min;
|
||||
float front_trans_duration;
|
||||
float back_trans_duration;
|
||||
float transition_airspeed;
|
||||
float front_trans_throttle;
|
||||
float back_trans_throttle;
|
||||
float airspeed_blend;
|
||||
bool airspeed_disabled;
|
||||
float front_trans_timeout;
|
||||
float mpc_xy_cruise;
|
||||
int32_t fw_motors_off; /**< bitmask of all motors that should be off in fixed wing mode */
|
||||
int32_t diff_thrust;
|
||||
float diff_thrust_scale;
|
||||
float pitch_min_rad;
|
||||
float land_pitch_min_rad;
|
||||
float forward_thrust_scale;
|
||||
float dec_to_pitch_ff;
|
||||
float dec_to_pitch_i;
|
||||
float back_trans_dec_sp;
|
||||
bool vt_mc_on_fmu;
|
||||
int32_t vt_forward_thrust_enable_mode;
|
||||
float mpc_land_alt1;
|
||||
float mpc_land_alt2;
|
||||
float vt_spoiler_mc_ld;
|
||||
};
|
||||
|
||||
// Has to match 1:1 msg/vtol_vehicle_status.msg
|
||||
enum class mode {
|
||||
|
|
@ -133,7 +99,7 @@ enum class pwm_limit_type {
|
|||
|
||||
class VtolAttitudeControl;
|
||||
|
||||
class VtolType
|
||||
class VtolType: public ModuleParams
|
||||
{
|
||||
public:
|
||||
|
||||
|
|
@ -246,8 +212,6 @@ protected:
|
|||
struct vehicle_thrust_setpoint_s *_thrust_setpoint_0;
|
||||
struct vehicle_thrust_setpoint_s *_thrust_setpoint_1;
|
||||
|
||||
struct Params *_params;
|
||||
|
||||
bool _flag_idle_mc = false; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
|
||||
|
||||
bool _pusher_active = false;
|
||||
|
|
@ -310,6 +274,46 @@ protected:
|
|||
|
||||
float _dt{0.0025f}; // time step [s]
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(ModuleParams,
|
||||
(ParamBool<px4::params::VT_ELEV_MC_LOCK>) _param_vt_elev_mc_lock,
|
||||
(ParamFloat<px4::params::VT_FW_MIN_ALT>) _param_vt_fw_min_alt,
|
||||
(ParamFloat<px4::params::VT_FW_ALT_ERR>) _param_vt_fw_alt_err,
|
||||
(ParamInt<px4::params::VT_FW_QC_P>) _param_vt_fw_qc_p,
|
||||
(ParamInt<px4::params::VT_FW_QC_R>) _param_vt_fw_qc_r,
|
||||
(ParamFloat<px4::params::VT_F_TR_OL_TM>) _param_vt_f_tr_ol_tm,
|
||||
(ParamFloat<px4::params::VT_TRANS_MIN_TM>) _param_vt_trans_min_tm,
|
||||
|
||||
(ParamFloat<px4::params::VT_F_TRANS_DUR>) _param_vt_f_trans_dur,
|
||||
(ParamFloat<px4::params::VT_B_TRANS_DUR>) _param_vt_b_trans_dur,
|
||||
(ParamFloat<px4::params::VT_ARSP_TRANS>) _param_vt_arsp_trans,
|
||||
(ParamFloat<px4::params::VT_F_TRANS_THR>) _param_vt_f_trans_thr,
|
||||
(ParamFloat<px4::params::VT_B_TRANS_THR>) _param_vt_b_trans_thr,
|
||||
(ParamFloat<px4::params::VT_ARSP_BLEND>) _param_vt_arsp_blend,
|
||||
(ParamBool<px4::params::FW_ARSP_MODE>) _param_fw_arsp_mode,
|
||||
(ParamFloat<px4::params::VT_TRANS_TIMEOUT>) _param_vt_trans_timeout,
|
||||
(ParamFloat<px4::params::MPC_XY_CRUISE>) _param_mpc_xy_cruise,
|
||||
(ParamBool<px4::params::VT_FW_DIFTHR_EN>) _param_vt_fw_difthr_en,
|
||||
(ParamFloat<px4::params::VT_FW_DIFTHR_SC>) _param_vt_fw_difthr_sc,
|
||||
(ParamFloat<px4::params::VT_B_DEC_FF>) _param_vt_b_dec_ff,
|
||||
(ParamFloat<px4::params::VT_B_DEC_I>) _param_vt_b_dec_i,
|
||||
(ParamFloat<px4::params::VT_B_DEC_MSS>) _param_vt_b_dec_mss,
|
||||
|
||||
(ParamFloat<px4::params::VT_PTCH_MIN>) _param_vt_ptch_min,
|
||||
(ParamFloat<px4::params::VT_FWD_THRUST_SC>) _param_vt_fwd_thrust_sc,
|
||||
(ParamInt<px4::params::VT_FWD_THRUST_EN>) _param_vt_fwd_thrust_en,
|
||||
(ParamFloat<px4::params::MPC_LAND_ALT1>) _param_mpc_land_alt1,
|
||||
(ParamFloat<px4::params::MPC_LAND_ALT2>) _param_mpc_land_alt2,
|
||||
(ParamFloat<px4::params::VT_LND_PTCH_MIN>) _param_vt_lnd_ptch_min,
|
||||
|
||||
(ParamBool<px4::params::SYS_CTRL_ALLOC>) _param_sys_ctrl_alloc,
|
||||
(ParamInt<px4::params::VT_IDLE_PWM_MC>) _param_vt_idle_pwm_mc,
|
||||
(ParamInt<px4::params::VT_MOT_ID>) _param_vt_mot_id,
|
||||
(ParamBool<px4::params::VT_MC_ON_FMU>) _param_vt_mc_on_fmu,
|
||||
(ParamInt<px4::params::VT_FW_MOT_OFFID>) _param_vt_fw_mot_offid,
|
||||
(ParamFloat<px4::params::VT_SPOILER_MC_LD>) _param_vt_spoiler_mc_ld
|
||||
|
||||
)
|
||||
|
||||
private:
|
||||
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue