Add option to ping chained devices (#127)

CMakeLists.txt

@@ -153,6 +153,7 @@ add_executable(ypspur-coordinator
   src/msq.win32.c
   src/odometry.c
   src/param.c
+  src/ping.c
   src/serial.c
   src/ssm_spur_handler.c
   src/utility.c

include/odometry.h

@@ -43,6 +43,7 @@ typedef struct _odometry
   double wtorque[YP_PARAM_MAX_MOTOR_NUM];
   double torque_trans;
   double torque_angular;
+  int ping_response[YP_PARAM_MAX_MOTOR_NUM + 1];
 } Odometry;
 
 typedef struct _error_state *ErrorStatePtr;

include/param.h

@@ -21,6 +21,8 @@
 #ifndef PARAM_H
 #define PARAM_H
 
+#include <stdio.h>
+
 #include <ypparam.h>
 #include <utility.h>
 
@@ -49,6 +51,7 @@ typedef enum
   OPTION_PASSIVE = 0x20000,
   OPTION_UPDATE_PARAM = 0x40000,
   OPTION_HIGH_PREC = 0x80000,
+  OPTION_PING = 0x100000,
 } ParamOptions;
 
 #define OPTION_DEFAULT (OPTION_HIGH_PREC)

include/ping.h

@@ -0,0 +1,26 @@
+// Copyright (c) 2020 The YP-Spur Authors, except where otherwise indicated.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#ifndef PING_H
+#define PING_H
+
+int ping();
+
+#endif  // PING_H

include/shvel-param.h

@@ -89,13 +89,15 @@ typedef enum
   PARAM_ad_mask,
   PARAM_phase_offset,
   PARAM_protocol_version,
+  PARAM_ping,
 } YPSpur_shvel_param;
 
 typedef enum
 {
   INT_enc_index_rise = 0,
   INT_enc_index_fall,
   INT_error_state,
+  INT_ping_response,
 } YPSpur_shvel_interrupt;
 
 typedef enum
@@ -109,4 +111,6 @@ typedef enum
   ERROR_INTERNAL = 0x0020,
 } YPSpur_shvel_error_state;
 
+#define MOTOR_ID_BROADCAST 0x7E
+
 #endif  // SHVEL_PARAM_H

src/odometry.c

@@ -250,14 +250,14 @@ void process_int(
   Parameters *param;
   param = get_param_ptr();
 
-  if (!param->motor_enable[id])
-    return;
-
   switch (param_id)
   {
     case INT_enc_index_rise:
     case INT_enc_index_fall:
     {
+      if (id >= YP_PARAM_MAX_MOTOR_NUM || !param->motor_enable[id])
+        return;
+
       // enc == value のときに INDEX_RISE/FALL_ANGLE [rad] だった
       const unsigned short enc_div =
           ((unsigned int)xp->enc[id] << ((int)p(YP_PARAM_ENCODER_DIV, id))) & 0xFFFF;
@@ -289,6 +289,9 @@ void process_int(
     }
     case INT_error_state:
     {
+      if (id >= YP_PARAM_MAX_MOTOR_NUM || !param->motor_enable[id])
+        return;
+
       if (err->state[id] != value)
       {
         if (value == ERROR_NONE)
@@ -319,6 +322,18 @@ void process_int(
       err->time[id] = receive_time;
       break;
     }
+    case INT_ping_response:
+      if (id == MOTOR_ID_BROADCAST)
+      {
+        xp->ping_response[YP_PARAM_MAX_MOTOR_NUM] = value;
+        yprintf(OUTPUT_LV_INFO, "Ping response received: broadcast, 0x%08x\n", value);
+      }
+      else
+      {
+        xp->ping_response[id] = value;
+        yprintf(OUTPUT_LV_INFO, "Ping response received: %d, 0x%08x\n", id, value);
+      }
+      break;
     default:
       yprintf(OUTPUT_LV_ERROR, "Error: Unknown interrput data (%d, %d, %d)\n", param_id, id, value);
       break;

src/param.c

@@ -152,6 +152,7 @@ void arg_longhelp(int argc, char *argv[])
   fprintf(stderr, "  --ssm-id <SSMID>         Change ssm id (default = 0).\n");
   fprintf(stderr, "  --socket <port>          Use socket ipc.\n");
   fprintf(stderr, "  --daemon                 Run in daemon mode.\n");
+  fprintf(stderr, "  --ping                   Ping RS485 chained devices.\n");
 }
 
 /* 引数の説明 */
@@ -198,6 +199,10 @@ int arg_analyze(int argc, char *argv[])
     {
       g_param.option |= OPTION_DAEMON;
     }
+    else if (!strcmp(argv[i], "--ping"))
+    {
+      g_param.option |= OPTION_PING;
+    }
     else if (!strcmp(argv[i], "--param-help"))
     {
       g_param.option |= OPTION_SHOW_PARAMHELP;

src/ping.c

@@ -0,0 +1,62 @@
+// Copyright (c) 2020 The YP-Spur Authors, except where otherwise indicated.
+//
+// Permission is hereby granted, free of charge, to any person obtaining a copy
+// of this software and associated documentation files (the "Software"), to
+// deal in the Software without restriction, including without limitation the
+// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
+// sell copies of the Software, and to permit persons to whom the Software is
+// furnished to do so, subject to the following conditions:
+//
+// The above copyright notice and this permission notice shall be included in
+// all copies or substantial portions of the Software.
+//
+// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+// SOFTWARE.
+
+#include <param.h>
+#include <utility.h>
+#include <odometry.h>
+#include <shvel-param.h>
+#include <yprintf.h>
+
+int ping()
+{
+  const int data = 0x123456;
+  yprintf(OUTPUT_LV_INFO, "Ping request: 0x%08x\n", data);
+
+  OdometryPtr odom = get_odometry_ptr();
+  int i;
+  for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM + 1; i++)
+    odom->ping_response[i] = 0;
+
+  const char target = MOTOR_ID_BROADCAST;
+  parameter_set(PARAM_servo, target, 0);
+  parameter_set(PARAM_ping, target, data);
+  odometry_receive_loop();
+
+  int ret = 1;
+  for (i = 0; i < YP_PARAM_MAX_MOTOR_NUM; i++)
+  {
+    if (odom->ping_response[i] == data)
+    {
+      ret = 0;
+      yprintf(OUTPUT_LV_INFO, "Ping response from ID: %d\n", i);
+    }
+  }
+  if (odom->ping_response[i] == data)
+  {
+    ret = 0;
+    yprintf(OUTPUT_LV_INFO, "Ping response from device without ID\n");
+  }
+  if (ret)
+  {
+    yprintf(OUTPUT_LV_ERROR, "No ping response!\n");
+  }
+
+  return 1;
+}

src/serial.c

@@ -536,6 +536,25 @@ int encode_write(char *data, int len)
   return 0;
 }
 
+int encode_int_write(char *data, int len)
+{
+  unsigned char buf[128];
+  int encode_len, ret;
+
+  buf[0] = COMMUNICATION_INT_BYTE;
+  encode_len = encode((unsigned char *)data, len, buf + 1, 126);
+  buf[encode_len + 1] = COMMUNICATION_END_BYTE;
+
+  ret = serial_write((char *)buf, encode_len + 2);
+  if (ret <= 0)
+  {
+    return -1;
+  }
+  serial_flush_out();
+
+  return 0;
+}
+
 int serial_write(char *buf, int len)
 {
 #if !defined(__MINGW32__)

src/ypspur-coordinator.c

@@ -51,6 +51,7 @@
 #include <ssm_spur_handler.h>
 #include <utility.h>
 #include <yprintf.h>
+#include <ping.h>
 
 /* ライブラリ用 */
 #include <ypspur.h>
@@ -332,6 +333,12 @@ int main(int argc, char *argv[])
     {
       yprintf(OUTPUT_LV_INFO, " Port    : n/a (--without-device mode)\n");
     }
+
+    if (option(OPTION_PING))
+    {
+      return ping();
+    }
+
     if (!(option(OPTION_PARAM_FILE)))
     {
       // パラメータファイルが指定されておらず、ドライバにパラメータが内蔵されている場合