From 526fb65dd140e5a3e0f6004a09ed69fbdf73be3b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 5 Jan 2016 07:17:44 +1100
Subject: [PATCH] HAL_PX4: fixed setting of rcout frequency on alt channels

---
 libraries/AP_HAL_PX4/RCOutput.cpp | 33 ++++++++++++++++++++++++++-----
 libraries/AP_HAL_PX4/RCOutput.h   | 27 +++++++++++++------------
 2 files changed, 42 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_HAL_PX4/RCOutput.cpp b/libraries/AP_HAL_PX4/RCOutput.cpp
index 69649fdae9..c8056da655 100644
--- a/libraries/AP_HAL_PX4/RCOutput.cpp
+++ b/libraries/AP_HAL_PX4/RCOutput.cpp
@@ -85,19 +85,22 @@ void PX4RCOutput::_init_alt_channels(void)
     }
 }
 
-void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) 
+/*
+  set output frequency on outputs associated with fd
+ */
+void PX4RCOutput::set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz) 
 {
-    // we can't set this per channel yet
+    // we can't set this per channel
     if (freq_hz > 50) {
         // we're being asked to set the alt rate
-        if (ioctl(_pwm_fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) {
+        if (ioctl(fd, PWM_SERVO_SET_UPDATE_RATE, (unsigned long)freq_hz) != 0) {
             hal.console->printf("RCOutput: Unable to set alt rate to %uHz\n", (unsigned)freq_hz);
             return;
         }
         _freq_hz = freq_hz;
     }
 
-    /* work out the new rate mask. The PX4IO board has 3 groups of servos. 
+    /* work out the new rate mask. The outputs have 3 groups of servos. 
 
        Group 0: channels 0 1
        Group 1: channels 4 5 6 7
@@ -133,11 +136,31 @@ void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
         }
     }
 
-    if (ioctl(_pwm_fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) {
+    if (ioctl(fd, PWM_SERVO_SET_SELECT_UPDATE_RATE, _rate_mask) != 0) {
         hal.console->printf("RCOutput: Unable to set alt rate mask to 0x%x\n", (unsigned)_rate_mask);
     }
 }
 
+/*
+  set output frequency
+ */
+void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz) 
+{
+    // greater than 400 doesn't give enough room at higher periods for
+    // the down pulse
+    if (freq_hz > 400) {
+        freq_hz = 400;
+    }
+    uint32_t primary_mask = chmask & ((1UL<<_servo_count)-1);
+    uint32_t alt_mask = chmask >> _servo_count;
+    if (primary_mask && _pwm_fd != -1) {
+        set_freq_fd(_pwm_fd, primary_mask, freq_hz);
+    }
+    if (alt_mask && _alt_fd != -1) {
+        set_freq_fd(_alt_fd, alt_mask, freq_hz);
+    }
+}
+
 uint16_t PX4RCOutput::get_freq(uint8_t ch) 
 {
     if (_rate_mask & (1U<<ch)) {
diff --git a/libraries/AP_HAL_PX4/RCOutput.h b/libraries/AP_HAL_PX4/RCOutput.h
index 5c246e406d..07ddee118a 100644
--- a/libraries/AP_HAL_PX4/RCOutput.h
+++ b/libraries/AP_HAL_PX4/RCOutput.h
@@ -12,19 +12,19 @@
 class PX4::PX4RCOutput : public AP_HAL::RCOutput 
 {
 public:
-    void     init();
-    void     set_freq(uint32_t chmask, uint16_t freq_hz);
-    uint16_t get_freq(uint8_t ch);
-    void     enable_ch(uint8_t ch);
-    void     disable_ch(uint8_t ch);
-    void     write(uint8_t ch, uint16_t period_us);
-    uint16_t read(uint8_t ch);
-    void     read(uint16_t* period_us, uint8_t len);
-    void     set_safety_pwm(uint32_t chmask, uint16_t period_us);
-    void     set_failsafe_pwm(uint32_t chmask, uint16_t period_us);
-    bool     force_safety_on(void);
-    void     force_safety_off(void);
-    void     set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) {
+    void     init() override;
+    void     set_freq(uint32_t chmask, uint16_t freq_hz) override;
+    uint16_t get_freq(uint8_t ch) override;
+    void     enable_ch(uint8_t ch) override;
+    void     disable_ch(uint8_t ch) override;
+    void     write(uint8_t ch, uint16_t period_us) override;
+    uint16_t read(uint8_t ch) override;
+    void     read(uint16_t* period_us, uint8_t len) override;
+    void     set_safety_pwm(uint32_t chmask, uint16_t period_us) override;
+    void     set_failsafe_pwm(uint32_t chmask, uint16_t period_us) override;
+    bool     force_safety_on(void) override;
+    void     force_safety_off(void) override;
+    void     set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) override {
         _esc_pwm_min = min_pwm;
         _esc_pwm_max = max_pwm;
     }
@@ -59,6 +59,7 @@ private:
     void _init_alt_channels(void);
     void _publish_actuators(void);
     void _arm_actuators(bool arm);
+    void set_freq_fd(int fd, uint32_t chmask, uint16_t freq_hz);
 };
 
 #endif // __AP_HAL_PX4_RCOUTPUT_H__