fixed rampup bug in Se3 and Mpc

Author: Tomas Baca

src/mpc_controller.cpp

@@ -655,14 +655,15 @@ bool MpcController::activate(const ControlOutput &last_control_output) {
       rampup_direction_ = 0;
     }
 
-    RCLCPP_INFO(node_->get_logger(), "[%s]: activating rampup with initial throttle: %.4f, target: %.4f", name_.c_str(), throttle_last_controller.value(), hover_throttle);
-
     rampup_active_     = true;
     rampup_start_time_ = clock_->now();
     rampup_last_time_  = clock_->now();
     rampup_throttle_   = throttle_last_controller.value();
 
     rampup_duration_ = std::abs(throttle_difference) / _rampup_speed_;
+
+    RCLCPP_INFO(node_->get_logger(), "[%s]: activating rampup with initial throttle: %.4f, target: %.4f", name_.c_str(), throttle_last_controller.value(), hover_throttle);
+
   }
 
   first_iteration_ = true;
@@ -1669,24 +1670,22 @@ void MpcController::MPC(const mrs_msgs::msg::UavState &uav_state, const mrs_msgs
 
   if (rampup_active_) {
 
-    // deactivate the rampup when the times up
-    if (std::abs((clock_->now() - rampup_start_time_).seconds()) >= rampup_duration_) {
+    double rampup_dt = (clock_->now() - rampup_last_time_).seconds();
 
-      rampup_active_ = false;
+    rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
 
-      RCLCPP_INFO(node_->get_logger(), "[%s]: rampup finished", name_.c_str());
-
-    } else {
+    rampup_last_time_ = clock_->now();
 
-      double rampup_dt = (clock_->now() - rampup_last_time_).seconds();
+    throttle = rampup_throttle_;
 
-      rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 100, "[%s]: ramping up throttle, %.4f", name_.c_str(), throttle);
 
-      rampup_last_time_ = clock_->now();
+    // deactivate the rampup when the times up
+    if (std::abs((clock_->now() - rampup_start_time_).seconds()) >= rampup_duration_) {
 
-      throttle = rampup_throttle_;
+      rampup_active_ = false;
 
-      RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 100, "[%s]: ramping up throttle, %.4f", name_.c_str(), throttle);
+      RCLCPP_INFO(node_->get_logger(), "[%s]: rampup finished", name_.c_str());
     }
 
   } else {

src/se3_controller.cpp

@@ -1533,24 +1533,22 @@ void Se3Controller::SE3Controller(const mrs_msgs::msg::UavState& uav_state, cons
 
   } else if (rampup_active_) {
 
-    // deactivate the rampup when the times up
-    if (std::abs((clock_->now() - rampup_start_time_).seconds()) >= rampup_duration_) {
+    double rampup_dt = (clock_->now() - rampup_last_time_).seconds();
 
-      rampup_active_ = false;
-
-      RCLCPP_INFO(node_->get_logger(), "[Se3Controller]: rampup finished");
+    rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
 
-    } else {
+    rampup_last_time_ = clock_->now();
 
-      double rampup_dt = (clock_->now() - rampup_last_time_).seconds();
+    throttle = rampup_throttle_;
 
-      rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+    RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 100, "[Se3Controller]: ramping up throttle, %.4f", throttle);
 
-      rampup_last_time_ = clock_->now();
+    // deactivate the rampup when the times up
+    if (std::abs((clock_->now() - rampup_start_time_).seconds()) >= rampup_duration_) {
 
-      throttle = rampup_throttle_;
+      rampup_active_ = false;
 
-      RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 100, "[Se3Controller]: ramping up throttle, %.4f", throttle);
+      RCLCPP_INFO(node_->get_logger(), "[Se3Controller]: rampup finished");
     }
 
   } else {