#include <orte.h>
#include <roboorte_robottype.h>
//#include <pthread.h>
-#include <robot.h>
-#include <uoled.h>
+#include <time.h>
+#include "uoled.h"
#include "displayd.h"
+#include <poll.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <signal.h>
+#include <stdbool.h>
+struct sercom_data* sercom;
+int interrupt = 0;
-#ifdef CONFIG_DEBUG_DISPLAYD
-/** Used to control verbosity */
-#define DEBUG
-#endif
+static void sig_handler(int sig)
+{
+ interrupt = 1;
+}
+
+
+/**
+ * Subtract the `struct timespec' values X and Y,
+ * storing the result in RESULT (result = x - y).
+ * Return 1 if the difference is negative, otherwise 0.
+ */
+int timespec_subtract (struct timespec *result,
+ struct timespec *x,
+ struct timespec *y)
+{
+ /* Perform the carry for the later subtraction by updating Y. */
+ if (x->tv_nsec < y->tv_nsec) {
+ int num_sec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
+ y->tv_nsec -= 1000000000 * num_sec;
+ y->tv_sec += num_sec;
+ }
+ if (x->tv_nsec - y->tv_nsec > 1000000000) {
+ int num_sec = (x->tv_nsec - y->tv_nsec) / 1000000000;
+ y->tv_nsec += 1000000000 * num_sec;
+ y->tv_sec -= num_sec;
+ }
+
+ /* Compute the time remaining to wait.
+ `tv_nsec' is certainly positive. */
+ result->tv_sec = x->tv_sec - y->tv_sec;
+ result->tv_nsec = x->tv_nsec - y->tv_nsec;
+
+ /* Return 1 if result is negative. */
+ return x->tv_sec < y->tv_sec;
+}
+
+bool miliseconds_elapsed_since(struct timespec *t, unsigned miliseconds)
+{
+ struct timespec now, diff;
+ unsigned long long elapsed;
+
+ if (t->tv_sec == 0 && t->tv_nsec == 0)
+ return true; /* Always elapsed after program start */
+
+ clock_gettime(CLOCK_MONOTONIC, &now);
+ timespec_subtract(&diff, &now, t);
+ elapsed = diff.tv_sec * 1000 + diff.tv_nsec/1000000;
+ return elapsed > miliseconds;
+}
-//#define DEBUG
-struct sercom_data* sercom;
void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
switch (info->status) {
case NEW_DATA:
- uoled_display_voltage(&orte_data->pwr_voltage);
+ uoled_display_voltage(orte_data->pwr_voltage.voltage33,orte_data->pwr_voltage.voltage50,
+ orte_data->pwr_voltage.voltage80, orte_data->pwr_voltage.voltageBAT);
+ status = STATUS_OK;
break;
case DEADLINE:
- printf("ORTE deadline occurred - PWR_VOLTAGE receive\n");
+ status = STATUS_FAILED;
break;
}
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(PWR, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
}
-void rcv_ref_pos_cb (const ORTERecvInfo *info, void *vinstance,
+void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
- struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
- switch (info->status) {
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ //struct odo_data_type *instance = (struct odo_data_type *)vinstance;
+ switch (info->status) {
+ case NEW_DATA:
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(ODO, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA:
+ if (instance->start_condition)
+ status = STATUS_WARNING; /* Start plug must be plugged before competition start */
+ else
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 500)) {
+ uoled_display_status(STA, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_robot_swicthes_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ UDE_hw_status_t status = STATUS_FAILED;
+ static char last_color;
+ static struct timespec last_sent;
+ struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
+
+ switch (info->status) {
+ case NEW_DATA:
+ if (instance->team_color != last_color ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_color(instance->team_color);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_color = instance->team_color;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+}
+
+void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct motion_status_type *m = (struct motion_status_type *)vinstance;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ if (m->err_left == 0 && m->err_right == 0)
+ status = STATUS_OK;
+ else
+ status = STATUS_WARNING;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(MOT, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_vidle_status_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ struct vidle_status_type *m = (struct vidle_status_type *)vinstance;
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ switch (info->status) {
+ case NEW_DATA:
+ if (m->flags == 0)
+ status = STATUS_OK;
+ else
+ status = STATUS_WARNING;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ /* Neni potreba aktualizovat stav na displeji 25x za sekundu */
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(VID, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ struct camera_result_type *instance = (struct camera_result_type *)vinstance;
+ char s, c;
+ switch (info->status) {
case NEW_DATA:
- uoled_display_position(&orte_data->ref_pos);
+ if (instance->error == 0)
+ status = STATUS_OK;
+ else
+ status = STATUS_WARNING;
break;
case DEADLINE:
- printf("ORTE deadline occurred - REF_POS receive\n");
+ status = STATUS_FAILED;
break;
}
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(CAM, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+
+ if (status == STATUS_OK) {
+ s = (instance->side >= 0 && instance->side < 10) ?
+ '0'+instance->side : 'E';
+ c = (instance->center >= 0 && instance->center < 10) ?
+ '0'+instance->center : 'E';
+ } else
+ s = c = '-';
+ uoled_display_corns(s, c);
+}
+
+void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
+ //struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
+ switch (info->status) {
+ case NEW_DATA:
+ status = STATUS_OK;
+ break;
+ case DEADLINE:
+ status = STATUS_FAILED;
+ break;
+ }
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_status(HOK, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
}
-void rcv_est_pos_odo_cb (const ORTERecvInfo *info, void *vinstance,
+void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance,
void *recvCallBackParam)
{
+ UDE_hw_status_t status = STATUS_FAILED;
+ static UDE_hw_status_t last_status;
+ static struct timespec last_sent;
struct robottype_orte_data *orte_data = (struct robottype_orte_data *)recvCallBackParam;
switch (info->status) {
case NEW_DATA:
- uoled_display_position(&orte_data->est_pos_odo);
+ status = STATUS_OK;
break;
case DEADLINE:
- printf("ORTE deadline occurred - EST_POS_ODO receive\n");
+ status = STATUS_FAILED;
break;
}
+ if (status != last_status ||
+ miliseconds_elapsed_since(&last_sent, 100)) {
+ uoled_display_status(APP, status);
+ if (status == STATUS_OK)
+ uoled_display_position(orte_data->est_pos_best.x, orte_data->est_pos_best.y, orte_data->est_pos_best.phi);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ last_status = status;
+}
+
+void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ char status[MGS_LENGTH_DISPLAY_SM];
+ static char last_status[MGS_LENGTH_DISPLAY_SM];
+ static struct timespec last_sent;
+ struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
+ switch (info->status) {
+ case NEW_DATA:
+ strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
+ status[MGS_LENGTH_DISPLAY_SM-1]='\0';
+ //uoled_display_fsm(FSM_MAIN, fsm_state->state_name);
+ break;
+ case DEADLINE:
+ strcpy(status, "?");
+ //uoled_display_fsm(FSM_MAIN, "?");
+ break;
+ }
+ if (strcmp(status,last_status) != 0 ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_fsm(FSM_MAIN, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
+}
+
+void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ char status[MGS_LENGTH_DISPLAY_SM];
+ static char last_status[MGS_LENGTH_DISPLAY_SM];
+ static struct timespec last_sent;
+ struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
+ switch (info->status) {
+ case NEW_DATA:
+ strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
+ status[MGS_LENGTH_DISPLAY_SM-1]='\0';
+ break;
+ case DEADLINE:
+ strcpy(status, "?");
+ break;
+ }
+ if (strcmp(status,last_status) != 0 ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_fsm(FSM_ACT, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
+}
+
+void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance,
+ void *recvCallBackParam)
+{
+ char status[MGS_LENGTH_DISPLAY_SM];
+ static char last_status[MGS_LENGTH_DISPLAY_SM];
+ static struct timespec last_sent;
+ struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
+ switch (info->status) {
+ case NEW_DATA:
+ strncpy(status, fsm_state->state_name, MGS_LENGTH_DISPLAY_SM);
+ status[MGS_LENGTH_DISPLAY_SM-1]='\0';
+ break;
+ case DEADLINE:
+ strcpy(status, "?");
+ break;
+ }
+ if (strcmp(status,last_status) != 0 ||
+ miliseconds_elapsed_since(&last_sent, 1000)) {
+ uoled_display_fsm(FSM_MOVE, status);
+ clock_gettime(CLOCK_MONOTONIC, &last_sent);
+ }
+ strncpy(last_status,status,MGS_LENGTH_DISPLAY_SM);
}
+
+struct robottype_orte_data orte;
+
int main(int argc, char *argv[])
{
//fd_set read_fd_set;
//struct timeval timeout;
- //int ret;
+ int ret;
//int size;
- struct robottype_orte_data orte;
char * tty;
- sigset_t sigset;
- int sig;
+
+ signal(SIGINT, sig_handler);
+ signal(SIGTERM, sig_handler);
//struct can_frame frame;
tty = getenv("DISPLAY_TTY");
- if (!tty)
+ if (!tty){
tty="/dev/ttyUSB0";
+ printf("displayd: enviroment variable 'DISPLAY_TTY' was not set, using default value '%s'.\n", tty);
+ }
- sercom = uoled_init(tty, NULL);
- if (strcmp(tty, "/dev/null") != 0) {
+ sercom = uoled_sercom_init(tty, NULL);
+ if (strcmp(tty, "/dev/null") == 0 || sercom != NULL) {
// :wq
printf("displayd: init serial communication OK\n");
} else {
- printf("displayd: init serial communication failed\n");
- //return 1;
+ fprintf(stderr, "displayd: init serial communication failed\n");
+//#ifdef COMPETITION
+ /* Sleep in order to that not be respawn too fast by init */
+ sleep(3);
+ return 1;
+//#endif
}
- orte.strength = 1;
-
/* orte initialization */
if(robottype_roboorte_init(&orte)) {
- printf("Roboorte initialization failed! Exiting...\n");
- return -1;
+ fprintf(stderr, "displayd: Roboorte initialization failed! Exiting...\n");
+#ifdef COMPETITION
+ /* Sleep in order to that not be respawn too fast by init */
+ sleep(3);
+#endif
+ return 1;
}
else
- printf("Roboorte OK\n");
+ printf("displayd: Roboorte OK\n");
/* creating publishers */
//robottype_publisher_pwr_voltage_create(&orte, NULL, NULL);
//printf("Publishers OK\n");
/* creating subscribers */
+ robottype_subscriber_odo_data_create(&orte, rcv_odo_data_cb, &orte);
+ robottype_subscriber_motion_status_create(&orte, rcv_motion_status_cb, NULL);
+ robottype_subscriber_vidle_status_create(&orte, rcv_vidle_status_cb, NULL);
robottype_subscriber_pwr_voltage_create(&orte, rcv_pwr_voltage_cb, &orte);
- robottype_subscriber_est_pos_odo_create(&orte, rcv_est_pos_odo_cb, &orte);
- robottype_subscriber_ref_pos_create(&orte, rcv_ref_pos_cb, &orte);
+ robottype_subscriber_est_pos_best_create(&orte, rcv_est_pos_best_cb, &orte);
//robottype_subscriber_pwr_alert_create(&orte, rcv_pwr_alert_cb, &orte);
//robottype_subscriber_motion_speed_create(&orte, rcv_motion_speed_cb, &orte);
//robottype_subscriber_can_msg_create(&orte, rcv_can_msg_cb, &orte);
- //robottype_subscriber_hokuyo_pitch_create(&orte, rcv_hokuyo_pitch_cb, &orte);
+ robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
+ robottype_subscriber_camera_result_create(&orte, rcv_camera_result_cb, &orte);
+ robottype_subscriber_fsm_main_create(&orte, rcv_fsm_main_cb, &orte);
+ robottype_subscriber_fsm_act_create(&orte, rcv_fsm_act_cb, &orte);
+ robottype_subscriber_fsm_motion_create(&orte, rcv_fsm_motion_cb, &orte);
+ robottype_subscriber_robot_cmd_create(&orte, rcv_robot_cmd_cb, &orte);
+ robottype_subscriber_robot_switches_create(&orte, rcv_robot_swicthes_cb, &orte);
+
+ printf("displayd: subscribers created OK\n");
- printf("subscribers created OK\n");
+
+ UDE_recieve_cmd_t rcmd;
+ struct pollfd pfd[1] = {{
+ .fd = sercom->fd,
+ .events = POLLIN,
+ }};
/* main loop */
- for(;;)
+ while(!interrupt)
{
if (strcmp(tty, "/dev/null") != 0) {
- sigfillset(&sigset);
- sigwait(&sigset, &sig);
- break;
+ ret = poll(pfd, 1, 500/*ms*/);
+ if (ret > 0) {
+ ret = uoled_recieve_cmd(&rcmd);
+/* orte.display_cmd.command=rcmd; */
+/* if (! rcmd){ */
+/* ORTEPublicationSend(orte.publication_display_cmd); */
+/* printf("displayd: publishing command recieved from display [ 0x%2x ]\n", rcmd); */
+
+/* } */
+ }
+ if (ret < 0) {
+ perror("Error: read from display");
+ break;
+ }
} else {
- // spracovani serioveho portu, pri prijmu dat...
+ usleep(500/*ms*/*1000);
+ ret = 0;
}
+ if (ret == 0)
+ uoled_display_alive();
}
+
+ // deinitialization
+ printf("displayd: closing serial communication...\n");
+ uoled_sercom_close();
+// printf("displayd: destroying display_cmd publisher...\n");
+// robottype_publisher_display_cmd_destroy(&orte);
+ printf("displayd: destroying orte instance...\n");
+ robottype_roboorte_destroy(&orte);
+
+ if (!interrupt) {
+ printf("Waiting 3 seconds\n");
+ sleep(3);
+ }
+
+ printf("displayd: finished!\n");
return 0;
}
projects / eurobot / public.git / blobdiff