Skip to content

Commit 273d293

Browse files
sogwmssogwms
committed
update command (2st
1 parent 4ed94bd commit 273d293

File tree

4 files changed

+181
-40
lines changed

4 files changed

+181
-40
lines changed

‎protocol/ano_cmd.c‎

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
#include "ano.h"
2+
#include "command.h"
3+
4+
static void request_pid_param_info(void)
5+
{
6+
command_handle(COMMAND_REQUEST_PID, RT_NULL, 0, RT_NULL);
7+
}
8+
static void reset_param(void)
9+
{
10+
11+
}
12+
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)
13+
{
14+
struct cmd_dt_pid pid;
15+
16+
pid.id = 1;
17+
pid.kp = k1_p;
18+
pid.ki = k1_i;
19+
pid.kd = k1_d;
20+
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), RT_NULL);
21+
pid.id = 2;
22+
pid.kp = k2_p;
23+
pid.ki = k2_i;
24+
pid.kd = k2_d;
25+
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), RT_NULL);
26+
pid.id = 3;
27+
pid.kp = k3_p;
28+
pid.ki = k3_i;
29+
pid.kd = k3_d;
30+
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), RT_NULL);
31+
32+
33+
}
34+
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)
35+
{
36+
37+
}
38+
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)
39+
{
40+
41+
}
42+
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)
43+
{
44+
45+
}
46+
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)
47+
{
48+
49+
}
50+
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)
51+
{
52+
53+
}
54+
55+
static struct ano_callback cb = {
56+
.request_pid_param_info = request_pid_param_info,
57+
.reset_param = reset_param,
58+
.set_pid_group1 = set_pid_group1,
59+
.set_pid_group2 = set_pid_group2,
60+
.set_pid_group3 = set_pid_group3,
61+
.set_pid_group4 = set_pid_group4,
62+
.set_pid_group5 = set_pid_group5,
63+
.set_pid_group6 = set_pid_group6,
64+
};
65+
66+
static void ano_command_register(void)
67+
{
68+
// set ano callback
69+
ano_set_callback(&cb);
70+
}
71+
72+
INIT_DEVICE_EXPORT(ano_command_register);

‎protocol/command.c‎

Lines changed: 32 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,12 @@
1111
static rt_thread_t trd_cmd = RT_NULL;
1212

1313
// Message Queue
14+
struct command_msg
15+
{
16+
struct command_info info;
17+
void *param;
18+
rt_uint16_t size;
19+
};
1420
#define MAX_MSGS 16
1521
static rt_mq_t msg_cmd = RT_NULL;
1622

@@ -19,12 +25,12 @@ static rt_mq_t msg_cmd = RT_NULL;
1925
struct command
2026
{
2127
rt_int16_t robot_cmd;
22-
void (*handler)(command_info_t info);
28+
void (*handler)(command_info_t info, void *param, rt_uint16_t size);
2329
};
2430
static struct command command_table[TABLE_MAX_SIZE];
2531
static uint16_t command_table_size = 0;
2632

27-
rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info))
33+
rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info, void *param, rt_uint16_t size))
2834
{
2935
if (command_table_size > TABLE_MAX_SIZE - 1)
3036
{
@@ -56,24 +62,42 @@ rt_err_t command_unregister(rt_int16_t cmd)
5662
return RT_EOK;
5763
}
5864

59-
rt_err_t command_send(command_info_t info)
65+
rt_err_t command_handle(rt_int16_t cmd, void *param, rt_uint16_t size, void *target)
66+
{
67+
struct command_msg msg = {
68+
.param = param,
69+
.size = size,
70+
.info = {
71+
.target = target,
72+
.cmd = cmd,
73+
}
74+
};
75+
return rt_mq_send(msg_cmd, &msg, sizeof(struct command_msg));
76+
}
77+
78+
rt_err_t command_send(command_sender_t sender, rt_int16_t cmd, void *param, rt_uint16_t size)
6079
{
61-
return rt_mq_send(msg_cmd, info, sizeof(struct command_info));
80+
if (sender->send != RT_NULL)
81+
{
82+
return sender->send(cmd, param, size);
83+
}
84+
85+
return RT_EOK;
6286
}
6387

6488
static void command_thread_entry(void *param)
6589
{
66-
struct command_info info;
90+
struct command_msg msg;
6791

6892
while (1)
6993
{
70-
rt_mq_recv(msg_cmd, &info, sizeof(struct command_info), RT_WAITING_FOREVER);
94+
rt_mq_recv(msg_cmd, &msg, sizeof(struct command_msg), RT_WAITING_FOREVER);
7195
// look-up table and call callback
7296
for (int i = 0; i < command_table_size; i++)
7397
{
74-
if (command_table[i].robot_cmd == info.cmd)
98+
if (command_table[i].robot_cmd == msg.info.cmd)
7599
{
76-
command_table[i].handler(&info);
100+
command_table[i].handler(&msg.info, msg.param, msg.size);
77101
break;
78102
}
79103
}

‎protocol/command.h‎

Lines changed: 63 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -3,30 +3,78 @@
33

44
#include <rtthread.h>
55

6-
// command
7-
#define COMMAND_NONE 0
8-
#define COMMAND_CAR_STOP 1
9-
#define COMMAND_CAR_FORWARD 2
10-
#define COMMAND_CAR_BACKWARD 3
11-
#define COMMAND_CAR_TURNLEFT 4
12-
#define COMMAND_CAR_TURNRIGHT 5
13-
#define COMMAND_GET_CAR_SPEED 6
6+
/* Command */
7+
// down-link
8+
#define COMMAND_NONE 0X0000
9+
#define COMMAND_CAR_STOP 0X0001
10+
#define COMMAND_CAR_FORWARD 0X0002
11+
#define COMMAND_CAR_BACKWARD 0X0003
12+
#define COMMAND_CAR_TURNLEFT 0X0004
13+
#define COMMAND_CAR_TURNRIGHT 0X0005
1414

15-
// #define COMMAND_CAR_FORWARD_WITH_PARAM
15+
#define COMMAND_CAR_FORWARD_WITH_PARAM 0X1002
16+
#define COMMAND_CAR_BACKWARD_WITH_PARAM 0X1003
17+
#define COMMAND_CAR_TURNLEFT_WITH_PARAM 0X1004
18+
#define COMMAND_CAR_TURNRIGHT_WITH_PARAM 0X1005
1619

17-
#define COMMAND_RC_VIBRATE -1
20+
#define COMMAND_SET_PID 0x3000
21+
#define COMMAND_RESET_PID 0x4000
22+
#define COMMAND_REQUEST_PID 0x5000
23+
24+
// up-link
25+
#define COMMAND_RC_VIBRATE 0x2000
26+
27+
#define COMMAND_SEND_PID 0x6000
28+
#define COMMAND_SEND_SENSOR 0x6001
29+
#define COMMAND_SEND_RPY 0x6002
30+
31+
struct cmd_dt_pid
32+
{
33+
int id;
34+
float kp;
35+
float ki;
36+
float kd;
37+
};
38+
39+
struct cmd_dt_sensor
40+
{
41+
int32_t acc_x;
42+
int32_t acc_y;
43+
int32_t acc_z;
44+
int32_t gyro_x;
45+
int32_t gyro_y;
46+
int32_t gyro_z;
47+
int32_t mag_x;
48+
int32_t mag_y;
49+
int32_t mag_z;
50+
};
51+
52+
struct cmd_dt_rpy
53+
{
54+
float roll;
55+
float pitch;
56+
float yaw;
57+
};
58+
59+
struct command_sender
60+
{
61+
char *name;
62+
rt_err_t (*send)(rt_int16_t cmd, void *param, rt_uint16_t size);
63+
};
64+
65+
typedef struct command_sender *command_sender_t;
1866

1967
struct command_info
2068
{
21-
void *target;
22-
rt_int16_t cmd;
23-
void *param;
69+
rt_int16_t cmd;
70+
void *target;
2471
};
2572

2673
typedef struct command_info *command_info_t;
2774

28-
rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info));
75+
rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info, void *param, rt_uint16_t size));
2976
rt_err_t command_unregister(rt_int16_t cmd);
30-
rt_err_t command_send(command_info_t info);
77+
rt_err_t command_handle(rt_int16_t cmd, void *param, rt_uint16_t size, void *target);
78+
rt_err_t command_send(command_sender_t sender, rt_int16_t cmd, void *param, rt_uint16_t size);
3179

3280
#endif

‎protocol/ps2.c‎

Lines changed: 14 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -26,8 +26,6 @@ static rt_base_t ps2_clk_pin;
2626
static rt_base_t ps2_do_pin;
2727
static rt_base_t ps2_di_pin;
2828

29-
static struct command_info cmd_info;
30-
3129
static void hal_cs_high(void)
3230
{
3331
rt_pin_write(ps2_cs_pin, PIN_HIGH);
@@ -153,8 +151,7 @@ static void ps2_thread_entry(void *param)
153151
{
154152
if (table[i].standard_cmd != COMMAND_NONE)
155153
{
156-
cmd_info.cmd = table[i].standard_cmd;
157-
command_send(&cmd_info);
154+
command_handle(table[i].standard_cmd, RT_NULL, 0, RT_NULL);
158155
}
159156
}
160157
}
@@ -164,27 +161,27 @@ static void ps2_thread_entry(void *param)
164161
{
165162
if (table[PS2_ROCKER_LX].standard_cmd != COMMAND_NONE)
166163
{
167-
cmd_info.cmd = table[PS2_ROCKER_LX].standard_cmd;
168-
cmd_info.param = &ctrl_data.left_stick_x;
169-
command_send(&cmd_info);
164+
// cmd_info.cmd = table[PS2_ROCKER_LX].standard_cmd;
165+
// cmd_info.param = &ctrl_data.left_stick_x;
166+
// command_handle(&cmd_info);
170167
}
171168
if (table[PS2_ROCKER_LY].standard_cmd != COMMAND_NONE)
172169
{
173-
cmd_info.cmd = table[PS2_ROCKER_LY].standard_cmd;
174-
cmd_info.param = &ctrl_data.left_stick_y;
175-
command_send(&cmd_info);
170+
// cmd_info.cmd = table[PS2_ROCKER_LY].standard_cmd;
171+
// cmd_info.param = &ctrl_data.left_stick_y;
172+
// command_handle(&cmd_info);
176173
}
177174
if (table[PS2_ROCKER_RX].standard_cmd != COMMAND_NONE)
178175
{
179-
cmd_info.cmd = table[PS2_ROCKER_RX].standard_cmd;
180-
cmd_info.param = &ctrl_data.right_stick_x;
181-
command_send(&cmd_info);
176+
// cmd_info.cmd = table[PS2_ROCKER_RX].standard_cmd;
177+
// cmd_info.param = &ctrl_data.right_stick_x;
178+
// command_handle(&cmd_info);
182179
}
183180
if (table[PS2_ROCKER_RY].standard_cmd != COMMAND_NONE)
184181
{
185-
cmd_info.cmd = table[PS2_ROCKER_RY].standard_cmd;
186-
cmd_info.param = &ctrl_data.right_stick_y;
187-
command_send(&cmd_info);
182+
// cmd_info.cmd = table[PS2_ROCKER_RY].standard_cmd;
183+
// cmd_info.param = &ctrl_data.right_stick_y;
184+
// command_handle(&cmd_info);
188185
}
189186
}
190187
}
@@ -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
205202
hal_cs_high();
206203
hal_clk_high();
207204

208-
cmd_info.target = target;
205+
// cmd_info.target = target;
209206

210207
tid_ps2 = rt_thread_create("ps2",
211208
ps2_thread_entry, RT_NULL,

0 commit comments

Comments
 (0)