5 * @brief Display-ORTE bridge
7 * Copyright: (c) 2010 CTU Dragons
8 * CTU FEE - Department of Control Engineering
15 #include <roboorte_robottype.h>
16 //#include <pthread.h>
26 struct sercom_data* sercom;
29 static void sig_handler(int sig)
36 * Subtract the `struct timespec' values X and Y,
37 * storing the result in RESULT (result = x - y).
38 * Return 1 if the difference is negative, otherwise 0.
40 int timespec_subtract (struct timespec *result,
44 /* Perform the carry for the later subtraction by updating Y. */
45 if (x->tv_nsec < y->tv_nsec) {
46 int num_sec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
47 y->tv_nsec -= 1000000000 * num_sec;
50 if (x->tv_nsec - y->tv_nsec > 1000000000) {
51 int num_sec = (x->tv_nsec - y->tv_nsec) / 1000000000;
52 y->tv_nsec += 1000000000 * num_sec;
56 /* Compute the time remaining to wait.
57 `tv_nsec' is certainly positive. */
58 result->tv_sec = x->tv_sec - y->tv_sec;
59 result->tv_nsec = x->tv_nsec - y->tv_nsec;
61 /* Return 1 if result is negative. */
62 return x->tv_sec < y->tv_sec;
65 bool miliseconds_elapsed_since(struct timespec *t, unsigned miliseconds)
67 struct timespec now, diff;
68 unsigned long long elapsed;
70 if (t->tv_sec == 0 && t->tv_nsec == 0)
71 return true; /* Always elapsed after program start */
73 clock_gettime(CLOCK_MONOTONIC, &now);
74 timespec_subtract(&diff, &now, t);
75 elapsed = diff.tv_sec * 1000 + diff.tv_nsec/1000000;
76 return elapsed > miliseconds;
81 void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance,
82 void *recvCallBackParam)
84 UDE_hw_status_t status = STATUS_FAILED;
85 static UDE_hw_status_t last_status;
86 static struct timespec last_sent;
87 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
88 switch (info->status) {
90 uoled_display_voltage(orte_data->pwr_voltage.voltage33,orte_data->pwr_voltage.voltage50,
91 orte_data->pwr_voltage.voltage80, orte_data->pwr_voltage.voltageBAT);
95 status = STATUS_FAILED;
98 if (status != last_status ||
99 miliseconds_elapsed_since(&last_sent, 1000)) {
100 uoled_display_status(PWR, status);
101 clock_gettime(CLOCK_MONOTONIC, &last_sent);
103 last_status = status;
106 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
107 void *recvCallBackParam)
109 UDE_hw_status_t status = STATUS_FAILED;
110 static UDE_hw_status_t last_status;
111 static struct timespec last_sent;
112 //struct odo_data_type *instance = (struct odo_data_type *)vinstance;
113 switch (info->status) {
118 status = STATUS_FAILED;
121 /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
122 if (status != last_status ||
123 miliseconds_elapsed_since(&last_sent, 1000)) {
124 uoled_display_status(ODO, status);
125 clock_gettime(CLOCK_MONOTONIC, &last_sent);
127 last_status = status;
130 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
131 void *recvCallBackParam)
133 UDE_hw_status_t status = STATUS_FAILED;
134 static UDE_hw_status_t last_status;
135 static struct timespec last_sent;
136 struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
138 switch (info->status) {
140 if (instance->start_condition)
141 status = STATUS_WARNING; /* Start plug must be plugged before competition start */
146 status = STATUS_FAILED;
149 if (status != last_status ||
150 miliseconds_elapsed_since(&last_sent, 500)) {
151 uoled_display_status(STA, status);
152 clock_gettime(CLOCK_MONOTONIC, &last_sent);
154 last_status = status;
157 void rcv_robot_swicthes_cb(const ORTERecvInfo *info, void *vinstance,
158 void *recvCallBackParam)
160 UDE_hw_status_t status = STATUS_FAILED;
161 static char last_color;
162 static struct timespec last_sent;
163 struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
165 switch (info->status) {
167 if (instance->team_color != last_color ||
168 miliseconds_elapsed_since(&last_sent, 1000)) {
169 uoled_display_color(instance->team_color);
170 clock_gettime(CLOCK_MONOTONIC, &last_sent);
172 last_color = instance->team_color;
174 status = STATUS_FAILED;
179 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
180 void *recvCallBackParam)
182 struct motion_status_type *m = (struct motion_status_type *)vinstance;
183 UDE_hw_status_t status = STATUS_FAILED;
184 static UDE_hw_status_t last_status;
185 static struct timespec last_sent;
186 switch (info->status) {
188 if (m->err_left == 0 && m->err_right == 0)
191 status = STATUS_WARNING;
194 status = STATUS_FAILED;
197 /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
198 if (status != last_status ||
199 miliseconds_elapsed_since(&last_sent, 1000)) {
200 uoled_display_status(MOT, status);
201 clock_gettime(CLOCK_MONOTONIC, &last_sent);
203 last_status = status;
206 void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
207 void *recvCallBackParam)
209 struct vidle_status_type *m = (struct vidle_status_type *)vinstance;
210 UDE_hw_status_t status = STATUS_FAILED;
211 static UDE_hw_status_t last_status;
212 static struct timespec last_sent;
213 switch (info->status) {
218 status = STATUS_WARNING;
221 status = STATUS_FAILED;
224 /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
225 if (status != last_status ||
226 miliseconds_elapsed_since(&last_sent, 1000)) {
227 uoled_display_status(VID, status);
228 clock_gettime(CLOCK_MONOTONIC, &last_sent);
230 last_status = status;
233 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
234 void *recvCallBackParam)
236 UDE_hw_status_t status = STATUS_FAILED;
237 static UDE_hw_status_t last_status;
238 static struct timespec last_sent;
239 struct camera_result_type *instance = (struct camera_result_type *)vinstance;
241 switch (info->status) {
243 if (instance->error == 0)
246 status = STATUS_WARNING;
249 status = STATUS_FAILED;
252 if (status != last_status ||
253 miliseconds_elapsed_since(&last_sent, 1000)) {
254 uoled_display_status(CAM, status);
255 clock_gettime(CLOCK_MONOTONIC, &last_sent);
257 last_status = status;
259 if (status == STATUS_OK) {
260 s = (instance->side >= 0 && instance->side < 10) ?
261 '0'+instance->side : 'E';
262 c = (instance->center >= 0 && instance->center < 10) ?
263 '0'+instance->center : 'E';
266 uoled_display_corns(s, c);
269 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
270 void *recvCallBackParam)
272 UDE_hw_status_t status = STATUS_FAILED;
273 static UDE_hw_status_t last_status;
274 static struct timespec last_sent;
275 //struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
276 switch (info->status) {
281 status = STATUS_FAILED;
284 if (status != last_status ||
285 miliseconds_elapsed_since(&last_sent, 1000)) {
286 uoled_display_status(HOK, status);
287 clock_gettime(CLOCK_MONOTONIC, &last_sent);
289 last_status = status;
292 void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance,
293 void *recvCallBackParam)
295 UDE_hw_status_t status = STATUS_FAILED;
296 static UDE_hw_status_t last_status;
297 static struct timespec last_sent;
298 struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
299 switch (info->status) {
304 status = STATUS_FAILED;
307 if (status != last_status ||
308 miliseconds_elapsed_since(&last_sent, 100)) {
309 uoled_display_status(APP, status);
310 if (status == STATUS_OK)
311 uoled_display_position(orte_data->est_pos_best.x, orte_data->est_pos_best.y, orte_data->est_pos_best.phi);
312 clock_gettime(CLOCK_MONOTONIC, &last_sent);
314 last_status = status;
317 void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance,
318 void *recvCallBackParam)
320 char status[MGS_LENGTH_DISPLAY_SM];
321 static char last_status[MGS_LENGTH_DISPLAY_SM];
322 static struct timespec last_sent;
323 struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
324 switch (info->status) {
326 strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
327 status[MGS_LENGTH_DISPLAY_SM-1]='\0';
328 //uoled_display_fsm(FSM_MAIN, fsm_state->state_name);
332 //uoled_display_fsm(FSM_MAIN, "?");
335 if (strcmp(status,last_status) != 0 ||
336 miliseconds_elapsed_since(&last_sent, 1000)) {
337 uoled_display_fsm(FSM_MAIN, status);
338 clock_gettime(CLOCK_MONOTONIC, &last_sent);
340 strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
343 void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance,
344 void *recvCallBackParam)
346 char status[MGS_LENGTH_DISPLAY_SM];
347 static char last_status[MGS_LENGTH_DISPLAY_SM];
348 static struct timespec last_sent;
349 struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
350 switch (info->status) {
352 strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
353 status[MGS_LENGTH_DISPLAY_SM-1]='\0';
359 if (strcmp(status,last_status) != 0 ||
360 miliseconds_elapsed_since(&last_sent, 1000)) {
361 uoled_display_fsm(FSM_ACT, status);
362 clock_gettime(CLOCK_MONOTONIC, &last_sent);
364 strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
367 void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance,
368 void *recvCallBackParam)
370 char status[MGS_LENGTH_DISPLAY_SM];
371 static char last_status[MGS_LENGTH_DISPLAY_SM];
372 static struct timespec last_sent;
373 struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
374 switch (info->status) {
376 strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
377 status[MGS_LENGTH_DISPLAY_SM-1]='\0';
383 if (strcmp(status,last_status) != 0 ||
384 miliseconds_elapsed_since(&last_sent, 1000)) {
385 uoled_display_fsm(FSM_MOVE, status);
386 clock_gettime(CLOCK_MONOTONIC, &last_sent);
388 strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
392 struct robottype_orte_data orte;
394 int main(int argc, char *argv[])
396 //fd_set read_fd_set;
397 //struct timeval timeout;
403 signal(SIGINT, sig_handler);
404 signal(SIGTERM, sig_handler);
406 //struct can_frame frame;
407 tty = getenv("DISPLAY_TTY");
410 printf("displayd: enviroment variable 'DISPLAY_TTY' was not set, using default value '%s'.\n", tty);
413 sercom = uoled_sercom_init(tty, NULL);
414 if (strcmp(tty, "/dev/null") == 0 || sercom != NULL) {
416 printf("displayd: init serial communication OK\n");
418 fprintf(stderr, "displayd: init serial communication failed\n");
420 /* Sleep in order to that not be respawn too fast by init */
426 /* orte initialization */
427 if(robottype_roboorte_init(&orte)) {
428 fprintf(stderr, "displayd: Roboorte initialization failed! Exiting...\n");
430 /* Sleep in order to that not be respawn too fast by init */
436 printf("displayd: Roboorte OK\n");
438 /* creating publishers */
439 //robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
440 //robottype_publisher_motion_status_create(&orte, NULL, NULL);
441 //robottype_publisher_odo_data_create(&orte, NULL, NULL);
442 //robottype_publisher_motion_irc_create(&orte, NULL, NULL);
443 //robottype_publisher_robot_cmd_create(&orte, NULL, NULL);
444 //robottype_publisher_corr_trig_create(&orte, NULL, NULL);
445 //robottype_publisher_corr_distances_create(&orte, NULL, NULL);
446 //robottype_publisher_actuator_status_create(&orte, NULL, NULL);
447 //printf("Publishers OK\n");
449 /* creating subscribers */
450 robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, &orte);
451 robottype_subscriber_motion_status_create(&orte, rcv_motion_status_cb, NULL);
452 robottype_subscriber_vidle_status_create(&orte, rcv_vidle_status_cb, NULL);
453 robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, &orte);
454 robottype_subscriber_est_pos_best_create(&orte, rcv_est_pos_best_cb, &orte);
455 //robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
456 //robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
457 //robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
458 robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
459 robottype_subscriber_camera_result_create(&orte, rcv_camera_result_cb, &orte);
460 robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, &orte);
461 robottype_subscriber_fsm_act_create(&orte, rcv_fsm_act_cb, &orte);
462 robottype_subscriber_fsm_motion_create(&orte, rcv_fsm_motion_cb, &orte);
463 robottype_subscriber_robot_cmd_create(&orte, rcv_robot_cmd_cb, &orte);
464 robottype_subscriber_robot_switches_create(&orte, rcv_robot_swicthes_cb, &orte);
466 printf("displayd: subscribers created OK\n");
469 UDE_recieve_cmd_t rcmd;
470 struct pollfd pfd[1] = {{
478 if (strcmp(tty, "/dev/null") != 0) {
479 ret = poll(pfd, 1, 500/*ms*/);
481 ret = uoled_recieve_cmd(&rcmd);
482 /* orte.display_cmd.command=rcmd; */
484 /* ORTEPublicationSend(orte.publication_display_cmd); */
485 /* printf("displayd: publishing command recieved from display [ 0x%2x ]\n", rcmd); */
490 perror("Error: read from display");
494 usleep(500/*ms*/*1000);
498 uoled_display_alive();
502 printf("displayd: closing serial communication...\n");
503 uoled_sercom_close();
504 // printf("displayd: destroying display_cmd publisher...\n");
505 // robottype_publisher_display_cmd_destroy(&orte);
506 printf("displayd: destroying orte instance...\n");
507 robottype_roboorte_destroy(&orte);
510 printf("Waiting 3 seconds\n");
514 printf("displayd: finished!\n");