@@ -21,15 +21,16 @@ extern "C" {
21
21
using namespace std ::chrono_literals;
22
22
23
23
BraccioClass::BraccioClass ()
24
- : serial485{Serial1, 0 , 7 , 8 } /* TX, DE, RE */
24
+ : _i2c_mtx{}
25
+ , serial485{Serial1, 0 , 7 , 8 } /* TX, DE, RE */
25
26
, servos{serial485}
26
27
, PD_UFP{PD_LOG_LEVEL_VERBOSE}
27
- , _expander{TCA6424A_ADDRESS_ADDR_HIGH, i2c_mutex }
28
+ , _expander{TCA6424A_ADDRESS_ADDR_HIGH, _i2c_mtx }
28
29
, _is_ping_allowed{true }
29
30
, _is_motor_connected{false }
30
31
, _motors_connected_mtx{}
31
32
, _motors_connected_thd{}
32
- , _bl{i2c_mutex }
33
+ , _bl{_i2c_mtx }
33
34
, _gfx{}
34
35
, _lvgl_disp_drv{}
35
36
, _lvgl_indev_drv{}
@@ -50,12 +51,10 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
50
51
51
52
pinMode (PIN_FUSB302_INT, INPUT_PULLUP);
52
53
53
- #ifdef __MBED__
54
54
static rtos::Thread th (osPriorityHigh);
55
55
th.start (mbed::callback (this , &BraccioClass::pd_thread));
56
56
attachInterrupt (PIN_FUSB302_INT, mbed::callback (this , &BraccioClass::unlock_pd_semaphore_irq), FALLING);
57
57
pd_timer.attach (mbed::callback (this , &BraccioClass::unlock_pd_semaphore), 10ms);
58
- #endif
59
58
60
59
PD_UFP.init_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
61
60
@@ -119,11 +118,11 @@ bool BraccioClass::begin(voidFuncPtr custom_menu)
119
118
{
120
119
if (!PD_UFP.is_PPS_ready ())
121
120
{
122
- i2c_mutex .lock ();
121
+ _i2c_mtx .lock ();
123
122
PD_UFP.print_status (Serial);
124
123
PD_UFP.set_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
125
124
delay (10 );
126
- i2c_mutex .unlock ();
125
+ _i2c_mtx .unlock ();
127
126
}
128
127
};
129
128
@@ -243,13 +242,13 @@ void BraccioClass::pd_thread() {
243
242
pd_timer.detach ();
244
243
pd_timer.attach (mbed::callback (this , &BraccioClass::unlock_pd_semaphore), 50ms);
245
244
}
246
- i2c_mutex .lock ();
245
+ _i2c_mtx .lock ();
247
246
if (millis () - last_time_ask_pps > 5000 ) {
248
247
PD_UFP.set_PPS (PPS_V (7.2 ), PPS_A (2.0 ));
249
248
last_time_ask_pps = millis ();
250
249
}
251
250
PD_UFP.run ();
252
- i2c_mutex .unlock ();
251
+ _i2c_mtx .unlock ();
253
252
if (PD_UFP.is_power_ready () && PD_UFP.is_PPS_ready ()) {
254
253
255
254
}
0 commit comments