diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp index 607aad69e6..00cf1a622e 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.cpp @@ -319,14 +319,23 @@ void LinuxRCOutput_Bebop::write(uint8_t ch, uint16_t period_us) _request_period_us[ch] = period_us; - /* write command to motors only when all channels are set */ - if (ch == (BEBOP_BLDC_MOTORS_NUM - 1)) { - pthread_mutex_lock(&_mutex); - memcpy(_period_us, _request_period_us, sizeof(_period_us)); - pthread_cond_signal(&_cond); - pthread_mutex_unlock(&_mutex); - memset(_request_period_us, 0 ,sizeof(_request_period_us)); - } + if (!_corking) + push(); +} + +void LinuxRCOutput_Bebop::cork() +{ + _corking = true; +} + +void LinuxRCOutput_Bebop::push() +{ + _corking = false; + pthread_mutex_lock(&_mutex); + memcpy(_period_us, _request_period_us, sizeof(_period_us)); + pthread_cond_signal(&_cond); + pthread_mutex_unlock(&_mutex); + memset(_request_period_us, 0 ,sizeof(_request_period_us)); } uint16_t LinuxRCOutput_Bebop::read(uint8_t ch) diff --git a/libraries/AP_HAL_Linux/RCOutput_Bebop.h b/libraries/AP_HAL_Linux/RCOutput_Bebop.h index c9e5c08849..311aa5ab23 100644 --- a/libraries/AP_HAL_Linux/RCOutput_Bebop.h +++ b/libraries/AP_HAL_Linux/RCOutput_Bebop.h @@ -61,6 +61,8 @@ public: void enable_ch(uint8_t ch); void disable_ch(uint8_t ch); void write(uint8_t ch, uint16_t period_us); + void cork() override; + void push() override; uint16_t read(uint8_t ch); void read(uint16_t* period_us, uint8_t len); void set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm); @@ -75,6 +77,7 @@ private: uint16_t _min_pwm; uint16_t _max_pwm; uint8_t _state; + bool _corking = false; uint8_t _checksum(uint8_t *data, unsigned int len); void _start_prop();