update command (2st

protocol/ano_cmd.c

@@ -0,0 +1,72 @@
+#include "ano.h"
+#include "command.h"
+
+static void request_pid_param_info(void)
+{
+    command_handle(COMMAND_REQUEST_PID, RT_NULL, 0, RT_NULL);
+}
+static void reset_param(void)
+{
+
+}
+static void set_pid_group1(float k1_p, float k1_i, float k1_d, float k2_p, float k2_i, float k2_d, float k3_p, float k3_i, float k3_d)
+{
+    struct cmd_dt_pid pid;
+
+    pid.id = 1;
+    pid.kp = k1_p;
+    pid.ki = k1_i;
+    pid.kd = k1_d;
+    command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), RT_NULL);
+    pid.id = 2;
+    pid.kp = k2_p;
+    pid.ki = k2_i;
+    pid.kd = k2_d;
+    command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), RT_NULL);
+    pid.id = 3;
+    pid.kp = k3_p;
+    pid.ki = k3_i;
+    pid.kd = k3_d;
+    command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), RT_NULL);
+
+    
+}
+static void set_pid_group2(float k1_p, float k1_i, float k1_d, float k2_p, float k2_i, float k2_d, float k3_p, float k3_i, float k3_d)
+{
+
+}
+static void set_pid_group3(float k1_p, float k1_i, float k1_d, float k2_p, float k2_i, float k2_d, float k3_p, float k3_i, float k3_d)
+{
+
+}
+static void set_pid_group4(float k1_p, float k1_i, float k1_d, float k2_p, float k2_i, float k2_d, float k3_p, float k3_i, float k3_d)
+{
+
+}
+static void set_pid_group5(float k1_p, float k1_i, float k1_d, float k2_p, float k2_i, float k2_d, float k3_p, float k3_i, float k3_d)
+{
+
+}
+static void set_pid_group6(float k1_p, float k1_i, float k1_d, float k2_p, float k2_i, float k2_d, float k3_p, float k3_i, float k3_d)
+{
+
+}
+
+static struct ano_callback cb = {
+    .request_pid_param_info = request_pid_param_info,
+    .reset_param = reset_param,
+    .set_pid_group1 = set_pid_group1,
+    .set_pid_group2 = set_pid_group2,
+    .set_pid_group3 = set_pid_group3,
+    .set_pid_group4 = set_pid_group4,
+    .set_pid_group5 = set_pid_group5,
+    .set_pid_group6 = set_pid_group6,
+};
+
+static void ano_command_register(void)
+{
+    // set ano callback
+    ano_set_callback(&cb);
+}
+
+INIT_DEVICE_EXPORT(ano_command_register);

protocol/command.c

@@ -11,6 +11,12 @@
 static rt_thread_t trd_cmd = RT_NULL;
 
 // Message Queue
+struct command_msg
+{
+    struct command_info info;
+    void                *param;
+    rt_uint16_t         size;
+};
 #define MAX_MSGS                    16
 static rt_mq_t msg_cmd = RT_NULL;
 
@@ -19,12 +25,12 @@ static rt_mq_t msg_cmd = RT_NULL;
 struct command
 {
     rt_int16_t   robot_cmd;
-    void         (*handler)(command_info_t info);
+    void         (*handler)(command_info_t info, void *param, rt_uint16_t size);
 };
 static struct command command_table[TABLE_MAX_SIZE];
 static uint16_t command_table_size = 0;
 
-rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info))
+rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info, void *param, rt_uint16_t size))
 {
     if (command_table_size > TABLE_MAX_SIZE - 1)
     {
@@ -56,24 +62,42 @@ rt_err_t command_unregister(rt_int16_t cmd)
     return RT_EOK;
 }
 
-rt_err_t command_send(command_info_t info)
+rt_err_t command_handle(rt_int16_t cmd, void *param, rt_uint16_t size, void *target)
+{
+    struct command_msg msg = {
+        .param = param,
+        .size = size,
+        .info = {
+            .target = target,
+            .cmd = cmd,
+        }
+    };
+    return rt_mq_send(msg_cmd, &msg, sizeof(struct command_msg));
+}
+
+rt_err_t command_send(command_sender_t sender, rt_int16_t cmd, void *param, rt_uint16_t size)
 {
-    return rt_mq_send(msg_cmd, info, sizeof(struct command_info));
+    if (sender->send != RT_NULL)
+    {
+        return sender->send(cmd, param, size);
+    }
+
+    return RT_EOK;
 }
 
 static void command_thread_entry(void *param)
 {
-    struct command_info info;
+    struct command_msg msg;
 
     while (1)
     {
-        rt_mq_recv(msg_cmd, &info, sizeof(struct command_info), RT_WAITING_FOREVER);
+        rt_mq_recv(msg_cmd, &msg, sizeof(struct command_msg), RT_WAITING_FOREVER);
         // look-up table and call callback
         for (int i = 0; i < command_table_size; i++)
         {
-            if (command_table[i].robot_cmd == info.cmd)
+            if (command_table[i].robot_cmd == msg.info.cmd)
             {
-                command_table[i].handler(&info);
+                command_table[i].handler(&msg.info, msg.param, msg.size);
                 break;
             }
         }

protocol/command.h

@@ -3,30 +3,78 @@
 
 #include <rtthread.h>
 
-// command
-#define COMMAND_NONE                    0
-#define COMMAND_CAR_STOP                1
-#define COMMAND_CAR_FORWARD             2
-#define COMMAND_CAR_BACKWARD            3
-#define COMMAND_CAR_TURNLEFT            4
-#define COMMAND_CAR_TURNRIGHT           5
-#define COMMAND_GET_CAR_SPEED           6
+/* Command */
+// down-link
+#define COMMAND_NONE                            0X0000
+#define COMMAND_CAR_STOP                        0X0001
+#define COMMAND_CAR_FORWARD                     0X0002
+#define COMMAND_CAR_BACKWARD                    0X0003
+#define COMMAND_CAR_TURNLEFT                    0X0004
+#define COMMAND_CAR_TURNRIGHT                   0X0005
 
-// #define COMMAND_CAR_FORWARD_WITH_PARAM
+#define COMMAND_CAR_FORWARD_WITH_PARAM          0X1002
+#define COMMAND_CAR_BACKWARD_WITH_PARAM         0X1003
+#define COMMAND_CAR_TURNLEFT_WITH_PARAM         0X1004
+#define COMMAND_CAR_TURNRIGHT_WITH_PARAM        0X1005
 
-#define COMMAND_RC_VIBRATE              -1
+#define COMMAND_SET_PID                         0x3000
+#define COMMAND_RESET_PID                       0x4000
+#define COMMAND_REQUEST_PID                     0x5000
+
+// up-link
+#define COMMAND_RC_VIBRATE                      0x2000
+
+#define COMMAND_SEND_PID                        0x6000
+#define COMMAND_SEND_SENSOR                     0x6001
+#define COMMAND_SEND_RPY                        0x6002
+
+struct cmd_dt_pid
+{
+    int   id;
+    float kp;
+    float ki;
+    float kd;
+};
+
+struct cmd_dt_sensor
+{
+    int32_t acc_x;
+    int32_t acc_y;
+    int32_t acc_z;
+    int32_t gyro_x;
+    int32_t gyro_y;
+    int32_t gyro_z;
+    int32_t mag_x;
+    int32_t mag_y;
+    int32_t mag_z;
+};
+
+struct cmd_dt_rpy
+{
+    float roll;
+    float pitch;
+    float yaw;
+};
+
+struct command_sender
+{
+    char *name;
+    rt_err_t (*send)(rt_int16_t cmd, void *param, rt_uint16_t size);
+};
+
+typedef struct command_sender *command_sender_t;
 
 struct command_info
 {
-    void       *target;
-    rt_int16_t cmd;
-    void       *param;
+    rt_int16_t  cmd;
+    void        *target;
 };
 
 typedef struct command_info *command_info_t;
 
-rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info));
+rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info, void *param, rt_uint16_t size));
 rt_err_t command_unregister(rt_int16_t cmd);
-rt_err_t command_send(command_info_t info);
+rt_err_t command_handle(rt_int16_t cmd, void *param, rt_uint16_t size, void *target);
+rt_err_t command_send(command_sender_t sender, rt_int16_t cmd, void *param, rt_uint16_t size);
 
 #endif

protocol/ps2.c

@@ -26,8 +26,6 @@ static rt_base_t ps2_clk_pin;
 static rt_base_t ps2_do_pin; 
 static rt_base_t ps2_di_pin;
 
-static struct command_info cmd_info;
-
 static void hal_cs_high(void)
 {
     rt_pin_write(ps2_cs_pin, PIN_HIGH);
@@ -153,8 +151,7 @@ static void ps2_thread_entry(void *param)
             {
                 if (table[i].standard_cmd != COMMAND_NONE)
                 {
-                    cmd_info.cmd = table[i].standard_cmd;
-                    command_send(&cmd_info);
+                    command_handle(table[i].standard_cmd, RT_NULL, 0, RT_NULL);
                 }
             }
         }
@@ -164,27 +161,27 @@ static void ps2_thread_entry(void *param)
         {
             if (table[PS2_ROCKER_LX].standard_cmd != COMMAND_NONE)
             {
-                cmd_info.cmd = table[PS2_ROCKER_LX].standard_cmd;
-                cmd_info.param = &ctrl_data.left_stick_x;
-                command_send(&cmd_info);
+                // cmd_info.cmd = table[PS2_ROCKER_LX].standard_cmd;
+                // cmd_info.param = &ctrl_data.left_stick_x;
+                // command_handle(&cmd_info);
             }
             if (table[PS2_ROCKER_LY].standard_cmd != COMMAND_NONE)
             {
-                cmd_info.cmd = table[PS2_ROCKER_LY].standard_cmd;
-                cmd_info.param = &ctrl_data.left_stick_y;
-                command_send(&cmd_info);
+                // cmd_info.cmd = table[PS2_ROCKER_LY].standard_cmd;
+                // cmd_info.param = &ctrl_data.left_stick_y;
+                // command_handle(&cmd_info);
             }
             if (table[PS2_ROCKER_RX].standard_cmd != COMMAND_NONE)
             {
-                cmd_info.cmd = table[PS2_ROCKER_RX].standard_cmd;
-                cmd_info.param = &ctrl_data.right_stick_x;
-                command_send(&cmd_info);
+                // cmd_info.cmd = table[PS2_ROCKER_RX].standard_cmd;
+                // cmd_info.param = &ctrl_data.right_stick_x;
+                // command_handle(&cmd_info);
             }
             if (table[PS2_ROCKER_RY].standard_cmd != COMMAND_NONE)
             {
-                cmd_info.cmd = table[PS2_ROCKER_RY].standard_cmd;
-                cmd_info.param = &ctrl_data.right_stick_y;
-                command_send(&cmd_info);
+                // cmd_info.cmd = table[PS2_ROCKER_RY].standard_cmd;
+                // cmd_info.param = &ctrl_data.right_stick_y;
+                // command_handle(&cmd_info);
             }
         }
     }
@@ -205,7 +202,7 @@ void ps2_init(rt_base_t cs_pin, rt_base_t clk_pin, rt_base_t do_pin, rt_base_t d
     hal_cs_high();
     hal_clk_high();
 
-    cmd_info.target = target;
+    // cmd_info.target = target;
 
     tid_ps2 = rt_thread_create("ps2",
                           ps2_thread_entry, RT_NULL,