add function flexPwmInvertPolarity to pwm.c for easy polarity inversi…

…on of pwm output

teensy3/core_pins.h

@@ -33,6 +33,7 @@
 
 #include "kinetis.h"
 #include "pins_arduino.h"
+#include <stdbool.h>
 
 #define HIGH		1
 #define LOW		0
@@ -2253,6 +2254,7 @@ static inline uint8_t digitalReadFast(uint8_t pin)
 void pinMode(uint8_t pin, uint8_t mode);
 void init_pins(void);
 void analogWrite(uint8_t pin, int val);
+void flexPwmInvertPolarity(uint8_t pin, bool inversePolarity);
 uint32_t analogWriteRes(uint32_t bits);
 static inline uint32_t analogWriteResolution(uint32_t bits) { return analogWriteRes(bits); }
 void analogWriteFrequency(uint8_t pin, float frequency);

teensy4/pwm.c

@@ -1,6 +1,7 @@
 #include "imxrt.h"
 #include "core_pins.h"
 #include "debug/printf.h"
+#include <stdbool.h>
 
 
 struct pwm_pin_info_struct {
@@ -151,6 +152,53 @@ void flexpwmWrite(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channel, u
 	p->MCTRL |= FLEXPWM_MCTRL_LDOK(mask);
 }
 
+void flexPwmInvertPolarity(uint8_t pin, bool inversePolarity)
+{
+	const struct pwm_pin_info_struct *info;
+
+	if (pin >= CORE_NUM_DIGITAL) return;
+	info = pwm_pin_info + pin;
+
+	//return if not a FlexPWM pin
+	if (info->type != 1) return;
+
+	// FlexPWM pin
+	IMXRT_FLEXPWM_t *flexpwm;
+	switch ((info->module >> 4) & 3) {
+		case 0: flexpwm = &IMXRT_FLEXPWM1; break;
+		case 1: flexpwm = &IMXRT_FLEXPWM2; break;
+		case 2: flexpwm = &IMXRT_FLEXPWM3; break;
+		default: flexpwm = &IMXRT_FLEXPWM4;
+	}
+
+	unsigned int submodule = info->module & 0x03;
+	uint8_t channel = info->channel;
+	uint8_t polarityShift = 0;
+
+	//find out offset for the channel
+	//TODO: move magic numbers to declarations 
+	switch (channel) {
+	  case 0: // X
+	  	polarityShift              = 8U;  //PWM_OCTRL_POLX_SHIFT
+		break;
+	  case 1: // A
+		polarityShift              = 10U; //PWM_OCTRL_POLA_SHIFT
+		break;
+	  case 2: // B
+	    polarityShift              = 9U;  //PWM_OCTRL_POLB_SHIFT
+	}
+
+	//if polarityShift was not initialized skip
+	if(!polarityShift) return;
+
+	//update polarity
+	if(inversePolarity) {
+		flexpwm->SM[submodule].OCTRL |= ((uint16_t)1U << (uint16_t)polarityShift);
+	} else {
+		flexpwm->SM[submodule].OCTRL &= ~((uint16_t)1U << (uint16_t)polarityShift);
+	}
+}
+
 void flexpwmFrequency(IMXRT_FLEXPWM_t *p, unsigned int submodule, uint8_t channel, float frequency)
 {
 	uint16_t mask = 1 << submodule;