Skip to content

Commit

Permalink
bmp280 spi fix
Browse files Browse the repository at this point in the history
  • Loading branch information
@rtlopez
rtlopez committed Jul 26, 2023
1 parent c48ebbd commit 82e0443
Show file tree
Hide file tree
Showing 9 changed files with 68 additions and 67 deletions.
2 changes: 1 addition & 1 deletion lib/Espfc/src/Cli.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1150,7 +1150,7 @@ class Cli

if(mag)
{
s.print(F(" mag device: "));
s.print(F(" mag device: "));
s.print(FPSTR(Device::MagDevice::getName(mag->getType())));
s.print('/');
s.println(FPSTR(Device::BusDevice::getName(mag->getBus()->getType())));
Expand Down
5 changes: 5 additions & 0 deletions lib/Espfc/src/Device/BaroBMP085.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -133,6 +133,11 @@ class BaroBMP085: public BaroDevice
else return 4500 + 50; // invalid mode
}

virtual int getDenom() const override
{
return 1;
}

bool testConnection() override
{
uint8_t whoami = 0;
Expand Down
87 changes: 38 additions & 49 deletions lib/Espfc/src/Device/BaroBMP280.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,14 @@
#define BMP280_PRESSURE_REG 0xF7
#define BMP280_TEMPERATURE_REG 0xFA

#define BMP280_OSPS_T_X1 (1 << 5)
#define BMP280_OSPS_P_X1 (1 << 2)
#define BMP280_FILTER_X2 (1 << 2)
#define BMP280_MODE_NORMAL 0x03

#define BMP280_SAMPLING_X1 1
#define BMP280_SAMPLING_X2 2
#define BMP280_SAMPLING_X4 3
#define BMP280_SAMPLING_X8 4

namespace Espfc {

namespace Device {
Expand Down Expand Up @@ -66,30 +69,16 @@ class BaroBMP280: public BaroDevice

if(!testConnection()) return 0;

readReg(BMP280_CALIB_REG, (uint8_t*)&_cal, sizeof(CalibrationData)); // read callibration
delay(2);
_bus->read(_addr, BMP280_CALIB_REG, sizeof(CalibrationData), (uint8_t*)&_cal); // read callibration

// FIXME: strange bug in ESP32 SPI that distorts some commands,
// call get status and then write command twice for sure.
readReg8(BMP280_STATUS_REG);
readReg8(BMP280_STATUS_REG);
writeReg(BMP280_RESET_REG, BMP280_RESET_VAL); // device reset
readReg8(BMP280_STATUS_REG);
writeReg(BMP280_RESET_REG, BMP280_RESET_VAL); // device reset
delay(2);

readReg8(BMP280_STATUS_REG);
readReg8(BMP280_STATUS_REG);
writeReg(BMP280_CONFIG_REG, BMP280_FILTER_X2); // set minimal standby and IIR filter X2
readReg8(BMP280_STATUS_REG);
writeReg(BMP280_CONFIG_REG, BMP280_FILTER_X2); // set minimal standby and IIR filter X2
delay(2);
//writeReg(BMP280_CONFIG_REG, 0); // set minimal standby and IIR filter off

writeReg(BMP280_CONTROL_REG, BMP280_SAMPLING_X1 << 5 | BMP280_SAMPLING_X4 << 2 | BMP280_MODE_NORMAL); // set sampling mode

readReg8(BMP280_STATUS_REG);
readReg8(BMP280_STATUS_REG);
writeReg(BMP280_CONTROL_REG, BMP280_OSPS_T_X1 | BMP280_OSPS_P_X1 | BMP280_MODE_NORMAL); // set sampling mode
readReg8(BMP280_STATUS_REG);
writeReg(BMP280_CONTROL_REG, BMP280_OSPS_T_X1 | BMP280_OSPS_P_X1 | BMP280_MODE_NORMAL); // set sampling mode
delay(20);

return 1;
Expand All @@ -102,21 +91,23 @@ class BaroBMP280: public BaroDevice

virtual float readTemperature() override
{
int32_t adc_T = readReg(BMP280_TEMPERATURE_REG);
adc_T >>= 4;

int32_t var1 = ((((adc_T >> 3) - ((int32_t)_cal.dig_T1 << 1))) * ((int32_t)_cal.dig_T2)) >> 11;
int32_t var2 = (((((adc_T >> 4) - ((int32_t)_cal.dig_T1)) * ((adc_T >> 4) - ((int32_t)_cal.dig_T1))) >> 12) * ((int32_t)_cal.dig_T3)) >> 14;
//_t_fine = var1 + var2;
_t_fine += ((var1 + var2) - _t_fine + 4) >> 3; // smooth t_fine

float T = (_t_fine * 5 + 128) >> 8;
return T * 0.01f;
}

virtual float readPressure() override
{
int32_t adc_P = readReg(BMP280_PRESSURE_REG);
readMesurment();

int32_t adc_T = _raw_temp;
adc_T >>= 4;

int32_t vart1 = ((((adc_T >> 3) - ((int32_t)_cal.dig_T1 << 1))) * ((int32_t)_cal.dig_T2)) >> 11;
int32_t vart2 = (((((adc_T >> 4) - ((int32_t)_cal.dig_T1)) * ((adc_T >> 4) - ((int32_t)_cal.dig_T1))) >> 12) * ((int32_t)_cal.dig_T3)) >> 14;
_t_fine = vart1 + vart2;
//_t_fine += ((var1 + var2) - _t_fine + 4) >> 3; // smooth t_fine

int32_t adc_P = _raw_pressure;
adc_P >>= 4;

int64_t var1 = ((int64_t)_t_fine) - 128000;
Expand Down Expand Up @@ -146,35 +137,31 @@ class BaroBMP280: public BaroDevice

virtual int getDelay() const override
{
return 5500;
//const int comp = 50;
//return 4500 + comp; // temp
//return 5500 / 2; // if sapling X1
//return 7500 / 2; // if sampling X2
return 11500 / 2; // if sampling X4
}

bool testConnection() override
// notify BaroSensor to sample lower as delay is divided by 2
virtual int getDenom() const override
{
uint8_t whoami = readReg8(BMP280_WHOAMI_REG);
return whoami == BMP280_WHOAMI_ID;
return 2;
}

protected:
uint8_t readReg8(uint8_t reg)
{
uint8_t buffer = 0;
_bus->read(_addr, reg, 1, &buffer);
return buffer;
}

int32_t readReg(uint8_t reg)
bool testConnection() override
{
uint8_t buffer[3] = {0, 0, 0};
_bus->read(_addr, reg, 3, buffer);
return buffer[2] | (buffer[1] << 8) | (buffer[0] << 16);
uint8_t whoami = 0;
_bus->read(_addr, BMP280_WHOAMI_REG, 1, &whoami);
return whoami == BMP280_WHOAMI_ID;
}

int32_t readReg(uint8_t reg, uint8_t * buffer, uint8_t length)
protected:
void readMesurment()
{
return _bus->read(_addr, reg, length, buffer);
uint8_t buffer[6] = {0, 0, 0, 0, 0, 0};
_bus->readFast(_addr, BMP280_PRESSURE_REG, 6, buffer);
_raw_pressure = buffer[2] | (buffer[1] << 8) | (buffer[0] << 16);
_raw_temp = buffer[5] | (buffer[4] << 8) | (buffer[3] << 16);
}

int8_t writeReg(uint8_t reg, uint8_t val)
Expand All @@ -184,6 +171,8 @@ class BaroBMP280: public BaroDevice

int8_t _mode;
int32_t _t_fine;
int32_t _raw_temp;
int32_t _raw_pressure;
CalibrationData _cal;
};

Expand Down
1 change: 1 addition & 0 deletions lib/Espfc/src/Device/BaroDevice.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@ class BaroDevice: public BusAwareDevice
virtual float readTemperature() = 0;
virtual float readPressure() = 0;
virtual int getDelay() const = 0;
virtual int getDenom() const = 0;
virtual void setMode(BaroDeviceMode mode) = 0;

virtual bool testConnection() = 0;
Expand Down
6 changes: 3 additions & 3 deletions lib/Espfc/src/Device/BusSPI.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,7 +16,7 @@ class BusSPI: public BusDevice
static const uint8_t SPI_WRITE = 0x7f;

static const uint32_t SPI_SPEED_NORMAL = 1000000;
static const uint32_t SPI_SPEED_FAST = 10000000;
static const uint32_t SPI_SPEED_FAST = 16000000;

BusType getType() const override { return BUS_SPI; }

Expand Down Expand Up @@ -53,13 +53,13 @@ class BusSPI: public BusDevice
private:
void transfer(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *in, uint8_t *out, uint32_t speed)
{
_dev.beginTransaction(SPISettings(speed, MSBFIRST, SPI_MODE0));
_dev.beginTransaction(SPISettings(speed, SPI_MSBFIRST, SPI_MODE0));
digitalWrite(devAddr, LOW);
#if defined (ARCH_RP2040)
_dev.transfer(regAddr);
_dev.transfer(in, out, length);
#else
_dev.write(regAddr);
_dev.transfer(regAddr);
_dev.transferBytes(in, out, length);
#endif
digitalWrite(devAddr, HIGH);
Expand Down
12 changes: 10 additions & 2 deletions lib/Espfc/src/Device/GyroMPU9250.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@

#define MPU9250_USER_CTRL 0x6A
#define MPU9250_I2C_MST_EN 0x20
#define MPU9250_I2C_IF_DIS 0x10
#define MPU9250_I2C_MST_400 0x0D
#define MPU9250_I2C_MST_500 0x09
#define MPU9250_I2C_MST_CTRL 0x24
Expand Down Expand Up @@ -53,8 +54,15 @@ class GyroMPU9250: public GyroMPU6050
setRate(9); // 1000 / (9+1) = 100hz
delay(100);

// enable I2C master mode
_bus->writeByte(_addr, MPU9250_USER_CTRL, MPU9250_I2C_MST_EN);
// enable I2C master mode, and disable I2C if SPI
if(_bus->getType() == BUS_SPI)
{
_bus->writeByte(_addr, MPU9250_USER_CTRL, MPU9250_I2C_MST_EN | MPU9250_I2C_IF_DIS);
}
else
{
_bus->writeByte(_addr, MPU9250_USER_CTRL, MPU9250_I2C_MST_EN);
}
//delay(100);

// set the I2C bus speed to 400 kHz
Expand Down
2 changes: 1 addition & 1 deletion lib/Espfc/src/Device/MagAK8963.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,7 @@
#define AK8963_RESET 0x01
#define AK8963_ASA 0x10
#define AK8963_WHO_AM_I 0x00
#define AK8963_INIT_DELAY 200
#define AK8963_INIT_DELAY 20

namespace Espfc {

Expand Down
6 changes: 3 additions & 3 deletions lib/Espfc/src/Hardware.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,8 +85,8 @@ class Hardware
#if defined(ESPFC_SPI_0)
if(_model.config.pin[PIN_SPI_CS0] != -1)
{
pinMode(_model.config.pin[PIN_SPI_CS0], OUTPUT);
digitalWrite(_model.config.pin[PIN_SPI_CS0], HIGH);
pinMode(_model.config.pin[PIN_SPI_CS0], OUTPUT);
if(!detectedGyro && detectDevice(mpu9250, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &mpu9250;
if(!detectedGyro && detectDevice(icm20602, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &icm20602;
if(!detectedGyro && detectDevice(lsm6dso, spiBus, _model.config.pin[PIN_SPI_CS0])) detectedGyro = &lsm6dso;
Expand All @@ -96,8 +96,8 @@ class Hardware
if(_model.config.pin[PIN_I2C_0_SDA] != -1 && _model.config.pin[PIN_I2C_0_SCL] != -1)
{
if(!detectedGyro && detectDevice(mpu9250, i2cBus)) detectedGyro = &mpu9250;
if(!detectedGyro && detectDevice(mpu6050, i2cBus)) detectedGyro = &mpu6050;
if(!detectedGyro && detectDevice(icm20602, i2cBus)) detectedGyro = &icm20602;
if(!detectedGyro && detectDevice(mpu6050, i2cBus)) detectedGyro = &mpu6050;
if(!detectedGyro && detectDevice(lsm6dso, i2cBus)) detectedGyro = &lsm6dso;
}
#endif
Expand Down Expand Up @@ -144,8 +144,8 @@ class Hardware
#if defined(ESPFC_SPI_0)
if(_model.config.pin[PIN_SPI_CS1] != -1)
{
pinMode(_model.config.pin[PIN_SPI_CS1], OUTPUT);
digitalWrite(_model.config.pin[PIN_SPI_CS1], HIGH);
pinMode(_model.config.pin[PIN_SPI_CS1], OUTPUT);
if(!detectedBaro && detectDevice(bmp280, spiBus, _model.config.pin[PIN_SPI_CS1])) detectedBaro = &bmp280;
if(!detectedBaro && detectDevice(bmp085, spiBus, _model.config.pin[PIN_SPI_CS1])) detectedBaro = &bmp085;
}
Expand Down
14 changes: 6 additions & 8 deletions lib/Espfc/src/Sensor/BaroSensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,14 +32,13 @@ class BaroSensor: public BaseSensor
int delay = _baro->getDelay();
int toGyroRate = (delay / _model.state.gyroTimer.interval) + 1; // number of gyro readings per cycle
int interval = _model.state.gyroTimer.interval * toGyroRate;
int rate = 1000000 / interval;
_model.state.baroRate = rate;
_model.state.baroRate = (1000000 / interval) / _baro->getDenom();

_temperatureFilter.begin(FilterConfig(FILTER_PT1, 5), (rate / 2) / _temp_denom);
_pressureFilter.begin(FilterConfig(FILTER_PT1, 5), (rate / 2));
_altitudeFilter.begin(_model.config.baroFilter, rate);
_temperatureFilter.begin(FilterConfig(FILTER_PT1, 5), _model.state.baroRate / _temp_denom);
_pressureFilter.begin(FilterConfig(FILTER_PT1, 5), _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(rate).logln(_model.config.baroFilter.freq);
_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);

return 1;
}
Expand Down Expand Up @@ -73,7 +72,6 @@ class BaroSensor: public BaseSensor
return 0;
case BARO_STATE_TEMP_GET:
readTemperature();
updateAltitude();
_baro->setMode(BARO_MODE_PRESS);
_state = BARO_STATE_PRESS_GET;
_wait = micros() + _baro->getDelay();
Expand Down Expand Up @@ -122,7 +120,7 @@ class BaroSensor: public BaseSensor
if(_model.state.baroAltitudeBiasSamples > 0)
{
_model.state.baroAltitudeBiasSamples--;
_model.state.baroAltitudeBias += (_model.state.baroAltitudeRaw - _model.state.baroAltitudeBias) * 0.2f;
_model.state.baroAltitudeBias += (_model.state.baroAltitudeRaw - _model.state.baroAltitudeBias) * 0.25f;
}
_model.state.baroAltitude = _model.state.baroAltitudeRaw - _model.state.baroAltitudeBias;

Expand Down

0 comments on commit 82e0443

Please sign in to comment.