File content as of revision 39:ddf38df9699e:

#include "DC_DC.h"
#include <math.h>

#define TURN_OFF dcdcControl = 1; isControlPinOn = false;
#define TURN_ON  dcdcControl = 0; isControlPinOn = true;

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 CURRENT_SENSOR_LLIM    = -1.0;                      // Lowest allowable reading before declaring sensor broken
const float CURRENT_SENSOR_ULIM    = 33;                        // Sensor is at 5V rail, broken

DC_DC::DC_DC(PinName _dcdcPin, PinName _dcdcCurrent, PinName _fan1, PinName _fan2, PinName _pump1, PinName _pump2, float period, float slew, unsigned int size) :
    dcdcControl(_dcdcPin, 1), dcdcCurrent(_dcdcCurrent), fan1(_fan1, period, slew), fan2(_fan2, period, slew), pump1(_pump1, period, slew), pump2(_pump2, period, slew)
{
    TURN_OFF
    currentOffset = CURRENT_OFFSET_DEFAULT;
    buffTracker = 0;
    wait_ms(10);            // Make sure Hall effect IC is ready
    startTimer.reset();
    stopTimer.reset();
    status=0;
    _size = 0;
    onThreshold = 0;
    startDelay = 0;
    stopDelay = 0;
    overCurrent = 0;
    
    filterBuff = new float[size];
    if (filterBuff == 0) {
        error("DC-DC failed to allocate memory");   
    }
    _size = size;
    
    // Fill up the buffer
    for (int i = 0; i < _size; i++) {
        filterBuff[i] = (dcdcCurrent - currentOffset) * CURRENT_MULTIPLIER_DEFAULT;
        wait_ms(10);
    }
    
    // Compute the zero
    currentOffset = (current / CURRENT_MULTIPLIER_DEFAULT) + CURRENT_OFFSET_DEFAULT;
    
    // Correct the buffer for the new zero
    for (int i = 0; i < _size; i++) {
       filterBuff[i] = ((CURRENT_OFFSET_DEFAULT - currentOffset) * CURRENT_MULTIPLIER_DEFAULT) + filterBuff[i];
    }
}

void DC_DC::setup(float* _onThreshold, float* _overCurrent, float* _startDelay, float* _stopDelay) {
    onThreshold = _onThreshold;
    overCurrent = _overCurrent;
    startDelay  = _startDelay;
    startDelay  = _startDelay;
}

bool DC_DC::size(unsigned int size) {
    if (_size == size) return false;
    
    // Get recent
    updateCurrent();
    float avg = getCurrent();
    
    // Allocate new
    float* newBuff = new float[size];
    if (newBuff == 0) return false;     // Not found, nothing has been changed
    else {
        delete[] filterBuff;            // Free old
        __disable_irq();
        
        _size = size;                   // Set size
        filterBuff = newBuff;           // Transfer to new
        for (int i = 0; i < size; i++) filterBuff[i] = avg;     // Pre-fill with old average
        
        __enable_irq();
        return true;
    }   
}
void DC_DC::updateCurrent()
{
    float avg=0;
    for (int i = 0; i < _size; i++) {
        avg += filterBuff[i];
    }
    avg /= _size;
    current = avg;
}

void DC_DC::set(bool on)
{
    if (onThreshold == 0) return;                               // Not setup yet

    // Do nothing if already on
    if (on && isControlPinOn) return;
    
    // Double check if already off
    if (!on && !isControlPinOn) {
        fan1.directOff();
        fan2.directOff();
        pump1.directOff();
        pump2.directOff();
        TURN_OFF
    }

    // If start requested and no error
    if (on && !critError && !(status & POWER_DOWN)) {
        status |= POWER_UP;
        status &= ~POWER_DOWN;
        TURN_ON
        startTimer.reset();
        startTimer.start();
        stopTimer.stop();
        stopTimer.reset();

    // If stop requested
    } else {
        status &= ~POWER_UP;
        status |= POWER_DOWN;
        fan1.directOff();
        fan2.directOff();
        pump1.directOff();
        pump2.directOff();
        TURN_OFF
        stopTimer.reset();
        stopTimer.start();
        startTimer.stop();
        startTimer.reset();
    }
}

void DC_DC::sample()
{
    // Get next current sample
    float curr = (dcdcCurrent.read() - currentOffset) * CURRENT_MULTIPLIER_DEFAULT;    

    filterBuff[buffTracker++] = curr;                           // Add to buffer filter
    buffTracker %= _size;
    updateCurrent();                                            // Compute average
    
    if (onThreshold == 0) return;                               // Not setup yet
    
    // Update status
    char stat = 0;
    if (current >= *onThreshold)    stat |= CONV_ON;            // The converter is actually on (positive current)
    if (isControlPinOn)             stat |= SET_ON;             // The converter is set on

    // During Timed Startup Phase
    if (stat & POWER_UP) {
        if (startTimer.read() >= *startDelay) {                 // Start time elapsed
            if (stat & CONV_ON) {                               // Positive current detected
                startTimer.stop();                              // Stop timer
                startTimer.reset();
                stat &= ~POWER_UP;                              // Done power-up
            }
        }
    }

    // During Timed Stopping Phase
    if (stat & POWER_DOWN) {
        if (stopTimer.read() >= *stopDelay) {                   // Stop time elapsed
            if (!(stat & CONV_ON)) {                            // Current dropped, its off
                stopTimer.stop();                               // Stop timer
                stopTimer.reset();
                stat &= ~POWER_DOWN;                            // Done power-down
            }
        }
    }
    
    if (current < CURRENT_SENSOR_LLIM || current > CURRENT_SENSOR_ULIM) stat |= SENSOR_FAULT;   // Sensor out of range
    else if (current > *overCurrent)                                    stat |= OVER_CURRENT;   // Over current

    // Process critical error conditions
    if (stat & (SENSOR_FAULT | OVER_CURRENT)) 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();
        TURN_OFF
        startTimer.stop();
        startTimer.reset();
    }
    
    // Don't allow to run if still powering on or off
    if (stat & (POWER_UP | POWER_DOWN)) {
        fan1.directOff();
        fan2.directOff();
        pump1.directOff();
        pump2.directOff();
    }
}

void DC_DC::setPwm(enum Channel_T chan, float duty)
{
    // Off if error present, starting in startup, or DC-DC not actually on, or not set on
    if (!onThreshold || critError || !(status & CONV_ON) || !isControlPinOn || (status & (POWER_UP | POWER_DOWN))) {
        fan1.directOff();
        fan2.directOff();
        pump1.directOff();
        pump2.directOff();
    } 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;
}