From 19a5712563bc9d44d86af65f67673555a8ddab8e Mon Sep 17 00:00:00 2001 From: rtlopez Date: Fri, 28 Jul 2023 00:34:40 +0200 Subject: [PATCH] baro code tidy --- lib/Espfc/src/Device/BaroBMP085.h | 33 +++++++++++++++---------------- lib/Espfc/src/Device/BaroBMP280.h | 20 +++++++++---------- lib/Espfc/src/Device/BaroDevice.h | 3 +-- lib/Espfc/src/Model.h | 4 ---- lib/Espfc/src/ModelState.h | 1 - lib/Espfc/src/Sensor/BaroSensor.h | 24 +++++++++++----------- lib/Espfc/src/Telemetry.h | 30 ---------------------------- 7 files changed, 40 insertions(+), 75 deletions(-) diff --git a/lib/Espfc/src/Device/BaroBMP085.h b/lib/Espfc/src/Device/BaroBMP085.h index b228ecfb..7985feea 100644 --- a/lib/Espfc/src/Device/BaroBMP085.h +++ b/lib/Espfc/src/Device/BaroBMP085.h @@ -79,7 +79,7 @@ class BaroBMP085: public BaroDevice int32_t x2 = ((int32_t)_cal.mc << 11) / (x1 + _cal.md); _t_fine = x1 + x2; //_t_fine = (_t_fine + x1 + x2 + 1) >> 1; // avg of last two samples - return (float)((_t_fine + 8) >> 4) / 10.0f; + return (float)((_t_fine + 8) >> 4) * 0.1f; } virtual float readPressure() override @@ -87,12 +87,13 @@ class BaroBMP085: public BaroDevice uint8_t buffer[3]; _bus->read(_addr, BMP085_MEASUREMENT_REG, 3, buffer); + uint8_t oss = (_mode & 0xC0) >> 6; + uint32_t up = ((uint32_t)buffer[0] << 16) + ((uint16_t)buffer[1] << 8) + buffer[2]; - if(_mode & 0x34) up = up >> (8 - ((_mode & 0xC0) >> 6)); + if(_mode & 0x34) up = up >> (8 - oss); if(up == 0) return NAN; - uint8_t oss = (_mode & 0xC0) >> 6; int32_t b6 = _t_fine - 4000; int32_t x1 = ((int32_t)_cal.b2 * ((b6 * b6) >> 12)) >> 11; int32_t x2 = ((int32_t)_cal.ac2 * b6) >> 11; @@ -118,24 +119,22 @@ class BaroBMP085: public BaroDevice void setMode(BaroDeviceMode mode) { - _mode = mode == BARO_MODE_TEMP ? BMP085_MEASURE_T : BMP085_MEASURE_P0; + _mode = mode == BARO_MODE_TEMP ? BMP085_MEASURE_T : BMP085_MEASURE_P2; _bus->writeByte(_addr, BMP085_CONTROL_REG, _mode); } - virtual int getDelay() const override - { - const int comp = 50; - if(_mode == BMP085_MEASURE_T) return 4500 + comp; // temp - else if(_mode == BMP085_MEASURE_P0) return 4500 + comp; // press_0 - else if(_mode == BMP085_MEASURE_P1) return 7500 + comp; // press_1 - else if(_mode == BMP085_MEASURE_P2) return 13500 + comp; // press_2 - else if(_mode == BMP085_MEASURE_P3) return 25500 + comp; // press_3 - else return 4500 + 50; // invalid mode - } - - virtual int getDenom() const override + virtual int getDelay(BaroDeviceMode mode) const override { - return 1; + switch(mode) + { + case BARO_MODE_TEMP: + return 4550; // temp + default: + //return 4550; // press_0 + //return 7550; // press_1 + return 13550; // press_2 + //return 25550; // press_3 + } } bool testConnection() override diff --git a/lib/Espfc/src/Device/BaroBMP280.h b/lib/Espfc/src/Device/BaroBMP280.h index 3842dfee..d680b504 100644 --- a/lib/Espfc/src/Device/BaroBMP280.h +++ b/lib/Espfc/src/Device/BaroBMP280.h @@ -135,17 +135,17 @@ class BaroBMP280: public BaroDevice (void)mode; } - virtual int getDelay() const override + virtual int getDelay(BaroDeviceMode mode) const override { - //return 5500 / 2; // if sapling X1 - //return 7500 / 2; // if sampling X2 - return 11500 / 2; // if sampling X4 - } - - // notify BaroSensor to sample lower as delay is divided by 2 - virtual int getDenom() const override - { - return 2; + switch(mode) + { + case BARO_MODE_TEMP: + return 0; + default: + //return 5500; // if sapling X1 + //return 7500; // if sampling X2 + return 11500; // if sampling X4 + } } bool testConnection() override diff --git a/lib/Espfc/src/Device/BaroDevice.h b/lib/Espfc/src/Device/BaroDevice.h index 87e85ddf..31aa8640 100644 --- a/lib/Espfc/src/Device/BaroDevice.h +++ b/lib/Espfc/src/Device/BaroDevice.h @@ -34,8 +34,7 @@ class BaroDevice: public BusAwareDevice virtual float readTemperature() = 0; virtual float readPressure() = 0; - virtual int getDelay() const = 0; - virtual int getDenom() const = 0; + virtual int getDelay(BaroDeviceMode mode) const = 0; virtual void setMode(BaroDeviceMode mode) = 0; virtual bool testConnection() = 0; diff --git a/lib/Espfc/src/Model.h b/lib/Espfc/src/Model.h index 2eef78a7..e622b43a 100644 --- a/lib/Espfc/src/Model.h +++ b/lib/Espfc/src/Model.h @@ -512,11 +512,7 @@ class Model } state.customMixer = MixerConfig(config.customMixerCount, config.customMixes); - state.telemetry = config.telemetry; - state.baroAltitudeBiasSamples = 300; - // override temporary - //state.telemetry = true; //state.telemetryTimer.setRate(100); } diff --git a/lib/Espfc/src/ModelState.h b/lib/Espfc/src/ModelState.h index 84ecd284..ef8074cc 100644 --- a/lib/Espfc/src/ModelState.h +++ b/lib/Espfc/src/ModelState.h @@ -261,7 +261,6 @@ struct ModelState VectorFloat magCalibrationScale; VectorFloat magCalibrationOffset; - bool telemetry; Timer telemetryTimer; Stats stats; diff --git a/lib/Espfc/src/Sensor/BaroSensor.h b/lib/Espfc/src/Sensor/BaroSensor.h index e79e207e..b43d3b04 100644 --- a/lib/Espfc/src/Sensor/BaroSensor.h +++ b/lib/Espfc/src/Sensor/BaroSensor.h @@ -19,7 +19,7 @@ class BaroSensor: public BaseSensor BARO_STATE_PRESS_GET, }; - BaroSensor(Model& model): _model(model), _state(BARO_STATE_INIT), _counter(0), _temp_denom(1) {} + BaroSensor(Model& model): _model(model), _state(BARO_STATE_INIT), _counter(0) {} int begin() { @@ -29,13 +29,15 @@ class BaroSensor: public BaseSensor if(!_baro) return 0; _baro->setMode(BARO_MODE_TEMP); - int delay = _baro->getDelay(); + int delay = _baro->getDelay(BARO_MODE_TEMP) + _baro->getDelay(BARO_MODE_PRESS); int toGyroRate = (delay / _model.state.gyroTimer.interval) + 1; // number of gyro readings per cycle int interval = _model.state.gyroTimer.interval * toGyroRate; - _model.state.baroRate = (1000000 / interval) / _baro->getDenom(); + _model.state.baroRate = 1000000 / interval; - _temperatureFilter.begin(FilterConfig(FILTER_PT1, 5), _model.state.baroRate / _temp_denom); - _pressureFilter.begin(FilterConfig(FILTER_PT1, 5), _model.state.baroRate); + _model.state.baroAltitudeBiasSamples = 2 * _model.state.baroRate; + + _temperatureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baroRate * 0.1f), _model.state.baroRate); + _pressureFilter.begin(FilterConfig(FILTER_PT1, _model.state.baroRate * 0.1f), _model.state.baroRate); _altitudeFilter.begin(_model.config.baroFilter, _model.state.baroRate); _model.logger.info().log(F("BARO INIT")).log(FPSTR(Device::BaroDevice::getName(_baro->getType()))).log(toGyroRate).log(_model.state.baroRate).logln(_model.config.baroFilter.freq); @@ -68,14 +70,14 @@ class BaroSensor: public BaseSensor case BARO_STATE_INIT: _baro->setMode(BARO_MODE_TEMP); _state = BARO_STATE_TEMP_GET; - _wait = micros() + _baro->getDelay(); + _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(); - _counter = _temp_denom; + _wait = micros() + _baro->getDelay(BARO_MODE_PRESS); + _counter = 1; return 1; case BARO_STATE_PRESS_GET: readPressure(); @@ -84,13 +86,14 @@ class BaroSensor: public BaseSensor { _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); } - _wait = micros() + _baro->getDelay(); return 1; break; default: @@ -120,7 +123,7 @@ class BaroSensor: public BaseSensor if(_model.state.baroAltitudeBiasSamples > 0) { _model.state.baroAltitudeBiasSamples--; - _model.state.baroAltitudeBias += (_model.state.baroAltitudeRaw - _model.state.baroAltitudeBias) * 0.25f; + _model.state.baroAltitudeBias += (_model.state.baroAltitudeRaw - _model.state.baroAltitudeBias) * 0.2f; } _model.state.baroAltitude = _model.state.baroAltitudeRaw - _model.state.baroAltitudeBias; @@ -140,7 +143,6 @@ class BaroSensor: public BaseSensor Filter _altitudeFilter; uint32_t _wait; int32_t _counter; - int32_t _temp_denom; }; } diff --git a/lib/Espfc/src/Telemetry.h b/lib/Espfc/src/Telemetry.h index 5b2416cc..fe8d8f2d 100644 --- a/lib/Espfc/src/Telemetry.h +++ b/lib/Espfc/src/Telemetry.h @@ -16,36 +16,6 @@ class Telemetry { Stats::Measure measure(_model.state.stats, COUNTER_TELEMETRY); - /* - _value = _value + (_up ? 1 : -1); - - if (_up && _value >= 1811) _up = false; - if (!_up && _value <= 172) _up = true; - - Rc::CrsfFrame frame; - Rc::CrsfData data; - data.chan0 = _value; - data.chan1 = _value; - data.chan2 = _value; - data.chan3 = _value; - data.chan4 = _value; - data.chan5 = _value; - data.chan6 = _value; - data.chan7 = _value; - data.chan8 = _value; - data.chan9 = _value; - data.chan10 = _value; - data.chan11 = _value; - data.chan12 = _value; - data.chan13 = _value; - data.chan14 = _value; - data.chan15 = _value; - - Rc::Crsf::encodeRcData(frame, data); - - size_t size = frame.message.size + 2; - s.write(frame.data, size); - */ //print(s, _model.state.gyro.x, 3); //println(s);