1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
|
#include "udp2lcm.h"
void robotCtrlInit(robot_control_t *robotCtrlData, int64_t utime,
int8_t commandid, int8_t robotid, int8_t ndparams,
int8_t niparams, int8_t nsparams, int64_t nbparams) {
if (robotCtrlData == NULL) {
fprintf(stderr, "Error: Invalid robot control data\n");
return;
}
robotCtrlData->utime = utime;
robotCtrlData->commandid = commandid;
robotCtrlData->robotid = robotid;
robotCtrlData->ndparams = ndparams;
robotCtrlData->dparams = NULL;
robotCtrlData->niparams = niparams;
robotCtrlData->iparams = NULL;
robotCtrlData->nsparams = nsparams;
robotCtrlData->sparams = NULL;
robotCtrlData->nbparams = nbparams;
robotCtrlData->bparams = NULL;
if (ndparams > 0) {
robotCtrlData->dparams = (double *)malloc(ndparams * sizeof(double));
memset(robotCtrlData->dparams, 0, ndparams * sizeof(double));
}
if (niparams > 0) {
robotCtrlData->iparams = (int8_t *)malloc(niparams * sizeof(int8_t));
memset(robotCtrlData->iparams, 0, niparams * sizeof(int8_t));
}
if (nsparams > 0) {
robotCtrlData->sparams = (char **)malloc(nsparams * sizeof(char *));
memset(robotCtrlData->sparams, 0, nsparams * sizeof(char *));
}
if (nbparams > 0) {
robotCtrlData->bparams = (uint8_t *)malloc(nbparams * sizeof(uint8_t));
memset(robotCtrlData->bparams, 0, nbparams * sizeof(uint8_t));
}
}
void freeRobotCtrl(robot_control_t *robotCtrlData) {
if (robotCtrlData == NULL) {
fprintf(stderr, "Error: Invalid robot control data\n");
return;
}
if (robotCtrlData->dparams != NULL) {
free(robotCtrlData->dparams);
robotCtrlData->dparams = NULL;
}
if (robotCtrlData->iparams != NULL) {
free(robotCtrlData->iparams);
robotCtrlData->iparams = NULL;
}
if (robotCtrlData->sparams != NULL) {
free(robotCtrlData->sparams);
robotCtrlData->sparams = NULL;
}
if (robotCtrlData->bparams != NULL) {
free(robotCtrlData->bparams);
robotCtrlData->bparams = NULL;
}
}
|
