AP_HAL: add toggle to GPIO

libraries/AP_HAL/GPIO.h

@@ -18,6 +18,7 @@ public:
     virtual void    mode(uint8_t output) = 0;
     virtual uint8_t read() = 0;
     virtual void    write(uint8_t value) = 0;
+    virtual void    toggle() = 0;
 };
 
 class AP_HAL::GPIO {
@@ -27,6 +28,7 @@ public:
     virtual void    pinMode(uint8_t pin, uint8_t output) = 0;
     virtual uint8_t read(uint8_t pin) = 0;
     virtual void    write(uint8_t pin, uint8_t value) = 0;
+    virtual void    toggle(uint8_t pin) = 0;
     virtual int8_t  analogPinToDigitalPin(uint8_t pin) = 0;
 
     /* Alternative interface: */

libraries/AP_HAL_AVR/GPIO.cpp

@@ -97,6 +97,23 @@ void AVRGPIO::write(uint8_t pin, uint8_t value) {
     SREG = oldSREG;
 }
 
+void AVRGPIO::toggle(uint8_t pin) {
+    uint8_t bit = digitalPinToBitMask(pin);
+    uint8_t port = digitalPinToPort(pin);
+    volatile uint8_t *out;
+
+    if (port == NOT_A_PIN) return;
+
+    out = portOutputRegister(port);
+
+    uint8_t oldSREG = SREG;
+    cli();
+
+    *out ^= bit;
+
+    SREG = oldSREG;
+}
+
 /* Implement GPIO Interrupt 6, used for MPU6000 data ready on APM2. */
 bool AVRGPIO::attach_interrupt(
         uint8_t interrupt_num, AP_HAL::Proc proc, uint8_t mode) {
@@ -171,5 +188,19 @@ void AVRDigitalSource::write(uint8_t value) {
     SREG = oldSREG;
 }
 
+void AVRDigitalSource::toggle() {
+    const uint8_t bit = _bit;
+    const uint8_t port = _port;
+    volatile uint8_t* out;
+    out = portOutputRegister(port);
+
+    uint8_t oldSREG = SREG;
+    cli();
+
+    *out ^= bit;
+
+    SREG = oldSREG;
+}
+
 
 #endif

libraries/AP_HAL_AVR/GPIO.h

@@ -5,12 +5,27 @@
 #include <AP_HAL.h>
 #include "AP_HAL_AVR_Namespace.h"
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_APM1
+ # define HAL_GPIO_A_LED_PIN        37
+ # define HAL_GPIO_B_LED_PIN        36
+ # define HAL_GPIO_C_LED_PIN        35
+ # define HAL_GPIO_LED_ON           HIGH
+ # define HAL_GPIO_LED_OFF          LOW
+#elif CONFIG_HAL_BOARD == HAL_BOARD_APM2
+ # define HAL_GPIO_A_LED_PIN        27
+ # define HAL_GPIO_B_LED_PIN        26
+ # define HAL_GPIO_C_LED_PIN        25
+ # define HAL_GPIO_LED_ON           LOW
+ # define HAL_GPIO_LED_OFF          HIGH
+#endif
+
 class AP_HAL_AVR::AVRDigitalSource : public AP_HAL::DigitalSource {
 public:
   AVRDigitalSource(uint8_t bit, uint8_t port) : _bit(bit), _port(port) {}
   void    mode(uint8_t output);
   uint8_t read();
   void    write(uint8_t value);
+  void    toggle();
 
 private:
   const uint8_t _bit;
@@ -25,6 +40,7 @@ public:
     int8_t  analogPinToDigitalPin(uint8_t pin);
     uint8_t read(uint8_t pin);
     void    write(uint8_t pin, uint8_t value);
+    void    toggle(uint8_t pin);
     AP_HAL::DigitalSource* channel(uint16_t);
     bool    attach_interrupt(uint8_t interrupt_num, AP_HAL::Proc proc,
             uint8_t mode);

libraries/AP_HAL_Empty/GPIO.cpp

@@ -25,6 +25,9 @@ uint8_t EmptyGPIO::read(uint8_t pin) {
 void EmptyGPIO::write(uint8_t pin, uint8_t value)
 {}
 
+void EmptyGPIO::toggle(uint8_t pin)
+{}
+
 /* Alternative interface: */
 AP_HAL::DigitalSource* EmptyGPIO::channel(uint16_t n) {
     return new EmptyDigitalSource(0);
@@ -52,3 +55,6 @@ void EmptyDigitalSource::write(uint8_t value) {
     _v = value;
 }
 
+void EmptyDigitalSource::toggle() {
+    _v = !_v;
+}

libraries/AP_HAL_Empty/GPIO.h

@@ -12,6 +12,7 @@ public:
     int8_t  analogPinToDigitalPin(uint8_t pin);
     uint8_t read(uint8_t pin);
     void    write(uint8_t pin, uint8_t value);
+    void    toggle(uint8_t pin);
 
     /* Alternative interface: */
     AP_HAL::DigitalSource* channel(uint16_t n);
@@ -28,6 +29,7 @@ public:
     void    mode(uint8_t output);
     uint8_t read();
     void    write(uint8_t value); 
+    void    toggle();
 private:
     uint8_t _v;
 };

libraries/AP_HAL_PX4/GPIO.cpp

@@ -120,7 +120,7 @@ void PX4GPIO::write(uint8_t pin, uint8_t value)
     switch (pin) {
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
-        case A_LED_PIN:    // Arming LED
+        case HAL_GPIO_A_LED_PIN:    // Arming LED
             if (value == LOW) {
                 ioctl(_led_fd, LED_OFF, LED_RED);
             } else {
@@ -128,10 +128,10 @@ void PX4GPIO::write(uint8_t pin, uint8_t value)
             }
             break;
 
-        case B_LED_PIN:    // not used yet 
+        case HAL_GPIO_B_LED_PIN:    // not used yet 
             break;
 
-        case C_LED_PIN:    // GPS LED 
+        case HAL_GPIO_C_LED_PIN:    // GPS LED 
             if (value == LOW) { 
                 ioctl(_led_fd, LED_OFF, LED_BLUE);
             } else { 
@@ -187,6 +187,35 @@ void PX4GPIO::write(uint8_t pin, uint8_t value)
     }
 }
 
+void PX4GPIO::toggle(uint8_t pin)
+{
+    switch (pin) {
+
+        case HAL_GPIO_A_LED_PIN:    // Arming LED
+            ioctl(_led_fd, LED_OFF, LED_RED);
+            ioctl(_led_fd, LED_ON, LED_RED);
+            break;
+
+        case HAL_GPIO_B_LED_PIN:    // not used yet 
+            break;
+
+        case HAL_GPIO_C_LED_PIN:    // GPS LED 
+            ioctl(_led_fd, LED_OFF, LED_BLUE);
+            ioctl(_led_fd, LED_ON, LED_BLUE);
+            break;
+
+        case PX4_GPIO_PIEZO_PIN:    // Piezo beeper 
+            ioctl(_tone_alarm_fd, TONE_SET_ALARM, 3);    // Alarm on !! 
+            ioctl(_tone_alarm_fd, TONE_SET_ALARM, 0);    // Alarm off !! 
+            break;
+
+        case PX4_GPIO_EXT_RELAY_PIN: // Ext Relay 
+            ioctl(_gpio_fd, GPIO_CLEAR, GPIO_EXT_1);
+            ioctl(_gpio_fd, GPIO_SET, GPIO_EXT_1);
+            break;
+    }
+}
+
 /* Alternative interface: */
 AP_HAL::DigitalSource* PX4GPIO::channel(uint16_t n) {
     return new PX4DigitalSource(0);
@@ -214,4 +243,8 @@ void PX4DigitalSource::write(uint8_t value) {
     _v = value;
 }
 
+void PX4DigitalSource::toggle() {
+    _v = !_v;
+}
+
 #endif // CONFIG_HAL_BOARD

libraries/AP_HAL_PX4/GPIO.h

@@ -13,6 +13,14 @@
 #define PX4_GPIO_EXT_IO_ACC1_PIN        115
 #define PX4_GPIO_EXT_IO_ACC2_PIN        116
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_PX4
+ # define HAL_GPIO_A_LED_PIN        27
+ # define HAL_GPIO_B_LED_PIN        26
+ # define HAL_GPIO_C_LED_PIN        25
+ # define HAL_GPIO_LED_ON           LOW
+ # define HAL_GPIO_LED_OFF          HIGH
+#endif
+
 class PX4::PX4GPIO : public AP_HAL::GPIO {
 public:
     PX4GPIO();
@@ -21,6 +29,7 @@ public:
     int8_t  analogPinToDigitalPin(uint8_t pin);
     uint8_t read(uint8_t pin);
     void    write(uint8_t pin, uint8_t value);
+    void    toggle(uint8_t pin);
 
     /* Alternative interface: */
     AP_HAL::DigitalSource* channel(uint16_t n);
@@ -41,6 +50,7 @@ public:
     void    mode(uint8_t output);
     uint8_t read();
     void    write(uint8_t value);
+    void    toggle();
 private:
     uint8_t _v;
 };

libraries/AP_HAL_SMACCM/GPIO.cpp

@@ -29,6 +29,10 @@ void SMACCMGPIO::write(uint8_t pin, uint8_t value)
 {
 }
 
+void SMACCMGPIO::toggle(uint8_t pin)
+{
+}
+
 /* Alternative interface: */
 AP_HAL::DigitalSource* SMACCMGPIO::channel(uint16_t n)
 {
@@ -60,3 +64,8 @@ void SMACCMDigitalSource::write(uint8_t value)
 {
   _v = value;
 }
+
+void SMACCMDigitalSource::toggle()
+{
+  _v = !_v;
+}

libraries/AP_HAL_SMACCM/GPIO.h

@@ -4,6 +4,15 @@
 
 #include <AP_HAL_SMACCM.h>
 
+#if CONFIG_HAL_BOARD == HAL_BOARD_SMACCM
+// XXX these are just copied, may not make sense
+ # define HAL_GPIO_A_LED_PIN        27
+ # define HAL_GPIO_B_LED_PIN        26
+ # define HAL_GPIO_C_LED_PIN        25
+ # define HAL_GPIO_LED_ON           LOW
+ # define HAL_GPIO_LED_OFF          HIGH
+#endif
+
 class SMACCM::SMACCMGPIO : public AP_HAL::GPIO {
 public:
     SMACCMGPIO();
@@ -12,6 +21,7 @@ public:
     int8_t  analogPinToDigitalPin(uint8_t pin);
     uint8_t read(uint8_t pin);
     void    write(uint8_t pin, uint8_t value);
+    void    toggle(uint8_t pin);
 
     /* Alternative interface: */
     AP_HAL::DigitalSource* channel(uint16_t n);
@@ -28,6 +38,7 @@ public:
     void    mode(uint8_t output);
     uint8_t read();
     void    write(uint8_t value);
+    void    toggle();
 private:
     uint8_t _v;
 };