cleaned up the rampup bug fix

Author: Tomas Baca

src/mpc_controller.cpp

@@ -663,7 +663,6 @@ bool MpcController::activate(const ControlOutput &last_control_output) {
     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;
@@ -1670,24 +1669,26 @@ void MpcController::MPC(const mrs_msgs::msg::UavState &uav_state, const mrs_msgs
 
   if (rampup_active_) {
 
-    double rampup_dt = (clock_->now() - rampup_last_time_).seconds();
+    // deactivate the rampup when the times up
+    if (std::abs((clock_->now() - rampup_start_time_).seconds()) >= rampup_duration_) {
 
-    rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
+      rampup_active_ = false;
 
-    rampup_last_time_ = clock_->now();
+      RCLCPP_INFO(node_->get_logger(), "[%s]: rampup finished", name_.c_str());
 
-    throttle = rampup_throttle_;
+    } else {
 
-    RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 100, "[%s]: ramping up throttle, %.4f", name_.c_str(), throttle);
+      double rampup_dt = (clock_->now() - rampup_last_time_).seconds();
 
-    // deactivate the rampup when the times up
-    if (std::abs((clock_->now() - rampup_start_time_).seconds()) >= rampup_duration_) {
+      rampup_throttle_ += double(rampup_direction_) * _rampup_speed_ * rampup_dt;
 
-      rampup_active_ = false;
+      rampup_last_time_ = clock_->now();
 
-      RCLCPP_INFO(node_->get_logger(), "[%s]: rampup finished", name_.c_str());
+      RCLCPP_INFO_THROTTLE(node_->get_logger(), *clock_, 100, "[%s]: ramping up throttle, %.4f", name_.c_str(), throttle);
     }
 
+    throttle = rampup_throttle_;
+
   } else {
 
     if (desired_thrust_force >= 0) {

src/se3_controller.cpp

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