Penn Electric Racing / Mbed 2 deprecated SystemManagement

System Management code

Dependencies:   mbed CANBuffer Watchdog MODSERIAL mbed-rtos xbeeRelay IAP

Fork of SystemManagement by Martin Deng

Libs/IMD/IMD.cpp@38:8efacce315ae, 2015-02-07 (annotated)

Committer:
pspatel321
Date:
Sat Feb 07 08:54:51 2015 +0000
Revision:
38:8efacce315ae
Parent:
36:0afc0fc8f86b 
Updated with profiles, operating info, etc. Just like the other programs.  Awaiting test in car.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
pspatel321 30:91af74a299e1 1 #include "IMD.h"
pspatel321 30:91af74a299e1 2 #include <math.h>
pspatel321 36:0afc0fc8f86b 3 #include "pinmap.h"
pspatel321 30:91af74a299e1 4
pspatel321 38:8efacce315ae 5
pspatel321 38:8efacce315ae 6 const float ZERO_HZ_TIMEOUT = 0.21; // Time (sec) that must pass without an edge to call it 0 Hz, set to greater than the longest expected pulse width
pspatel321 38:8efacce315ae 7 const float EXTRA = 0.02; // Margin on the IMD PWM limits
pspatel321 30:91af74a299e1 8
pspatel321 38:8efacce315ae 9 const uint32_t PCLK = 24000000; // Timer counting clock = 24Mhz
pspatel321 30:91af74a299e1 10 const uint32_t TIMEOUT_TICKS = PCLK*ZERO_HZ_TIMEOUT; // Zeroing timeout in clock ticks = seconds * PCLK
pspatel321 38:8efacce315ae 11
pspatel321 38:8efacce315ae 12 static IMD* instance[4] = { 0 }; // Access member from static frame, one IMD permitted per timer module (4 total IMD objects)
pspatel321 30:91af74a299e1 13
pspatel321 30:91af74a299e1 14 // Interrupt functions, must be static context
pspatel321 30:91af74a299e1 15 void tim0_IRQ() {
pspatel321 30:91af74a299e1 16 if (LPC_TIM0->IR & (1<<4|1<<5)) instance[0]->edgeIRQ(); // Capture pin interrupt
pspatel321 30:91af74a299e1 17 if (LPC_TIM0->IR & 1) instance[0]->zeroIRQ(); // MR0 interrupt
pspatel321 30:91af74a299e1 18 LPC_TIM0->IR=0x3F; // Clear interrupt flags
pspatel321 30:91af74a299e1 19 }
pspatel321 30:91af74a299e1 20 void tim1_IRQ() {
pspatel321 30:91af74a299e1 21 if (LPC_TIM1->IR & (1<<4|1<<5)) instance[1]->edgeIRQ();
pspatel321 30:91af74a299e1 22 if (LPC_TIM1->IR & 1) instance[1]->zeroIRQ();
pspatel321 30:91af74a299e1 23 LPC_TIM1->IR=0x3F; // Clear interrupt flags
pspatel321 30:91af74a299e1 24 }
pspatel321 30:91af74a299e1 25 void tim2_IRQ() {
pspatel321 30:91af74a299e1 26 if (LPC_TIM2->IR & (1<<4|1<<5)) instance[2]->edgeIRQ();
pspatel321 30:91af74a299e1 27 if (LPC_TIM2->IR & 1) instance[2]->zeroIRQ();
pspatel321 30:91af74a299e1 28 LPC_TIM2->IR=0x3F; // Clear interrupt flags
pspatel321 30:91af74a299e1 29 }
pspatel321 30:91af74a299e1 30 void tim3_IRQ() {
pspatel321 30:91af74a299e1 31 if (LPC_TIM3->IR & (1<<4|1<<5)) instance[3]->edgeIRQ();
pspatel321 30:91af74a299e1 32 if (LPC_TIM3->IR & 1) instance[3]->zeroIRQ();
pspatel321 30:91af74a299e1 33 LPC_TIM3->IR=0x3F; // Clear interrupt flags
pspatel321 30:91af74a299e1 34 }
pspatel321 30:91af74a299e1 35
pspatel321 36:0afc0fc8f86b 36 IMD::IMD(PinName _pin) {
pspatel321 30:91af74a299e1 37 // Setup the timer/pin access variables
pspatel321 36:0afc0fc8f86b 38 if (_pin == P1_26) { // CAP0.0
pspatel321 30:91af74a299e1 39 timer=0;
pspatel321 30:91af74a299e1 40 pin=0;
pspatel321 30:91af74a299e1 41 timerBase=LPC_TIM0;
pspatel321 36:0afc0fc8f86b 42 } else if (_pin == P1_27) { // CAP0.1
pspatel321 30:91af74a299e1 43 timer=0;
pspatel321 30:91af74a299e1 44 pin=1;
pspatel321 30:91af74a299e1 45 timerBase=LPC_TIM0;
pspatel321 36:0afc0fc8f86b 46 } else if (_pin == P1_18) { // CAP1.0
pspatel321 30:91af74a299e1 47 timer=1;
pspatel321 30:91af74a299e1 48 pin=0;
pspatel321 30:91af74a299e1 49 timerBase=LPC_TIM1;
pspatel321 36:0afc0fc8f86b 50 } else if (_pin == P1_19) { // CAP1.1
pspatel321 30:91af74a299e1 51 timer=1;
pspatel321 30:91af74a299e1 52 pin=1;
pspatel321 30:91af74a299e1 53 timerBase=LPC_TIM1;
pspatel321 36:0afc0fc8f86b 54 } else if (_pin == P0_4) { // CAP2.0
pspatel321 30:91af74a299e1 55 timer=2;
pspatel321 30:91af74a299e1 56 pin=0;
pspatel321 30:91af74a299e1 57 timerBase=LPC_TIM2;
pspatel321 36:0afc0fc8f86b 58 } else if (_pin == P0_5) { // CAP2.1
pspatel321 30:91af74a299e1 59 timer=2;
pspatel321 30:91af74a299e1 60 pin=1;
pspatel321 30:91af74a299e1 61 timerBase=LPC_TIM2;
pspatel321 36:0afc0fc8f86b 62 } else if (_pin == P0_23) { // CAP3.0
pspatel321 30:91af74a299e1 63 timer=3;
pspatel321 30:91af74a299e1 64 pin=0;
pspatel321 30:91af74a299e1 65 timerBase=LPC_TIM3;
pspatel321 36:0afc0fc8f86b 66 } else if (_pin == P0_24) { // CAP3.1
pspatel321 30:91af74a299e1 67 timer=3;
pspatel321 30:91af74a299e1 68 pin=1;
pspatel321 30:91af74a299e1 69 timerBase=LPC_TIM3;
pspatel321 30:91af74a299e1 70 } else { // Invalid pin
pspatel321 30:91af74a299e1 71 timerBase=0;
pspatel321 30:91af74a299e1 72 pin=0;
pspatel321 30:91af74a299e1 73 timer=0;
pspatel321 30:91af74a299e1 74 return;
pspatel321 30:91af74a299e1 75 }
pspatel321 30:91af74a299e1 76
pspatel321 30:91af74a299e1 77 instance[timer] = this;
pspatel321 30:91af74a299e1 78 first = true;
pspatel321 30:91af74a299e1 79
pspatel321 30:91af74a299e1 80 startTime = 0;
pspatel321 30:91af74a299e1 81 widthTicks = 0; // Zero low, so that duty = 0/1 = 0%
pspatel321 30:91af74a299e1 82 periodTicks = 1;
pspatel321 30:91af74a299e1 83
pspatel321 30:91af74a299e1 84 // Enable power and set pclk at 24Mhz
pspatel321 30:91af74a299e1 85 if (timer==0) {
pspatel321 30:91af74a299e1 86 LPC_SC->PCONP |= (1<<1);
pspatel321 30:91af74a299e1 87 LPC_SC->PCLKSEL0 &= ~(3<<2);
pspatel321 30:91af74a299e1 88 } if (timer==1) {
pspatel321 30:91af74a299e1 89 LPC_SC->PCONP |= (1<<2);
pspatel321 30:91af74a299e1 90 LPC_SC->PCLKSEL0 &= ~(3<<4);
pspatel321 30:91af74a299e1 91 } if (timer==2) {
pspatel321 30:91af74a299e1 92 LPC_SC->PCONP |= (1<<22);
pspatel321 30:91af74a299e1 93 LPC_SC->PCLKSEL1 &= ~(3<<12);
pspatel321 30:91af74a299e1 94 } if (timer==3) {
pspatel321 30:91af74a299e1 95 LPC_SC->PCONP |= (1<<23);
pspatel321 30:91af74a299e1 96 LPC_SC->PCLKSEL1 &= ~(3<<14);
pspatel321 30:91af74a299e1 97 }
pspatel321 30:91af74a299e1 98
pspatel321 36:0afc0fc8f86b 99 pin_function(_pin, 3); // Capture input
pspatel321 36:0afc0fc8f86b 100 pin_mode(_pin, PullDown); // Pull-down
pspatel321 36:0afc0fc8f86b 101
pspatel321 30:91af74a299e1 102 timerBase->TCR=2; // Stop counter and hold at 0, for configuration
pspatel321 30:91af74a299e1 103 timerBase->IR=0x3F; // Clear any interrupt flags
pspatel321 30:91af74a299e1 104 timerBase->CTCR=0; // Use pclk, not external pin
pspatel321 30:91af74a299e1 105 timerBase->PR=0; // No prescale value, clock at full pclk=24Mhz
pspatel321 30:91af74a299e1 106 timerBase->EMR=0; // Do not use external match pins
pspatel321 30:91af74a299e1 107
pspatel321 30:91af74a299e1 108 if (timer == 0) NVIC_SetVector(TIMER0_IRQn, (uint32_t)&tim0_IRQ); // Set irq handler for timer0
pspatel321 30:91af74a299e1 109 if (timer == 1) NVIC_SetVector(TIMER1_IRQn, (uint32_t)&tim1_IRQ); // Set irq handler for timer1
pspatel321 30:91af74a299e1 110 if (timer == 2) NVIC_SetVector(TIMER2_IRQn, (uint32_t)&tim2_IRQ); // Set irq handler for timer2
pspatel321 30:91af74a299e1 111 if (timer == 3) NVIC_SetVector(TIMER3_IRQn, (uint32_t)&tim3_IRQ); // Set irq handler for timer3
pspatel321 30:91af74a299e1 112
pspatel321 30:91af74a299e1 113 NVIC_EnableIRQ((IRQn_Type)(timer+1));
pspatel321 30:91af74a299e1 114 NVIC_SetPriority((IRQn_Type)(timer+1), 0); // Highest priority (default)
pspatel321 30:91af74a299e1 115
pspatel321 30:91af74a299e1 116 timerBase->CCR = pin?5<<3:5; // Generate interrupt on rising edge of capture pin
pspatel321 30:91af74a299e1 117 timerBase->MCR = 1; // Interrupt on MR0 to establish the zero speed timeout
pspatel321 30:91af74a299e1 118 zeroIRQ(); // Zero the values
pspatel321 30:91af74a299e1 119 timerBase->TCR = 1; // Start counting, GO!
pspatel321 30:91af74a299e1 120 }
pspatel321 30:91af74a299e1 121
pspatel321 30:91af74a299e1 122 void IMD::edgeIRQ() {
pspatel321 30:91af74a299e1 123 bool rising = pin?timerBase->CCR&(1<<3):timerBase->CCR&1;
pspatel321 30:91af74a299e1 124 uint32_t capTime = pin?timerBase->CR1:timerBase->CR0; // Get the time of the capture event
pspatel321 30:91af74a299e1 125 timerBase->MR0 = capTime + TIMEOUT_TICKS; // Move the zero timeout ahead
pspatel321 30:91af74a299e1 126
pspatel321 36:0afc0fc8f86b 127 // Special case - on first pulse after a timeout or on startup == Period cannot be calculated
pspatel321 30:91af74a299e1 128 // so set startTime such that periodTicks remains unchanged from its zero state (periodTicks=1)
pspatel321 30:91af74a299e1 129 if (first) {
pspatel321 30:91af74a299e1 130 first = false;
pspatel321 30:91af74a299e1 131 startTime = capTime - 1;
pspatel321 30:91af74a299e1 132 }
pspatel321 30:91af74a299e1 133 if (rising) {
pspatel321 30:91af74a299e1 134 periodTicks = capTime - startTime; // Get the period on Rising edge
pspatel321 30:91af74a299e1 135 startTime = capTime; // Set the start of the next pulse
pspatel321 30:91af74a299e1 136 } else {
pspatel321 30:91af74a299e1 137 widthTicks = capTime - startTime; // Get the pulse width on Falling edge
pspatel321 30:91af74a299e1 138 }
pspatel321 30:91af74a299e1 139
pspatel321 30:91af74a299e1 140 // Switch interrupt types to capture the next edge
pspatel321 30:91af74a299e1 141 if (rising) timerBase->CCR = pin?6<<3:6;
pspatel321 30:91af74a299e1 142 else timerBase->CCR = pin?5<<3:5;
pspatel321 30:91af74a299e1 143 }
pspatel321 30:91af74a299e1 144
pspatel321 30:91af74a299e1 145 void IMD::zeroIRQ() {
pspatel321 30:91af74a299e1 146 periodTicks = 1;
pspatel321 30:91af74a299e1 147 first = true; // Reset the first edge detection case
pspatel321 30:91af74a299e1 148
pspatel321 30:91af74a299e1 149 bool rising = pin?timerBase->CCR&(1<<3):timerBase->CCR&1;
pspatel321 30:91af74a299e1 150 // Timeout occurred after FALLING edge, RISING edge never happened
pspatel321 30:91af74a299e1 151 if (rising) {
pspatel321 30:91af74a299e1 152 widthTicks = 0; // Signal is low = 0/1 = 0% duty
pspatel321 30:91af74a299e1 153 } else {
pspatel321 30:91af74a299e1 154 widthTicks = 1; // Signal is high = 1/1 = 100% duty
pspatel321 30:91af74a299e1 155 }
pspatel321 30:91af74a299e1 156 }
pspatel321 30:91af74a299e1 157 float IMD::frequency() {
pspatel321 30:91af74a299e1 158 // Handle the case where we want to say 0Hz not infinity Hz
pspatel321 30:91af74a299e1 159 if (periodTicks == 1 || periodTicks == 0) return 0;
pspatel321 30:91af74a299e1 160 else return (float)(PCLK)/(float)(periodTicks);
pspatel321 30:91af74a299e1 161 }
pspatel321 30:91af74a299e1 162 float IMD::duty() {
pspatel321 30:91af74a299e1 163 return (float)(widthTicks)/(float)(periodTicks);
pspatel321 30:91af74a299e1 164 }
pspatel321 30:91af74a299e1 165
pspatel321 30:91af74a299e1 166 char IMD::status() {
pspatel321 30:91af74a299e1 167 float freq = frequency();
pspatel321 30:91af74a299e1 168 if (freq == 0) return OFF; // IMD off
pspatel321 30:91af74a299e1 169 else if (05 < freq && freq <= 15) return NORMAL; // 10Hz normal mode
pspatel321 30:91af74a299e1 170 else if (15 < freq && freq <= 25) return UNDERVOLT; // 20Hz undervoltage mode
pspatel321 30:91af74a299e1 171 else if (25 < freq && freq <= 35) return SPEEDSTART; // 30Hz speed start mode
pspatel321 30:91af74a299e1 172 else if (35 < freq && freq <= 45) return ERROR; // 40Hz IMD error
pspatel321 30:91af74a299e1 173 else if (45 < freq && freq <= 55) return GROUNDERR; // 50Hz Ground error
pspatel321 30:91af74a299e1 174 else return INVALID; // Invalid
pspatel321 30:91af74a299e1 175 }
pspatel321 30:91af74a299e1 176
pspatel321 30:91af74a299e1 177 float IMD::resistance() {
pspatel321 30:91af74a299e1 178 char stat = status();
pspatel321 30:91af74a299e1 179 float dut = duty();
pspatel321 30:91af74a299e1 180
pspatel321 30:91af74a299e1 181 // In normal or undervoltage mode, where Rf is available
pspatel321 30:91af74a299e1 182 if (stat == NORMAL || stat == UNDERVOLT) {
pspatel321 30:91af74a299e1 183 if (0.05-EXTRA <= dut && dut <= 0.95+EXTRA) {
pspatel321 30:91af74a299e1 184 if (dut > 0.95) dut = 0.95; // Ceiling
pspatel321 30:91af74a299e1 185 if (dut < 0.05) dut = 0.05; // Floor so it doesnt mess up the equation below
pspatel321 30:91af74a299e1 186 float rf = (0.9*1200e3/(dut-0.05)) - 1200e3;
pspatel321 30:91af74a299e1 187 if (rf < 1000) rf = 0; // Floor to hit 0
pspatel321 30:91af74a299e1 188 if (rf > 50e6) rf = 50e6; // Ceil to stay at 50e6
pspatel321 30:91af74a299e1 189 return rf;
pspatel321 30:91af74a299e1 190 }
pspatel321 30:91af74a299e1 191 else return NAN;
pspatel321 30:91af74a299e1 192 }
pspatel321 30:91af74a299e1 193 // In speed start, where only good/bad estimate is available
pspatel321 30:91af74a299e1 194 if (stat == SPEEDSTART) {
pspatel321 30:91af74a299e1 195 if (0.05-EXTRA <= dut && dut <= 0.10+EXTRA) return 50e6; // Good
pspatel321 30:91af74a299e1 196 else if (0.90-EXTRA <= dut && dut <= 0.95+EXTRA) return 0; // Bad
pspatel321 30:91af74a299e1 197 else return NAN;
pspatel321 30:91af74a299e1 198 }
pspatel321 30:91af74a299e1 199 return NAN; // Measurement not available in this mode
pspatel321 30:91af74a299e1 200 }