diff --git a/os/hal/ports/STM32/LLD/GPIOv2/pal_lld.c b/os/hal/ports/STM32/LLD/GPIOv2/pal_lld.c
index 10fc1a72b..b4545f094 100644
--- a/os/hal/ports/STM32/LLD/GPIOv2/pal_lld.c
+++ b/os/hal/ports/STM32/LLD/GPIOv2/pal_lld.c
@@ -171,17 +171,30 @@ void _pal_lld_setgroupmode(ioportid_t port,
       uint32_t altrmask, m1, m2, m4;
 
       altrmask = altr << ((bit & 7) * 4);
-      m4 = 15 << ((bit & 7) * 4);
-      if (bit < 8)
-        port->AFRL = (port->AFRL & ~m4) | altrmask;
-      else
-        port->AFRH = (port->AFRH & ~m4) | altrmask;
       m1 = 1 << bit;
-      port->OTYPER  = (port->OTYPER & ~m1) | otyper;
       m2 = 3 << (bit * 2);
+      m4 = 15 << ((bit & 7) * 4);
+      port->OTYPER  = (port->OTYPER & ~m1) | otyper;
       port->OSPEEDR = (port->OSPEEDR & ~m2) | ospeedr;
       port->PUPDR   = (port->PUPDR & ~m2) | pupdr;
-      port->MODER   = (port->MODER & ~m2) | moder;
+      if (moder == PAL_STM32_MODE_ALTERNATE) {
+        /* If going in alternate mode then the alternate number is set
+           before switching mode in order to avoid glitches.*/
+        if (bit < 8)
+          port->AFRL = (port->AFRL & ~m4) | altrmask;
+        else
+          port->AFRH = (port->AFRH & ~m4) | altrmask;
+        port->MODER   = (port->MODER & ~m2) | moder;
+      }
+      else {
+        /* If going into a non-alternate mode then the mode is switched
+           before setting the alternate mode in order to avoid glitches.*/
+        port->MODER   = (port->MODER & ~m2) | moder;
+        if (bit < 8)
+          port->AFRL = (port->AFRL & ~m4) | altrmask;
+        else
+          port->AFRH = (port->AFRH & ~m4) | altrmask;
+      }
     }
     mask >>= 1;
     if (!mask)
diff --git a/os/hal/ports/STM32/LLD/GPIOv3/pal_lld.c b/os/hal/ports/STM32/LLD/GPIOv3/pal_lld.c
index 5f96bb785..b45f7fe15 100644
--- a/os/hal/ports/STM32/LLD/GPIOv3/pal_lld.c
+++ b/os/hal/ports/STM32/LLD/GPIOv3/pal_lld.c
@@ -156,18 +156,31 @@ void _pal_lld_setgroupmode(ioportid_t port,
       uint32_t altrmask, m1, m2, m4;
 
       altrmask = altr << ((bit & 7) * 4);
-      m4 = 15 << ((bit & 7) * 4);
-      if (bit < 8)
-        port->AFRL = (port->AFRL & ~m4) | altrmask;
-      else
-        port->AFRH = (port->AFRH & ~m4) | altrmask;
       m1 = 1 << bit;
+      m2 = 3 << (bit * 2);
+      m4 = 15 << ((bit & 7) * 4);
       port->OTYPER  = (port->OTYPER & ~m1) | otyper;
       port->ASCR    = (port->ASCR & ~m1) | ascr;
-      m2 = 3 << (bit * 2);
       port->OSPEEDR = (port->OSPEEDR & ~m2) | ospeedr;
       port->PUPDR   = (port->PUPDR & ~m2) | pupdr;
-      port->MODER   = (port->MODER & ~m2) | moder;
+       if (moder == PAL_STM32_MODE_ALTERNATE) {
+        /* If going in alternate mode then the alternate number is set
+           before switching mode in order to avoid glitches.*/
+        if (bit < 8)
+          port->AFRL = (port->AFRL & ~m4) | altrmask;
+        else
+          port->AFRH = (port->AFRH & ~m4) | altrmask;
+        port->MODER   = (port->MODER & ~m2) | moder;
+      }
+      else {
+        /* If going into a non-alternate mode then the mode is switched
+           before setting the alternate mode in order to avoid glitches.*/
+        port->MODER   = (port->MODER & ~m2) | moder;
+        if (bit < 8)
+          port->AFRL = (port->AFRL & ~m4) | altrmask;
+        else
+          port->AFRH = (port->AFRH & ~m4) | altrmask;
+      }
       port->LOCKR   = (port->LOCKR & ~m1) | lockr;
     }
     mask >>= 1;
diff --git a/readme.txt b/readme.txt
index e1b4de613..b9eda38c4 100644
--- a/readme.txt
+++ b/readme.txt
@@ -95,6 +95,8 @@
 - RT:  Merged RT4.
 - NIL: Merged NIL2.
 - NIL: Added STM32F7 demo.
+- VAR: Fixed palSetMode glitching outputs (bug #723)(backported to 3.0.6
+       and 16.1.4).
 - VAR: Fixed error in STM32 PWM driver regarding channels 4 and 5 (bug #722)
        (backported to 3.0.6 and 16.1.4).
 - VAR: Fixed wrong flash and ram size in linker script for maple mini