change input reference logic

tiiuae · Nov 15, 2023 · d11a26d · d11a26d
1 parent b71f08f
commit d11a26d
Showing 1 changed file with 3 additions and 7 deletions.
diff --git a/src/gazebo_mavlink_interface.cpp b/src/gazebo_mavlink_interface.cpp
@@ -40,10 +40,7 @@ GZ_ADD_PLUGIN(
 using namespace mavlink_interface;
 
 GazeboMavlinkInterface::GazeboMavlinkInterface() :
-    input_offset_ {},
     input_scaling_ {},
-    zero_position_disarmed_ {},
-    zero_position_armed_ {},
     input_index_ {}
     {
       mavlink_interface_ = std::make_shared<MavlinkInterface>();
@@ -450,14 +447,13 @@ void GazeboMavlinkInterface::handle_actuator_controls(const gz::sim::UpdateInfo
 
   for (int i = 0; i < input_reference_.size(); i++) {
     if (armed) {
-      input_reference_[i] = (actuator_controls[input_index_[i]] + input_offset_[i])
-          * input_scaling_(i) + zero_position_armed_[i];
+      input_reference_[i] = actuator_controls[input_index_[i]] * input_scaling_(i);
     } else {
-      input_reference_[i] = zero_position_disarmed_[i];
+      input_reference_[i] = 0;
       // std::cout << input_reference_ << ", ";
     }
   }
-  // std::cout << "Input Reference: " << input_reference_.transpose() << std::endl;
+  std::cout << "Input Reference: " << input_reference_.transpose() << std::endl;
   received_first_actuator_ = mavlink_interface_->GetReceivedFirstActuator();
 }