HAL_Linux: RCOutput_PCA9685: implement force_safety_on/off

ArduPilot · Sep 14, 2021 · 68cbb47 · 68cbb47
1 parent 38e8020
commit 68cbb47
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 0 deletions.
diff --git 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
@@ -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;