Diff: Libs/DC_DC/DC_DC.cpp

Revision:

30:91af74a299e1

Child:

34:18bcf276d3bf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Libs/DC_DC/DC_DC.cpp	Thu Nov 13 10:53:10 2014 +0000
@@ -0,0 +1,181 @@
+#include "DC_DC.h"
+#include <math.h>
+
+#define OFF 1
+#define ON 0
+
+const float CURRENT_MULTIPLIER_DEFAULT = 41.35338345864663;      // Full scale amps
+const float CURRENT_OFFSET_DEFAULT = 0.0909090909090909;         // Float adc reading for 0 amps
+
+const float DC_DC_ON_THRESHOLD = 0.5;                    // Current draw required in order to declare it as ON
+const int STARTUP_DELAY_MS = 500;                        // DC-DC converter startup time in milliseconds
+const float OVER_CURRENT_THRESHOLD = 25;                 // Overcurrent threshold
+const float CURRENT_SENSOR_LLIM = -0.5;                  // Lowest allowable reading before declaring sensor broken
+const int ZEROING_SAMPLES = 50;                          // Number of samples to average for zeroing
+const int STOP_DELAY_MS = 500;                           // Amount of time given to turn-off before flagging error
+
+DC_DC::DC_DC(PinName _dcdcPin, PinName _dcdcCurrent, PinName _fan1, PinName _fan2, PinName _pump1, PinName _pump2, float period, float slew) : 
+    dcdcControl(_dcdcPin, OFF), dcdcCurrent(_dcdcCurrent), fan1(_fan1, period, slew), fan2(_fan2, period, slew), pump1(_pump1, period, slew), pump2(_pump2, period, slew) {
+
+    currentOffset = CURRENT_OFFSET_DEFAULT;
+    starting = false;
+    stopping = false;
+    buffTracker = 0;
+    wait_ms(10);            // Make sure Hall effect IC is ready
+    startTimer.reset();
+    stopTimer.reset();
+    status=0;
+    
+    // Fill up the buffer
+    for (int i = 0; i < DC_DC_FILTER_TAPS; i++) {
+        filterBuff[i] = (dcdcCurrent - currentOffset) * CURRENT_MULTIPLIER_DEFAULT;
+    }
+    updateCurrent();
+    sample();
+    if (!critError) zeroCurrent();
+}
+
+void DC_DC::zeroCurrent() {
+    float avg=0;
+    for (int i = 0; i < ZEROING_SAMPLES; i++) {
+        avg+=dcdcCurrent;
+        wait_ms(1);   
+    }
+    avg /= ZEROING_SAMPLES;
+    currentOffset = avg;
+}
+
+void DC_DC::updateCurrent() {
+    float avg=0;
+    for (int i = 0; i < DC_DC_FILTER_TAPS; i++) {
+        avg += filterBuff[i];
+    }
+    avg /= DC_DC_FILTER_TAPS;
+    current = avg;
+}
+
+void DC_DC::set(bool on) {
+    // Do nothing if already on
+    if (on && dcdcControl == ON) return;
+    
+    // Do nothing if error present
+    if (critError) return;
+    
+    // If start requested
+    if (on) {
+        dcdcControl = ON;
+        starting = true;
+        startTimer.reset();
+        startTimer.start();
+        stopTimer.stop();
+        stopTimer.reset();
+        stopping = false;
+    // If stop requested
+    } else {
+        stopping=true;
+        fan1.directOff();
+        fan2.directOff();
+        pump1.directOff();
+        pump2.directOff();
+        dcdcControl = OFF;
+        stopTimer.reset();
+        stopTimer.start();
+        startTimer.stop();
+        startTimer.reset();
+        starting = false;
+    }
+}
+
+void DC_DC::sample() {
+    
+    // Get next current sample
+    float curr = (dcdcCurrent - currentOffset) * CURRENT_MULTIPLIER_DEFAULT;
+    if (curr < CURRENT_SENSOR_LLIM) curr = -INFINITY;               // Check if sensor out of range
+    else if (curr < 0) curr = 0;                                    // Floor to zero             
+    filterBuff[buffTracker] = curr;                                 // Add to buffer filter
+    buffTracker++;
+    if (buffTracker >= DC_DC_FILTER_TAPS) buffTracker = 0;
+    updateCurrent();                                                // Compute average
+    
+    // Update status
+    register char stat = status & 0xF0;                        // The the upper 4 bits (locking errors)
+    if (current >= DC_DC_ON_THRESHOLD) stat |= ConvOn;         // The converter is actually on (positive current)
+    if (dcdcControl == ON) stat |= SetOn;                      // The converter is set on
+    
+    // During Timed Startup Phase
+    if (starting) {
+        stat |= PowerUp;                                        // Indicate state in status byte
+        if (startTimer.read_us() >= STARTUP_DELAY_MS*1000) {    // Start time elapsed
+            if (stat & ConvOn) {                                // Positive current detected
+                startTimer.stop();      // Stop timer
+                startTimer.reset();     // Reset to zero
+                starting = false;       // Indicate running
+            } else {
+                startTimer.stop();
+                startTimer.reset();
+                stat |= FailStart;      // Failed to start
+            }
+        }
+    }
+   
+    // During Timed Stopping Phase
+    if (stopping) {
+        stat |= PowerDown;                                    // Indicate state in status byte      
+        if (stopTimer.read_us() >= STOP_DELAY_MS*1000) {      // Stop time elapsed
+            if (stat & ConvOn) {                              // Converter still on
+                stopTimer.stop();
+                stopTimer.reset();
+                stat |= FailStop;         // It didn't turn off even after timer expired!
+            } else {                      // Its actually off
+                stopping = false;
+                stopTimer.stop();
+                stopTimer.reset();   
+            }
+        }
+    }
+    
+    if (current > OVER_CURRENT_THRESHOLD) stat |= OverCurrent;  // Over current
+    if (current == -INFINITY) stat |= SensorFault;              // Sensor out of range
+   
+    // While in steady state
+    if (!stopping && !starting) {
+        if ((stat & SetOn) && !(stat & 1)) stat |= FailStart;    // Should be running but its not
+        if (!(stat & SetOn) && (stat & 1)) stat |= FailStop;     // Should be stopped but its not
+    }
+    
+    // Process critical error conditions
+    if (stat & 0xF0) critError = true;
+    else critError =  false;
+    status = stat;
+
+    // Arrest error, all channels off, DC-DC off
+    if (critError) {
+        fan1.directOff();
+        fan2.directOff();
+        pump1.directOff();
+        pump2.directOff();
+        dcdcControl = OFF;
+        startTimer.stop();
+        startTimer.reset();
+        starting = false;
+    }
+}
+
+void DC_DC::setPwm(enum Channel_T chan, float duty) {
+    // Do nothing if error present, starting in startup, or DC-DC not actually on, or not set on
+    if (critError || starting || stopping || !(status & 1) || dcdcControl == OFF) return;
+    
+    else {
+        if (chan == FAN1) fan1.write(duty);
+        if (chan == FAN2) fan2.write(duty);
+        if (chan == PUMP1) pump1.write(duty);
+        if (chan == PUMP2) pump2.write(duty);   
+    }
+}
+float DC_DC::readPwm(enum Channel_T chan) {
+    if (chan == FAN1) return fan1.readRaw();
+    if (chan == FAN2) return fan2.readRaw();
+    if (chan == PUMP1) return pump1.readRaw();
+    if (chan == PUMP2) return pump2.readRaw();
+    else return 0;
+}
\ No newline at end of file