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@36:0afc0fc8f86b, 2015-01-22 (annotated)

Committer:
pspatel321
Date:
Thu Jan 22 07:58:51 2015 +0000
Revision:
36:0afc0fc8f86b
Parent:
30:91af74a299e1
Child:
38:8efacce315ae
Tested in car with other systems.  Most features are good to go.  Except xbees need work.  The DC-DC protection features were giving problems due to spurious current measurements.  They have been edited to reduce glitchy errors.

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