add support for more pwm output channels

5 files changed, 54 insertions, 13 deletions

diff --git a/ROMFS/px4fmu_trifle/init.d/rcS b/ROMFS/px4fmu_trifle/init.d/rcS
index 8295875d9..e0f3676d9 100644
--- a/ROMFS/px4fmu_trifle/init.d/rcS
+++ b/ROMFS/px4fmu_trifle/init.d/rcS
@@ -1,7 +1,13 @@
 #!nsh
 # empty
 
-pwm 2000
-sleep 3
-pwm 1000
+pwm 0 2000
+pwm 1 2000
+pwm 2 2000
+pwm 3 2000
 sleep 3
+pwm 0 1000
+pwm 1 1000
+pwm 2 1000
+pwm 3 1000
+sleep 2
diff --git a/src/drivers/boards/trifle/devconfig.c b/src/drivers/boards/trifle/devconfig.c
index 47b69a27e..1fec4acd6 100644
--- a/src/drivers/boards/trifle/devconfig.c
+++ b/src/drivers/boards/trifle/devconfig.c
@@ -21,7 +21,32 @@ __EXPORT const struct  pwm_servo_timer pwm_servo_timers[PWM_SERVO_MAX_TIMERS] =
 __EXPORT const struct pwm_servo_channel pwm_servo_channels[PWM_SERVO_MAX_CHANNELS] = {
   {
     .timer = &pwm_servo_timers[0],
+    .ftm_channel = 1,
+    .pinconfig = PIN_PORTC | PIN2 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+  },
+  {
+    .timer = &pwm_servo_timers[0],
+    .ftm_channel = 0,
+    .pinconfig = PIN_PORTC | PIN1 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+  },
+  {
+    .timer = &pwm_servo_timers[0],
+    .ftm_channel = 6,
+    .pinconfig = PIN_PORTD | PIN6 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+  },
+  {
+    .timer = &pwm_servo_timers[0],
     .ftm_channel = 5,
-    .pinconfig = PIN_PORTD | PIN5 |  PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+    .pinconfig = PIN_PORTD | PIN5 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+  },
+    {
+    .timer = &pwm_servo_timers[0],
+    .ftm_channel = 2,
+    .pinconfig = PIN_PORTC | PIN3 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
+  },
+  {
+    .timer = &pwm_servo_timers[0],
+    .ftm_channel = 3,
+    .pinconfig = PIN_PORTC | PIN4 | PIN_ALT4_OUTPUT | PIN_ALT4_HIGHDRIVE
   }
 };
diff --git a/src/drivers/kinetis/drv_pwm_servo.c b/src/drivers/kinetis/drv_pwm_servo.c
index 7e7756983..e64a82764 100644
--- a/src/drivers/kinetis/drv_pwm_servo.c
+++ b/src/drivers/kinetis/drv_pwm_servo.c
@@ -81,7 +81,6 @@
        // configure pin
        kinetis_pinconfig(channel->pinconfig); 
      }
-
       
    }
 
@@ -167,6 +166,10 @@ up_pwm_servo_get_rate_group(unsigned group)
 int
 up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
 {
+
+  if (group > PWM_SERVO_MAX_TIMERS) {
+    return -ERANGE;
+  }
   
   /*
    * ftm rate = bus frequency / prescaler
@@ -184,10 +187,6 @@ up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
   }
     
   const struct pwm_servo_timer* timer = &pwm_servo_timers[group];
-
-  //  uint32_t regval = getreg32(timer->ftm_base + KINETIS_FTM_MODE_OFFSET);
-  //putreg32(regval | FTM_MODE_WPDIS, timer->ftm_base + KINETIS_FTM_MODE_OFFSET);
-
   
   uint32_t regval = getreg32(timer->ftm_base + KINETIS_FTM_SC_OFFSET);
   regval = regval & ~FTM_SC_PS_MASK;
@@ -211,6 +210,10 @@ up_pwm_servo_set_rate_group_update(unsigned group, unsigned rate)
 int
 up_pwm_servo_set(unsigned channel, servo_position_t value)
 {
+  if (channel > PWM_SERVO_MAX_CHANNELS) {
+    return -ERANGE;
+  }
+  
   const struct pwm_servo_channel* ch = &pwm_servo_channels[channel];
 
   putreg32(
@@ -229,6 +232,10 @@ up_pwm_servo_set(unsigned channel, servo_position_t value)
 servo_position_t
 up_pwm_servo_get(unsigned channel)
 {
+  if (channel > PWM_SERVO_MAX_CHANNELS) {
+    return -ERANGE;
+  }
+  
   const struct pwm_servo_channel* ch = &pwm_servo_channels[channel];
   uint32_t reg = ch->timer->ftm_base + KINETIS_FTM_CV_OFFSET(ch->ftm_channel);
   uint32_t mod = getreg32(reg);
diff --git a/src/drivers/kinetis/drv_pwm_servo.h b/src/drivers/kinetis/drv_pwm_servo.h
index 160b7aad7..d5b4afc3d 100644
--- a/src/drivers/kinetis/drv_pwm_servo.h
+++ b/src/drivers/kinetis/drv_pwm_servo.h
@@ -40,7 +40,7 @@
 #define KINETIS_PWM_SERVO_H
 
 #define PWM_SERVO_MAX_TIMERS 1
-#define PWM_SERVO_MAX_CHANNELS 1
+#define PWM_SERVO_MAX_CHANNELS 6
 
 #include <stdbool.h>
 #include <drivers/drv_pwm_output.h>
diff --git a/src/examples/pwm/pwm.c b/src/examples/pwm/pwm.c
index a40da17e7..a67e24ba6 100644
--- a/src/examples/pwm/pwm.c
+++ b/src/examples/pwm/pwm.c
@@ -9,10 +9,13 @@
 __EXPORT int pwm_main(int argc, char *argv[]);
 
 int pwm_main(int args, char *argv[]) {
-  printf("set pwm0 to %d us\n", atoi(argv[1]));
+  uint8_t channel = (uint8_t) atoi(argv[1]);
+  uint16_t us = (uint16_t) atoi(argv[2]);
+  
+  printf("set pwm channel %d to %d us\n", channel, us);
 
-  up_pwm_servo_init(1);
+  up_pwm_servo_init(1 << channel);
   up_pwm_servo_arm(true);
-  up_pwm_servo_set(0, atoi(argv[1]));
+  up_pwm_servo_set(channel, us);
   return 0;
 }