From 687ec98e5dd2ab9525d8bca187a7dda87ed3a55e Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Tue, 25 Jul 2017 12:13:40 +0900
Subject: [PATCH] AP_HAL_PX4: set default servo update rate only when changed

This removes the chance a small switch occurring whenever this function is called
---
 libraries/AP_HAL_PX4/RCOutput.cpp | 13 +++++++++----
 libraries/AP_HAL_PX4/RCOutput.h   |  1 +
 2 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp
index 831d1941d2..288bd01df7 100644
--- a/libraries/AP_HAL_PX4/RCOutput.cpp
+++ b/libraries/AP_HAL_PX4/RCOutput.cpp
@@ -644,10 +644,15 @@ void PX4RCOutput::set_output_mode(enum output_mode mode)
 // set default output update rate
 void PX4RCOutput::set_default_rate(uint16_t rate_hz)
 {
-    ioctl(_pwm_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz);
-    if (_alt_fd != -1) {
-        ioctl(_alt_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz);
-    }    
+    if (rate_hz != _default_rate_hz) {
+        // set servo update rate for first 8 pwm channels
+        ioctl(_pwm_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz);
+        if (_alt_fd != -1) {
+            // set servo update rate for auxiliary channels
+            ioctl(_alt_fd, PWM_SERVO_SET_DEFAULT_UPDATE_RATE, rate_hz);
+        }
+        _default_rate_hz = rate_hz;
+    }
 }
 
 #endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h
index 9436882cc5..c5e4792f58 100644
--- a/libraries/AP_HAL_PX4/RCOutput.h
+++ b/libraries/AP_HAL_PX4/RCOutput.h
@@ -82,4 +82,5 @@ private:
     enum AP_HAL::Util::safety_state _safety_state_request = AP_HAL::Util::SAFETY_NONE;
     uint32_t _safety_state_request_last_ms = 0;
     void force_safety_pending_requests(void);
+    uint16_t _default_rate_hz = 50;
 };