@@ -58,6 +58,7 @@ BraccioClass::BraccioClass()
58
58
, _pd_events{}
59
59
, _pd_timer{}
60
60
, _start_pd_burst{0xFFFFFFFF }
61
+ , _pd_thd{osPriorityHigh}
61
62
{
62
63
63
64
}
@@ -77,19 +78,11 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
77
78
pinMode (PIN_FUSB302_INT, INPUT_PULLUP);
78
79
pinMode (RS485_RX_PIN, INPUT_PULLUP);
79
80
80
- static rtos::Thread th (osPriorityHigh);
81
- th.start (mbed::callback (this , &BraccioClass::pd_thread));
81
+ _pd_thd.start (mbed::callback (this , &BraccioClass::pd_thread_func));
82
82
attachInterrupt (PIN_FUSB302_INT, braccio_unlock_pd_semaphore_irq, FALLING);
83
83
_pd_timer.attach (braccio_unlock_pd_semaphore, 10ms);
84
-
85
84
_PD_UFP.init_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
86
85
87
- /*
88
- while (millis() < 200) {
89
- _PD_UFP.run();
90
- }
91
- */
92
-
93
86
button_init ();
94
87
95
88
if (!expander_init ()) return false ;
@@ -444,10 +437,13 @@ void BraccioClass::lvgl_defaultMenu()
444
437
lv_obj_set_pos (label1, 0 , 0 );
445
438
}
446
439
447
- void BraccioClass::pd_thread () {
440
+ void BraccioClass::pd_thread_func ()
441
+ {
448
442
_start_pd_burst = millis ();
449
443
size_t last_time_ask_pps = 0 ;
450
- while (1 ) {
444
+
445
+ for (;;)
446
+ {
451
447
auto ret = _pd_events.wait_any (0xFF );
452
448
if ((ret & 1 ) && (millis () - _start_pd_burst > 1000 )) {
453
449
_pd_timer.detach ();
0 commit comments