File content as of revision 23:0927e605af4b:

#include "PWM.h"
#include "HomePosition.h"

//static menber variable
volatile uint8_t PWM::SRV_Idx = 0;
volatile uint32_t PWM::SRV_dutyTable[SRV_BANK_NUM][SRV_IDX_NUM];

PWM::PWM()
{
    for (int i = 0; i < SRV_CH_NUM; i++) {
        setDuty(i, HOMEPOS[i]);
    }
    InitPWM();
}

void PWM::InitPWM()
{
    LPC_PINCON->PINSEL3 &= ~(3 << 4); // GPIO (00)
    LPC_GPIO1->FIODIR |= (1 << 18);  // output
    
    LPC_PINCON->PINSEL4 |= 0x555; // GPIO (00)
    LPC_GPIO2->FIODIR |= 0x3F;  // output

    LPC_SC->PCLKSEL0 &= ~(3 << 12); // PCLK_TIMER0 ck/4 (00)
    LPC_SC->PCONP |= (1 << 6); // PCPWM1
    
    LPC_PWM1->IR |= (1<<0);//reset PWMMR0 flag
    LPC_PWM1->TCR = (1 << 1);//reset
    LPC_PWM1->PR = SystemCoreClock / 4 / 1000000 - 1; // prescale 1000kHz
    LPC_PWM1->CTCR = 0;
    LPC_PWM1->MCR |= (1 << 1)|(1 << 0); // MR0R, MR0

    LPC_PWM1->MR0 = SRV_PERIOD;

    LPC_PWM1->TCR = (1 << 0)|(1 << 3); // enable
    LPC_PWM1->PCR |= (0x3F << 9);//1-6 pwm
    LPC_PWM1->LER |= 0x7E;
    
    NVIC_SetPriority(PWM1_IRQn,0);//priority level (high)0~255(low)
    NVIC_EnableIRQ(PWM1_IRQn);//enable
}

void PWM::setDuty(uint8_t ch, uint32_t duty)
{
    if (ch >= SRV_CH_NUM) {
        return;
    } else if (duty < SRV_MIN_DUTY) {
        duty = SRV_MIN_DUTY;
    } else if (duty > SRV_MAX_DUTY) {
        duty = SRV_MAX_DUTY;
    }
    
    SRV_dutyTable[ch >> SRV_IDX_SHIFT][ch & SRV_IDX_MASK] = duty;
}

void PWM1_IRQHandler (void)
{ 
    LPC_PWM1->MR1 = SRV_PWMTable[0][PWM::SRV_Idx];
    LPC_PWM1->MR2 = SRV_PWMTable[1][PWM::SRV_Idx];
    LPC_PWM1->MR3 = SRV_PWMTable[2][PWM::SRV_Idx];
    LPC_PWM1->MR4 = PWM::SRV_dutyTable[0][PWM::SRV_Idx];
    LPC_PWM1->MR5 = PWM::SRV_dutyTable[1][PWM::SRV_Idx];
    LPC_PWM1->MR6 = PWM::SRV_dutyTable[2][PWM::SRV_Idx];
    
    LPC_PWM1->LER |= 0x7E;
    
    LPC_PWM1->IR |= (1<<0); // reset PWMMR0 flag
    
    PWM::SRV_Idx++;

    if(PWM::SRV_Idx >= SRV_IDX_NUM) {
        PWM::SRV_Idx = 0;
    }
}