CoulombCounter.cpp

00001 #include "CoulombCounter.h"
00002 
00003 const float MSEC_HRS = 2.77778e-7;                          // Multiplier to convert milliseconds to hours
00004 const float BAT_ISENSE_MULTIPLIER = 6.640114;               // Multiplier to convert float to amps, calibrated using 3 points with Fluke 87V meter and code on 1/9/15
00005 const float BAT_ISENSE_OFFSET = -3.344968;                  // Offset to convert float to amps, calibrated using 3 points with Fluke 87V meter and code on 1/9/15
00006 
00007 CoulombCounter::CoulombCounter(PinName _pin, int _mSec, unsigned int size) : BatISense(_pin)
00008 {
00009     mSec = _mSec;
00010     _size = 0;
00011     errorPacket = 0;
00012     overChargeCurrent = 0;
00013     overDischargeCurrent = 0;
00014     
00015     // Default capacity if blank or corrupted
00016     float capReg = store.read(GPREG_AH_CAPACITY);
00017     if (capReg < MIN_CAPACITY_SETTING || capReg > MAX_CAPACITY_SETTING) {   // Bad, write default
00018         store.write(DEFAULT_AH, GPREG_AH_CAPACITY);
00019         errorPacket |= CAP_INIT;
00020     }
00021 
00022     // Default SOC if blank or corrupted
00023     float Ah = store.read(GPREG_AH_COUNTER);
00024     if (Ah < 0 || Ah > MAX_CAPACITY_SETTING) {                              // Bad, write default
00025         store.write(DEFAULT_SOC*DEFAULT_AH, GPREG_AH_COUNTER);
00026         errorPacket |= SOC_INIT;
00027     }
00028 
00029     // Allocate the buffer
00030     buffArr = new float [size];
00031     if (buffArr == 0) {
00032         error("Coulomb Counter failed to allocate memory");
00033     }
00034     _size = size;
00035     tracker = 0;
00036 
00037     // Take the initial readings, fill the buffer
00038     for (int i = 0; i < _size; i++) {
00039         buffArr[i] = BatISense.read()*BAT_ISENSE_MULTIPLIER+BAT_ISENSE_OFFSET;
00040     }
00041     updateAvg();      // Get avg and fill out overCurrent flag
00042 }
00043 void CoulombCounter::setup(float *charge, float *discharge) {
00044     overChargeCurrent = charge;
00045     overDischargeCurrent = discharge;
00046 }
00047 bool CoulombCounter::size(unsigned int size)
00048 {
00049     if (_size == size) return false;
00050 
00051     // Get a current sample, this will be used to fill the new buffer
00052     updateAvg();
00053     float avg = current();
00054 
00055     float* newArr = new float [size];
00056     if (newArr == 0) return false;
00057     else {
00058         delete[] buffArr;   // Free old
00059         __disable_irq();
00060 
00061         buffArr = newArr;   // Transfer
00062         _size = size;       // Set new size
00063         for (int i = 0; i < _size; i++) buffArr[i] = avg;   // Fill with old avg
00064 
00065         __enable_irq();
00066         return true;
00067     }
00068 }
00069 void CoulombCounter::sample()
00070 {
00071     // Take the reading
00072     float currentSample = BatISense.read()*BAT_ISENSE_MULTIPLIER+BAT_ISENSE_OFFSET;
00073 
00074     // Integrate
00075     float f = ampHours()-currentSample*mSec*MSEC_HRS;
00076 
00077     // Bound to valid range
00078     float cap = capacity();
00079     if (f > cap) f = cap;
00080     if (f < 0) f = 0;
00081 
00082     // Write
00083     store.write(f, GPREG_AH_COUNTER);
00084 
00085     // Add to filter
00086     buffArr[tracker++] = currentSample;
00087     tracker %= _size;
00088     
00089     // Update the avg, check for over-current
00090     updateAvg();
00091 }
00092 
00093 void CoulombCounter::updateAvg()
00094 {
00095     float avg = 0;
00096     for (int i = 0; i < _size; i++) {
00097         avg += buffArr[i];
00098     }
00099     avg /= _size;
00100     currentFiltered = avg;
00101     
00102     errorPacket = 0;
00103     if (overChargeCurrent == 0) return;     // setup() not run yet
00104 
00105     if (avg > *overDischargeCurrent)    errorPacket |= OVER_DISCHARGE_I;
00106     else                                errorPacket &= ~OVER_DISCHARGE_I;
00107     if (avg < *overChargeCurrent)       errorPacket |= OVER_CHARGE_I;
00108     else                                errorPacket &= ~OVER_CHARGE_I;
00109 }