fix(velocity_controller): debug value for feedforward (#3782)

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
autowarefoundation · May 27, 2023 · 42d147b · 42d147b
1 parent 3950f81
commit 42d147b
Show file tree

Hide file tree

Showing 2 changed files with 3 additions and 1 deletion.
diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/debug_values.hpp
@@ -56,6 +56,7 @@ class DebugValues
     PITCH_RAW_TRAJ_DEG = 27,
     STOP_DIST = 28,
     FF_SCALE = 29,
+    ACC_CMD_FF = 30,
     SIZE  // this is the number of enum elements
   };
 

diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -946,7 +946,8 @@ double PidLongitudinalController::applyVelocityFeedback(
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_P_CONTRIBUTION, pid_contributions.at(0));
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_I_CONTRIBUTION, pid_contributions.at(1));
   m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FB_D_CONTRIBUTION, pid_contributions.at(2));
-  m_debug_values.setValues(DebugValues::TYPE::FF_SCALE, ff_acc);
+  m_debug_values.setValues(DebugValues::TYPE::FF_SCALE, ff_scale);
+  m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_FF, ff_acc);
 
   return feedback_acc;
 }