Skip to content

Commit

Permalink
Merge pull request #50 from rtlopez/baro-fix
Browse files Browse the repository at this point in the history 
baro code tidy
  • Loading branch information
@rtlopez
rtlopez authored Jul 31, 2023
2 parents 210b815 + 19a5712 commit 2b9f3a5
Show file tree
Hide file tree
Showing 7 changed files with 40 additions and 75 deletions.
33 changes: 16 additions & 17 deletions lib/Espfc/src/Device/BaroBMP085.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,20 +79,21 @@ 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
{
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;
Expand All @@ -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
Expand Down
20 changes: 10 additions & 10 deletions lib/Espfc/src/Device/BaroBMP280.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
3 changes: 1 addition & 2 deletions lib/Espfc/src/Device/BaroDevice.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
4 changes: 0 additions & 4 deletions lib/Espfc/src/Model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}

Expand Down
1 change: 0 additions & 1 deletion lib/Espfc/src/ModelState.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -261,7 +261,6 @@ struct ModelState
VectorFloat magCalibrationScale;
VectorFloat magCalibrationOffset;

bool telemetry;
Timer telemetryTimer;

Stats stats;
Expand Down
24 changes: 13 additions & 11 deletions lib/Espfc/src/Sensor/BaroSensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
{
Expand All @@ -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);
Expand Down Expand Up @@ -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();
Expand All @@ -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:
Expand Down Expand Up @@ -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;

Expand All @@ -140,7 +143,6 @@ class BaroSensor: public BaseSensor
Filter _altitudeFilter;
uint32_t _wait;
int32_t _counter;
int32_t _temp_denom;
};

}
Expand Down
30 changes: 0 additions & 30 deletions lib/Espfc/src/Telemetry.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down

0 comments on commit 2b9f3a5

Please sign in to comment.