1 #include "display_orte.h"
2 #include "ortesignals.h"
3 //#include <can_msg_def.h>
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.
13 int timespec_subtract (struct timespec *result,
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;
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;
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;
34 /* Return 1 if result is negative. */
35 return x->tv_sec < y->tv_sec;
38 bool miliseconds_elapsed_since(struct timespec *t, unsigned miliseconds)
40 struct timespec now, diff;
41 unsigned long long elapsed;
43 if (t->tv_sec == 0 && t->tv_nsec == 0)
44 return true; /* Always elapsed after program start */
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;
52 ////////////////////////////////////////////////////////////////////////////////////////////////
54 void rcv_match_time_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
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) {
66 status = STATUS_FAILED;
69 if (status != last_status ||
70 miliseconds_elapsed_since(&last_sent, 1000)) {
71 inst->status_con(TIM, status);
72 clock_gettime(CLOCK_MONOTONIC, &last_sent);
77 void rcv_pwr_voltage_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
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) {
89 status = STATUS_FAILED;
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);
100 void rcv_odo_data_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
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) {
111 status = STATUS_FAILED;
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);
119 last_status = status;
122 void rcv_robot_cmd_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
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;
130 switch (info->status) {
132 if (instance->start_condition)
133 status = STATUS_WARNING; /* Start plug must be plugged before competition start */
138 status = STATUS_FAILED;
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);
147 last_status = status;
150 void rcv_robot_swicthes_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
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;
158 switch (info->status) {
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);
165 last_color = instance->team_color;
167 status = STATUS_FAILED;
172 void rcv_motion_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
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) {
181 if (m->err_left == 0 && m->err_right == 0)
184 status = STATUS_WARNING;
187 status = STATUS_FAILED;
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);
195 last_status = status;
198 void rcv_lift_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
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) {
210 status = STATUS_WARNING;
213 status = STATUS_FAILED;
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);
221 last_status = status;
224 void rcv_jaws_status_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
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) {
233 if (m->flags.left == 0 && m->flags.right == 0)
236 status = STATUS_WARNING;
239 status = STATUS_FAILED;
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);
247 last_status = status;
250 void rcv_camera_result_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
255 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
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) {
266 status = STATUS_FAILED;
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);
274 last_status = status;
277 void rcv_est_pos_best_cb (const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
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) {
288 status = STATUS_FAILED;
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);
298 last_status = status;
301 void rcv_fsm_main_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
303 OrteSignals *inst=(OrteSignals *)recvCallBackParam;
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) {
310 status=fsm_state->state_name;
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);
324 void rcv_fsm_act_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
326 OrteSignals *inst=(OrteSignals *)recvCallBackParam;
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) {
333 status=fsm_state->state_name;
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);
347 void rcv_fsm_motion_cb(const ORTERecvInfo *info, void *vinstance, void *recvCallBackParam)
349 OrteSignals *inst=(OrteSignals *)recvCallBackParam;
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) {
356 status=fsm_state->state_name;
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);