Merge pull request #196 from ye-luo-xi-tui/master

Add anti-shake to stick
rm-controls · Aug 8, 2023 · 4077a1e · 4077a1e
2 parents 8da34c2 + c3ce967
commit 4077a1e
Show file tree

Hide file tree

Showing 2 changed files with 13 additions and 2 deletions.
diff --git a/rm_dbus/include/rm_dbus/dbus.h b/rm_dbus/include/rm_dbus/dbus.h
@@ -72,6 +72,7 @@ class DBus
 
 private:
   DBusData_t d_bus_data_{};
+  int s0_count_ = 0, s1_count_ = 0;
   int port_{};
   int16_t buff_[18]{};
   bool is_success{};

diff --git a/rm_dbus/src/dbus.cpp b/rm_dbus/src/dbus.cpp
@@ -149,8 +149,18 @@ void DBus::unpack()
   if (d_bus_data_.ch3 <= 10 && d_bus_data_.ch3 >= -10)
     d_bus_data_.ch3 = 0;
 
-  d_bus_data_.s1 = ((buff_[5] >> 4) & 0x000C) >> 2;
-  d_bus_data_.s0 = (buff_[5] >> 4) & 0x0003;
+  if (d_bus_data_.s0 != ((buff_[5] >> 4) & 0x0003))
+    s0_count_++;
+  else
+    s0_count_ = 0;
+  if (d_bus_data_.s1 != ((buff_[5] >> 4) & 0x000C) >> 2)
+    s1_count_++;
+  else
+    s1_count_ = 0;
+  if (s0_count_ > 3)
+    d_bus_data_.s0 = (buff_[5] >> 4) & 0x0003;
+  if (s1_count_ > 3)
+    d_bus_data_.s1 = ((buff_[5] >> 4) & 0x000C) >> 2;
 
   if ((abs(d_bus_data_.ch0) > 660) || (abs(d_bus_data_.ch1) > 660) || (abs(d_bus_data_.ch2) > 660) ||
       (abs(d_bus_data_.ch3) > 660))