src/display-qt/display_orte.cpp

   1 #include "display_orte.h"
   2 #include "ortesignals.h"
   3 //#include <can_msg_def.h>
   4
   5 #include <time.h>
   6
   7
   8 /**
   9  * Subtract the `struct timespec' values X and Y,
  10  *  storing the result in RESULT (result = x - y).
  11  *  Return 1 if the difference is negative, otherwise 0.
  12  */
  13 int timespec_subtract (struct timespec *result,
  14                    struct timespec *x,
  15                    struct timespec *y)
  16 {
  17   /* Perform the carry for the later subtraction by updating Y. */
  18   if (x->tv_nsec < y->tv_nsec) {
  19     int num_sec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
  20     y->tv_nsec -= 1000000000 * num_sec;
  21     y->tv_sec += num_sec;
  22   }
  23   if (x->tv_nsec - y->tv_nsec > 1000000000) {
  24     int num_sec = (x->tv_nsec - y->tv_nsec) / 1000000000;
  25     y->tv_nsec += 1000000000 * num_sec;
  26     y->tv_sec -= num_sec;
  27   }
  28
  29   /* Compute the time remaining to wait.
  30      `tv_nsec' is certainly positive. */
  31   result->tv_sec = x->tv_sec - y->tv_sec;
  32   result->tv_nsec = x->tv_nsec - y->tv_nsec;
  33
  34   /* Return 1 if result is negative. */
  35   return x->tv_sec < y->tv_sec;
  36 }
  37
  38 bool miliseconds_elapsed_since(struct timespec *t, unsigned miliseconds)
  39 {
  40         struct timespec now, diff;
  41         unsigned long long elapsed;
  42
  43         if (t->tv_sec == 0 && t->tv_nsec == 0)
  44                 return true;    /* Always elapsed after program start */
  45
  46         clock_gettime(CLOCK_MONOTONIC, &now);
  47         timespec_subtract(&diff, &now, t);
  48         elapsed = diff.tv_sec * 1000 + diff.tv_nsec/1000000;
  49         return elapsed > miliseconds;
  50 }
  51
  52 ////////////////////////////////////////////////////////////////////////////////////////////////
  53
  54 void rcv_match_time_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
  55 {
  56         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
  57         UDE_hw_status_t status = STATUS_FAILED;
  58         static UDE_hw_status_t last_status;
  59         static struct timespec last_sent;
  60         switch (info->status) {
  61                 case NEW_DATA:
  62                         inst->time_con();
  63                         status = STATUS_OK;
  64                         break;
  65                 case DEADLINE:
  66                         status = STATUS_FAILED;
  67                         break;
  68         }
  69 //      if (status != last_status ||
  70 //          miliseconds_elapsed_since(&last_sent, 1000)) {
  71 //              inst->status_con(PWR, status);
  72 //              clock_gettime(CLOCK_MONOTONIC, &last_sent);
  73 //      }
  74 //      last_status = status;
  75 }
  76
  77 void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
  78 {
  79         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
  80         UDE_hw_status_t status = STATUS_FAILED;
  81         static UDE_hw_status_t last_status;
  82         static struct timespec last_sent;
  83         switch (info->status) {
  84                 case NEW_DATA:
  85                         inst->pwr_con();
  86                         status = STATUS_OK;
  87                         break;
  88                 case DEADLINE:
  89                         status = STATUS_FAILED;
  90                         break;
  91         }
  92         if (status != last_status ||
  93             miliseconds_elapsed_since(&last_sent, 1000)) {
  94                 inst->status_con(PWR, status);
  95                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
  96         }
  97         last_status = status;
  98 }
  99
 100 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 101 {
 102         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 103         UDE_hw_status_t status = STATUS_FAILED;
 104         static UDE_hw_status_t last_status;
 105         static struct timespec last_sent;
 106         switch (info->status) {
 107                 case NEW_DATA:
 108                         status = STATUS_OK;
 109                         break;
 110                 case DEADLINE:
 111                         status = STATUS_FAILED;
 112                         break;
 113         }
 114         if (status != last_status ||
 115             miliseconds_elapsed_since(&last_sent, 1000)) {
 116                 inst->status_con(ODO, status);
 117                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 118         }
 119         last_status = status;
 120 }
 121
 122 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 123 {
 124         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 125         UDE_hw_status_t status = STATUS_FAILED;
 126         static UDE_hw_status_t last_status;
 127         static struct timespec last_sent;
 128         struct robot_cmd_type *instance = (struct robot_cmd_type *)vinstance;
 129
 130         switch (info->status) {
 131                 case NEW_DATA:
 132                         if (instance->start_condition)
 133                                 status = STATUS_WARNING; /* Start plug must be plugged before competition start */
 134                         else
 135                                 status = STATUS_OK;
 136                         break;
 137                 case DEADLINE:
 138                         status = STATUS_FAILED;
 139                         break;
 140         }
 141         if (status != last_status ||
 142             miliseconds_elapsed_since(&last_sent, 500)) {
 143                 //uoled_display_status(STA, status);
 144                 inst->status_con(STA, status);
 145                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 146         }
 147         last_status = status;
 148 }
 149
 150 void rcv_robot_swicthes_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 151 {
 152         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 153         UDE_hw_status_t status = STATUS_FAILED;
 154         static char last_color;
 155         static struct timespec last_sent;
 156         struct robot_switches_type *instance = (struct robot_switches_type *)vinstance;
 157
 158         switch (info->status) {
 159                 case NEW_DATA:
 160                         if (instance->team_color != last_color ||
 161                             miliseconds_elapsed_since(&last_sent, 1000)) {
 162                                 inst->color_con(instance->team_color);
 163                                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 164                         }
 165                         last_color = instance->team_color;
 166                 case DEADLINE:
 167                         status = STATUS_FAILED;
 168                         break;
 169         }
 170 }
 171
 172 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 173 {
 174         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 175         struct motion_status_type *m = (struct motion_status_type *)vinstance;
 176         UDE_hw_status_t status = STATUS_FAILED;
 177         static UDE_hw_status_t last_status;
 178         static struct timespec last_sent;
 179         switch (info->status) {
 180                 case NEW_DATA:
 181                         if (m->err_left == 0 && m->err_right == 0)
 182                                 status = STATUS_OK;
 183                         else
 184                                 status = STATUS_WARNING;
 185                         break;
 186                 case DEADLINE:
 187                         status = STATUS_FAILED;
 188                         break;
 189         }
 190         if (status != last_status ||
 191             miliseconds_elapsed_since(&last_sent, 1000)) {
 192                 inst->status_con(MOT, status);
 193                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 194         }
 195         last_status = status;
 196 }
 197
 198 void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 199 {
 200         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 201         struct lift_status_type *m = (struct lift_status_type *)vinstance;
 202         UDE_hw_status_t status = STATUS_FAILED;
 203         static UDE_hw_status_t last_status;
 204         static struct timespec last_sent;
 205         switch (info->status) {
 206                 case NEW_DATA:
 207                         if (m->flags)
 208                                 status = STATUS_OK;
 209                         else
 210                                 status = STATUS_WARNING;
 211                         break;
 212                 case DEADLINE:
 213                         status = STATUS_FAILED;
 214                         break;
 215         }
 216         if (status != last_status ||
 217             miliseconds_elapsed_since(&last_sent, 1000)) {
 218                 inst->status_con(LFT, status);
 219                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 220         }
 221         last_status = status;
 222 }
 223
 224 void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 225 {
 226         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 227         struct jaws_status_type *m = (struct jaws_status_type *)vinstance;
 228         UDE_hw_status_t status = STATUS_FAILED;
 229         static UDE_hw_status_t last_status;
 230         static struct timespec last_sent;
 231         switch (info->status) {
 232                 case NEW_DATA:
 233                         if (m->flags.left == 0 && m->flags.right == 0)
 234                                 status = STATUS_OK;
 235                         else
 236                                 status = STATUS_WARNING;
 237                         break;
 238                 case DEADLINE:
 239                         status = STATUS_FAILED;
 240                         break;
 241         }
 242         if (status != last_status ||
 243             miliseconds_elapsed_since(&last_sent, 1000)) {
 244                 inst->status_con(JAW, status);
 245                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 246         }
 247         last_status = status;
 248 }
 249
 250 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 251 {
 252
 253 }
 254
 255 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 256 {
 257         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 258         UDE_hw_status_t status = STATUS_FAILED;
 259         static UDE_hw_status_t last_status;
 260         static struct timespec last_sent;
 261         switch (info->status) {
 262                 case NEW_DATA:
 263                         status = STATUS_OK;
 264                         break;
 265                 case DEADLINE:
 266                         status = STATUS_FAILED;
 267                         break;
 268         }
 269         if (status != last_status ||
 270             miliseconds_elapsed_since(&last_sent, 1000)) {
 271                 inst->status_con(HOK, status);
 272                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 273         }
 274         last_status = status;
 275 }
 276
 277 void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 278 {
 279         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 280         UDE_hw_status_t status = STATUS_FAILED;
 281         static UDE_hw_status_t last_status;
 282         static struct timespec last_sent;
 283         switch (info->status) {
 284                 case NEW_DATA:
 285                         status = STATUS_OK;
 286                         break;
 287                 case DEADLINE:
 288                         status = STATUS_FAILED;
 289                         break;
 290         }
 291         if (status != last_status ||
 292             miliseconds_elapsed_since(&last_sent, 100)) {
 293                 inst->status_con(APP, status);
 294                 if (status == STATUS_OK)
 295                         inst->position_con();
 296                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 297         }
 298         last_status = status;
 299 }
 300
 301 void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 302 {
 303         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 304         QString status;
 305         static QString last_status;
 306         static struct timespec last_sent;
 307         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 308         switch (info->status) {
 309                 case NEW_DATA:
 310                         status=fsm_state->state_name;
 311                         break;
 312                 case DEADLINE:
 313                         status="?";
 314                         break;
 315         }
 316         if (status!=last_status ||
 317             miliseconds_elapsed_since(&last_sent, 1000)) {
 318                 inst->fsm_con(FSM_MAIN, status);
 319                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 320         }
 321         last_status=status;
 322 }
 323
 324 void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 325 {
 326         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 327         QString status;
 328         static QString last_status;
 329         static struct timespec last_sent;
 330         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 331         switch (info->status) {
 332                 case NEW_DATA:
 333                         status=fsm_state->state_name;
 334                         break;
 335                 case DEADLINE:
 336                         status="?";
 337                         break;
 338         }
 339         if (status!=last_status ||
 340             miliseconds_elapsed_since(&last_sent, 1000)) {
 341                 inst->fsm_con(FSM_ACT, status);
 342                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 343         }
 344         last_status=status;
 345 }
 346
 347 void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
 348 {
 349         OrteSignals *inst=(OrteSignals *)recvCallBackParam;
 350         QString status;
 351         static QString last_status;
 352         static struct timespec last_sent;
 353         struct fsm_state_type *fsm_state = (struct fsm_state_type *)vinstance;
 354         switch (info->status) {
 355                 case NEW_DATA:
 356                         status=fsm_state->state_name;
 357                         break;
 358                 case DEADLINE:
 359                         status="?";
 360                         break;
 361         }
 362         if (status!=last_status ||
 363             miliseconds_elapsed_since(&last_sent, 1000)) {
 364                 inst->fsm_con(FSM_MOVE, status);
 365                 clock_gettime(CLOCK_MONOTONIC, &last_sent);
 366         }
 367         last_status=status;
 368 }