diff --git a/mirte_telemetrix_cpp/libs/tmx-cpp b/mirte_telemetrix_cpp/libs/tmx-cpp
index 4b16b1e4..7bf2034a 160000
--- a/mirte_telemetrix_cpp/libs/tmx-cpp
+++ b/mirte_telemetrix_cpp/libs/tmx-cpp
@@ -1 +1 @@
-Subproject commit 4b16b1e4120406aea951213600634c06d269809b
+Subproject commit 7bf2034a9ace21492a7f48c2852bd3ebc33aaea0
diff --git a/mirte_telemetrix_cpp/src/modules/hiwonder/hiwonder_servo.cpp b/mirte_telemetrix_cpp/src/modules/hiwonder/hiwonder_servo.cpp
index ffec6c9f..df65e510 100644
--- a/mirte_telemetrix_cpp/src/modules/hiwonder/hiwonder_servo.cpp
+++ b/mirte_telemetrix_cpp/src/modules/hiwonder/hiwonder_servo.cpp
@@ -21,6 +21,12 @@ Hiwonder_servo::Hiwonder_servo(
                  "not found]",
                  this->servo_data->name.c_str(), this->servo_data->id);
 
+  if (this->bus_mod->register_servo_id(this->servo_data->id))
+    RCLCPP_ERROR(logger,
+                 "HiWonder Servo '%s' ID is out of range [Requesed ID %d, but "
+                 "range is 0-253]",
+                 this->servo_data->name.c_str(), this->servo_data->id);
+
   auto range = this->bus_mod->get_range(servo_data->id);
   assert(range.has_value());
   auto [lower, upper] = range.value();
@@ -252,4 +258,4 @@ void Hiwonder_servo::set_motor_speed_service_callback(
   // output: speed from -1000 to 1000
   int16_t speed = std::clamp(req->speed, -100, 100) * 10;
   res->status = this->bus_mod->motor_mode_write(this->servo_data->id, speed);
-}
\ No newline at end of file
+}
diff --git a/mirte_telemetrix_cpp/src/modules/hiwonder_module.cpp b/mirte_telemetrix_cpp/src/modules/hiwonder_module.cpp
index 50adadd9..5b10fcb3 100644
--- a/mirte_telemetrix_cpp/src/modules/hiwonder_module.cpp
+++ b/mirte_telemetrix_cpp/src/modules/hiwonder_module.cpp
@@ -28,8 +28,9 @@ HiWonderBus_module::HiWonderBus_module(
 
   // Create a list of ID's
   std::vector<uint8_t> servo_ids;
-  for (auto servo : this->data.servos)
-    servo_ids.push_back(servo->id);
+  // Don't pre-add ids since it can cause errors on missing servos
+  // for (auto servo : this->data.servos)
+  //   servo_ids.push_back(servo->id);
 
   this->bus = std::make_shared<tmx_cpp::HiwonderServo_module>(
       this->data.uart_port, this->data.rx_pin, this->data.tx_pin, servo_ids,