5 * @brief CAN-ORTE bridge
7 * Copyright: (c) 2008 CTU Dragons
8 * CTU FEE - Department of Control Engineering
19 #include <sys/socket.h>
20 #include <sys/ioctl.h>
28 #include <can_msg_masks.h>
29 #include <can_msg_def.h>
32 #include <roboorte_robottype.h>
33 #include <can_msg_masks.h>
38 if ((sock = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
39 perror("cand: unable to create a socket");
43 can_addr.can_family = AF_CAN;
44 std::strcpy(can_ifreq.ifr_name, "can0");
46 if (ioctl(sock, SIOCGIFINDEX, &can_ifreq) < 0) {
47 perror("cand: SIOCGIFINDEX");
51 can_addr.can_ifindex = can_ifreq.ifr_ifindex;
53 if (!can_addr.can_ifindex) {
54 perror("cand: invalid socket interface");
58 if (bind(sock, (struct sockaddr *)&can_addr, sizeof(can_addr)) < 0) {
59 perror("cand: unable to bind");
66 int set_pwr_ctrl(struct robottype_orte_data *orte_data)
69 if(orte_data->pwr_ctrl.voltage33)
70 data |= CAN_PWR_CTRL_33_ON;
72 data |= CAN_PWR_CTRL_33_OFF;
74 if(orte_data->pwr_ctrl.voltage50)
75 data |= CAN_PWR_CTRL_50_ON;
77 data |= CAN_PWR_CTRL_50_OFF;
79 if(orte_data->pwr_ctrl.voltage80)
80 data |= CAN_PWR_CTRL_80_ON;
82 data |= CAN_PWR_CTRL_80_OFF;
84 can_send(CAN_PWR, 1, &data);
89 int set_pwr_alert(struct robottype_orte_data *orte_data)
92 data = orte_data->pwr_alert.value;
94 can_send(CAN_PWR_ALERT, 1, &data);
99 int send_can_msg(struct robottype_orte_data *orte_data)
101 return can_send(orte_data->can_msg.id, orte_data->can_msg.len, orte_data->can_msg.data);
104 int set_motor_speed(struct robottype_orte_data *orte_data)
106 unsigned char data[4];
108 data[0] = orte_data->motion_speed.right >> 8;
109 data[1] = orte_data->motion_speed.right & 0xff;
110 data[2] = orte_data->motion_speed.left >> 8;
111 data[3] = orte_data->motion_speed.left & 0xff;
112 can_send(CAN_MOTION_CMD, 4, data);
117 int set_vidle_cmd(uint16_t req_pos)
119 unsigned char data[2];
121 data[0] = req_pos >> 8 && 0xff;
122 data[1] = req_pos & 0xff;
123 can_send(CAN_VIDLE_CMD, 2, data);
129 * Sends #CAN_HOKUYO_PITCH message.
131 * - data = orte_data->pusher.pos
133 int set_hokuyo_pitch(struct robottype_orte_data *orte_data)
135 unsigned char data = orte_data->hokuyo_pitch.angle;
137 can_send(CAN_HOKUYO_PITCH, sizeof(data), &data);
142 int can_send(canid_t id, unsigned char length, unsigned char *data)
144 struct can_frame frame;
148 frame.can_dlc = length;
150 for(int i=0; i<length; i++)
151 frame.data[i] = data[i];
153 if ((size = write(sock, &frame, sizeof(struct can_frame))) < 0) {
154 perror("cand: can send: write()");
156 } else if (size < sizeof(struct can_frame)) {
157 perror("cand: can send: incomplete CAN frame\n");
165 * Parse frame ID and react as required
167 void cand_parse_frame(struct robottype_orte_data *orte, struct can_frame frame)
169 static int status_cnt = 0;
171 switch(frame.can_id) {
172 /* voltage measurements from power board */
174 /* robot commands (start, ..) */
175 case CAN_VIDLE_STATUS:
176 orte->vidle_status.act_pos = (frame.data[0] << 8) | frame.data[1];
177 orte->vidle_status.response = (frame.data[2] << 8) | frame.data[3];
178 orte->vidle_status.flags = frame.data[5];
179 ORTEPublicationSend(orte->publication_vidle_status);
182 orte->robot_cmd.start_condition = frame.data[0];
183 ORTEPublicationSend(orte->publication_robot_cmd);
186 /* positioning by odometry */
188 orte->odo_data.left =
189 ((frame.data[0]<<24)|
192 orte->odo_data.right =
193 ((frame.data[3]<<24)|
196 ORTEPublicationSend(orte->publication_odo_data);
199 /* positioning by odometry */
200 case CAN_MOTION_ODOMETRY_SIMPLE:
201 orte->motion_irc.right =
202 ((frame.data[0]<<24)|
205 orte->motion_irc.left =
206 ((frame.data[3]<<24)|
209 orte->motion_irc.seq = frame.data[6];
210 ORTEPublicationSend(orte->publication_motion_irc);
214 case CAN_MOTION_STATUS:
215 orte->motion_status.err_left =
216 (frame.data[0]<<8)|(frame.data[1]);
217 orte->motion_status.err_right =
218 (frame.data[2]<<8)|(frame.data[3]);
219 if(++status_cnt == 2) { // h8eurobot sends duplicite can messages
220 ORTEPublicationSend(orte->publication_motion_status);
225 double volt33, voltBAT;
226 voltBAT = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
227 (frame.data[2] << 8) | (frame.data[3]))/10000.0;
228 volt33 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
229 (frame.data[6] << 8) | (frame.data[7]))/10000.0;
230 orte->pwr_voltage.voltage33 = volt33;
231 orte->pwr_voltage.voltageBAT = voltBAT;
235 double volt80, volt50;
236 volt50 = ((frame.data[0] << 24) | (frame.data[1] << 16) | \
237 (frame.data[2] << 8) | (frame.data[3]))/10000.0;
238 volt80 = ((frame.data[4] << 24) | (frame.data[5] << 16) | \
239 (frame.data[6] << 8) | (frame.data[7]))/10000.0;
240 orte->pwr_voltage.voltage80 = volt80;
241 orte->pwr_voltage.voltage50 = volt50;
243 ORTEPublicationSend(orte->publication_pwr_voltage);
247 //FIXME: add logging here (Filip)
248 // printf("received CAN msg with unknown id: %x\n",frame.can_id);
253 void rcv_pwr_ctrl_cb (const ORTERecvInfo *info, void *vinstance,
254 void *recvCallBackParam)
256 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
258 switch (info->status) {
260 set_pwr_ctrl(orte_data);
263 //printf("ORTE deadline occurred - PWR_CTRL receive\n");
268 void rcv_can_msg_cb (const ORTERecvInfo *info, void *vinstance,
269 void *recvCallBackParam)
271 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
273 switch (info->status) {
275 send_can_msg(orte_data);
278 //printf("ORTE deadline occurred - PWR_CTRL receive\n");
282 void rcv_pwr_alert_cb (const ORTERecvInfo *info, void *vinstance,
283 void *recvCallBackParam)
286 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
287 switch (info->status) {
289 set_pwr_alert(orte_data);
292 //printf("ORTE deadline occurred - PWR_CTRL receive\n");
297 void rcv_motion_speed_cb(const ORTERecvInfo *info, void *vinstance,
298 void *recvCallBackParam)
300 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
302 switch (info->status) {
304 /* reversing motion direction, as it is different, than last year */
305 orte_data->motion_speed.left *= -1;
306 orte_data->motion_speed.right *=-1;
307 set_motor_speed(orte_data);
308 /*printf("motor cmd received\n");*/
311 //printf("motor cmd deadline occurred, stopping motors\n");
312 orte_data->motion_speed.left = 0;
313 orte_data->motion_speed.right = 0;
314 set_motor_speed(orte_data);
319 void rcv_vidle_cmd_cb(const ORTERecvInfo *info, void *vinstance,
320 void *recvCallBackParam)
322 struct vidle_cmd_type *vidle_cmd = (struct vidle_cmd_type *)vinstance;
324 switch (info->status) {
326 set_vidle_cmd(vidle_cmd->req_pos);
333 void rcv_hokuyo_pitch_cb (const ORTERecvInfo *info, void *vinstance,
334 void *recvCallBackParam)
336 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
338 switch (info->status) {
340 set_hokuyo_pitch(orte_data);
343 // printf("ORTE deadline occurred - hokuyo pitch receive\n");
349 int main(int argc, char *argv[])
354 struct robottype_orte_data orte;
355 struct can_frame frame;
357 if (cand_init() < 0) {
358 printf("cand: init failed\n");
361 printf("cand: init OK\n");
366 /* orte initialization */
367 if(robottype_roboorte_init(&orte)) {
368 printf("Roboorte initialization failed! Exiting...\n");
372 printf("Roboorte OK\n");
374 /* creating publishers */
375 robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
376 robottype_publisher_motion_status_create(&orte, NULL, NULL);
377 robottype_publisher_odo_data_create(&orte, NULL, NULL);
378 robottype_publisher_motion_irc_create(&orte, NULL, NULL);
379 robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
380 robottype_publisher_vidle_status_create(&orte, NULL, NULL);
381 printf("Publishers OK\n");
383 /* creating subscribers */
384 robottype_subscriber_pwr_ctrl_create(&orte, rcv_pwr_ctrl_cb, &orte);
385 robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
386 robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
387 robottype_subscriber_vidle_cmd_create(&orte, rcv_vidle_cmd_cb, &orte);
388 robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
391 printf("subscribers OK\n");
395 struct pollfd pfd[1] = {{sock, POLLIN, 0}};
397 ret = poll(pfd, 1, -1);
399 perror("cand: poll() error");
403 /* timeout expired, nothing to be done */
407 /* read data from SocketCAN */
408 size = read(sock, &frame, sizeof(struct can_frame));
412 cand_parse_frame(&orte, frame);