diff --git a/external_modules/src/modules/rl_tools_commander/RLtoolsCommander.cpp b/external_modules/src/modules/rl_tools_commander/RLtoolsCommander.cpp index b957e68..5617fa7 100644 --- a/external_modules/src/modules/rl_tools_commander/RLtoolsCommander.cpp +++ b/external_modules/src/modules/rl_tools_commander/RLtoolsCommander.cpp @@ -20,6 +20,7 @@ RLtoolsCommander::RLtoolsCommander() : ModuleParams(nullptr), ScheduledWorkItem( overwrite = false; mode = DEFAULT_MODE; trajectory.initialized = false; + parameters_update(); } RLtoolsCommander::~RLtoolsCommander(){ @@ -37,6 +38,54 @@ bool RLtoolsCommander::init() return true; } +void RLtoolsCommander::parameters_update() +{ + // check for parameter updates + if (_parameter_update_sub.updated()) + { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + ModuleParams::updateParams(); + _xy_vel_i_acc = _param_mpc_xy_vel_i_acc.get() > 0.0f ? _param_mpc_xy_vel_i_acc.get() : _xy_vel_i_acc; + _z_vel_i_acc = _param_mpc_z_vel_i_acc.get() > 0.0f ? _param_mpc_z_vel_i_acc.get() : _z_vel_i_acc; + _rollrate_i = _param_mc_rollrate_i.get() > 0.0f ? _param_mc_rollrate_i.get() : _rollrate_i; + _pitchrate_i = _param_mc_pitchrate_i.get() > 0.0f ? _param_mc_pitchrate_i.get() : _pitchrate_i; + _yawrate_i = _param_mc_yawrate_i.get() > 0.0f ? _param_mc_yawrate_i.get() : _yawrate_i; + } +} + +void RLtoolsCommander::toggle_integral_gain(bool use_model) +{ + if (use_model) + { + _param_mpc_xy_vel_i_acc.set(0.0f); + _param_mpc_xy_vel_i_acc.commit(); + _param_mpc_z_vel_i_acc.set(0.0f); + _param_mpc_z_vel_i_acc.commit(); + _param_mc_rollrate_i.set(0.0f); + _param_mc_rollrate_i.commit(); + _param_mc_pitchrate_i.set(0.0f); + _param_mc_pitchrate_i.commit(); + _param_mc_yawrate_i.set(0.0f); + _param_mc_yawrate_i.commit(); + PX4_INFO("Integral gain removed"); + } + else + { + _param_mpc_xy_vel_i_acc.set(_xy_vel_i_acc); + _param_mpc_xy_vel_i_acc.commit(); + _param_mpc_z_vel_i_acc.set(_z_vel_i_acc); + _param_mpc_z_vel_i_acc.commit(); + _param_mc_rollrate_i.set(_rollrate_i); + _param_mc_rollrate_i.commit(); + _param_mc_pitchrate_i.set(_pitchrate_i); + _param_mc_pitchrate_i.commit(); + _param_mc_yawrate_i.set(_yawrate_i); + _param_mc_yawrate_i.commit(); + PX4_INFO("Integral gain restored"); + } +} + void RLtoolsCommander::FigureEight::update(hrt_abstime now){ if(!initialized){ reset(now); @@ -62,6 +111,7 @@ void RLtoolsCommander::FigureEight::update(hrt_abstime now){ void RLtoolsCommander::Run() { if (should_exit()) { + toggle_integral_gain(false); _vehicle_local_position_sub.unregisterCallback(); ScheduleClear(); exit_and_cleanup(); @@ -73,6 +123,8 @@ void RLtoolsCommander::Run() bool prev_command_active = command_active; bool next_command_active = command_active; + parameters_update(); + { manual_control_setpoint_s manual_control_input; if(_manual_control_input_sub.update(&manual_control_input)) { @@ -145,6 +197,7 @@ void RLtoolsCommander::Run() else{ PX4_INFO("Command disabled"); } + toggle_integral_gain(next_command_active); } command_active = next_command_active; diff --git a/external_modules/src/modules/rl_tools_commander/RLtoolsCommander.hpp b/external_modules/src/modules/rl_tools_commander/RLtoolsCommander.hpp index a02bbcb..34849bd 100644 --- a/external_modules/src/modules/rl_tools_commander/RLtoolsCommander.hpp +++ b/external_modules/src/modules/rl_tools_commander/RLtoolsCommander.hpp @@ -13,6 +13,7 @@ #include #include #include +#include #include #include #include @@ -73,8 +74,10 @@ class RLtoolsCommander : public ModuleBase, public ModuleParam STEP_RESPONSE = 2 }; - + void parameters_update(); + void toggle_integral_gain(bool use_model); + uORB::SubscriptionInterval _parameter_update_sub{ ORB_ID(parameter_update), 1_s }; uORB::Subscription _manual_control_input_sub{ORB_ID(manual_control_input)}; uORB::SubscriptionCallbackWorkItem _vehicle_local_position_sub{this, ORB_ID(vehicle_local_position)}; uORB::SubscriptionCallbackWorkItem _vehicle_attitude_sub{this, ORB_ID(vehicle_attitude)}; @@ -104,4 +107,19 @@ class RLtoolsCommander : public ModuleBase, public ModuleParam float step_response_offset[3] = {1, 0, 0}; perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + + // integral gain parameters + float _xy_vel_i_acc = 0.0f; + float _z_vel_i_acc = 0.0f; + float _rollrate_i = 0.0f; + float _pitchrate_i = 0.0f; + float _yawrate_i = 0.0f; + + DEFINE_PARAMETERS( + (ParamFloat)_param_mpc_xy_vel_i_acc, + (ParamFloat)_param_mpc_z_vel_i_acc, + (ParamFloat)_param_mc_rollrate_i, + (ParamFloat)_param_mc_pitchrate_i, + (ParamFloat)_param_mc_yawrate_i + ) };