From 4d0d521e0d09c044a8516bd5eb77e9643fde1adc Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Wed, 8 Sep 2021 13:24:45 -0300 Subject: [PATCH 1/2] HAL_Linux: try to stop PWMs before shutting down due to SIGTERM --- libraries/AP_HAL_Linux/HAL_Linux_Class.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp index 4e5831464567b..80168c5dddb8c 100644 --- a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp +++ b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp @@ -413,6 +413,8 @@ void HAL_Linux::run(int argc, char* const argv[], Callbacks* callbacks) const callbacks->loop(); } + // At least try to stop all PWM before shutting down + rcout->force_safety_on(); rcin->teardown(); I2CDeviceManager::from(i2c_mgr)->teardown(); SPIDeviceManager::from(spi)->teardown(); From 1543b36f9d2283673911b124bfd88a96670da5ce Mon Sep 17 00:00:00 2001 From: Willian Galvani Date: Wed, 8 Sep 2021 13:26:36 -0300 Subject: [PATCH 2/2] HAL_Linux: RCOutput_PCA9685: implement force_safety_on/off --- libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp | 23 +++++++++++++++++++++ libraries/AP_HAL_Linux/RCOutput_PCA9685.h | 2 ++ 2 files changed, 25 insertions(+) diff --git a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp index f64005ac14769..cf84ba465230b 100644 --- a/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp +++ b/libraries/AP_HAL_Linux/RCOutput_PCA9685.cpp @@ -167,6 +167,29 @@ void RCOutput_PCA9685::disable_ch(uint8_t ch) write(ch, 0); } +bool RCOutput_PCA9685::force_safety_on() { + if (!_dev || !_dev->get_semaphore()->take(10)) { + return false; + } + /* Shutdown before sleeping. */ + _dev->write_register(PCA9685_RA_ALL_LED_OFF_H, PCA9685_ALL_LED_OFF_H_SHUT); + /* Put PCA9685 to sleep */ + _dev->write_register(PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT); + + _dev->get_semaphore()->give(); + return true; +} + +void RCOutput_PCA9685::force_safety_off() { + if (!_dev || !_dev->get_semaphore()->take(10)) { + return; + } + /* Restart the device and enable auto-incremented write */ + _dev->write_register(PCA9685_RA_MODE1, + PCA9685_MODE1_RESTART_BIT | PCA9685_MODE1_AI_BIT); + _dev->get_semaphore()->give(); +} + void RCOutput_PCA9685::write(uint8_t ch, uint16_t period_us) { if (ch >= (PWM_CHAN_COUNT - _channel_offset)) { diff --git a/libraries/AP_HAL_Linux/RCOutput_PCA9685.h b/libraries/AP_HAL_Linux/RCOutput_PCA9685.h index 8e43aa2fa6bf2..c4b55d953ed0f 100644 --- a/libraries/AP_HAL_Linux/RCOutput_PCA9685.h +++ b/libraries/AP_HAL_Linux/RCOutput_PCA9685.h @@ -26,6 +26,8 @@ class RCOutput_PCA9685 : public AP_HAL::RCOutput { uint16_t get_freq(uint8_t ch) override; void enable_ch(uint8_t ch) override; void disable_ch(uint8_t ch) override; + bool force_safety_on() override; + void force_safety_off() override; void write(uint8_t ch, uint16_t period_us) override; void cork() override; void push() override;