Skip to content

Commit dc09cc2

Browse files
sogwmssogwms
committed
add respone to request-command
1 parent 6e7e8a7 commit dc09cc2

File tree

9 files changed

+138
-81
lines changed

9 files changed

+138
-81
lines changed

‎chassis/chassis.c‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -259,6 +259,24 @@ static void chassis_set_pid(command_info_t info, void *param, rt_uint16_t size)
259259
}
260260
}
261261

262+
static void chassis_response_request_pid(command_info_t info, void *param, rt_uint16_t size)
263+
{
264+
// TODO
265+
if (info->target != RT_NULL)
266+
{
267+
chassis_t chas = (chassis_t)info->target;
268+
struct controller_param parameter;
269+
struct cmd_dt_pid pid_info;
270+
271+
controller_get_param(chas->c_wheels[0]->w_controller, &parameter);
272+
pid_info.id = PID_ID_WHEEL_0;
273+
pid_info.kp = parameter.data.pid.kp;
274+
pid_info.ki = parameter.data.pid.ki;
275+
pid_info.kd = parameter.data.pid.kd;
276+
command_send(info->sender, COMMAND_SEND_PID, &pid_info, sizeof(struct cmd_dt_pid));
277+
}
278+
}
279+
262280
static int chassis_command_register(void)
263281
{
264282
// TODO
@@ -270,6 +288,8 @@ static int chassis_command_register(void)
270288

271289
command_register(COMMAND_SET_PID, chassis_set_pid);
272290

291+
command_register(COMMAND_REQUEST_PID, chassis_response_request_pid);
292+
273293
return RT_EOK;
274294
}
275295

‎controller/controller.c‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -96,3 +96,11 @@ rt_err_t controller_set_param(controller_t controller, controller_param_t param)
9696

9797
return controller->set_param(controller, param);
9898
}
99+
100+
rt_err_t controller_get_param(controller_t controller, controller_param_t param)
101+
{
102+
RT_ASSERT(controller != RT_NULL);
103+
104+
return controller->get_param(controller, param);
105+
}
106+

‎controller/controller.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@ struct controller
3131
rt_err_t (*reset)(void *controller);
3232
rt_err_t (*destroy)(void *controller);
3333
rt_err_t (*set_param)(void *controller, controller_param_t param);
34+
rt_err_t (*get_param)(void *controller, controller_param_t param);
3435
};
3536

3637
typedef struct controller *controller_t;
@@ -44,4 +45,6 @@ rt_err_t controller_reset(controller_t controller);
4445
rt_err_t controller_set_target(controller_t controller, rt_int16_t target);
4546
rt_err_t controller_set_sample_time(controller_t controller, rt_uint16_t sample_time);
4647
rt_err_t controller_set_param(controller_t controller, controller_param_t param);
48+
rt_err_t controller_get_param(controller_t controller, controller_param_t param);
49+
4750
#endif // __CONTROLLER_H__

‎controller/inc_pid_controller.c‎

Lines changed: 13 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,17 @@ static rt_err_t inc_pid_controller_set_param(void *pid, controller_param_t param
2727
return RT_EOK;
2828
}
2929

30+
static rt_err_t inc_pid_controller_get_param(void *pid, controller_param_t param)
31+
{
32+
inc_pid_controller_t inc_pid = (inc_pid_controller_t)pid;
33+
34+
param->data.pid.kp = inc_pid->kp;
35+
param->data.pid.ki = inc_pid->ki;
36+
param->data.pid.kd = inc_pid->kd;
37+
38+
return RT_EOK;
39+
}
40+
3041
static rt_err_t inc_pid_controller_update(void *pid, float current_point)
3142
{
3243
inc_pid_controller_t inc_pid = (inc_pid_controller_t)pid;
@@ -100,7 +111,8 @@ inc_pid_controller_t inc_pid_controller_create(float kp, float ki, float kd, rt_
100111
new_pid->controller.destroy = inc_pid_controller_destroy;
101112
new_pid->controller.update = inc_pid_controller_update;
102113
new_pid->controller.set_param = inc_pid_controller_set_param;
103-
114+
new_pid->controller.get_param = inc_pid_controller_get_param;
115+
104116
return new_pid;
105117
}
106118

‎controller/pos_pid_controller.c‎

Lines changed: 12 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,17 @@ static rt_err_t pos_pid_controller_set_param(void *pid, controller_param_t param
2727
return RT_EOK;
2828
}
2929

30+
static rt_err_t pos_pid_controller_get_param(void *pid, controller_param_t param)
31+
{
32+
pos_pid_controller_t pos_pid = (pos_pid_controller_t)pid;
33+
34+
param->data.pid.kp = pos_pid->kp;
35+
param->data.pid.ki = pos_pid->ki;
36+
param->data.pid.kd = pos_pid->kd;
37+
38+
return RT_EOK;
39+
}
40+
3041
static rt_err_t pos_pid_controller_update(void *pid, float current_point)
3142
{
3243
pos_pid_controller_t pos_pid = (pos_pid_controller_t)pid;
@@ -109,6 +120,7 @@ pos_pid_controller_t pos_pid_controller_create(float kp, float ki, float kd, rt_
109120
new_pid->controller.destroy = pos_pid_controller_destroy;
110121
new_pid->controller.update = pos_pid_controller_update;
111122
new_pid->controller.set_param = pos_pid_controller_set_param;
123+
new_pid->controller.get_param = pos_pid_controller_get_param;
112124

113125
return new_pid;
114126
}

‎protocol/ano.c‎

Lines changed: 74 additions & 74 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,73 @@ static rt_sem_t rx_sem = RT_NULL;
2323
// ABOUT COMMAND
2424
static void *ano_target;
2525

26+
static rt_err_t ano_sender_send(rt_int16_t cmd, void *param, rt_uint16_t size)
27+
{
28+
switch (cmd)
29+
{
30+
case COMMAND_SEND_PID:
31+
if (size == 3 * sizeof(struct cmd_dt_pid))
32+
{
33+
struct cmd_dt_pid *pid_info = (struct cmd_dt_pid *)param;
34+
int group = (int)((pid_info[0].id + pid_info[1].id + pid_info[2].id) / 9) + 1;
35+
36+
if (group > 6)
37+
{
38+
group = 6;
39+
}
40+
if (group < 1)
41+
{
42+
group = 1;
43+
}
44+
ano_send_pid(group, pid_info[0].kp, pid_info[0].ki, pid_info[0].kd,
45+
pid_info[1].kp, pid_info[1].ki, pid_info[1].kd,
46+
pid_info[2].kp, pid_info[2].ki, pid_info[2].kd);
47+
}
48+
else
49+
{
50+
LOG_D("You need send three groups pid paramter at once when use COMMAND_SEND_PID");
51+
return RT_ERROR;
52+
}
53+
54+
break;
55+
case COMMAND_SEND_SENSOR:
56+
if (size == sizeof(struct cmd_dt_sensor))
57+
{
58+
struct cmd_dt_sensor *sensor_info = (struct cmd_dt_sensor *)param;
59+
ano_send_senser(sensor_info->acc_x, sensor_info->acc_y, sensor_info->acc_z,
60+
sensor_info->gyro_x, sensor_info->gyro_y, sensor_info->gyro_z,
61+
sensor_info->mag_x, sensor_info->mag_y, sensor_info->mag_z, 0);
62+
}
63+
else
64+
{
65+
return RT_ERROR;
66+
}
67+
68+
break;
69+
case COMMAND_SEND_RPY:
70+
if (size == sizeof(struct cmd_dt_rpy))
71+
{
72+
struct cmd_dt_rpy *rpy_info = (struct cmd_dt_rpy *)param;
73+
ano_send_status(rpy_info->roll, rpy_info->pitch, rpy_info->yaw, 0,0,0);
74+
}
75+
else
76+
{
77+
return RT_ERROR;
78+
}
79+
80+
break;
81+
default:
82+
break;
83+
}
84+
85+
return RT_EOK;
86+
}
87+
88+
static struct command_sender ano_sender = {
89+
.name = "ano",
90+
.send = ano_sender_send
91+
};
92+
2693
static int _send_data(uint8_t *buffer, uint8_t length)
2794
{
2895
if (dev_ano != RT_NULL)
@@ -94,38 +161,38 @@ static void ano_parse_frame(uint8_t *buffer, uint8_t length)
94161
{
95162
if (*(buffer + 4) == 0X01)
96163
{
97-
command_handle(COMMAND_REQUEST_PID, RT_NULL, 0, ano_target);
164+
command_handle(COMMAND_REQUEST_PID, RT_NULL, 0, &ano_sender, ano_target);
98165
}
99166
else if (*(buffer + 4) == 0XA0)
100167
{
101168
// request version info
102169
}
103170
else if (*(buffer + 4) == 0XA1)
104171
{
105-
command_handle(COMMAND_RESET_PID, RT_NULL, 0, ano_target);
172+
command_handle(COMMAND_RESET_PID, RT_NULL, 0, &ano_sender, ano_target);
106173
}
107174
}
108175
else if (*(buffer + 2) == 0X10) //PID1
109176
{
110177
struct cmd_dt_pid pid;
111178
float kpid[9];
112179
_get_pid_param(buffer, kpid);
113-
180+
114181
pid.id = PID_ID_WHEEL_0;
115182
pid.kp = kpid[0];
116183
pid.ki = kpid[1];
117184
pid.kd = kpid[2];
118-
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
185+
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), &ano_sender, ano_target);
119186
pid.id = PID_ID_WHEEL_1;
120187
pid.kp = kpid[3];
121188
pid.ki = kpid[4];
122189
pid.kd = kpid[5];
123-
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
190+
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), &ano_sender, ano_target);
124191
pid.id = PID_ID_WHEEL_2;
125192
pid.kp = kpid[6];
126193
pid.ki = kpid[7];
127194
pid.kd = kpid[8];
128-
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
195+
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), &ano_sender, ano_target);
129196

130197
ano_send_check(*(buffer + 2), sum);
131198
}
@@ -139,7 +206,7 @@ static void ano_parse_frame(uint8_t *buffer, uint8_t length)
139206
pid.kp = kpid[0];
140207
pid.ki = kpid[1];
141208
pid.kd = kpid[2];
142-
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), ano_target);
209+
command_handle(COMMAND_SET_PID, &pid, sizeof(struct cmd_dt_pid), &ano_sender, ano_target);
143210
// pid.id = 5;
144211
// pid.kp = kpid[3];
145212
// pid.ki = kpid[4];
@@ -683,73 +750,6 @@ int ano_set_device(const char *device_name)
683750
return RT_EOK;
684751
}
685752

686-
static rt_err_t ano_sender_send(rt_int16_t cmd, void *param, rt_uint16_t size)
687-
{
688-
switch (cmd)
689-
{
690-
case COMMAND_SEND_PID:
691-
if (size == 3 * sizeof(struct cmd_dt_pid))
692-
{
693-
struct cmd_dt_pid *pid_info = (struct cmd_dt_pid *)param;
694-
int group = (int)((pid_info[0].id + pid_info[1].id + pid_info[2].id) / 9) + 1;
695-
696-
if (group > 6)
697-
{
698-
group = 6;
699-
}
700-
if (group < 1)
701-
{
702-
group = 1;
703-
}
704-
ano_send_pid(group, pid_info[0].kp, pid_info[0].ki, pid_info[0].kd,
705-
pid_info[1].kp, pid_info[1].ki, pid_info[1].kd,
706-
pid_info[2].kp, pid_info[2].ki, pid_info[2].kd);
707-
}
708-
else
709-
{
710-
LOG_D("You need send three groups pid paramter at once when use COMMAND_SEND_PID");
711-
return RT_ERROR;
712-
}
713-
714-
break;
715-
case COMMAND_SEND_SENSOR:
716-
if (size == sizeof(struct cmd_dt_sensor))
717-
{
718-
struct cmd_dt_sensor *sensor_info = (struct cmd_dt_sensor *)param;
719-
ano_send_senser(sensor_info->acc_x, sensor_info->acc_y, sensor_info->acc_z,
720-
sensor_info->gyro_x, sensor_info->gyro_y, sensor_info->gyro_z,
721-
sensor_info->mag_x, sensor_info->mag_y, sensor_info->mag_z, 0);
722-
}
723-
else
724-
{
725-
return RT_ERROR;
726-
}
727-
728-
break;
729-
case COMMAND_SEND_RPY:
730-
if (size == sizeof(struct cmd_dt_rpy))
731-
{
732-
struct cmd_dt_rpy *rpy_info = (struct cmd_dt_rpy *)param;
733-
ano_send_status(rpy_info->roll, rpy_info->pitch, rpy_info->yaw, 0,0,0);
734-
}
735-
else
736-
{
737-
return RT_ERROR;
738-
}
739-
740-
break;
741-
default:
742-
break;
743-
}
744-
745-
return RT_EOK;
746-
}
747-
748-
static struct command_sender ano_sender = {
749-
.name = "ano",
750-
.send = ano_sender_send
751-
};
752-
753753
command_sender_t ano_get_sender(void)
754754
{
755755
return &ano_sender;

‎protocol/command.c‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -62,12 +62,13 @@ rt_err_t command_unregister(rt_int16_t cmd)
6262
return RT_EOK;
6363
}
6464

65-
rt_err_t command_handle(rt_int16_t cmd, void *param, rt_uint16_t size, void *target)
65+
rt_err_t command_handle(rt_int16_t cmd, void *param, rt_uint16_t size, command_sender_t sender, void *target)
6666
{
6767
struct command_msg msg = {
6868
.param = param,
6969
.size = size,
7070
.info = {
71+
.sender = sender,
7172
.target = target,
7273
.cmd = cmd,
7374
}

‎protocol/command.h‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -85,15 +85,16 @@ typedef struct command_sender *command_sender_t;
8585

8686
struct command_info
8787
{
88-
rt_int16_t cmd;
89-
void *target;
88+
rt_int16_t cmd;
89+
void *target;
90+
command_sender_t sender;
9091
};
9192

9293
typedef struct command_info *command_info_t;
9394

9495
rt_err_t command_register(rt_int16_t cmd, void (*handler)(command_info_t info, void *param, rt_uint16_t size));
9596
rt_err_t command_unregister(rt_int16_t cmd);
96-
rt_err_t command_handle(rt_int16_t cmd, void *param, rt_uint16_t size, void *target);
97+
rt_err_t command_handle(rt_int16_t cmd, void *param, rt_uint16_t size, command_sender_t sender, void *target);
9798
rt_err_t command_send(command_sender_t sender, rt_int16_t cmd, void *param, rt_uint16_t size);
9899

99100
#endif

‎protocol/ps2.c‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -182,7 +182,7 @@ static void ps2_thread_entry(void *param)
182182
{
183183
if (table[i].standard_cmd != COMMAND_NONE)
184184
{
185-
command_handle(table[i].standard_cmd, RT_NULL, 0, RT_NULL);
185+
command_handle(table[i].standard_cmd, RT_NULL, 0, &ps2_sender, ps2_target);
186186
}
187187
}
188188
}
@@ -210,7 +210,7 @@ static void ps2_thread_entry(void *param)
210210
continue;
211211
}
212212

213-
command_handle(table[PS2_ROCKER_LX].standard_cmd, &target_velocity, sizeof(struct cmd_dt_velocity), ps2_target);
213+
command_handle(table[PS2_ROCKER_LX].standard_cmd, &target_velocity, sizeof(struct cmd_dt_velocity), &ps2_sender, ps2_target);
214214
}
215215
if (table[PS2_ROCKER_LY].standard_cmd != COMMAND_NONE)
216216
{

0 commit comments

Comments
 (0)