HAL_PX4: enable the FMU PWM pins to be used as GPIO when needed

libraries/AP_HAL_PX4/GPIO.cpp

@@ -48,11 +48,11 @@ void PX4GPIO::init()
         hal.scheduler->panic("Unable to open /dev/tone_alarm");
     }
 
-#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+    _gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, 0);
-    _gpio_fmu_fd = open(PX4FMU_DEVICE_PATH, O_RDWR);
     if (_gpio_fmu_fd == -1) {
         hal.scheduler->panic("Unable to open GPIO");
     }
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
     if (ioctl(_gpio_fmu_fd, GPIO_CLEAR, GPIO_EXT_1) != 0) {
         hal.console->printf("GPIO: Unable to setup GPIO_1\n");
     }
@@ -66,7 +66,13 @@ void PX4GPIO::init()
 }
 
 void PX4GPIO::pinMode(uint8_t pin, uint8_t output)
-{}
+{
+    switch (pin) {
+    case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5):
+        ioctl(_gpio_fmu_fd, output?GPIO_SET_OUTPUT:GPIO_SET_INPUT, 1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0)));
+        break;
+    }
+}
 
 int8_t PX4GPIO::analogPinToDigitalPin(uint8_t pin)
 {
@@ -114,7 +120,14 @@ uint8_t PX4GPIO::read(uint8_t pin) {
             ioctl(_gpio_io_fd, GPIO_GET, (unsigned long)&relays);
             return (relays & PX4IO_P_SETUP_RELAYS_ACC2)?HIGH:LOW;
 #endif
+
+    case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5): {
+        uint32_t v = 0;
+        ioctl(_gpio_fmu_fd, GPIO_GET, (unsigned long)&v);
+        return (v & (1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0))))?HIGH:LOW;
     }
+    }
+    return LOW;
 }
 
 void PX4GPIO::write(uint8_t pin, uint8_t value)
@@ -186,6 +199,10 @@ void PX4GPIO::write(uint8_t pin, uint8_t value)
             ioctl(_gpio_io_fd, value==LOW?GPIO_CLEAR:GPIO_SET, PX4IO_P_SETUP_RELAYS_ACC2);
             break;
 #endif
+
+    case PX4_GPIO_FMU_SERVO_PIN(0) ... PX4_GPIO_FMU_SERVO_PIN(5):
+        ioctl(_gpio_fmu_fd, value==LOW?GPIO_CLEAR:GPIO_SET, 1U<<(pin-PX4_GPIO_FMU_SERVO_PIN(0)));
+        break;
     }
 }
 

libraries/AP_HAL_PX4/GPIO.h

@@ -13,6 +13,12 @@
 #define PX4_GPIO_EXT_IO_ACC1_PIN        115
 #define PX4_GPIO_EXT_IO_ACC2_PIN        116
 
+/*
+  start servo channels used as GPIO at 50. Pin 50 is
+  the first FMU servo pin
+ */
+#define PX4_GPIO_FMU_SERVO_PIN(n)       (n+50)
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4
  # define HAL_GPIO_A_LED_PIN        27
  # define HAL_GPIO_B_LED_PIN        26

libraries/AP_HAL_PX4/RCOutput.cpp

@@ -41,29 +41,29 @@ void PX4RCOutput::init(void* unused)
         return;
     }
 
-    /* if we have 8 servos, then we are attached to PX4IO. In that
-     * case, we want to open the PX4FMU driver for the 4 additional
-     * channels
-     */
-    if (_servo_count <= 4) {
-        return;
-    }
     _alt_fd = open("/dev/px4fmu", O_RDWR);
     if (_alt_fd == -1) {
         hal.console->printf("RCOutput: failed to open /dev/px4fmu");
         return;
     }
+}
+
+
+void PX4RCOutput::_init_alt_channels(void) 
+{
+    if (_alt_fd == -1) {
+        return;
+    }
     if (ioctl(_alt_fd, PWM_SERVO_ARM, 0) != 0) {
         hal.console->printf("RCOutput: Unable to setup alt IO arming\n");
+        return;
     }
-#ifdef PWM_SERVO_SET_ARM_OK
     if (ioctl(_alt_fd, PWM_SERVO_SET_ARM_OK, 0) != 0) {
         hal.console->printf("RCOutput: Unable to setup alt IO arming OK\n");
+        return;
     }
-#endif
     if (ioctl(_alt_fd, PWM_SERVO_GET_COUNT, (unsigned long)&_alt_servo_count) != 0) {
         hal.console->printf("RCOutput: Unable to get servo count\n");        
-        return;
     }
 }
 
@@ -122,7 +122,7 @@ void PX4RCOutput::set_freq(uint32_t chmask, uint16_t freq_hz)
 
 uint16_t PX4RCOutput::get_freq(uint8_t ch) 
 {
-    if (_rate_mask & (1<<ch)) {
+    if (_rate_mask & (1U<<ch)) {
         return _freq_hz;
     }
     return 50;
@@ -130,12 +130,18 @@ uint16_t PX4RCOutput::get_freq(uint8_t ch)
 
 void PX4RCOutput::enable_ch(uint8_t ch)
 {
-    // channels are always enabled ...
+    if (ch >= 8 && !(_enabled_channels & (1U<<ch))) {
+        // this is the first enable of an auxillary channel - setup
+        // aux channels now. This delayed setup makes it possible to
+        // use BRD_PWM_COUNT to setup the number of PWM channels.
+        _init_alt_channels();
+    }
+    _enabled_channels |= (1U<<ch);
 }
 
 void PX4RCOutput::disable_ch(uint8_t ch)
 {
-    // channels are always enabled ...
+    _enabled_channels &= ~(1U<<ch);
 }
 
 void PX4RCOutput::set_safety_pwm(uint32_t chmask, uint16_t period_us)
@@ -159,6 +165,10 @@ void PX4RCOutput::write(uint8_t ch, uint16_t period_us)
     if (ch >= _servo_count + _alt_servo_count) {
         return;
     }
+    if (!(_enabled_channels & (1U<<ch))) {
+        // not enabled
+        return;
+    }
     if (ch >= _max_channel) {
         _max_channel = ch + 1;
     }

libraries/AP_HAL_PX4/RCOutput.h

@@ -35,6 +35,9 @@ private:
     unsigned _servo_count;
     unsigned _alt_servo_count;
     uint32_t _rate_mask;
+    uint16_t _enabled_channels;
+
+    void _init_alt_channels(void);
 };
 
 #endif // __AP_HAL_PX4_RCOUTPUT_H__