openXsensor/oXs_voltage.cpp

   1 #include "oXs_voltage.h"
   2
   3 #ifdef DEBUG
   4 //#define DEBUGNEWVALUE
   5 //#define DEBUGDELAY
   6 //#define DEBUGCELLCALCULATION
   7 //#define DEBUGLOWVOLTAGE
   8 //#define DEBUGNTC
   9 #endif
  10
  11 #if defined(ARDUINO_MEASURES_VOLTAGES) && (ARDUINO_MEASURES_VOLTAGES == YES)
  12
  13 extern unsigned long micros( void ) ;
  14 extern unsigned long millis( void ) ;
  15 extern void delay(unsigned long ms) ;
  16
  17
  18 #ifdef DEBUG
  19 OXS_VOLTAGE::OXS_VOLTAGE(HardwareSerial &print)
  20 #else
  21 OXS_VOLTAGE::OXS_VOLTAGE(uint8_t x)
  22 #endif
  23 {
  24 #ifdef DEBUG
  25   printer = &print; //operate on the address of print
  26 #endif
  27 }
  28
  29 // **************** Setup the Current sensor *********************
  30 void OXS_VOLTAGE::setupVoltage( void ) {
  31   uint16_t tempRef ;
  32 #ifdef DEBUG
  33   printer->println("Enter setup voltage");
  34 #endif
  35
  36 #ifdef USE_INTERNAL_REFERENCE
  37   analogReference(INTERNAL) ;
  38 #elif defined(USE_EXTERNAL_REFERENCE)
  39   analogReference(EXTERNAL) ;
  40 #endif
  41 #ifdef PIN_VOLTAGE
  42   uint8_t tempPin[6 ] = { PIN_VOLTAGE };
  43 #else
  44   uint8_t tempPin[6 ] = { 8 , 8 , 8 , 8 , 8 , 8 } ;
  45 #endif
  46 #ifdef RESISTOR_TO_GROUND
  47   float tempResistorToGround[6] = { RESISTOR_TO_GROUND } ;
  48 #else
  49   float tempResistorToGround[6] = { 0 , 0 , 0 , 0 , 0 , 0 } ;
  50 #endif
  51 #ifdef RESISTOR_TO_VOLTAGE
  52   float tempResistorToVoltage[6] = { RESISTOR_TO_VOLTAGE } ;
  53 #else
  54   float tempResistorToVoltage[6] = { 0 , 0 , 0 , 0 , 0 , 0 } ;
  55 #endif
  56 #ifdef OFFSET_VOLTAGE
  57   int tempOffsetVoltage[6] = { OFFSET_VOLTAGE} ;
  58 #else
  59   int tempOffsetVoltage[6] =  { 0 , 0 , 0 , 0 , 0 , 0 } ;
  60 #endif
  61 #ifdef SCALE_VOLTAGE
  62   float tempScaleVoltage[6] = { SCALE_VOLTAGE }  ;
  63 #else
  64   float tempScaleVoltage[6] =  { 1 , 1 , 1 , 1 , 1 , 1 } ;
  65 #endif
  66
  67 #if defined(USE_INTERNAL_REFERENCE) && defined(REFERENCE_VOLTAGE) && REFERENCE_VOLTAGE < 2000
  68   tempRef = REFERENCE_VOLTAGE  ;
  69 #elif defined(USE_INTERNAL_REFERENCE) && defined(REFERENCE_VOLTAGE)
  70   #error REFERENCE_VOLTAGE must be less than 2000 when USE_INTERNAL_REFERENCE is defined
  71 #elif defined(USE_EXTERNAL_REFERENCE)
  72 #ifndef REFERENCE_VOLTAGE
  73   #error REFERENCE_VOLTAGE must be defined when USE_EXTERNAL_REFERENCE is defined
  74 #else
  75   tempRef = REFERENCE_VOLTAGE  ;
  76 #endif
  77 #elif defined(USE_INTERNAL_REFERENCE)
  78   tempRef = 1100 ;
  79 #elif defined(REFERENCE_VOLTAGE) && REFERENCE_VOLTAGE > 2000
  80   tempRef = REFERENCE_VOLTAGE  ;
  81 #elif defined(REFERENCE_VOLTAGE)
  82   #error REFERENCE_VOLTAGE must be greater than 2000 when USE_INTERNAL_REFERENCE is not defined
  83 #else
  84   tempRef = 5000 ;
  85 #endif
  86
  87 #ifdef DEBUG
  88   printer->print("Reference voltage:");
  89   printer->println(tempRef);
  90 #endif
  91   voltageData.atLeastOneVolt = false ;
  92   for (int cntInit = 0 ; cntInit < 6 ; cntInit++) {
  93     if ( tempPin[ cntInit ] < 8 ) {
  94       voltageData.mVoltPin[cntInit] =  tempPin[ cntInit ] ;
  95       // pinMode(voltageData.mVoltPin[cntInit],INPUT);  // this instruction was wrong because pinMode need A0... A7 for analog pin - and not 0...7
  96       voltageData.atLeastOneVolt = true ;
  97     } else {
  98       voltageData.mVoltPin[cntInit] = 8 ;
  99     }
 100     voltageData.offset[cntInit] = tempOffsetVoltage[ cntInit ] ;
 101     if ( tempResistorToGround[cntInit] > 0 && tempResistorToVoltage[cntInit] > 0 && tempScaleVoltage[cntInit] > 0 ) {
 102       voltageData.mVoltPerStep[cntInit] = tempRef / 1023.0 * ( tempResistorToGround[cntInit] + tempResistorToVoltage[cntInit] ) / tempResistorToGround[cntInit]  * tempScaleVoltage[cntInit];
 103     } else {
 104       voltageData.mVoltPerStep[cntInit] = tempRef / 1023.0  * tempScaleVoltage[cntInit];
 105     }
 106     voltageData.sumVoltage[cntInit] = 0 ;
 107     voltageData.mVolt[cntInit].available = false ;
 108 #ifdef DEBUG
 109     printer->print("Voltage:"); printer->print( cntInit + 1 );
 110     printer->print(" , pin="); printer->print( voltageData.mVoltPin[cntInit] );
 111     printer->print(" , offset="); printer->print( voltageData.offset[cntInit] );
 112     printer->print("  , mVoltPerStep="); printer->println( voltageData.mVoltPerStep[cntInit] );
 113 #endif
 114
 115   }
 116 //  voltageData.atLeastOneVoltage = ( voltageData.mVoltPin[0] < 8 || voltageData.mVoltPin[1] < 8 || voltageData.mVoltPin[2] < 8 ||voltageData.mVoltPin[3] < 8 ||voltageData.mVoltPin[4] < 8 || voltageData.mVoltPin[5] < 8 ) ;
 117 }
 118
 119
 120
 121 // Maximum voltage that is allowed (theoretical) on the voltage divider is 5/R2/(R1+R2);
 122
 123   static byte voltageNr = 0;
 124
 125 void OXS_VOLTAGE::readSensor() {
 126
 127     if (voltageData.atLeastOneVolt) { // no calculation if there is no voltage to calculate.
 128 #ifdef DEBUGDELAY
 129         long milliVoltBegin = micros() ;
 130 #endif
 131
 132         while ( voltageData.mVoltPin[voltageNr] > 7) { // Skip nr if voltageNr have not a pin defined between 0 and 7
 133             voltageNrIncrease();                       // Find next voltage to be read; if overlap, calculate average for each voltage
 134         }
 135         voltageData.sumVoltage[voltageNr] += readVoltage(voltageNr) ;   // read voltage
 136 #ifdef DEBUGDELAY
 137         milliVoltBegin = milliVoltBegin - micros() ;
 138         printer->print("VoltageNr ");
 139         printer->print(voltageNr);
 140         printer->print(" in ");
 141         printer->println(milliVoltBegin);
 142 #endif
 143
 144         voltageNrIncrease();                          // Find next voltage to be read; if overlap, calculate average for each voltage
 145     }
 146 }
 147
 148
 149
 150 // Select next voltage to read ; if all voltages have been read and 500 ms are enlapsed since previous average calculation, calculates the new averages for each voltage
 151 void OXS_VOLTAGE::voltageNrIncrease() {
 152   static int cnt = 0;
 153   static unsigned long lastVoltMillis = millis() ;
 154 #if defined ( NUMBEROFCELLS ) && (NUMBEROFCELLS > 0)
 155     static int32_t secondMVolt ;
 156 #endif
 157   //static int32_t previousMVolt ;
 158
 159   voltageNr++;
 160   if(voltageNr == 6) {
 161       voltageNr = 0 ;
 162       cnt++;
 163       if(millis() > ( lastVoltMillis + 500) ){   // calculate average only once every 500 msec
 164         for (uint8_t cntVolt = 0 ; cntVolt < 6 ; cntVolt++) {
 165           if ( voltageData.mVoltPin[cntVolt] < 8) {
 166             voltageData.mVolt[cntVolt].value = round( ((float) voltageData.sumVoltage[cntVolt]  * voltageData.mVoltPerStep[cntVolt] ) / (float) cnt  ) + voltageData.offset[cntVolt];
 167 //            voltageData.mVolt[cntVolt].value = (voltageData.sumVoltage[cntVolt] / cnt  * voltageData.mVoltPerStep[cntVolt] ) + voltageData.offset[cntVolt];
 168 //            voltageData.mVolt[cntVolt].value = (1 + cntVolt) * 3000 + cntVolt * (millis() & 0xFF)  ; // this is just to test the cell calculation ; !!!!!!!!!!!to be removed
 169             voltageData.mVolt[cntVolt].available = true ;
 170             voltageData.sumVoltage[cntVolt] = 0 ;
 171 #ifdef DEBUGNEWVALUE
 172             printer->print("At ");
 173             printer->print(millis());
 174             printer->print(" Cnt = ");
 175             printer->print(cnt);
 176             printer->print(" mVolt ");
 177             printer->print(cntVolt);
 178             printer->print(" = ");
 179             printer->println( voltageData.mVolt[cntVolt].value );
 180 #endif
 181           } // if
 182         } // End For
 183
 184 #if defined ( NUMBEROFCELLS ) && (NUMBEROFCELLS > 0)
 185         voltageData.maxNumberOfCells = 0 ;
 186         int32_t mVoltOneCell ;
 187         uint8_t prevIndex ;
 188         for (uint8_t cellIndex = 0; cellIndex < NUMBEROFCELLS ; cellIndex++) {
 189           if (cellIndex == 0) {
 190             mVoltOneCell = voltageData.mVolt[cellIndex].value;
 191           } else {
 192             mVoltOneCell = voltageData.mVolt[cellIndex].value - voltageData.mVolt[prevIndex].value ;
 193           }
 194           prevIndex = cellIndex ;
 195           if (mVoltOneCell  < 500) {
 196             mVoltOneCell = 0 ;
 197           } else {
 198             voltageData.maxNumberOfCells = cellIndex + 1 ;
 199           }
 200           voltageData.mVoltCell[cellIndex]  = mVoltOneCell ;
 201           voltageData.mVoltCell_Available[cellIndex] = true ;
 202         }
 203         voltageData.mVoltCellMin = 0 ;
 204         voltageData.mVoltCellTot = 0 ;
 205         for (uint8_t cellIndex = 0; cellIndex < voltageData.maxNumberOfCells ; cellIndex++) {
 206           if (voltageData.mVoltCell[cellIndex] == 0 ) {
 207             break ;
 208           } else {
 209             if ( (voltageData.mVoltCellMin == 0) || ( voltageData.mVoltCellMin > voltageData.mVoltCell[cellIndex] ) ){
 210               voltageData.mVoltCellMin = voltageData.mVoltCell[cellIndex] ;
 211             }
 212               voltageData.mVoltCellTot = voltageData.mVolt[cellIndex].value ;
 213           }
 214         }
 215         if ( voltageData.mVoltCellMin > 0 ) {
 216           voltageData.mVoltCellMin_Available = true ;
 217         }
 218         voltageData.mVoltCellTot_Available = true ;
 219 #if ( PROTOCOL  ==  FRSKY_SPORT ) || ( PROTOCOL  ==  FRSKY_HUB ) || ( PROTOCOL  ==  FRSKY_SPORT_HUB )  // For FRSKY protocols
 220         if (voltageData.maxNumberOfCells > 0) {
 221             if (voltageData.maxNumberOfCells == 1) {
 222               secondMVolt = 0 ;
 223             }
 224             else {
 225               secondMVolt = voltageData.mVolt[1].value;
 226             }
 227             voltageData.mVoltCell_1_2.value = calculateCell(0, voltageData.mVolt[0].value , secondMVolt , 0, voltageData.maxNumberOfCells ) ;
 228             voltageData.mVoltCell_1_2.available = true ;
 229         }
 230         if (voltageData.maxNumberOfCells > 2) {
 231             if (voltageData.maxNumberOfCells == 3) {
 232               secondMVolt = 0 ;
 233             }
 234             else {
 235               secondMVolt = voltageData.mVolt[3].value ;
 236             }
 237             voltageData.mVoltCell_3_4.value = calculateCell(voltageData.mVolt[1].value , voltageData.mVolt[2].value , secondMVolt , 2 , voltageData.maxNumberOfCells) ;
 238             voltageData.mVoltCell_3_4.available = true ;
 239         }
 240         if (voltageData.maxNumberOfCells > 4) {
 241             if (voltageData.maxNumberOfCells == 5) {
 242               secondMVolt = 0 ;
 243             }
 244             else {
 245               secondMVolt = voltageData.mVolt[5].value ;
 246             }
 247             voltageData.mVoltCell_5_6.value = calculateCell(voltageData.mVolt[3].value , voltageData.mVolt[4].value , secondMVolt , 4 , voltageData.maxNumberOfCells) ;
 248             voltageData.mVoltCell_5_6.available = true ;
 249         }
 250 #endif // Enf of Frsky protocols
 251 #endif // ( NUMBEROFCELLS ) && (NUMBEROFCELLS > 0)
 252
 253 #if defined  ( FIRST_NTC_ON_VOLT_NR ) && defined ( LAST_NTC_ON_VOLT_NR ) && ( FIRST_NTC_ON_VOLT_NR <= 6 ) && ( LAST_NTC_ON_VOLT_NR <= 6) &&  ( FIRST_NTC_ON_VOLT_NR >= 1 ) && ( LAST_NTC_ON_VOLT_NR >= 1 ) && ( FIRST_NTC_ON_VOLT_NR <= LAST_NTC_ON_VOLT_NR )
 254         uint8_t numberOfNtc = LAST_NTC_ON_VOLT_NR - FIRST_NTC_ON_VOLT_NR + 1;
 255         uint8_t minNtcIdx ;
 256         uint8_t maxNtcIdx ;
 257         int32_t minTemp = 99999999 ;
 258         int32_t maxTemp = 0 ;
 259
 260        for ( uint8_t iNtc = FIRST_NTC_ON_VOLT_NR - 1 ; iNtc <= LAST_NTC_ON_VOLT_NR - 1 ; iNtc++ ) {
 261           convertNtcVoltToTemp( voltageData.mVolt[iNtc].value ) ;
 262           if (voltageData.mVolt[iNtc].value < minTemp ) {
 263             minTemp = voltageData.mVolt[iNtc].value ;
 264             minNtcIdx = iNtc + 1 ;
 265           }
 266           if (voltageData.mVolt[iNtc].value > maxTemp ) {
 267             maxTemp = voltageData.mVolt[iNtc].value ;
 268             maxNtcIdx = iNtc + 1 ;
 269           }
 270         }  // end for
 271         if ( numberOfNtc  > 2) {
 272           voltageData.mVolt[FIRST_NTC_ON_VOLT_NR - 1].value =  minTemp ;
 273           voltageData.mVolt[FIRST_NTC_ON_VOLT_NR ].value =  maxNtcIdx ;
 274           voltageData.mVolt[LAST_NTC_ON_VOLT_NR - 1].value =  maxTemp ;
 275         }
 276 #endif  // end of NTC calculations
 277
 278         cnt=0;
 279         lastVoltMillis = millis() ;
 280 #ifdef SEQUENCE_OUTPUTS
 281         lowVoltage = false ;
 282 #if defined( SEQUENCE_MIN_CELL)
 283         if ( voltageData.mVoltCellMin < SEQUENCE_MIN_CELL) {
 284           lowVoltage = true ;
 285         }
 286 #endif //SEQUENCE_MIN_CELL
 287 #if defined( SEQUENCE_MIN_VOLT_6) && defined (PIN_VOLTAGE_6 )
 288         if ( voltageData.mVolt[5].value < SEQUENCE_MIN_VOLT_6) {
 289           lowVoltage = true ;
 290         }
 291 #endif //( SEQUENCE_MIN_VOLT_6) && defined (PIN_VOLTAGE_6 )
 292 #ifdef DEBUGLOWVOLTAGE
 293             printer->print("LowVoltage= ");
 294             printer->println(lowVoltage) ;
 295 #endif
 296 #endif // SEQUENCE_OUTPUTS
 297
 298       }   // End if VOLT_BUFFER_LENGTH
 299    }     // End if == 6
 300 }
 301
 302
 303
 304
 305 int OXS_VOLTAGE::readVoltage( int value ) { // value is the index in an aray giving the pin to read
 306   //******** First discharge the capacitor of ADCMux to ground in order to avoid that measurement from another pin has an impact on this measurement
 307 #ifdef USE_INTERNAL_REFERENCE
 308   ADMUX = _BV(REFS1) | _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1) | _BV(MUX0); // binary = 11 00 1111 (11 = use internal VRef as max, 1111 = measure ground level)
 309 #elif defined(USE_EXTERNAL_REFERENCE)
 310   ADMUX =  _BV(MUX3) | _BV(MUX2) | _BV(MUX1) | _BV(MUX0);                          // binary = 00 00 1111 (00 = use external VRef as max, 1111 = measure ground level)
 311 #else
 312   ADMUX =  _BV(REFS0) | _BV(MUX3) | _BV(MUX2) | _BV(MUX1) | _BV(MUX0);             // binary = 01 00 1111 (01 = use Vcc as max, 1111 = measure ground level)
 313 #endif
 314     delayMicroseconds(200); // Wait for Vref to settle
 315   ADCSRA |= _BV(ADSC); // Start conversion
 316   while (bit_is_set(ADCSRA,ADSC)); // wait that conversion is done ; this takes 13 cycles of 125 khz (clock set in oXs_general.ccp so 104 usec
 317
 318   analogRead( voltageData.mVoltPin[value]); // read the value from the sensor ; it requires about 120 usec
 319   // discard the first measurement
 320   delayMicroseconds(100); // Wait for ADMux to settle
 321   return analogRead(voltageData.mVoltPin[value]); // use the second measurement ; it requires about 120 usec
 322 }
 323
 324
 325 void OXS_VOLTAGE::resetValues() {
 326   // not used currently
 327 }
 328
 329
 330
 331 #if defined ( NUMBEROFCELLS ) && (NUMBEROFCELLS > 0 )
 332 // calculate 2 cell voltages, make some checks and format in Frsky format.
 333 uint32_t OXS_VOLTAGE::calculateCell(int32_t V0 , int32_t V1 , int32_t V2 , uint8_t cellId , uint8_t  maxNumberOfCells) {
 334   int32_t cell_1 ;
 335   int32_t cell_2 ;
 336   if (V0 < 500) V0 = 0 ;
 337   if (V1 < 500) V1 = 0 ;
 338   if (V2 < 500) V2 = 0 ;
 339   cell_1= V1 - V0 ;
 340   cell_2= V2 - V1 ;
 341   if (cell_1 < 500) cell_1 = 0 ;
 342   if (cell_2 < 500) cell_2 = 0 ;
 343   cell_1 = (cell_1 >> 1) & 0xFFF ; //Frsky expects that the value are divided by 2
 344   cell_2 = (cell_2 >> 1) & 0xFFF ; //Frsky expects that the value are divided by 2
 345 #ifdef DEBUGCELLCALCULATION
 346             printer->print("Cell calculation for cellId ");
 347             printer->print(cellId) ;
 348             printer->print(" Frist Cell = ");
 349             printer->print(cell_1) ;
 350             printer->print(" Second Cell = ");
 351             printer->print(cell_2) ;
 352             printer->print(" Frsky value = ");
 353             printer->println( (cell_2 << 20) | (cell_1 << 8) | ( ( (int32_t) voltageData.maxNumberOfCells)<<4 ) | (int32_t) cellId  , HEX );
 354 #endif
 355   return (cell_2 << 20) | (cell_1 << 8) | ( ( (int32_t) voltageData.maxNumberOfCells )<<4 ) | (int32_t) cellId ;
 356 }
 357 #endif // end calculateCell
 358
 359
 360 #if defined(ARDUINO_MEASURES_VOLTAGES) && (ARDUINO_MEASURES_VOLTAGES == YES) && defined ( PIN_VOLTAGE ) && defined ( FIRST_NTC_ON_VOLT_NR )
 361 void  OXS_VOLTAGE::convertNtcVoltToTemp (int32_t &voltage ) {     //Calculate temperature using the Beta Factor equation
 362         // Convert the thermal stress value to resistance
 363         // we reuse here the mVolt calculated by oXs. The config must be adapted in a such a way that this mVolt is equal to the raw value returned by the ADC * 1000 (for better accuracy)
 364         // therefore, the mVoltPerStep calculated must be equal to 1000 and so :
 365         // USE_INTERNAL_REFERENCE must be as comment (so with // in front off)
 366         // USE_EXTERNAL_REFERENCE must be as comment (so with // in front off)
 367         // REFERENCE_VOLTAGE must be as comment (so with // in front off)
 368         // PIN_VOLTAGE must be defined and the analog pin used for NTC must be specified in one of the 6 index
 369         //  RESISTOR_TO_GROUND must be set on 0 (for the index being used)
 370         //  OFFSET_VOLTAGE must (normally) be set on 0 (for the index being used)
 371         //  SCALE_VOLTAGE  must be set on 204.6 (=1000 * 1023/5000) (for the index being used)
 372         // The calculated temperature is filled back in the voltage field
 373         // If there are more than 2 NTC, voltage from FIRST_NTC is filled with the lowest temp, voltage from LAST_NTC is filled with highest temp and voltage from FIRST_NTC+1 is filled with the index of the highest temp
 374 #if defined( DEBUG) && defined (DEBUGNTC)
 375          Serial.print( "Voltage= " ) ; Serial.print( voltage );
 376 #endif
 377
 378         float media =  SERIE_RESISTOR /  ( (1023000.0 / voltage ) - 1 ) ;
 379 /*
 380         float temperatura = media / TERMISTOR_NOMINAL;     // (R/Ro)
 381         temperatura = log(temperatura); // ln(R/Ro)
 382         temperatura /= B_COEFFICIENT;                   // 1/B * ln(R/Ro)
 383         temperatura += 1.0 / (TEMPERATURE_NOMINAL + 273.15); // + (1/To)
 384         temperatura = 1.0 / temperatura ;                 // Invert the value
 385         temperatura -= 273.15 ;                         // Convert it to Celsius
 386         voltage = temperatura ;
 387 */
 388         // T = 1/(A + B* ln(R) + C * ln(R) *ln(R) *ln(R))
 389         float steinhart = log(media) ;
 390         steinhart = 1 / ( STEINHART_A + STEINHART_B * steinhart + STEINHART_C * steinhart * steinhart * steinhart) ;
 391         steinhart -= 273.15 ;
 392         voltage = steinhart ;
 393
 394 #if defined( DEBUG) && defined (DEBUGNTC)
 395          Serial.print( "Temp= " ) ; Serial.println( voltage );
 396 #endif
 397
 398     }  // end convertNtcVoltToTemp
 399 #endif
 400
 401
 402
 403 #endif // end of PIN_VOLTAGE