From 7292ce483c83779bf1a0bc4299e73b6f6b5d36de Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 18 Jan 2022 09:12:23 +0100 Subject: [PATCH] VTOL: move to cpp params API Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/standard.cpp | 87 +++----- src/modules/vtol_att_control/standard.h | 28 +-- src/modules/vtol_att_control/tailsitter.cpp | 35 ++-- src/modules/vtol_att_control/tailsitter.h | 14 +- src/modules/vtol_att_control/tiltrotor.cpp | 108 ++++------ src/modules/vtol_att_control/tiltrotor.h | 27 +-- .../vtol_att_control_main.cpp | 193 ++---------------- .../vtol_att_control/vtol_att_control_main.h | 55 +---- .../vtol_att_control_params.c | 2 +- src/modules/vtol_att_control/vtol_type.cpp | 95 +++++---- src/modules/vtol_att_control/vtol_type.h | 86 ++++---- 11 files changed, 241 insertions(+), 489 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 2feee91ee0..b56d7b979a 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -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(); diff --git a/src/modules/vtol_att_control/standard.h b/src/modules/vtol_att_control/standard.h index baecb4375d..70963e008f 100644 --- a/src/modules/vtol_att_control/standard.h +++ b/src/modules/vtol_att_control/standard.h @@ -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 -#include 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) _param_vt_psher_rmp_dt, + (ParamFloat) _param_vt_b_trans_ramp, + (ParamFloat) _param_fw_psp_off, + (ParamFloat) _param_vt_b_rev_out, + (ParamFloat) _param_vt_b_rev_del + ) }; #endif diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index d583ab4473..17013a35ec 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -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; diff --git a/src/modules/vtol_att_control/tailsitter.h b/src/modules/vtol_att_control/tailsitter.h index c91a82d97d..36dd05b39a 100644 --- a/src/modules/vtol_att_control/tailsitter.h +++ b/src/modules/vtol_att_control/tailsitter.h @@ -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) _param_fw_psp_off + ) + + }; #endif diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 6b32d0f901..84e6e4adc6 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -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]; diff --git a/src/modules/vtol_att_control/tiltrotor.h b/src/modules/vtol_att_control/tiltrotor.h index 1435fb832e..a5601f723f 100644 --- a/src/modules/vtol_att_control/tiltrotor.h +++ b/src/modules/vtol_att_control/tiltrotor.h @@ -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) _param_vt_tilt_mc, + (ParamFloat) _param_vt_tilt_trans, + (ParamFloat) _param_vt_tilt_fw, + (ParamFloat) _param_vt_tilt_spinup, + (ParamFloat) _param_vt_trans_p2_dur + ) + }; #endif diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index 089e6567d7..5262232385 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -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(_params.vtol_type) == vtol_type::TAILSITTER) { + if (static_cast(_param_vt_type.get()) == vtol_type::TAILSITTER) { _vtol_type = new Tailsitter(this); - } else if (static_cast(_params.vtol_type) == vtol_type::TILTROTOR) { + } else if (static_cast(_param_vt_type.get()) == vtol_type::TILTROTOR) { _vtol_type = new Tiltrotor(this); - } else if (static_cast(_params.vtol_type) == vtol_type::STANDARD) { + } else if (static_cast(_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; - - 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; - - /* vtol motor count */ - param_get(_params_handles.fw_motors_off, &_params.fw_motors_off); - } - - /* 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 VT_F_TR_OL_TM set to {1:.1}. - */ - events::send(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); + // check for parameter updates + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); - param_get(_params_handles.diff_thrust_scale, &v); - _params.diff_thrust_scale = math::constrain(v, -1.0f, 1.0f); + // update parameters from storage + updateParams(); - /* 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); + if (_vtol_type != nullptr) { + _vtol_type->parameters_update(); + } - // update the parameters of the instances of base VtolType - if (_vtol_type != nullptr) { - _vtol_type->parameters_update(); + _vtol_vehicle_status.fw_permanent_stab = _param_vt_fw_perm_stab.get(); } - - 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(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 9ad9b90356..2da2045a04 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -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 #include #include -#include #include #include #include #include #include +#include #include #include #include @@ -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, public px4::WorkItem +class VtolAttitudeControl : public ModuleBase, 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) _param_vt_type, + (ParamBool) _param_vt_fw_perm_stab + ) }; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index c903ce070c..d954f63427 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -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 diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index 745dbb9423..726d7cdc04 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -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(_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(_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(); } diff --git a/src/modules/vtol_att_control/vtol_type.h b/src/modules/vtol_att_control/vtol_type.h index 6d30489669..8c66253aba 100644 --- a/src/modules/vtol_att_control/vtol_type.h +++ b/src/modules/vtol_att_control/vtol_type.h @@ -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 #include #include +#include #include +#include + 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) _param_vt_elev_mc_lock, + (ParamFloat) _param_vt_fw_min_alt, + (ParamFloat) _param_vt_fw_alt_err, + (ParamInt) _param_vt_fw_qc_p, + (ParamInt) _param_vt_fw_qc_r, + (ParamFloat) _param_vt_f_tr_ol_tm, + (ParamFloat) _param_vt_trans_min_tm, + + (ParamFloat) _param_vt_f_trans_dur, + (ParamFloat) _param_vt_b_trans_dur, + (ParamFloat) _param_vt_arsp_trans, + (ParamFloat) _param_vt_f_trans_thr, + (ParamFloat) _param_vt_b_trans_thr, + (ParamFloat) _param_vt_arsp_blend, + (ParamBool) _param_fw_arsp_mode, + (ParamFloat) _param_vt_trans_timeout, + (ParamFloat) _param_mpc_xy_cruise, + (ParamBool) _param_vt_fw_difthr_en, + (ParamFloat) _param_vt_fw_difthr_sc, + (ParamFloat) _param_vt_b_dec_ff, + (ParamFloat) _param_vt_b_dec_i, + (ParamFloat) _param_vt_b_dec_mss, + + (ParamFloat) _param_vt_ptch_min, + (ParamFloat) _param_vt_fwd_thrust_sc, + (ParamInt) _param_vt_fwd_thrust_en, + (ParamFloat) _param_mpc_land_alt1, + (ParamFloat) _param_mpc_land_alt2, + (ParamFloat) _param_vt_lnd_ptch_min, + + (ParamBool) _param_sys_ctrl_alloc, + (ParamInt) _param_vt_idle_pwm_mc, + (ParamInt) _param_vt_mot_id, + (ParamBool) _param_vt_mc_on_fmu, + (ParamInt) _param_vt_fw_mot_offid, + (ParamFloat) _param_vt_spoiler_mc_ld + + ) + private: