DC_DC.cpp

00001 #include "DC_DC.h"
00002 #include <math.h>
00003 
00004 #define TURN_OFF dcdcControl = 1; isControlPinOn = false;
00005 #define TURN_ON  dcdcControl = 0; isControlPinOn = true;
00006 
00007 const float CURRENT_MULTIPLIER_DEFAULT = 41.35338345864663;     // Full scale amps
00008 const float CURRENT_OFFSET_DEFAULT = 0.0909090909090909;        // Float adc reading for 0 amps
00009 
00010 const float CURRENT_SENSOR_LLIM    = -1.0;                      // Lowest allowable reading before declaring sensor broken
00011 const float CURRENT_SENSOR_ULIM    = 33;                        // Sensor is at 5V rail, broken
00012 
00013 DC_DC::DC_DC(PinName _dcdcPin, PinName _dcdcCurrent, PinName _fan1, PinName _fan2, PinName _pump1, PinName _pump2, float period, float slew, unsigned int size) :
00014     dcdcControl(_dcdcPin, 1), dcdcCurrent(_dcdcCurrent), fan1(_fan1, period, slew), fan2(_fan2, period, slew), pump1(_pump1, period, slew), pump2(_pump2, period, slew)
00015 {
00016     TURN_OFF
00017     currentOffset = CURRENT_OFFSET_DEFAULT;
00018     buffTracker = 0;
00019     wait_ms(10);            // Make sure Hall effect IC is ready
00020     startTimer.reset();
00021     stopTimer.reset();
00022     status=0;
00023     _size = 0;
00024     onThreshold = 0;
00025     startDelay = 0;
00026     stopDelay = 0;
00027     overCurrent = 0;
00028     
00029     filterBuff = new float[size];
00030     if (filterBuff == 0) {
00031         error("DC-DC failed to allocate memory");   
00032     }
00033     _size = size;
00034     
00035     // Fill up the buffer
00036     for (int i = 0; i < _size; i++) {
00037         filterBuff[i] = (dcdcCurrent - currentOffset) * CURRENT_MULTIPLIER_DEFAULT;
00038         wait_ms(10);
00039     }
00040     
00041     // Compute the zero
00042     currentOffset = (current / CURRENT_MULTIPLIER_DEFAULT) + CURRENT_OFFSET_DEFAULT;
00043     
00044     // Correct the buffer for the new zero
00045     for (int i = 0; i < _size; i++) {
00046        filterBuff[i] = ((CURRENT_OFFSET_DEFAULT - currentOffset) * CURRENT_MULTIPLIER_DEFAULT) + filterBuff[i];
00047     }
00048 }
00049 
00050 void DC_DC::setup(float* _onThreshold, float* _overCurrent, float* _startDelay, float* _stopDelay) {
00051     onThreshold = _onThreshold;
00052     overCurrent = _overCurrent;
00053     startDelay  = _startDelay;
00054     startDelay  = _startDelay;
00055 }
00056 
00057 bool DC_DC::size(unsigned int size) {
00058     if (_size == size) return false;
00059     
00060     // Get recent
00061     updateCurrent();
00062     float avg = getCurrent();
00063     
00064     // Allocate new
00065     float* newBuff = new float[size];
00066     if (newBuff == 0) return false;     // Not found, nothing has been changed
00067     else {
00068         delete[] filterBuff;            // Free old
00069         __disable_irq();
00070         
00071         _size = size;                   // Set size
00072         filterBuff = newBuff;           // Transfer to new
00073         for (int i = 0; i < size; i++) filterBuff[i] = avg;     // Pre-fill with old average
00074         
00075         __enable_irq();
00076         return true;
00077     }   
00078 }
00079 void DC_DC::updateCurrent()
00080 {
00081     float avg=0;
00082     for (int i = 0; i < _size; i++) {
00083         avg += filterBuff[i];
00084     }
00085     avg /= _size;
00086     current = avg;
00087 }
00088 
00089 void DC_DC::set(bool on)
00090 {
00091     if (onThreshold == 0) return;                               // Not setup yet
00092 
00093     // Do nothing if already on
00094     if (on && isControlPinOn) return;
00095     
00096     // Double check if already off
00097     if (!on && !isControlPinOn) {
00098         fan1.directOff();
00099         fan2.directOff();
00100         pump1.directOff();
00101         pump2.directOff();
00102         TURN_OFF
00103     }
00104 
00105     // If start requested and no error
00106     if (on && !critError && !(status & POWER_DOWN)) {
00107         status |= POWER_UP;
00108         status &= ~POWER_DOWN;
00109         TURN_ON
00110         startTimer.reset();
00111         startTimer.start();
00112         stopTimer.stop();
00113         stopTimer.reset();
00114 
00115     // If stop requested
00116     } else {
00117         status &= ~POWER_UP;
00118         status |= POWER_DOWN;
00119         fan1.directOff();
00120         fan2.directOff();
00121         pump1.directOff();
00122         pump2.directOff();
00123         TURN_OFF
00124         stopTimer.reset();
00125         stopTimer.start();
00126         startTimer.stop();
00127         startTimer.reset();
00128     }
00129 }
00130 
00131 void DC_DC::sample()
00132 {
00133     // Get next current sample
00134     float curr = (dcdcCurrent.read() - currentOffset) * CURRENT_MULTIPLIER_DEFAULT;    
00135 
00136     filterBuff[buffTracker++] = curr;                           // Add to buffer filter
00137     buffTracker %= _size;
00138     updateCurrent();                                            // Compute average
00139     
00140     if (onThreshold == 0) return;                               // Not setup yet
00141     
00142     // Update status
00143     char stat = 0;
00144     if (current >= *onThreshold)    stat |= CONV_ON;            // The converter is actually on (positive current)
00145     if (isControlPinOn)             stat |= SET_ON;             // The converter is set on
00146 
00147     // During Timed Startup Phase
00148     if (stat & POWER_UP) {
00149         if (startTimer.read() >= *startDelay) {                 // Start time elapsed
00150             if (stat & CONV_ON) {                               // Positive current detected
00151                 startTimer.stop();                              // Stop timer
00152                 startTimer.reset();
00153                 stat &= ~POWER_UP;                              // Done power-up
00154             }
00155         }
00156     }
00157 
00158     // During Timed Stopping Phase
00159     if (stat & POWER_DOWN) {
00160         if (stopTimer.read() >= *stopDelay) {                   // Stop time elapsed
00161             if (!(stat & CONV_ON)) {                            // Current dropped, its off
00162                 stopTimer.stop();                               // Stop timer
00163                 stopTimer.reset();
00164                 stat &= ~POWER_DOWN;                            // Done power-down
00165             }
00166         }
00167     }
00168     
00169     if (current < CURRENT_SENSOR_LLIM || current > CURRENT_SENSOR_ULIM) stat |= SENSOR_FAULT;   // Sensor out of range
00170     else if (current > *overCurrent)                                    stat |= OVER_CURRENT;   // Over current
00171 
00172     // Process critical error conditions
00173     if (stat & (SENSOR_FAULT | OVER_CURRENT)) critError = true;
00174     else critError =  false;
00175     status = stat;
00176 
00177     // Arrest error, all channels off, DC-DC off
00178     if (critError) {
00179         fan1.directOff();
00180         fan2.directOff();
00181         pump1.directOff();
00182         pump2.directOff();
00183         TURN_OFF
00184         startTimer.stop();
00185         startTimer.reset();
00186     }
00187     
00188     // Don't allow to run if still powering on or off
00189     if (stat & (POWER_UP | POWER_DOWN)) {
00190         fan1.directOff();
00191         fan2.directOff();
00192         pump1.directOff();
00193         pump2.directOff();
00194     }
00195 }
00196 
00197 void DC_DC::setPwm(enum Channel_T chan, float duty)
00198 {
00199     // Off if error present, starting in startup, or DC-DC not actually on, or not set on
00200     if (!onThreshold || critError || !(status & CONV_ON) || !isControlPinOn || (status & (POWER_UP | POWER_DOWN))) {
00201         fan1.directOff();
00202         fan2.directOff();
00203         pump1.directOff();
00204         pump2.directOff();
00205     } else {
00206         if (chan == FAN1)  fan1.write(duty);
00207         if (chan == FAN2)  fan2.write(duty);
00208         if (chan == PUMP1) pump1.write(duty);
00209         if (chan == PUMP2) pump2.write(duty);
00210     }
00211 }
00212 float DC_DC::readPwm(enum Channel_T chan)
00213 {
00214     if (chan == FAN1)   return fan1.readRaw();
00215     if (chan == FAN2)   return fan2.readRaw();
00216     if (chan == PUMP1)  return pump1.readRaw();
00217     if (chan == PUMP2)  return pump2.readRaw();
00218     else return 0;
00219 }