@@ -58,6 +58,7 @@ BraccioClass::BraccioClass()
5858, _pd_events{}
5959, _pd_timer{}
6060, _start_pd_burst{0xFFFFFFFF }
61+ , _pd_thd{osPriorityHigh}
6162{
6263
6364}
@@ -77,19 +78,11 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
7778 pinMode (PIN_FUSB302_INT, INPUT_PULLUP);
7879 pinMode (RS485_RX_PIN, INPUT_PULLUP);
7980
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));
8282 attachInterrupt (PIN_FUSB302_INT, braccio_unlock_pd_semaphore_irq, FALLING);
8383 _pd_timer.attach (braccio_unlock_pd_semaphore, 10ms);
84-
8584 _PD_UFP.init_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
8685
87- /*
88- while (millis() < 200) {
89- _PD_UFP.run();
90- }
91- */
92-
9386 button_init ();
9487
9588 if (!expander_init ()) return false ;
@@ -444,10 +437,13 @@ void BraccioClass::lvgl_defaultMenu()
444437 lv_obj_set_pos (label1, 0 , 0 );
445438}
446439
447- void BraccioClass::pd_thread () {
440+ void BraccioClass::pd_thread_func ()
441+ {
448442 _start_pd_burst = millis ();
449443 size_t last_time_ask_pps = 0 ;
450- while (1 ) {
444+
445+ for (;;)
446+ {
451447 auto ret = _pd_events.wait_any (0xFF );
452448 if ((ret & 1 ) && (millis () - _start_pd_burst > 1000 )) {
453449 _pd_timer.detach ();
0 commit comments