Skip to content

Commit 36540cf

Browse files
Tomas BacaTomas Baca
Tomas Baca
authored and
Tomas Baca
committed
updated dynparam interface
1 parent b8db839 commit 36540cf

File tree

2 files changed

+14
-26
lines changed

2 files changed

+14
-26
lines changed

‎src/mpc_controller.cpp‎

Lines changed: 6 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -315,6 +315,8 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
315315
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") + "/config/private/" + private_handlers->name_space + ".yaml");
316316
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") + "/config/public/" + private_handlers->name_space + ".yaml");
317317

318+
dynparam_mgr_->get_param_provider().copyYamls(private_handlers->param_loader->getParamProvider());
319+
318320
// load the dynamicall model parameters
319321
private_handlers->param_loader->loadParam("mpc/mpc_model/number_of_states", _n_states_);
320322
private_handlers->param_loader->loadParam("mpc/mpc_model/dt1", _dt1_);
@@ -365,9 +367,7 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
365367

366368
// mass estimator
367369
private_handlers->param_loader->loadParam("so3/mass_estimator/km", gains_.km);
368-
369-
dynparam_mgr_->register_param("se3/mass_estimator/fuse_acceleration", &drs_params_.fuse_acceleration, drs_params_.fuse_acceleration);
370-
370+
dynparam_mgr_->register_param("so3/mass_estimator/fuse_acceleration", &drs_params_.fuse_acceleration);
371371
private_handlers->param_loader->loadParam("so3/mass_estimator/km_lim", gains_.km_lim);
372372

373373
// constraints
@@ -390,12 +390,10 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
390390
private_handlers->param_loader->loadParam("so3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
391391

392392
// angular rate feed forward
393-
private_handlers->param_loader->loadParam("so3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
394-
dynparam_mgr_->register_param("so3/angular_rate_feedforward/jerk", &drs_params_.jerk_feedforward, drs_params_.jerk_feedforward);
393+
dynparam_mgr_->register_param("so3/angular_rate_feedforward/jerk", &drs_params_.jerk_feedforward);
395394

396395
// output mode
397-
private_handlers->param_loader->loadParam("so3/preferred_output", drs_params_.preferred_output_mode);
398-
dynparam_mgr_->register_param("so3/preferred_output", &drs_params_.preferred_output_mode, drs_params_.preferred_output_mode, mrs_lib::DynparamMgr::range_t<int>(0, 3));
396+
dynparam_mgr_->register_param("so3/preferred_output", &drs_params_.preferred_output_mode, mrs_lib::DynparamMgr::range_t<int>(0, 3));
399397

400398
// | ------------------- position pid params ------------------ |
401399

@@ -409,7 +407,7 @@ bool MpcController::initialize(const rclcpp::Node::SharedPtr &node, std::shared_
409407

410408
// | ------------------ finish loading params ----------------- |
411409

412-
if (!private_handlers->param_loader->loadedSuccessfully()) {
410+
if (!private_handlers->param_loader->loadedSuccessfully() || !dynparam_mgr_->loaded_successfully()) {
413411
RCLCPP_ERROR(node_->get_logger(), "[%s]: Could not load all parameters!", this->name_.c_str());
414412
return false;
415413
}
@@ -1463,8 +1461,6 @@ void MpcController::MPC(const mrs_msgs::msg::UavState &uav_state, const mrs_msgs
14631461

14641462
if (last_throttle_ < (_throttle_saturation_ - 0.01) && last_throttle_ > 0) {
14651463
uav_mass_difference_ += 1.0 * gains.km * (desired_bodyz_acc - measured_bodyz_acc) * dt;
1466-
1467-
RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 100, "[%s]: mass estimation using IMU acc runs, mass difference %.3f kg", this->name_.c_str(), uav_mass_difference_);
14681464
}
14691465

14701466
} else if (tracker_command.use_position_vertical) {

‎src/se3_controller.cpp‎

Lines changed: 8 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -278,6 +278,8 @@ bool Se3Controller::initialize(const rclcpp::Node::SharedPtr& node, std::shared_
278278
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") + "/config/private/se3_controller.yaml");
279279
private_handlers->param_loader->addYamlFile(ament_index_cpp::get_package_share_directory("mrs_uav_controllers") + "/config/public/se3_controller.yaml");
280280

281+
dynparam_mgr_->get_param_provider().copyYamls(private_handlers->param_loader->getParamProvider());
282+
281283
// lateral gains
282284
private_handlers->param_loader->loadParam("se3/default_gains/horizontal/kp", gains_.kpxy);
283285
private_handlers->param_loader->loadParam("se3/default_gains/horizontal/kv", gains_.kvxy);
@@ -306,10 +308,8 @@ bool Se3Controller::initialize(const rclcpp::Node::SharedPtr& node, std::shared_
306308

307309
// mass estimator
308310
private_handlers->param_loader->loadParam("se3/default_gains/mass_estimator/km", gains_.km);
309-
private_handlers->param_loader->loadParam("se3/mass_estimator/fuse_acceleration", drs_params_.fuse_acceleration);
310311
private_handlers->param_loader->loadParam("se3/default_gains/mass_estimator/km_lim", gains_.km_lim);
311-
312-
dynparam_mgr_->register_param("se3/mass_estimator/fuse_acceleration", &drs_params_.fuse_acceleration, drs_params_.fuse_acceleration);
312+
dynparam_mgr_->register_param("se3/mass_estimator/fuse_acceleration", &drs_params_.fuse_acceleration);
313313

314314
// integrator limits
315315
private_handlers->param_loader->loadParam("se3/default_gains/horizontal/kiw_lim", gains_.kiwxy_lim);
@@ -335,18 +335,12 @@ bool Se3Controller::initialize(const rclcpp::Node::SharedPtr& node, std::shared_
335335
private_handlers->param_loader->loadParam("se3/gain_filtering/gain_mute_coefficient", _gain_mute_coefficient_);
336336

337337
// output mode
338-
private_handlers->param_loader->loadParam("se3/preferred_output", drs_params_.preferred_output_mode);
339-
dynparam_mgr_->register_param("se3/preferred_output", &drs_params_.preferred_output_mode, drs_params_.preferred_output_mode, mrs_lib::DynparamMgr::range_t<int>(0, 3));
340-
341-
private_handlers->param_loader->loadParam("se3/rotation_matrix", drs_params_.rotation_type);
342-
dynparam_mgr_->register_param("se3/rotation_matrix", &drs_params_.rotation_type, drs_params_.rotation_type, mrs_lib::DynparamMgr::range_t<int>(0, 1));
338+
dynparam_mgr_->register_param("se3/preferred_output", &drs_params_.preferred_output_mode, mrs_lib::DynparamMgr::range_t<int>(0, 3));
339+
dynparam_mgr_->register_param("se3/rotation_matrix", &drs_params_.rotation_type, mrs_lib::DynparamMgr::range_t<int>(0, 1));
343340

344341
// angular rate feed forward
345-
private_handlers->param_loader->loadParam("se3/angular_rate_feedforward/parasitic_pitch_roll", drs_params_.pitch_roll_heading_rate_compensation);
346-
dynparam_mgr_->register_param("se3/angular_rate_feedforward/parasitic_pitch_roll", &drs_params_.pitch_roll_heading_rate_compensation, drs_params_.pitch_roll_heading_rate_compensation);
347-
348-
private_handlers->param_loader->loadParam("se3/angular_rate_feedforward/jerk", drs_params_.jerk_feedforward);
349-
dynparam_mgr_->register_param("se3/angular_rate_feedforward/jerk", &drs_params_.jerk_feedforward, drs_params_.jerk_feedforward);
342+
dynparam_mgr_->register_param("se3/angular_rate_feedforward/parasitic_pitch_roll", &drs_params_.pitch_roll_heading_rate_compensation);
343+
dynparam_mgr_->register_param("se3/angular_rate_feedforward/jerk", &drs_params_.jerk_feedforward);
350344

351345
// | ------------------- position pid params ------------------ |
352346

@@ -360,7 +354,7 @@ bool Se3Controller::initialize(const rclcpp::Node::SharedPtr& node, std::shared_
360354

361355
// | ------------------ finish loading params ----------------- |
362356

363-
if (!private_handlers->param_loader->loadedSuccessfully()) {
357+
if (!private_handlers->param_loader->loadedSuccessfully() || !dynparam_mgr_->loaded_successfully()) {
364358
RCLCPP_ERROR(node_->get_logger(), "[Se3Controller]: could not load all parameters!");
365359
return false;
366360
}
@@ -1259,8 +1253,6 @@ void Se3Controller::SE3Controller(const mrs_msgs::msg::UavState& uav_state, cons
12591253

12601254
if (last_throttle_ < (_throttle_saturation_ - 0.01) && last_throttle_ > 0) {
12611255
uav_mass_difference_ += 1.0 * gains.km * (desired_bodyz_acc - measured_bodyz_acc) * dt;
1262-
1263-
RCLCPP_DEBUG_THROTTLE(node_->get_logger(), *clock_, 100, "[Se3Controller]: mass estimation using IMU acc runs, mass difference %.3f kg", uav_mass_difference_);
12641256
}
12651257

12661258
} else if (tracker_command.use_position_vertical) {

0 commit comments

Comments
 (0)