[Gate] Add target_torque management

Luos-io · Aug 1, 2024 · e42e024 · e42e024
1 parent 2bbf414
commit e42e024
Showing 1 changed file with 36 additions and 0 deletions.
diff --git a/tool_services/gate/TinyJSON/convert.c b/tool_services/gate/TinyJSON/convert.c
@@ -374,6 +374,42 @@ void Convert_JsonToMsg(service_t *service, uint16_t id, luos_type_t type, char *
             return;
         }
     }
+    // target Torque
+    if ((property && !strcmp(property, "target_torque")))
+    {
+        if ((property_type == JSON_REAL) || (property_type == JSON_INTEGER))
+        {
+            torque_t torque = ForceOD_TorqueFrom_N_m((float)json_getReal(jobj));
+            ForceOD_TorqueToMsg(&torque, &msg);
+            Luos_SendMsg(service, &msg);
+            return;
+        }
+        if (property_type == JSON_ARRAY)
+        {
+            int i = 0;
+            // this is a trajectory
+            int size = (int)json_getInteger(json_getChild(jobj));
+            if (size == 0)
+            {
+                // This trajaectory is empty
+                return;
+            }
+            // find the first \r of the current buf
+            for (i = 0; i < GATE_BUFF_SIZE; i++)
+            {
+                if (bin_data[i] == '\n')
+                {
+                    i++;
+                    break;
+                }
+            }
+            if (i < GATE_BUFF_SIZE - 1)
+            {
+                msg.header.cmd = TORQUE;
+                Luos_SendData(service, &msg, &bin_data[i], (unsigned int)size);
+            }
+        }
+    }
     // target Linear speed
     if ((property && !strcmp(property, "target_trans_speed")) && ((property_type == JSON_REAL) || (property_type == JSON_INTEGER)))
     {