From e58669bddbcd755b78428c46c3a9430368ca460a Mon Sep 17 00:00:00 2001 From: rtlopez Date: Thu, 31 Oct 2024 22:54:42 +0100 Subject: [PATCH] split baro, mag and voltage sensors --- lib/Espfc/src/Sensor/BaroSensor.cpp | 125 +++++++++++++++++++ lib/Espfc/src/Sensor/BaroSensor.h | 121 ++---------------- lib/Espfc/src/Sensor/MagSensor.cpp | 163 +++++++++++++++++++++++++ lib/Espfc/src/Sensor/MagSensor.h | 163 ++----------------------- lib/Espfc/src/Sensor/VoltageSensor.cpp | 115 +++++++++++++++++ lib/Espfc/src/Sensor/VoltageSensor.h | 114 ++--------------- 6 files changed, 429 insertions(+), 372 deletions(-) create mode 100644 lib/Espfc/src/Sensor/BaroSensor.cpp create mode 100644 lib/Espfc/src/Sensor/MagSensor.cpp create mode 100644 lib/Espfc/src/Sensor/VoltageSensor.cpp diff --git a/lib/Espfc/src/Sensor/BaroSensor.cpp b/lib/Espfc/src/Sensor/BaroSensor.cpp new file mode 100644 index 0000000..cba071b --- /dev/null +++ b/lib/Espfc/src/Sensor/BaroSensor.cpp @@ -0,0 +1,125 @@ +#include "BaroSensor.h" + +namespace Espfc { + +namespace Sensor { + +BaroSensor::BaroSensor(Model& model): _model(model), _state(BARO_STATE_INIT), _counter(0) {} + +int BaroSensor::begin() +{ + _model.state.baro.rate = 0; + if(!_model.baroActive()) return 0; + _baro = _model.state.baro.dev; + if(!_baro) return 0; + + _baro->setMode(BARO_MODE_TEMP); + int delay = _baro->getDelay(BARO_MODE_TEMP) + _baro->getDelay(BARO_MODE_PRESS); + int toGyroRate = (delay / _model.state.gyro.timer.interval) + 1; // number of gyro readings per cycle + int interval = _model.state.gyro.timer.interval * toGyroRate; + _model.state.baro.rate = 1000000 / interval; + + _model.state.baro.altitudeBiasSamples = 2 * _model.state.baro.rate; + + // TODO: move filters to BaroState + _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); + _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); + _altitudeFilter.begin(_model.config.baro.filter, _model.state.baro.rate); + + _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baro.rate).logln(_model.config.baro.filter.freq); + + return 1; +} + +int BaroSensor::update() +{ + int status = read(); + + return status; +} + +int BaroSensor::read() +{ + if(!_baro || !_model.baroActive()) return 0; + + if(_wait > micros()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_BARO); + + if(_model.config.debug.mode == DEBUG_BARO) + { + _model.state.debug[0] = _state; + } + + switch(_state) + { + case BARO_STATE_INIT: + _baro->setMode(BARO_MODE_TEMP); + _state = BARO_STATE_TEMP_GET; + _wait = micros() + _baro->getDelay(BARO_MODE_TEMP); + return 0; + case BARO_STATE_TEMP_GET: + readTemperature(); + _baro->setMode(BARO_MODE_PRESS); + _state = BARO_STATE_PRESS_GET; + _wait = micros() + _baro->getDelay(BARO_MODE_PRESS); + _counter = 1; + return 1; + case BARO_STATE_PRESS_GET: + readPressure(); + updateAltitude(); + if(--_counter > 0) + { + _baro->setMode(BARO_MODE_PRESS); + _state = BARO_STATE_PRESS_GET; + _wait = micros() + _baro->getDelay(BARO_MODE_PRESS); + } + else + { + _baro->setMode(BARO_MODE_TEMP); + _state = BARO_STATE_TEMP_GET; + _wait = micros() + _baro->getDelay(BARO_MODE_TEMP); + } + return 1; + break; + default: + _state = BARO_STATE_INIT; + break; + } + + return 0; +} + +void BaroSensor::readTemperature() +{ + _model.state.baro.temperatureRaw = _baro->readTemperature(); + _model.state.baro.temperature = _temperatureFilter.update(_model.state.baro.temperatureRaw); +} + +void BaroSensor::readPressure() +{ + _model.state.baro.pressureRaw = _baro->readPressure(); + _model.state.baro.pressure = _pressureFilter.update(_model.state.baro.pressureRaw); +} + +void BaroSensor::updateAltitude() +{ + _model.state.baro.altitudeRaw = _altitudeFilter.update(Math::toAltitude(_model.state.baro.pressure)); + if(_model.state.baro.altitudeBiasSamples > 0) + { + _model.state.baro.altitudeBiasSamples--; + _model.state.baro.altitudeBias += (_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias) * 0.2f; + } + _model.state.baro.altitude = _model.state.baro.altitudeRaw - _model.state.baro.altitudeBias; + + if(_model.config.debug.mode == DEBUG_BARO) + { + _model.state.debug[1] = lrintf(_model.state.baro.pressureRaw * 0.1f); // hPa x 10 + _model.state.debug[2] = lrintf(_model.state.baro.temperatureRaw * 100.f); // deg C x 100 + _model.state.debug[3] = lrintf(_model.state.baro.altitudeRaw * 10.f); + } +} + +} + +} diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h index dbeda6d..d5f8201 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.h +++ b/lib/Espfc/src/Sensor/BaroSensor.h @@ -2,6 +2,7 @@ #define _ESPFC_SENSOR_BARO_SENSOR_H_ #include "BaseSensor.h" +#include "Model.h" #include "Device/BaroDevice.h" #include "Filter.h" @@ -19,122 +20,16 @@ class BaroSensor: public BaseSensor BARO_STATE_PRESS_GET, }; - BaroSensor(Model& model): _model(model), _state(BARO_STATE_INIT), _counter(0) {} + BaroSensor(Model& model); - int begin() - { - _model.state.baro.rate = 0; - if(!_model.baroActive()) return 0; - _baro = _model.state.baro.dev; - if(!_baro) return 0; - - _baro->setMode(BARO_MODE_TEMP); - int delay = _baro->getDelay(BARO_MODE_TEMP) + _baro->getDelay(BARO_MODE_PRESS); - int toGyroRate = (delay / _model.state.gyro.timer.interval) + 1; // number of gyro readings per cycle - int interval = _model.state.gyro.timer.interval * toGyroRate; - _model.state.baro.rate = 1000000 / interval; - - _model.state.baro.altitudeBiasSamples = 2 * _model.state.baro.rate; - - // TODO: move filters to BaroState - _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); - _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baro.rate * 0.1f), _model.state.baro.rate); - _altitudeFilter.begin(_model.config.baro.filter, _model.state.baro.rate); - - _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(_baro->getAddress()).log(toGyroRate).log(_model.state.baro.rate).logln(_model.config.baro.filter.freq); - - return 1; - } - - int update() - { - int status = read(); - - return status; - } - - int read() - { - if(!_baro || !_model.baroActive()) return 0; - - if(_wait > micros()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_BARO); - - if(_model.config.debug.mode == DEBUG_BARO) - { - _model.state.debug[0] = _state; - } - - switch(_state) - { - case BARO_STATE_INIT: - _baro->setMode(BARO_MODE_TEMP); - _state = BARO_STATE_TEMP_GET; - _wait = micros() + _baro->getDelay(BARO_MODE_TEMP); - return 0; - case BARO_STATE_TEMP_GET: - readTemperature(); - _baro->setMode(BARO_MODE_PRESS); - _state = BARO_STATE_PRESS_GET; - _wait = micros() + _baro->getDelay(BARO_MODE_PRESS); - _counter = 1; - return 1; - case BARO_STATE_PRESS_GET: - readPressure(); - updateAltitude(); - if(--_counter > 0) - { - _baro->setMode(BARO_MODE_PRESS); - _state = BARO_STATE_PRESS_GET; - _wait = micros() + _baro->getDelay(BARO_MODE_PRESS); - } - else - { - _baro->setMode(BARO_MODE_TEMP); - _state = BARO_STATE_TEMP_GET; - _wait = micros() + _baro->getDelay(BARO_MODE_TEMP); - } - return 1; - break; - default: - _state = BARO_STATE_INIT; - break; - } - - return 0; - } + int begin(); + int update(); + int read(); private: - void readTemperature() - { - _model.state.baro.temperatureRaw = _baro->readTemperature(); - _model.state.baro.temperature = _temperatureFilter.update(_model.state.baro.temperatureRaw); - } - - void readPressure() - { - _model.state.baro.pressureRaw = _baro->readPressure(); - _model.state.baro.pressure = _pressureFilter.update(_model.state.baro.pressureRaw); - } - - void updateAltitude() - { - _model.state.baro.altitudeRaw = _altitudeFilter.update(Math::toAltitude(_model.state.baro.pressure)); - if(_model.state.baro.altitudeBiasSamples > 0) - { - _model.state.baro.altitudeBiasSamples--; - _model.state.baro.altitudeBias += (_model.state.baro.altitudeRaw - _model.state.baro.altitudeBias) * 0.2f; - } - _model.state.baro.altitude = _model.state.baro.altitudeRaw - _model.state.baro.altitudeBias; - - if(_model.config.debug.mode == DEBUG_BARO) - { - _model.state.debug[1] = lrintf(_model.state.baro.pressureRaw * 0.1f); // hPa x 10 - _model.state.debug[2] = lrintf(_model.state.baro.temperatureRaw * 100.f); // deg C x 100 - _model.state.debug[3] = lrintf(_model.state.baro.altitudeRaw * 10.f); - } - } + void readTemperature(); + void readPressure(); + void updateAltitude(); Model& _model; Device::BaroDevice * _baro; diff --git a/lib/Espfc/src/Sensor/MagSensor.cpp b/lib/Espfc/src/Sensor/MagSensor.cpp new file mode 100644 index 0000000..f98cf63 --- /dev/null +++ b/lib/Espfc/src/Sensor/MagSensor.cpp @@ -0,0 +1,163 @@ +#include "MagSensor.h" +#include + +#ifdef abs +#undef abs +#endif + +namespace Espfc { + +namespace Sensor { + +MagSensor::MagSensor(Model& model): _model(model) {} + +int MagSensor::begin() +{ + if(!_model.magActive()) return 0; + + _mag = _model.state.mag.dev; + if(!_mag) return 0; + + if(_model.state.mag.timer.rate < 5) return 0; + + // by default use eeprom calibration settings + _model.state.mag.calibrationState = CALIBRATION_IDLE; + _model.state.mag.calibrationValid = true; + + _model.logger.info().log(F("MAG INIT")).log(FPSTR(Device::MagDevice::getName(_mag->getType()))).log(_mag->getAddress()).logln(_model.state.mag.timer.rate); + + return 1; +} + +int MagSensor::update() +{ + int status = read(); + + if (status) filter(); + + return status; +} + +int MagSensor::read() +{ + if(!_mag || !_model.magActive() || !_model.state.mag.timer.check()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_MAG_READ); + _mag->readMag(_model.state.mag.raw); + + return 1; +} + +int MagSensor::filter() +{ + if(!_mag || !_model.magActive()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER); + + _model.state.mag.adc = _mag->convert(_model.state.mag.raw); + + align(_model.state.mag.adc, _model.config.mag.align); + _model.state.mag.adc = _model.state.boardAlignment.apply(_model.state.mag.adc); + + for(size_t i = 0; i < AXIS_COUNT_RPY; i++) + { + _model.state.mag.adc.set(i, _model.state.mag.filter[i].update(_model.state.mag.adc[i])); + } + + calibrate(); + + return 1; +} + +void MagSensor::calibrate() +{ + switch(_model.state.mag.calibrationState) + { + case CALIBRATION_IDLE: + if(_model.state.mag.calibrationValid) + { + _model.state.mag.adc -= _model.state.mag.calibrationOffset; + _model.state.mag.adc *= _model.state.mag.calibrationScale; + } + break; + case CALIBRATION_START: + resetCalibration(); + _model.state.mag.calibrationSamples = 30 * _model.state.mag.timer.rate; + _model.state.mag.calibrationState = CALIBRATION_UPDATE; + break; + case CALIBRATION_UPDATE: + updateCalibration(); + _model.state.mag.calibrationSamples--; + if(_model.state.mag.calibrationSamples <= 0) _model.state.mag.calibrationState = CALIBRATION_APPLY; + break; + case CALIBRATION_APPLY: + applyCalibration(); + _model.state.mag.calibrationState = CALIBRATION_SAVE; + break; + case CALIBRATION_SAVE: + _model.finishCalibration(); + _model.state.mag.calibrationState = CALIBRATION_IDLE; + break; + default: + _model.state.mag.calibrationState = CALIBRATION_IDLE; + break; + } +} + +void MagSensor::resetCalibration() +{ + _model.state.mag.calibrationMin = VectorFloat(); + _model.state.mag.calibrationMax = VectorFloat(); + _model.state.mag.calibrationValid = false; +} + +void MagSensor::updateCalibration() +{ + for(int i = 0; i < AXIS_COUNT_RPY; i++) + { + if(_model.state.mag.adc[i] < _model.state.mag.calibrationMin[i]) _model.state.mag.calibrationMin.set(i, _model.state.mag.adc[i]); + if(_model.state.mag.adc[i] > _model.state.mag.calibrationMax[i]) _model.state.mag.calibrationMax.set(i, _model.state.mag.adc[i]); + } +} + +void MagSensor::applyCalibration() +{ + const float EPSILON = 0.001f; + + // verify calibration data and find biggest range + float maxRange = -1; + for(int i = 0; i < AXIS_COUNT_RPY; i++) + { + if(_model.state.mag.calibrationMin[i] > -EPSILON) return; + if(_model.state.mag.calibrationMax[i] < EPSILON) return; + if((_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]) > maxRange) + { + maxRange = _model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]; + } + } + + // probably incomplete data, must be positive + if(maxRange <= 0) return; + + VectorFloat scale(1.f, 1.f, 1.f); + VectorFloat offset(0.f, 0.f, 0.f); + + for (int i = 0; i < AXIS_COUNT_RPY; i++) + { + const float range = (_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]); + const float bias = (_model.state.mag.calibrationMax[i] + _model.state.mag.calibrationMin[i]) * 0.5f; + + if(std::abs(range) < EPSILON) return; // incomplete data + + scale.set(i, maxRange / range); // makes everything the same range + offset.set(i, bias); // level bias + } + + _model.state.mag.calibrationScale = scale; + _model.state.mag.calibrationOffset = offset; + _model.state.mag.calibrationValid = true; +} + +} + +} diff --git a/lib/Espfc/src/Sensor/MagSensor.h b/lib/Espfc/src/Sensor/MagSensor.h index 01f00c7..0c42417 100644 --- a/lib/Espfc/src/Sensor/MagSensor.h +++ b/lib/Espfc/src/Sensor/MagSensor.h @@ -1,11 +1,9 @@ -#ifndef _ESPFC_SENSOR_MAG_SENSOR_H_ -#define _ESPFC_SENSOR_MAG_SENSOR_H_ +#pragma once +#include "Model.h" #include "BaseSensor.h" #include "Device/MagDevice.h" -#include - namespace Espfc { namespace Sensor { @@ -13,156 +11,18 @@ namespace Sensor { class MagSensor: public BaseSensor { public: - MagSensor(Model& model): _model(model) {} - - int begin() - { - if(!_model.magActive()) return 0; - - _mag = _model.state.mag.dev; - if(!_mag) return 0; - - if(_model.state.mag.timer.rate < 5) return 0; - - // by default use eeprom calibration settings - _model.state.mag.calibrationState = CALIBRATION_IDLE; - _model.state.mag.calibrationValid = true; - - _model.logger.info().log(F("MAG INIT")).log(FPSTR(Device::MagDevice::getName(_mag->getType()))).log(_mag->getAddress()).logln(_model.state.mag.timer.rate); - - return 1; - } - - int update() - { - int status = read(); - - if (status) filter(); - - return status; - } - - int read() - { - if(!_mag || !_model.magActive() || !_model.state.mag.timer.check()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_MAG_READ); - _mag->readMag(_model.state.mag.raw); - - return 1; - } - - int filter() - { - if(!_mag || !_model.magActive()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_MAG_FILTER); + MagSensor(Model& model); - _model.state.mag.adc = _mag->convert(_model.state.mag.raw); - - - align(_model.state.mag.adc, _model.config.mag.align); - _model.state.mag.adc = _model.state.boardAlignment.apply(_model.state.mag.adc); - - for(size_t i = 0; i < AXIS_COUNT_RPY; i++) - { - _model.state.mag.adc.set(i, _model.state.mag.filter[i].update(_model.state.mag.adc[i])); - } - - calibrate(); - - return 1; - } + int begin(); + int update(); + int read(); + int filter(); private: - void calibrate() - { - switch(_model.state.mag.calibrationState) - { - case CALIBRATION_IDLE: - if(_model.state.mag.calibrationValid) - { - _model.state.mag.adc -= _model.state.mag.calibrationOffset; - _model.state.mag.adc *= _model.state.mag.calibrationScale; - } - break; - case CALIBRATION_START: - resetCalibration(); - _model.state.mag.calibrationSamples = 30 * _model.state.mag.timer.rate; - _model.state.mag.calibrationState = CALIBRATION_UPDATE; - break; - case CALIBRATION_UPDATE: - updateCalibration(); - _model.state.mag.calibrationSamples--; - if(_model.state.mag.calibrationSamples <= 0) _model.state.mag.calibrationState = CALIBRATION_APPLY; - break; - case CALIBRATION_APPLY: - applyCalibration(); - _model.state.mag.calibrationState = CALIBRATION_SAVE; - break; - case CALIBRATION_SAVE: - _model.finishCalibration(); - _model.state.mag.calibrationState = CALIBRATION_IDLE; - break; - default: - _model.state.mag.calibrationState = CALIBRATION_IDLE; - break; - } - } - - void resetCalibration() - { - _model.state.mag.calibrationMin = VectorFloat(); - _model.state.mag.calibrationMax = VectorFloat(); - _model.state.mag.calibrationValid = false; - } - - void updateCalibration() - { - for(int i = 0; i < AXIS_COUNT_RPY; i++) - { - if(_model.state.mag.adc[i] < _model.state.mag.calibrationMin[i]) _model.state.mag.calibrationMin.set(i, _model.state.mag.adc[i]); - if(_model.state.mag.adc[i] > _model.state.mag.calibrationMax[i]) _model.state.mag.calibrationMax.set(i, _model.state.mag.adc[i]); - } - } - - void applyCalibration() - { - const float EPSILON = 0.001f; - - // verify calibration data and find biggest range - float maxRange = -1; - for(int i = 0; i < AXIS_COUNT_RPY; i++) - { - if(_model.state.mag.calibrationMin[i] > -EPSILON) return; - if(_model.state.mag.calibrationMax[i] < EPSILON) return; - if((_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]) > maxRange) - { - maxRange = _model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]; - } - } - - // probably incomplete data, must be positive - if(maxRange <= 0) return; - - VectorFloat scale(1.f, 1.f, 1.f); - VectorFloat offset(0.f, 0.f, 0.f); - - for (int i = 0; i < AXIS_COUNT_RPY; i++) - { - const float range = (_model.state.mag.calibrationMax[i] - _model.state.mag.calibrationMin[i]); - const float bias = (_model.state.mag.calibrationMax[i] + _model.state.mag.calibrationMin[i]) * 0.5f; - - if(abs(range) < EPSILON) return; // incomplete data - - scale.set(i, maxRange / range); // makes everything the same range - offset.set(i, bias); // level bias - } - - _model.state.mag.calibrationScale = scale; - _model.state.mag.calibrationOffset = offset; - _model.state.mag.calibrationValid = true; - } + void calibrate(); + void resetCalibration(); + void updateCalibration(); + void applyCalibration(); Model& _model; Device::MagDevice * _mag; @@ -171,4 +31,3 @@ class MagSensor: public BaseSensor } } -#endif \ No newline at end of file diff --git a/lib/Espfc/src/Sensor/VoltageSensor.cpp b/lib/Espfc/src/Sensor/VoltageSensor.cpp new file mode 100644 index 0000000..6718ba0 --- /dev/null +++ b/lib/Espfc/src/Sensor/VoltageSensor.cpp @@ -0,0 +1,115 @@ +#include "VoltageSensor.h" + +#include + +namespace Espfc { + +namespace Sensor { + +VoltageSensor::VoltageSensor(Model &model) : _model(model) {} + +int VoltageSensor::begin() +{ + _model.state.battery.timer.setRate(100); + _model.state.battery.samples = 50; + + _vFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); + _vFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); + + _iFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); + _iFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); + + _state = VBAT; + + return 1; +} + +int VoltageSensor::update() +{ + if (!_model.state.battery.timer.check()) return 0; + + Stats::Measure measure(_model.state.stats, COUNTER_BATTERY); + + switch (_state) + { + case VBAT: + _state = IBAT; + return readVbat(); + case IBAT: + _state = VBAT; + return readIbat(); + } + + return 0; +} + +int VoltageSensor::readVbat() +{ +#ifdef ESPFC_ADC_0 + if (_model.config.vbat.source != 1 || _model.config.pin[PIN_INPUT_ADC_0] == -1) return 0; + // wemos d1 mini has divider 3.2:1 (220k:100k) + // additionaly I've used divider 5.7:1 (4k7:1k) + // total should equals ~18.24:1, 73:4 resDiv:resMult should be ideal, + // but ~52:1 is real, did I miss something? + _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_0]); + float volts = _vFilterFast.update(_model.state.battery.rawVoltage * ESPFC_ADC_SCALE); + + volts *= _model.config.vbat.scale * 0.1f; + volts *= _model.config.vbat.resMult; + volts /= _model.config.vbat.resDiv; + + _model.state.battery.voltageUnfiltered = volts; + _model.state.battery.voltage = _vFilter.update(_model.state.battery.voltageUnfiltered); + + // cell count detection + if (_model.state.battery.samples > 0) + { + _model.state.battery.cells = std::ceil(_model.state.battery.voltage / 4.2f); + _model.state.battery.samples--; + } + + _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6); + _model.state.battery.percentage = Math::clamp(Math::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f); + + if (_model.config.debug.mode == DEBUG_BATTERY) + { + _model.state.debug[0] = constrain(lrintf(_model.state.battery.voltageUnfiltered * 100.0f), 0, 32000); + _model.state.debug[1] = constrain(lrintf(_model.state.battery.voltage * 100.0f), 0, 32000); + } + return 1; +#else + return 0; +#endif +} + +int VoltageSensor::readIbat() +{ +#ifdef ESPFC_ADC_1 + if (_model.config.ibat.source != 1 && _model.config.pin[PIN_INPUT_ADC_1] == -1) return 0; + + _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_1]); + float volts = _iFilterFast.update(_model.state.battery.rawCurrent * ESPFC_ADC_SCALE); + float milivolts = volts * 1000.0f; + + volts += _model.config.ibat.offset * 0.001f; + volts *= _model.config.ibat.scale * 0.1f; + + _model.state.battery.currentUnfiltered = volts; + _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered); + + if (_model.config.debug.mode == DEBUG_CURRENT_SENSOR) + { + _model.state.debug[0] = lrintf(milivolts); + _model.state.debug[1] = constrain(lrintf(_model.state.battery.currentUnfiltered * 100.0f), 0, 32000); + _model.state.debug[2] = _model.state.battery.rawCurrent; + } + + return 1; +#else + return 0; +#endif +} + +} + +} diff --git a/lib/Espfc/src/Sensor/VoltageSensor.h b/lib/Espfc/src/Sensor/VoltageSensor.h index 3e4af7a..8c8688b 100644 --- a/lib/Espfc/src/Sensor/VoltageSensor.h +++ b/lib/Espfc/src/Sensor/VoltageSensor.h @@ -1,9 +1,8 @@ -#ifndef _ESPFC_SENSOR_VOLTAGE_SENSOR_H_ -#define _ESPFC_SENSOR_VOLTAGE_SENSOR_H_ +#pragma once #include "Model.h" #include "BaseSensor.h" -#include +#include "Filter.h" namespace Espfc { @@ -16,108 +15,11 @@ class VoltageSensor: public BaseSensor VBAT, IBAT }; - VoltageSensor(Model& model): _model(model) {} - - int begin() - { - _model.state.battery.timer.setRate(100); - _model.state.battery.samples = 50; - - _vFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); - _vFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); - - _iFilterFast.begin(FilterConfig(FILTER_PT1, 20), _model.state.battery.timer.rate); - _iFilter.begin(FilterConfig(FILTER_PT2, 2), _model.state.battery.timer.rate); - - _state = VBAT; - - return 1; - } - - int update() - { - if(!_model.state.battery.timer.check()) return 0; - - Stats::Measure measure(_model.state.stats, COUNTER_BATTERY); - - switch(_state) { - case VBAT: - _state = IBAT; - return readVbat(); - case IBAT: - _state = VBAT; - return readIbat(); - } - - return 0; - } - - int readVbat() - { -#ifdef ESPFC_ADC_0 - if(_model.config.vbat.source != 1 || _model.config.pin[PIN_INPUT_ADC_0] == -1) return 0; - // wemos d1 mini has divider 3.2:1 (220k:100k) - // additionaly I've used divider 5.7:1 (4k7:1k) - // total should equals ~18.24:1, 73:4 resDiv:resMult should be ideal, - // but ~52:1 is real, did I miss something? - _model.state.battery.rawVoltage = analogRead(_model.config.pin[PIN_INPUT_ADC_0]); - float volts = _vFilterFast.update(_model.state.battery.rawVoltage * ESPFC_ADC_SCALE); - - volts *= _model.config.vbat.scale * 0.1f; - volts *= _model.config.vbat.resMult; - volts /= _model.config.vbat.resDiv; - - _model.state.battery.voltageUnfiltered = volts; - _model.state.battery.voltage = _vFilter.update(_model.state.battery.voltageUnfiltered); - - // cell count detection - if(_model.state.battery.samples > 0) - { - _model.state.battery.cells = std::ceil(_model.state.battery.voltage / 4.2f); - _model.state.battery.samples--; - } - - _model.state.battery.cellVoltage = _model.state.battery.voltage / constrain(_model.state.battery.cells, 1, 6); - _model.state.battery.percentage = Math::clamp(Math::map(_model.state.battery.cellVoltage, 3.4f, 4.2f, 0.0f, 100.0f), 0.0f, 100.0f); - - if(_model.config.debug.mode == DEBUG_BATTERY) - { - _model.state.debug[0] = constrain(lrintf(_model.state.battery.voltageUnfiltered * 100.0f), 0, 32000); - _model.state.debug[1] = constrain(lrintf(_model.state.battery.voltage * 100.0f), 0, 32000); - } - return 1; -#else - return 0; -#endif - } - - int readIbat() - { -#ifdef ESPFC_ADC_1 - if(_model.config.ibat.source != 1 && _model.config.pin[PIN_INPUT_ADC_1] == -1) return 0; - - _model.state.battery.rawCurrent = analogRead(_model.config.pin[PIN_INPUT_ADC_1]); - float volts = _iFilterFast.update(_model.state.battery.rawCurrent * ESPFC_ADC_SCALE); - float milivolts = volts * 1000.0f; - - volts += _model.config.ibat.offset * 0.001f; - volts *= _model.config.ibat.scale * 0.1f; - - _model.state.battery.currentUnfiltered = volts; - _model.state.battery.current = _iFilter.update(_model.state.battery.currentUnfiltered); - - if(_model.config.debug.mode == DEBUG_CURRENT_SENSOR) - { - _model.state.debug[0] = lrintf(milivolts); - _model.state.debug[1] = constrain(lrintf(_model.state.battery.currentUnfiltered * 100.0f), 0, 32000); - _model.state.debug[2] = _model.state.battery.rawCurrent; - } - - return 1; -#else - return 0; -#endif - } + VoltageSensor(Model& model); + int begin(); + int update(); + int readVbat(); + int readIbat(); private: Model& _model; @@ -131,5 +33,3 @@ class VoltageSensor: public BaseSensor } } - -#endif \ No newline at end of file