Fix protocol version send target (#106)

openspur · Apr 25, 2019 · 4fb5b6d · 4fb5b6d
1 parent 2b7f724
commit 4fb5b6d
Showing 1 changed file with 9 additions and 7 deletions.
diff --git a/src/param.c b/src/param.c
@@ -1020,18 +1020,16 @@ int apply_robot_params()
   // ウォッチドックタイマの設定
   for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
   {
-    if (!g_param.motor_enable[j])
-      continue;
-    parameter_set(PARAM_watch_dog_limit, j, 1200);
+    if (g_param.motor_enable[j])
+      parameter_set(PARAM_watch_dog_limit, j, 1200);
   }
 
   if (g_param_init)
   {
     for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
     {
-      if (!g_param.motor_enable[j])
-        continue;
-      parameter_set(PARAM_w_ref, j, 0);
+      if (g_param.motor_enable[j])
+        parameter_set(PARAM_w_ref, j, 0);
     }
     g_param_init = 0;
   }
@@ -1040,7 +1038,11 @@ int apply_robot_params()
   {
     int version, age;
     sscanf(YP_PROTOCOL_NAME, "YPP:%d:%d", &version, &age);
-    parameter_set(PARAM_protocol_version, 0, version);
+    for (j = 0; j < YP_PARAM_MAX_MOTOR_NUM; j++)
+    {
+      if (g_param.motor_enable[j])
+        parameter_set(PARAM_protocol_version, j, version);
+    }
   }
 
   /* モータのパラメータ */