1 files changed, 53 insertions, 6 deletions
diff --git a/testhal/AVR/MEGA/PWM/main.c b/testhal/AVR/MEGA/PWM/main.c
index 29c97d96a..41a539b90 100644
--- a/testhal/AVR/MEGA/PWM/main.c
+++ b/testhal/AVR/MEGA/PWM/main.c
@@ -22,6 +22,9 @@
 #include "ch.h"
 #endif
 
+#define PERIOD_VALUE1 0x7FFF
+#define PERIOD_VALUE2 0xFF
+
 #ifdef _NIL_
 THD_WORKING_AREA(waThread1, 128);
 THD_FUNCTION(Thread1, arg) {
@@ -45,9 +48,53 @@ int main(void) {
    * more can be done in this thread so we first initialize PWM subsystem.
    */
 
+#if AVR_PWM_USE_TIM1 && PWM_CHANNELS == 2
+  static PWMConfig pwm1cfg = {
+    F_CPU,
+    PERIOD_VALUE1,
+    NULL,
+    {
+      {PWM_OUTPUT_ACTIVE_HIGH, NULL},
+      {PWM_OUTPUT_ACTIVE_HIGH, NULL}
+    },
+  };
+
+  /* PB1-2 are timer 1 pwm channel outputs for ATMega328p */
+  palSetGroupMode(IOPORT2, 0x3, 1, PAL_MODE_OUTPUT_PUSHPULL);
+
+  pwmStart(&PWMD1, &pwm1cfg);
+
+  /* channel 0 with 75% duty cycle and 1 with 25% */
+  pwmEnableChannel(&PWMD1, 0, (PERIOD_VALUE1 >> 2)*3);
+  pwmEnableChannel(&PWMD1, 1, PERIOD_VALUE1 >> 2);
+#endif
+
+#if AVR_PWM_USE_TIM2
+  static PWMConfig pwm2cfg = {
+    F_CPU >> 5,
+    PERIOD_VALUE2,
+    NULL,
+    {
+      {PWM_OUTPUT_ACTIVE_HIGH, NULL},
+      {PWM_OUTPUT_ACTIVE_HIGH, NULL}
+    },
+  };
+
+  /* PB3 and PD3 are timer 2 pwm channel outputs for ATMega328p */
+  palSetPadMode(IOPORT2, 3, PAL_MODE_OUTPUT_PUSHPULL);
+  palSetPadMode(IOPORT4, 3, PAL_MODE_OUTPUT_PUSHPULL);
+
+  pwmStart(&PWMD2, &pwm2cfg);
+
+  /* channel 0 with 80% duty cycle and 1 with 20% */
+  pwmEnableChannel(&PWMD2, 0, PERIOD_VALUE2/5*4);
+  pwmEnableChannel(&PWMD2, 1, PERIOD_VALUE2/5);
+#endif
+
+#if AVR_PWM_USE_TIM3
   static PWMConfig pwm3cfg = {
-    1023,   /* Not real clock */
-    1023,   /* Maximum PWM count */
+    F_CPU,
+    PERIOD_VALUE1,
     NULL,
     {
       {PWM_OUTPUT_ACTIVE_HIGH, NULL},
@@ -62,11 +109,11 @@ int main(void) {
   palSetPadMode(IOPORT5, 5, PAL_MODE_OUTPUT_PUSHPULL);
 
   pwmStart(&PWMD3, &pwm3cfg);
-
   /* channel 0 with 50% duty cycle, 1 with 25% and 2 with 75% */
-  pwmEnableChannel(&PWMD3, 0, 511);
-  pwmEnableChannel(&PWMD3, 1, 255);
-  pwmEnableChannel(&PWMD3, 2, 767);
+  pwmEnableChannel(&PWMD3, 0, PERIOD_VALUE1 >> 1);
+  pwmEnableChannel(&PWMD3, 1, PERIOD_VALUE1 >> 2);
+  pwmEnableChannel(&PWMD3, 2, (PERIOD_VALUE1 >> 2)*3);
+#endif
 
   chSysInit();