From 257bc9187251ebd12305f23515036ad1274b8612 Mon Sep 17 00:00:00 2001
From: Iori Yanokura <ab.ioryz@gmail.com>
Date: Fri, 12 Jan 2024 01:21:18 +0900
Subject: [PATCH] Ignore worm servo from flash

---
 rcb4/armh7interface.py | 4 +++-
 1 file changed, 3 insertions(+), 1 deletion(-)

diff --git a/rcb4/armh7interface.py b/rcb4/armh7interface.py
index b3b895bf..2189a75e 100644
--- a/rcb4/armh7interface.py
+++ b/rcb4/armh7interface.py
@@ -546,7 +546,9 @@ def search_worm_ids(self):
         for idx in range(max_sensor_num):
             worm = self.memory_cstruct(WormmoduleStruct, idx)
             if worm.module_type == 1:
-                indices.append(idx)
+                servo = self.memory_cstruct(ServoStruct, worm.servo_id)
+                if servo.rotation == 1:
+                    indices.append(idx)
         self.worm_sorted_ids = indices
         return indices