PPMout.cpp

00001 #include "PPMout.h"
00002 
00003 void PPMout::CMT_IRQHandler() {
00004          
00005      uint8_t tmp = CMT_MSC;  // reset interrupt
00006      counter = (counter + 1)%ARRAY_SIZE;
00007      CMT_CMD1 = (count_array[counter] >> 8) & 0xFF;
00008      CMT_CMD2 = count_array[counter] & 0xFF;
00009 }
00010 
00011 PPMout::PPMout() {
00012     
00013     // Fill the count array with initial values
00014     for (int i = 0; i < CHANNELS; i++) {
00015 
00016         count_array[i] = (int) (RANGE_STEPS*0.5 + ZERO_STEPS);
00017     }
00018 
00019     set_sync_length();
00020     init();
00021 }
00022 
00023 void PPMout::update_channel(int channel, float value) {
00024     
00025     // Constrain value between 0 and 1
00026     value = PIN(value, 0, 1);
00027 
00028     // Constrain channel
00029     channel = PIN(channel, 0, (CHANNELS-1));
00030     
00031     // Correction
00032     channel = (channel*2 + 1)%CHANNELS;
00033 
00034     // Calculate new step value
00035     int steps = (int) (RANGE_STEPS*value + ZERO_STEPS);
00036 
00037     // Insert new value into array
00038     count_array[channel] = steps;
00039 
00040     set_sync_length();
00041 }
00042     
00043 void PPMout::init() {
00044     
00045     SIM_SCGC4 |= (1 << SIM_SCGC4_CMT_SHIFT);
00046     SIM_SOPT2 |= (1 << SIM_SOPT2_PTD7PAD_SHIFT);   // drive strength
00047     SIM_SCGC5 |= (1 << SIM_SCGC5_PORTD_SHIFT);  // power PORTD
00048     PORTD_PCR7 = PORT_PCR_MUX(2)|(1<< PORT_PCR_DSE_SHIFT)|(1 << PORT_PCR_SRE_SHIFT);
00049     CMT_PPS = CMT_PPS_VAL;    // 5 mhz
00050     CMT_CGH1 = 1;
00051     CMT_CGL1 = 1;
00052     CMT_CMD1 = (count_array[counter] >> 8) & 0xFF;
00053     CMT_CMD2 = count_array[counter] & 0xFF;
00054     CMT_CMD3 = (GAP_STEPS >> 8) & 0xFF;
00055     CMT_CMD4 = GAP_STEPS & 0xFF;
00056     CMT_OC = 0x60;     // enable IRO
00057     CMT_MSC = (1 << 3) | (1 << 1) | (1 << 0);     // enable CMT, interrupt and baseband
00058     
00059     NVIC_SetVector(CMT_IRQn, (uint32_t) &PPMout::CMT_IRQHandler);
00060     NVIC_EnableIRQ(CMT_IRQn);
00061 }
00062 
00063 void PPMout::set_sync_length() {
00064 
00065     int total = 0;
00066 
00067     for (int i = 0; i < CHANNELS; i++) {
00068 
00069         total += count_array[i];
00070     }
00071 
00072     // Update sync length to ensure it lasts 20ms
00073     count_array[CHANNELS] = MAX_NON_GAP_STEPS - total;
00074 }
00075 
00076 int PPMout::count_array[ARRAY_SIZE] = {};
00077 int PPMout::counter = 0;