Diff: targets/hal/TARGET_Freescale/TARGET_KL25Z/pwmout_api.c

Revision:

72:248c61396e08

Parent:

20:4263a77256ae

Child:

74:847f030b50ee

--- a/targets/hal/TARGET_Freescale/TARGET_KL25Z/pwmout_api.c	Thu Jan 09 09:00:05 2014 +0000
+++ b/targets/hal/TARGET_Freescale/TARGET_KL25Z/pwmout_api.c	Mon Jan 13 10:45:05 2014 +0000
@@ -64,14 +64,25 @@
     {NC , NC    , 0}
 };
 
-#define PWM_CLOCK_MHZ       (0.75) // (48)MHz / 64 = (0.75)MHz
+static float pwm_clock;
 
 void pwmout_init(pwmout_t* obj, PinName pin) {
     // determine the channel
     PWMName pwm = (PWMName)pinmap_peripheral(pin, PinMap_PWM);
     if (pwm == (PWMName)NC)
         error("PwmOut pin mapping failed");
-
+    
+    uint32_t clkdiv = 0;
+    float clkval = SystemCoreClock / 1000000.0f;
+    
+    while (clkval > 1) {
+        clkdiv++;
+        clkval /= 2.0;  
+        if (clkdiv == 7)
+            break;
+    }
+    
+    pwm_clock = clkval;
     unsigned int port = (unsigned int)pin >> PORT_SHIFT;
     unsigned int tpm_n = (pwm >> TPM_SHIFT);
     unsigned int ch_n = (pwm & 0xFF);
@@ -81,7 +92,7 @@
     SIM->SOPT2 |= SIM_SOPT2_TPMSRC(1); // Clock source: MCGFLLCLK or MCGPLLCLK
 
     TPM_Type *tpm = (TPM_Type *)(TPM0_BASE + 0x1000 * tpm_n);
-    tpm->SC = TPM_SC_CMOD(1) | TPM_SC_PS(6); // (48)MHz / 64 = (0.75)MHz
+    tpm->SC = TPM_SC_CMOD(1) | TPM_SC_PS(clkdiv); // (clock)MHz / clkdiv ~= (0.75)MHz
     tpm->CONTROLS[ch_n].CnSC = (TPM_CnSC_MSB_MASK | TPM_CnSC_ELSB_MASK); /* No Interrupts; High True pulses on Edge Aligned PWM */
 
     obj->CnV = &tpm->CONTROLS[ch_n].CnV;
@@ -125,7 +136,7 @@
 // Set the PWM period, keeping the duty cycle the same.
 void pwmout_period_us(pwmout_t* obj, int us) {
     float dc = pwmout_read(obj);
-    *obj->MOD = PWM_CLOCK_MHZ * us;
+    *obj->MOD = (uint32_t)(pwm_clock * (float)us);
     pwmout_write(obj, dc);
 }
 
@@ -138,5 +149,5 @@
 }
 
 void pwmout_pulsewidth_us(pwmout_t* obj, int us) {
-    *obj->CnV = PWM_CLOCK_MHZ * us;
+    *obj->CnV = (uint32_t)(pwm_clock * (float)us);
 }