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

vtol_att_control: consolidated standard parameters & fix usage of hrt_elapsed time

Browse Source 
- standard vtol was implementing many custom parameters although they
are generic and should be shared between the vtol types
- removed heavy usage of hrt_elapsed_time() which is a system call and
could be computationally expensive
This commit is contained in:
Roman 2018-02-26 17:25:19 +01:00 committed by Roman Bapst
parent 9c6b1a0f04
commit daa6c6ffc8
7 changed files with 112 additions and 118 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
src/modules/vtol_att_control/standard.cpp
View File

@ -62,23 +62,13 @@ Standard::Standard(VtolAttitudeControl *attc) :
_mc_yaw_weight = 1.0f;
_mc_throttle_weight = 1.0f;
_params_handles_standard.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_standard.back_trans_dur = param_find("VT_B_TRANS_DUR");
_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.pusher_trans = param_find("VT_TRANS_THR");
_params_handles_standard.airspeed_blend = param_find("VT_ARSP_BLEND");
_params_handles_standard.airspeed_trans = param_find("VT_ARSP_TRANS");
_params_handles_standard.front_trans_timeout = param_find("VT_TRANS_TIMEOUT");
_params_handles_standard.front_trans_time_min = param_find("VT_TRANS_MIN_TM");
_params_handles_standard.down_pitch_max = param_find("VT_DWN_PITCH_MAX");
_params_handles_standard.forward_thrust_scale = param_find("VT_FWD_THRUST_SC");
_params_handles_standard.airspeed_disabled = param_find("FW_ARSP_MODE");
_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");
_params_handles_standard.back_trans_throttle = param_find("VT_B_TRANS_THR");
_params_handles_standard.mpc_xy_cruise = param_find("MPC_XY_CRUISE");
}
Standard::~Standard()
@ -89,39 +79,16 @@ void
Standard::parameters_update()
{
float v;
int i;
/* duration of a forwards transition to fw mode */
param_get(_params_handles_standard.front_trans_dur, &v);
_params_standard.front_trans_dur = math::constrain(v, 0.0f, 20.0f);
/* duration of a back transition to mc mode */
param_get(_params_handles_standard.back_trans_dur, &v);
_params_standard.back_trans_dur = math::constrain(v, 0.0f, 20.0f);
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_standard.back_trans_dur);
_params_standard.back_trans_ramp = math::constrain(v, 0.0f, _params->back_trans_duration);
/* target throttle value for pusher motor during the transition to fw mode */
param_get(_params_handles_standard.pusher_trans, &v);
_params_standard.pusher_trans = math::constrain(v, 0.0f, 5.0f);
/* airspeed at which we should switch to fw mode */
param_get(_params_handles_standard.airspeed_trans, &v);
_params_standard.airspeed_trans = math::constrain(v, 1.0f, 20.0f);
/* airspeed at which we start blending mc/fw controls */
param_get(_params_handles_standard.airspeed_blend, &v);
_params_standard.airspeed_blend = math::constrain(v, 0.0f, 20.0f);
_airspeed_trans_blend_margin = _params_standard.airspeed_trans - _params_standard.airspeed_blend;
/* timeout for transition to fw mode */
param_get(_params_handles_standard.front_trans_timeout, &_params_standard.front_trans_timeout);
/* minimum time for transition to fw mode */
param_get(_params_handles_standard.front_trans_time_min, &_params_standard.front_trans_time_min);
_airspeed_trans_blend_margin = _params->transition_airspeed - _params->airspeed_blend;
/* maximum down pitch allowed */
param_get(_params_handles_standard.down_pitch_max, &v);
@ -130,10 +97,6 @@ Standard::parameters_update()
/* scale for fixed wing thrust used for forward acceleration in multirotor mode */
param_get(_params_handles_standard.forward_thrust_scale, &_params_standard.forward_thrust_scale);
/* airspeed mode */
param_get(_params_handles_standard.airspeed_disabled, &i);
_params_standard.airspeed_disabled = math::constrain(i, 0, 1);
/* pitch setpoint offset */
param_get(_params_handles_standard.pitch_setpoint_offset, &v);
_params_standard.pitch_setpoint_offset = math::radians(v);
@ -146,13 +109,6 @@ Standard::parameters_update()
param_get(_params_handles_standard.reverse_delay, &v);
_params_standard.reverse_delay = math::constrain(v, 0.0f, 10.0f);
/* reverse throttle */
param_get(_params_handles_standard.back_trans_throttle, &v);
_params_standard.back_trans_throttle = math::constrain(v, -1.0f, 1.0f);
/* mpc cruise speed */
param_get(_params_handles_standard.mpc_xy_cruise, &_params_standard.mpc_xy_cruise);
}
void Standard::update_vtol_state()
@ -163,6 +119,8 @@ void Standard::update_vtol_state()
*/
float mc_weight = _mc_roll_weight;
float time_now = (float)hrt_absolute_time();
float time_since_trans_start = time_now - (float)_vtol_schedule.transition_start;
if (!_attc->is_fixed_wing_requested()) {
@ -209,9 +167,8 @@ void Standard::update_vtol_state()
float x_vel = vel(0);
if (hrt_elapsed_time(&_vtol_schedule.transition_start) >
(_params_standard.back_trans_dur * 1000000.0f) ||
(_local_pos->v_xy_valid && x_vel <= _params_standard.mpc_xy_cruise)) {
if (time_since_trans_start > _params->back_trans_duration * 1e6f ||
(_local_pos->v_xy_valid && x_vel <= _params->mpc_xy_cruise)) {
_vtol_schedule.flight_mode = MC_MODE;
}
@ -233,10 +190,9 @@ void Standard::update_vtol_state()
} else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
// continue the transition to fw mode while monitoring airspeed for a final switch to fw mode
if (((_params_standard.airspeed_disabled == 1 ||
_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_trans) &&
(float)hrt_elapsed_time(&_vtol_schedule.transition_start)
> (_params_standard.front_trans_time_min * 1000000.0f)) ||
if (((_params->airspeed_mode == 1 ||
_airspeed->indicated_airspeed_m_s >= _params->transition_airspeed) &&
time_since_trans_start > _params->front_trans_time_min * 1e6f) ||
can_transition_on_ground()) {
_vtol_schedule.flight_mode = FW_MODE;
@ -277,6 +233,9 @@ void Standard::update_vtol_state()
void Standard::update_transition_state()
{
float mc_weight = 1.0f;
float time_now = (float)hrt_absolute_time();
float time_since_trans_start = time_now - (float)
_vtol_schedule.transition_start; // time in microseconds since transition started
VtolType::update_transition_state();
@ -284,32 +243,28 @@ void Standard::update_transition_state()
memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
if (_params_standard.front_trans_dur <= 0.0f) {
if (_params_standard.pusher_ramp_dt <= 0.0f) {
// just set the final target throttle value
_pusher_throttle = _params_standard.pusher_trans;
_pusher_throttle = _params->front_trans_throttle;
} else if (_pusher_throttle <= _params_standard.pusher_trans) {
} else if (_pusher_throttle <= _params->front_trans_throttle) {
// ramp up throttle to the target throttle value
_pusher_throttle = _params_standard.pusher_trans *
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) / (_params_standard.front_trans_dur * 1000000.0f);
_pusher_throttle = _params->front_trans_throttle * time_since_trans_start / (_params_standard.pusher_ramp_dt * 1e6f);
}
// 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 &&
_airspeed->indicated_airspeed_m_s >= _params_standard.airspeed_blend &&
(float)hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_time_min * 1000000.0f)
) {
mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params_standard.airspeed_blend) /
_airspeed->indicated_airspeed_m_s >= _params->airspeed_blend &&
time_since_trans_start > _params->front_trans_time_min * 1e6f) {
mc_weight = 1.0f - fabsf(_airspeed->indicated_airspeed_m_s - _params->airspeed_blend) /
_airspeed_trans_blend_margin;
// time based blending when no airspeed sensor is set
} else if (_params_standard.airspeed_disabled &&
hrt_elapsed_time(&_vtol_schedule.transition_start) < (_params_standard.front_trans_time_min * 1e6f) &&
hrt_elapsed_time(&_vtol_schedule.transition_start) > ((_params_standard.front_trans_time_min / 2.0f) * 1e6f)
) {
mc_weight = 1.0f - ((float)(hrt_elapsed_time(&_vtol_schedule.transition_start) - ((
_params_standard.front_trans_time_min / 2.0f) * 1000000.0f)) /
((_params_standard.front_trans_time_min / 2.0f) * 1000000.0f));
} else if (_params->airspeed_mode == 1 &&
time_since_trans_start < _params->front_trans_time_min * 1e6f &&
time_since_trans_start > _params->front_trans_time_min * 1e6f / 2.0f) {
mc_weight = 1.0f - ((time_since_trans_start - _params->front_trans_time_min * 1e6f / 2.0f) /
(_params->front_trans_time_min * 1e6f / 2.0f));
}
@ -320,8 +275,8 @@ void Standard::update_transition_state()
_v_att_sp->q_d_valid = true;
// check front transition timeout
if (_params_standard.front_trans_timeout > FLT_EPSILON) {
if (hrt_elapsed_time(&_vtol_schedule.transition_start) > (_params_standard.front_trans_timeout * 1e6f)) {
if (_params->front_trans_timeout > FLT_EPSILON) {
if (time_since_trans_start > _params->front_trans_timeout * 1e6f) {
// transition timeout occured, abort transition
_attc->abort_front_transition("Transition timeout");
}
@ -335,22 +290,21 @@ void Standard::update_transition_state()
q_sp.copyTo(_v_att_sp->q_d);
_v_att_sp->q_d_valid = true;
hrt_abstime btrans_start;
btrans_start = _vtol_schedule.transition_start + uint64_t(_params_standard.reverse_delay) * 1000000.0f;
float btrans_start = (float)_vtol_schedule.transition_start + _params_standard.reverse_delay * 1e6f;
_pusher_throttle = 0.0f;
if (hrt_absolute_time() >= btrans_start) {
if (time_now >= btrans_start) {
// Handle throttle reversal for active breaking
float thrscale = (float)hrt_elapsed_time(&btrans_start) / (_params_standard.front_trans_dur *
1000000.0f);
float thrscale = (time_now - btrans_start) / (_params_standard.pusher_ramp_dt * 1e6f);
thrscale = math::constrain(thrscale, 0.0f, 1.0f);
_pusher_throttle = thrscale * _params_standard.back_trans_throttle;
_pusher_throttle = thrscale * _params->back_trans_throttle;
}
// continually increase mc attitude control as we transition back to mc mode
if (_params_standard.back_trans_ramp > FLT_EPSILON) {
mc_weight = (float)hrt_elapsed_time(&_vtol_schedule.transition_start) /
((_params_standard.back_trans_ramp) * 1000000.0f);
mc_weight = time_since_trans_start /
((_params_standard.back_trans_ramp) * 1e6f);
}

22
src/modules/vtol_att_control/standard.h
View File

@ -67,41 +67,23 @@ public:
private:
struct {
float front_trans_dur;
float back_trans_dur;
float pusher_ramp_dt;
float back_trans_ramp;
float pusher_trans;
float airspeed_blend;
float airspeed_trans;
float front_trans_timeout;
float front_trans_time_min;
float down_pitch_max;
float forward_thrust_scale;
int32_t airspeed_disabled;
float pitch_setpoint_offset;
float reverse_output;
float reverse_delay;
float back_trans_throttle;
float mpc_xy_cruise;
} _params_standard;
struct {
param_t front_trans_dur;
param_t back_trans_dur;
param_t pusher_ramp_dt;
param_t back_trans_ramp;
param_t pusher_trans;
param_t airspeed_blend;
param_t airspeed_trans;
param_t front_trans_timeout;
param_t front_trans_time_min;
param_t down_pitch_max;
param_t forward_thrust_scale;
param_t airspeed_disabled;
param_t pitch_setpoint_offset;
param_t reverse_output;
param_t reverse_delay;
param_t back_trans_throttle;
param_t mpc_xy_cruise;
} _params_handles_standard;
enum vtol_mode {

20
src/modules/vtol_att_control/standard_params.c
View File

@ -39,16 +39,6 @@
* @author Roman Bapst <roman@px4.io>
*/
/**
* Target throttle value for pusher/puller motor during the transition to fw mode
*
* @min 0.0
* @max 1.0
* @increment 0.01
* @decimal 3
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TRANS_THR, 0.6f);
/**
* Maximum allowed down-pitch the controller is able to demand. This prevents large, negative
@ -113,14 +103,12 @@ PARAM_DEFINE_FLOAT(VT_B_REV_OUT, 0.0f);
PARAM_DEFINE_FLOAT(VT_B_REV_DEL, 0.0f);
/**
* Thottle output during back transition
* For ESCs and mixers that support reverse thrust on low PWM values set this to a negative value to apply active breaking
* For ESCs that support thrust reversal with a control channel please set VT_B_REV_OUT and set this to a positive value to apply active breaking
* Defines the time window during which the pusher throttle will be ramped up linearly to VT_F_TRANS_THR during a transition
* to fixed wing mode. Zero or negative values will produce an instant throttle rise to VT_F_TRANS_THR.
*
* @min -1
* @max 1
* @max 20
* @increment 0.01
* @decimal 2
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_B_TRANS_THR, 0.0f);
PARAM_DEFINE_FLOAT(VT_PSHER_RMP_DT, 3.0f);

22
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -77,6 +77,17 @@ VtolAttitudeControl::VtolAttitudeControl()
_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.wv_takeoff = param_find("VT_WV_TKO_EN");
_params_handles.wv_land = param_find("VT_WV_LND_EN");
_params_handles.wv_loiter = param_find("VT_WV_LTR_EN");
@ -487,6 +498,17 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.wv_land, &l);
_params.wv_land = (l == 1);
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, &_params.airspeed_mode);
param_get(_params_handles.front_trans_timeout, &_params.front_trans_timeout);
param_get(_params_handles.mpc_xy_cruise, &_params.mpc_xy_cruise);
// update the parameters of the instances of base VtolType
if (_vtol_type != nullptr) {
_vtol_type->parameters_update();

9
src/modules/vtol_att_control/vtol_att_control_main.h
View File

@ -192,6 +192,15 @@ private:
param_t wv_takeoff;
param_t wv_loiter;
param_t wv_land;
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;
} _params_handles{};
/* for multicopters it is usual to have a non-zero idle speed of the engines

30
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -137,6 +137,36 @@ PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR, 5.0f);
*/
PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR, 4.0f);
/**
* Target throttle value for the transition to fixed wing flight.
* standard vtol: pusher
* tailsitter, tiltrotor: main throttle
*
* @min 0.0
* @max 1.0
* @increment 0.01
* @decimal 3
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_F_TRANS_THR, 0.7f);
/**
* Target throttle value for the transition to hover flight.
* standard vtol: pusher
* tailsitter, tiltrotor: main throttle
*
* Note for standard vtol:
* For ESCs and mixers that support reverse thrust on low PWM values set this to a negative value to apply active breaking
* For ESCs that support thrust reversal with a control channel please set VT_B_REV_OUT and set this to a positive value to apply active breaking
*
* @min -1
* @max 1
* @increment 0.01
* @decimal 2
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_B_TRANS_THR, 0.0f);
/**
* Approximate deceleration during back transition
*

9
src/modules/vtol_att_control/vtol_type.h
View File

@ -61,6 +61,15 @@ struct Params {
bool wv_takeoff;
bool wv_loiter;
bool wv_land;
float front_trans_duration;
float back_trans_duration;
float transition_airspeed;
float front_trans_throttle;
float back_trans_throttle;
float airspeed_blend;
int32_t airspeed_mode;
float front_trans_timeout;
float mpc_xy_cruise;
};
// Has to match 1:1 msg/vtol_vehicle_status.msg