From f23746053dc126abd2ce17ee32e4ebab96468943 Mon Sep 17 00:00:00 2001
From: Siddharth Purohit <siddharthbharatpurohit@gmail.com>
Date: Mon, 6 May 2019 16:09:47 +0530
Subject: [PATCH] AP_IOMCU_FW: autodetect active high/low on heater control pin

---
 libraries/AP_IOMCU/iofirmware/iofirmware.cpp | 14 ++++++++++++--
 libraries/AP_IOMCU/iofirmware/iofirmware.h   |  3 ++-
 2 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
index 85c33c84ba..b28b0fc277 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.cpp
@@ -171,6 +171,15 @@ void AP_IOMCU_FW::init()
         has_heater = true;
     }
 
+    //Set Heater PWM Polarity, 0 for Active Low and 1 for Active High
+    heater_pwm_polarity = !palReadLine(HAL_GPIO_PIN_HEATER);
+    //Set Heater pin mode
+    if (heater_pwm_polarity) {
+        palSetLineMode(HAL_GPIO_PIN_HEATER, PAL_MODE_OUTPUT_PUSHPULL);
+    } else {
+        palSetLineMode(HAL_GPIO_PIN_HEATER, PAL_MODE_OUTPUT_OPENDRAIN);
+    }
+
     adc_init();
     rcin_serial_init();
 
@@ -284,10 +293,11 @@ void AP_IOMCU_FW::heater_update()
         }
     } else if (reg_setup.heater_duty_cycle == 0 || (now - last_heater_ms > 3000UL)) {
         // turn off the heater
-        HEATER_SET(0);
+        HEATER_SET(!heater_pwm_polarity);
     } else {
         uint8_t cycle = ((now / 10UL) % 100U);
-        HEATER_SET(!(cycle >= reg_setup.heater_duty_cycle));
+        //Turn off heater when cycle is greater than specified duty cycle
+        HEATER_SET((cycle >= reg_setup.heater_duty_cycle) ? !heater_pwm_polarity : heater_pwm_polarity);
     }
 }
 
diff --git a/libraries/AP_IOMCU/iofirmware/iofirmware.h b/libraries/AP_IOMCU/iofirmware/iofirmware.h
index 43850e298b..febd730330 100644
--- a/libraries/AP_IOMCU/iofirmware/iofirmware.h
+++ b/libraries/AP_IOMCU/iofirmware/iofirmware.h
@@ -126,6 +126,7 @@ private:
     bool update_default_rate;
     bool update_rcout_freq;
     bool has_heater;
+    bool heater_pwm_polarity;
     uint32_t last_blue_led_ms;
     uint32_t safety_update_ms;
     uint32_t safety_button_counter;
@@ -144,7 +145,7 @@ private:
 };
 
 // GPIO macros
-#define HEATER_SET(on) palWriteLine(HAL_GPIO_PIN_HEATER, !(on));
+#define HEATER_SET(on) palWriteLine(HAL_GPIO_PIN_HEATER, (on));
 #define BLUE_TOGGLE() palToggleLine(HAL_GPIO_PIN_HEATER);
 #define AMBER_SET(on) palWriteLine(HAL_GPIO_PIN_AMBER_LED, !(on));
 #define SPEKTRUM_POWER(on) palWriteLine(HAL_GPIO_PIN_SPEKTRUM_PWR_EN, on);