Skip to content

Commit

Permalink
split baro, mag and voltage sensors
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Oct 31, 2024
1 parent b034b67 commit e58669b
Show file tree
Hide file tree
Showing 6 changed files with 429 additions and 372 deletions.
125 changes: 125 additions & 0 deletions lib/Espfc/src/Sensor/BaroSensor.cpp
View file
Original file line number Diff line number Diff line change
@@ -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);
}
}

}

}
121 changes: 8 additions & 113 deletions lib/Espfc/src/Sensor/BaroSensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,7 @@
#define _ESPFC_SENSOR_BARO_SENSOR_H_

#include "BaseSensor.h"
#include "Model.h"
#include "Device/BaroDevice.h"
#include "Filter.h"

Expand All @@ -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;
Expand Down
Loading

0 comments on commit e58669b

Please sign in to comment.