final fixes, read correct sensor id from reg

main/sensors.go

@@ -8,6 +8,8 @@ import (
 	"strings"
 	"time"
 
+	"github.com/b3nn0/stratux/sensors/bmp388"
+
 	"github.com/b3nn0/goflying/ahrs"
 	"github.com/b3nn0/goflying/ahrsweb"
 	"github.com/b3nn0/stratux/common"
@@ -23,14 +25,15 @@ const (
 	calDLimit        = 10.0
 
 	// WHO_AM_I values to differentiate between the different IMUs.
-	MPUREG_WHO_AM_I     = 0x75
-	MPUREG_WHO_AM_I_VAL = 0x71 // Expected value.
-	MPUREG_WHO_AM_I_VAL_9255 = 0x73 // Expected value for MPU9255, seems to be compatible to 9250
-	MPUREG_WHO_AM_I_VAL_6500 = 0x70 // Expected value for MPU6500, seems to be same as 9250 but without magnetometer
-	MPUREG_WHO_AM_I_VAL_60X0 = 0x68 // Expected value for MPU6000 and MPU6050 (and MPU9150)
+	MPUREG_WHO_AM_I             = 0x75
+	MPUREG_WHO_AM_I_VAL         = 0x71 // Expected value.
+	MPUREG_WHO_AM_I_VAL_9255    = 0x73 // Expected value for MPU9255, seems to be compatible to 9250
+	MPUREG_WHO_AM_I_VAL_6500    = 0x70 // Expected value for MPU6500, seems to be same as 9250 but without magnetometer
+	MPUREG_WHO_AM_I_VAL_60X0    = 0x68 // Expected value for MPU6000 and MPU6050 (and MPU9150)
 	MPUREG_WHO_AM_I_VAL_UNKNOWN = 0x75 // Unknown MPU found on recent batch of gy91 boards see discussion 182
-	ICMREG_WHO_AM_I     = 0x00
-	ICMREG_WHO_AM_I_VAL = 0xEA // Expected value.
+	ICMREG_WHO_AM_I             = 0x00
+	ICMREG_WHO_AM_I_VAL         = 0xEA             // Expected value.
+	PRESSURE_WHO_AM_I           = bmp388.RegChipId // Expected address for bosch pressure sensors bmpXXX.
 )
 
 var (
@@ -78,13 +81,28 @@ func pollSensors() {
 }
 
 func initPressureSensor() (ok bool) {
-	bmp, err := sensors.NewBMP280(&i2cbus, 100*time.Millisecond)
-	if err == nil {
-		myPressureReader = bmp
-		return true
-	}
 
-	//TODO westphae: make bmp180.go to fit bmp interface
+	v, err := i2cbus.ReadByteFromReg(0x76, PRESSURE_WHO_AM_I)
+
+	if err != nil {
+		log.Printf("Error identifying IMU: %s\n", err.Error())
+		return false
+	}
+	if v == bmp388.ChipId {
+		log.Printf("BMP-388 detected")
+		bmp, err := sensors.NewBMP388(&i2cbus)
+		if err == nil {
+			myPressureReader = bmp
+			return true
+		}
+	} else {
+		log.Printf("using BMP-280")
+		bmp, err := sensors.NewBMP280(&i2cbus, 100*time.Millisecond)
+		if err == nil {
+			myPressureReader = bmp
+			return true
+		}
+	}
 
 	return false
 }
@@ -133,7 +151,7 @@ func tempAndPressureSender() {
 		}
 
 		altitude = common.CalcAltitude(press, globalSettings.AltitudeOffset)
-		if altitude > 70000 || (isGPSValid() && mySituation.GPSAltitudeMSL != 0 && math.Abs(float64(mySituation.GPSAltitudeMSL) - altitude) > 5000) {
+		if altitude > 70000 || (isGPSValid() && mySituation.GPSAltitudeMSL != 0 && math.Abs(float64(mySituation.GPSAltitudeMSL)-altitude) > 5000) {
 			addSingleSystemErrorf("BaroBroken", "Barometric altitude %d' out of expected range. Ignoring. Pressure sensor potentially broken.", int32(altitude))
 			continue
 		}

sensors/bmp388.go

@@ -0,0 +1,78 @@
+package sensors
+
+import (
+	"github.com/b3nn0/stratux/sensors/bmp388"
+	"github.com/kidoman/embd"
+	"time"
+)
+
+type BMP388 struct {
+	sensor      *bmp388.BMP388
+	temperature float64
+	pressure    float64
+	running     bool
+}
+
+func NewBMP388(i2cbus *embd.I2CBus) (*BMP388, error) {
+
+	bmp := bmp388.BMP388{Address: bmp388.Address, Config: bmp388.Config{
+		Temperature: bmp388.Sampling8X,
+		Pressure:    bmp388.Sampling2X,
+		IIR:         bmp388.Coeff0,
+	}, Bus: i2cbus} //new sensor
+	// retry to connect until sensor connected
+	var connected bool
+	for n := 0; n < 5; n++ {
+		if bmp.Connected() {
+			connected = true
+		} else {
+			time.Sleep(time.Millisecond)
+		}
+	}
+	if !connected {
+		return nil, bmp388.ErrNotConnected
+	}
+	err := bmp.Configure(bmp.Config)
+	if err != nil {
+		return nil, err
+	}
+	newBmp := BMP388{sensor: &bmp}
+
+	go newBmp.run()
+	return &newBmp, nil
+}
+func (bmp *BMP388) run() {
+	bmp.running = true
+	clock := time.NewTicker(100 * time.Millisecond)
+	for bmp.running {
+		for _ = range clock.C {
+			var p, _ = bmp.sensor.ReadPressure()
+			bmp.pressure = p
+			var t, _ = bmp.sensor.ReadTemperature()
+			bmp.temperature = t
+		}
+
+	}
+}
+
+func (bmp *BMP388) Close() {
+	bmp.running = false
+	bmp.sensor.Config.Mode = bmp388.Sleep
+	_ = bmp.sensor.Configure(bmp.sensor.Config)
+}
+
+// Temperature returns the current temperature in degrees C measured by the BMP280
+func (bmp *BMP388) Temperature() (float64, error) {
+	if !bmp.running {
+		return 0, bmp388.ErrNotConnected
+	}
+
+	return bmp.temperature, nil
+}
+
+func (bmp *BMP388) Pressure() (float64, error) {
+	if !bmp.running {
+		return 0, bmp388.ErrNotConnected
+	}
+	return bmp.pressure, nil
+}

sensors/bmp388/bmp388.go

@@ -0,0 +1,216 @@
+package bmp388
+
+/*
+taken from: https://github.com/tinygo-org/drivers/blob/release/bmp388/bmp388.go
+and converted to use embed
+*/
+import (
+	"errors"
+	"github.com/kidoman/embd"
+)
+
+var (
+	errConfigWrite  = errors.New("bmp388: failed to configure sensor, check connection")
+	errConfig       = errors.New("bmp388: there is a problem with the configuration, try reducing ODR")
+	errCaliRead     = errors.New("bmp388: failed to read calibration coefficient register")
+	errSoftReset    = errors.New("bmp388: failed to perform a soft reset")
+	ErrNotConnected = errors.New("bmp388: not connected")
+)
+
+type Oversampling byte
+type Mode byte
+type OutputDataRate byte
+type FilterCoefficient byte
+type Config struct {
+	Pressure    Oversampling
+	Temperature Oversampling
+	Mode        Mode
+	ODR         OutputDataRate
+	IIR         FilterCoefficient
+}
+
+// BMP388 wraps the I2C connection and configuration values for the BMP388
+type BMP388 struct {
+	Bus     *embd.I2CBus
+	Address uint8
+	cali    calibrationCoefficients
+	Config  Config
+}
+
+type calibrationCoefficients struct {
+	// Temperature compensation
+	t1 uint16
+	t2 uint16
+	t3 int8
+
+	// Pressure compensation
+	p1  int16
+	p2  int16
+	p3  int8
+	p4  int8
+	p5  uint16
+	p6  uint16
+	p7  int8
+	p8  int8
+	p9  int16
+	p10 int8
+	p11 int8
+}
+
+func (d *BMP388) Configure(config Config) (err error) {
+	d.Config = config
+
+	if d.Config == (Config{}) {
+		d.Config.Mode = Normal
+	}
+
+	// Turning on the pressure and temperature sensors and setting the measurement mode
+	err = d.writeRegister(RegPwrCtrl, PwrPress|PwrTemp|byte(d.Config.Mode))
+
+	// Configure the oversampling, output data rate, and iir filter coefficient settings
+	err = d.writeRegister(RegOSR, byte(d.Config.Pressure|d.Config.Temperature<<3))
+	err = d.writeRegister(RegODR, byte(d.Config.ODR))
+	err = d.writeRegister(RegIIR, byte(d.Config.IIR<<1))
+
+	if err != nil {
+		return errConfigWrite
+	}
+
+	// Check if there is a problem with the given configuration
+	if d.configurationError() {
+		return errConfig
+	}
+
+	// Reading the builtin calibration coefficients and parsing them per the datasheet. The compensation formula given
+	// in the datasheet is implemented in floating point
+	buffer, err := d.readRegister(RegCali, 21)
+	if err != nil {
+		return errCaliRead
+	}
+
+	d.cali.t1 = uint16(buffer[1])<<8 | uint16(buffer[0])
+	d.cali.t2 = uint16(buffer[3])<<8 | uint16(buffer[2])
+	d.cali.t3 = int8(buffer[4])
+
+	d.cali.p1 = int16(buffer[6])<<8 | int16(buffer[5])
+	d.cali.p2 = int16(buffer[8])<<8 | int16(buffer[7])
+	d.cali.p3 = int8(buffer[9])
+	d.cali.p4 = int8(buffer[10])
+	d.cali.p5 = uint16(buffer[12])<<8 | uint16(buffer[11])
+	d.cali.p6 = uint16(buffer[14])<<8 | uint16(buffer[13])
+	d.cali.p7 = int8(buffer[15])
+	d.cali.p8 = int8(buffer[16])
+	d.cali.p9 = int16(buffer[18])<<8 | int16(buffer[17])
+	d.cali.p10 = int8(buffer[19])
+	d.cali.p11 = int8(buffer[20])
+
+	return nil
+}
+func (d *BMP388) tlinCompensate() (int64, error) {
+	rawTemp, err := d.readSensorData(RegTemp)
+	if err != nil {
+		return 0, err
+	}
+
+	// pulled from C driver: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c
+	partialData1 := rawTemp - (256 * int64(d.cali.t1))
+	partialData2 := int64(d.cali.t2) * partialData1
+	partialData3 := (partialData1 * partialData1)
+	partialData4 := partialData3 * int64(d.cali.t3)
+	partialData5 := (partialData2 * 262144) + partialData4
+	return partialData5 / 4294967296, nil
+
+}
+func (d *BMP388) ReadTemperature() (float64, error) {
+
+	tlin, err := d.tlinCompensate()
+	if err != nil {
+		return 0, err
+	}
+
+	temp := (tlin * 25) / 16384
+	return float64(temp) / 100, nil
+}
+func (d *BMP388) ReadPressure() (float64, error) {
+
+	tlin, err := d.tlinCompensate()
+	if err != nil {
+		return 0, err
+	}
+	rawPress, err := d.readSensorData(RegPress)
+	if err != nil {
+		return 0, err
+	}
+
+	// code pulled from bmp388 C driver: https://github.com/BoschSensortec/BMP3-Sensor-API/blob/master/bmp3.c
+	partialData1 := tlin * tlin
+	partialData2 := partialData1 / 64
+	partialData3 := (partialData2 * tlin) / 256
+	partialData4 := (int64(d.cali.p8) * partialData3) / 32
+	partialData5 := (int64(d.cali.p7) * partialData1) * 16
+	partialData6 := (int64(d.cali.p6) * tlin) * 4194304
+	offset := (int64(d.cali.p5) * 140737488355328) + partialData4 + partialData5 + partialData6
+	partialData2 = (int64(d.cali.p4) * partialData3) / 32
+	partialData4 = (int64(d.cali.p3) * partialData1) * 4
+	partialData5 = (int64(d.cali.p2) - 16384) * tlin * 2097152
+	sensitivity := ((int64(d.cali.p1) - 16384) * 70368744177664) + partialData2 + partialData4 + partialData5
+	partialData1 = (sensitivity / 16777216) * rawPress
+	partialData2 = int64(d.cali.p10) * tlin
+	partialData3 = partialData2 + (65536 * int64(d.cali.p9))
+	partialData4 = (partialData3 * rawPress) / 8192
+
+	// dividing by 10 followed by multiplying by 10
+	// To avoid overflow caused by (pressure * partial_data4)
+	partialData5 = (rawPress * (partialData4 / 10)) / 512
+	partialData5 = partialData5 * 10
+	partialData6 = (int64)(uint64(rawPress) * uint64(rawPress))
+	partialData2 = (int64(d.cali.p11) * partialData6) / 65536
+	partialData3 = (partialData2 * rawPress) / 128
+	partialData4 = (offset / 4) + partialData1 + partialData5 + partialData3
+	compPress := ((uint64(partialData4) * 25) / uint64(1099511627776))
+	return float64(compPress) / 10000, nil
+}
+func (d *BMP388) Connected() bool {
+	data, err := d.readRegister(RegChipId, 1)
+	return err == nil && data[0] == ChipId // returns true if i2c comm was good and response equals 0x50
+}
+func (d *BMP388) SetMode(mode Mode) error {
+	d.Config.Mode = mode
+	return d.writeRegister(RegPwrCtrl, PwrPress|PwrTemp|byte(d.Config.Mode))
+}
+func (d *BMP388) readSensorData(register byte) (data int64, err error) {
+
+	if !d.Connected() {
+		return 0, ErrNotConnected
+	}
+
+	// put the sensor back into forced mode to get a reading, the sensor goes back to sleep after taking one read in
+	// forced mode
+	if d.Config.Mode != Normal {
+		err = d.SetMode(Forced)
+		if err != nil {
+			return
+		}
+	}
+
+	bytes, err := d.readRegister(register, 3)
+	if err != nil {
+		return
+	}
+	data = int64(bytes[2])<<16 | int64(bytes[1])<<8 | int64(bytes[0])
+	return
+}
+func (d *BMP388) configurationError() bool {
+	data, err := d.readRegister(RegErr, 1)
+	return err == nil && (data[0]&0x04) != 0
+}
+
+func (d *BMP388) readRegister(register byte, len int) (data []byte, err error) {
+	data = make([]byte, len)
+	err = (*d.Bus).ReadFromReg(d.Address, register, data)
+	return
+}
+
+func (d *BMP388) writeRegister(register byte, data byte) error {
+	return (*d.Bus).WriteToReg(d.Address, register, []byte{data})
+}

sensors/bmp388/registers.go

@@ -0,0 +1,84 @@
+// Package bmp388 provides a driver for Bosch's BMP388 digital temperature & pressure sensor.
+// The datasheet can be found here: https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bmp388-ds001.pdf
+package bmp388
+
+const Address byte = 0x76 // default I2C address
+
+const (
+	RegChipId  byte = 0x00 // useful for checking the connection
+	RegCali    byte = 0x31 // pressure & temperature compensation calibration coefficients
+	RegPress   byte = 0x04 // start of pressure data registers
+	RegTemp    byte = 0x07 // start of temperature data registers
+	RegPwrCtrl byte = 0x1B // measurement mode & pressure/temperature sensor power register
+	RegOSR     byte = 0x1C // oversampling settings register
+	RegODR     byte = 0x1D //
+	RegCmd     byte = 0x7E // miscellaneous command register
+	RegStat    byte = 0x03 // sensor status register
+	RegErr     byte = 0x02 // error status register
+	RegIIR     byte = 0x1F
+)
+
+const (
+	ChipId    byte = 0x50 // correct response if reading from chip id register
+	PwrPress  byte = 0x01 // power on pressure sensor
+	PwrTemp   byte = 0x02 // power on temperature sensor
+	SoftReset byte = 0xB6 // command to reset all user configuration
+	DRDYPress byte = 0x20 // for checking if pressure data is ready
+	DRDYTemp  byte = 0x40 // for checking if pressure data is ready
+)
+
+// The difference between forced and normal mode is the bmp388 goes to sleep after taking a measurement in forced mode.
+// Set it to forced if you intend to take measurements sporadically and want to save power. The driver will handle
+// waking the sensor up when the sensor is in forced mode.
+const (
+	Normal Mode = 0x30
+	Forced Mode = 0x16
+	Sleep  Mode = 0x00
+)
+
+// Increasing sampling rate increases precision but also the wait time for measurements. The datasheet has a table of
+// suggested values for oversampling, output data rates, and iir filter coefficients by use case.
+const (
+	Sampling1X Oversampling = iota
+	Sampling2X
+	Sampling4X
+	Sampling8X
+	Sampling16X
+	Sampling32X
+)
+
+// Output data rates in Hz. If increasing the sampling rates you need to decrease the output data rates, else the bmp388
+// will freeze and Configure() will return a configuration error message. In that case keep decreasing the data rate
+// until the bmp is happy
+const (
+	Odr200 OutputDataRate = iota
+	Odr100
+	Odr50
+	Odr25
+	Odr12p5
+	Odr6p25
+	Odr3p1
+	Odr1p5
+	Odr0p78
+	Odr0p39
+	Odr0p2
+	Odr0p1
+	Odr0p05
+	Odr0p02
+	Odr0p01
+	Odr0p006
+	Odr0p003
+	Odr0p0015
+)
+
+// IIR filter coefficients, higher values means steadier measurements but slower reaction times
+const (
+	Coeff0 FilterCoefficient = iota
+	Coeff1
+	Coeff3
+	Coeff7
+	Coeff15
+	Coeff31
+	Coeff63
+	Coeff127
+)

test/real_read_bmp388.go

@@ -0,0 +1,30 @@
+package main
+
+import (
+	"fmt"
+	"time"
+
+	"github.com/b3nn0/stratux/sensors/bmp388"
+	"github.com/kidoman/embd"
+	_ "github.com/kidoman/embd/host/all"
+)
+
+func main() {
+
+	i2cbus := embd.NewI2CBus(1)
+	bmp := bmp388.BMP388{Bus: &i2cbus, Address: bmp388.Address}
+	bmp.Configure(bmp.Config)
+
+	fmt.Println("t,temp,press,alt")
+
+	clock := time.NewTicker(time.Millisecond)
+	for {
+		for _ = range clock.C {
+
+			p, _ := bmp.ReadPressure()
+			t, _ := bmp.ReadTemperature()
+			fmt.Printf("%3.2f,%4.2f\n", p, t)
+		}
+
+	}
+}