|
@@ -26,6 +26,7 @@
|
26
|
26
|
#define PRIORITY_TRECEIVEFROMMON 25
|
27
|
27
|
#define PRIORITY_TSTARTROBOT 20
|
28
|
28
|
#define PRIORITY_TCAMERA 21
|
|
29
|
+#define PRIORITY_TBATTERY 31
|
29
|
30
|
|
30
|
31
|
/*
|
31
|
32
|
* Some remarks:
|
|
@@ -123,6 +124,10 @@ void Tasks::Init() {
|
123
|
124
|
cerr << "Error task create: " << strerror(-err) << endl << flush;
|
124
|
125
|
exit(EXIT_FAILURE);
|
125
|
126
|
}
|
|
127
|
+ if (err = rt_task_create(&th_checkBattery, "th_checkBattery", 0, PRIORITY_TBATTERY, 0)) {
|
|
128
|
+ cerr << "Error task create: " << strerror(-err) << endl << flush;
|
|
129
|
+ exit(EXIT_FAILURE);
|
|
130
|
+ }
|
126
|
131
|
cout << "Tasks created successfully" << endl << flush;
|
127
|
132
|
|
128
|
133
|
/**************************************************************************************/
|
|
@@ -167,6 +172,10 @@ void Tasks::Run() {
|
167
|
172
|
cerr << "Error task start: " << strerror(-err) << endl << flush;
|
168
|
173
|
exit(EXIT_FAILURE);
|
169
|
174
|
}
|
|
175
|
+ if (err = rt_task_start(&th_checkBattery, (void(*)(void*)) & Tasks::CheckBatteryTask, this)) {
|
|
176
|
+ cerr << "Error task start: " << strerror(-err) << endl << flush;
|
|
177
|
+ exit(EXIT_FAILURE);
|
|
178
|
+ }
|
170
|
179
|
|
171
|
180
|
cout << "Tasks launched" << endl << flush;
|
172
|
181
|
}
|
|
@@ -384,6 +393,41 @@ void Tasks::MoveTask(void *arg) {
|
384
|
393
|
}
|
385
|
394
|
|
386
|
395
|
/**
|
|
396
|
+ * @brief Thread printing the robot battery
|
|
397
|
+ */
|
|
398
|
+void Tasks::CheckBatteryTask(void *arg) {
|
|
399
|
+ cout << "Start " << __PRETTY_FUNCTION__ << endl << flush;
|
|
400
|
+ // Synchronization barrier (waiting that all tasks are starting)
|
|
401
|
+ rt_sem_p(&sem_barrier, TM_INFINITE);
|
|
402
|
+
|
|
403
|
+ /**************************************************************************************/
|
|
404
|
+ /* The task starts here */
|
|
405
|
+ /**************************************************************************************/
|
|
406
|
+
|
|
407
|
+ MessageBattery * batLevel;
|
|
408
|
+ int rs;
|
|
409
|
+
|
|
410
|
+ rt_task_set_periodic(NULL, TM_NOW, 500000000);
|
|
411
|
+
|
|
412
|
+ while(1) {
|
|
413
|
+ rt_task_wait_period(NULL);
|
|
414
|
+
|
|
415
|
+ rt_mutex_acquire(&mutex_robotStarted, TM_INFINITE);
|
|
416
|
+ rs = robotStarted;
|
|
417
|
+ rt_mutex_release(&mutex_robotStarted);
|
|
418
|
+ if (rs == 1) {
|
|
419
|
+ rt_mutex_acquire(&mutex_robot, TM_INFINITE);
|
|
420
|
+ batLevel = (MessageBattery*)robot.Write(new Message(MESSAGE_ROBOT_BATTERY_GET));
|
|
421
|
+ rt_mutex_release(&mutex_robot);
|
|
422
|
+
|
|
423
|
+ rt_mutex_acquire(&mutex_monitor, TM_INFINITE);
|
|
424
|
+ monitor.Write(batLevel);
|
|
425
|
+ rt_mutex_release(&mutex_monitor);
|
|
426
|
+ }
|
|
427
|
+ }
|
|
428
|
+}
|
|
429
|
+
|
|
430
|
+/**
|
387
|
431
|
* Write a message in a given queue
|
388
|
432
|
* @param queue Queue identifier
|
389
|
433
|
* @param msg Message to be stored
|