diff --git a/bno08x/bno08x.go b/bno08x/bno08x.go new file mode 100644 index 000000000..21d59c87e --- /dev/null +++ b/bno08x/bno08x.go @@ -0,0 +1,288 @@ +// Package bno08x provides a TinyGo driver for the Adafruit BNO08x 9-DOF IMU sensors. +// +// This driver implements the CEVA SH-2 protocol over the SHTP transport layer, +// providing access to orientation, motion, and environmental sensors. +// +// Datasheet: https://www.ceva-ip.com/wp-content/uploads/BNO080_085-Datasheet.pdf +package bno08x + +import ( + "time" + + "tinygo.org/x/drivers" + "tinygo.org/x/drivers/internal/pin" +) + +// Device represents a BNO08x sensor device. +type Device struct { + bus drivers.I2C + address uint16 + resetPin pin.OutputFunc + readChunk int + + hal *halI2C + shtp *shtp + sh2 *sh2Protocol + + queue [8]SensorValue + queueHead int + queueTail int + queueCount int + + productIDs ProductIDs + lastReset bool +} + +// Config holds configuration options for the device. +type Config struct { + // Address is the I2C address (default: 0x4A). + Address uint16 + + // ResetPin is the optional hardware reset pin. + ResetPin pin.OutputFunc + + // ReadChunk is the I2C read chunk size (default: 32 bytes). + ReadChunk int + + // StartupDelay is the delay after reset (default: 10ms). + StartupDelay time.Duration +} + +const ( + // DefaultAddress is the default I2C address. + DefaultAddress = 0x4A +) + +// New creates a new BNO08x device. +func New(bus drivers.I2C) *Device { + return &Device{ + bus: bus, + address: DefaultAddress, + readChunk: i2cDefaultChunk, + } +} + +// Configure initializes the sensor and prepares it for use. +func (d *Device) Configure(cfg Config) error { + if cfg.Address != 0 { + d.address = cfg.Address + } + if cfg.ReadChunk > 0 { + d.readChunk = cfg.ReadChunk + } + if cfg.ResetPin != nil { + d.resetPin = cfg.ResetPin + } + if cfg.StartupDelay <= 0 { + cfg.StartupDelay = 100 * time.Millisecond + } + + d.hal = newHAL(d) + d.shtp = newSHTP(d.hal) + d.sh2 = newSH2Protocol(d) + + d.queueHead = 0 + d.queueTail = 0 + d.queueCount = 0 + d.productIDs = ProductIDs{} + d.lastReset = false + + if err := d.hal.open(); err != nil { + return err + } + + // Now that handlers are registered, perform reset + // Try hardware reset first if available + if d.resetPin != nil { + d.hardwareReset() + time.Sleep(cfg.StartupDelay) + } else { + // No hardware reset pin - try soft reset via I2C raw packet first + // This is what Adafruit does in hal_open + if err := d.softResetI2C(); err != nil { + // If that fails, try soft reset via SHTP protocol + _ = d.sh2.softReset() + time.Sleep(50 * time.Millisecond) + } + } + + // Wait for reset notification by actively polling + // The sensor should send reset complete message shortly after reset + deadline := time.Now().Add(1000 * time.Millisecond) + pollCount := 0 + for time.Now().Before(deadline) { + pollCount++ + if err := d.service(); err != nil { + // Ignore errors during initial polling - sensor might not be ready + time.Sleep(1 * time.Millisecond) + continue + } + if d.lastReset { + break + } + time.Sleep(1 * time.Millisecond) + } + + if !d.lastReset { + return errTimeout + } + + // NOTE: We intentionally skip the Initialize command (sh2_initialize) + // Testing revealed that sending the Initialize command (0xF2 0x00 0x04 0x01...) + // prevents the BNO08x from sending sensor reports on channel 3. + // The sensor works correctly without this command after a soft reset. + // The Arduino library likely works because it does a hardware reset which + // may put the sensor in a different state, or their initialization sequence + // differs in a way that doesn't trigger this issue. + + // Request product IDs + if err := d.sh2.requestProductIDs(); err != nil { + return err + } + + // Wait for product IDs with polling delay + deadline = time.Now().Add(500 * time.Millisecond) + for time.Now().Before(deadline) { + if err := d.service(); err != nil { + time.Sleep(10 * time.Millisecond) + continue + } + if d.productIDs.NumEntries > 0 { + break + } + time.Sleep(10 * time.Millisecond) + } + + if d.productIDs.NumEntries == 0 { + return errTimeout + } + + return nil +} + +// EnableReport enables a specific sensor report at the given interval. +func (d *Device) EnableReport(id SensorID, intervalUs uint32) error { + err := d.sh2.enableReport(id, intervalUs) + if err != nil { + return err + } + + // Poll a few times to let the sensor process the command + // and potentially send acknowledgment + for i := 0; i < 10; i++ { + _ = d.service() + time.Sleep(10 * time.Millisecond) + } + + return nil +} + +// GetSensorConfig retrieves the current configuration for a sensor. +func (d *Device) GetSensorConfig(id SensorID) (SensorConfig, error) { + return d.sh2.getSensorConfig(id) +} + +// SetSensorConfig sets the configuration for a sensor. +func (d *Device) SetSensorConfig(id SensorID, config SensorConfig) error { + return d.sh2.setSensorConfig(id, config) +} + +// WasReset returns true if the sensor signaled a reset since the last call. +func (d *Device) WasReset() bool { + if d.lastReset { + d.lastReset = false + return true + } + return false +} + +// GetSensorEvent retrieves the next available sensor event if present. +func (d *Device) GetSensorEvent() (SensorValue, bool) { + if d.queueCount == 0 { + if err := d.service(); err != nil { + return SensorValue{}, false + } + if d.queueCount == 0 { + return SensorValue{}, false + } + } + + value := d.queue[d.queueHead] + d.queueHead = (d.queueHead + 1) % len(d.queue) + d.queueCount-- + + return value, true +} + +// ProductIDs returns the cached product identification information. +func (d *Device) ProductIDs() ProductIDs { + return d.productIDs +} + +// Service processes pending sensor data. +// This is called automatically by GetSensorEvent but can be called manually +// for more control over timing. +func (d *Device) Service() error { + return d.service() +} + +func (d *Device) enqueue(value SensorValue) { + next := (d.queueTail + 1) % len(d.queue) + if d.queueCount == len(d.queue) { + // Queue full, drop oldest + d.queueHead = (d.queueHead + 1) % len(d.queue) + d.queueCount-- + } + d.queue[d.queueTail] = value + d.queueTail = next + d.queueCount++ +} + +func (d *Device) service() error { + if d.shtp == nil { + return nil + } + for { + processed, err := d.shtp.poll() + if err != nil { + return err + } + if !processed { + break + } + } + return nil +} + +func (d *Device) hardwareReset() { + if d.resetPin == nil { + return + } + d.resetPin.High() + time.Sleep(10 * time.Millisecond) + d.resetPin.Low() + time.Sleep(10 * time.Millisecond) + d.resetPin.High() + time.Sleep(10 * time.Millisecond) +} + +func (d *Device) softResetI2C() error { + // Send soft reset packet via I2C as per Adafruit implementation + // Format: [length_low, length_high, channel, sequence, command] + // This is: 5 bytes total, channel 1 (executable), command 1 (reset) + softResetPacket := []byte{5, 0, 1, 0, 1} + + // Try up to 5 times + var err error + for attempts := 0; attempts < 5; attempts++ { + err = d.bus.Tx(d.address, softResetPacket, nil) + if err == nil { + // Success - wait for sensor to process reset + time.Sleep(300 * time.Millisecond) + return nil + } + time.Sleep(30 * time.Millisecond) + } + + return err +} diff --git a/bno08x/constants.go b/bno08x/constants.go new file mode 100644 index 000000000..7073f1feb --- /dev/null +++ b/bno08x/constants.go @@ -0,0 +1,179 @@ +package bno08x + +// I2C and protocol constants +const ( + shtpHeaderLength = 4 + maxTransferOut = 256 + maxTransferIn = 384 + + i2cDefaultChunk = 32 + continueMask = 0x8000 +) + +// SHTP channel numbers +const ( + channelCommand = 0 + channelExecutable = 1 + channelControl = 2 + channelSensorReport = 3 + channelWakeReport = 4 + channelGyroRV = 5 +) + +// SH-2 report IDs +const ( + reportProdIDReq = 0xF9 + reportProdIDResp = 0xF8 + reportSetFeature = 0xFD + reportGetFeature = 0xFE + reportGetFeatureResp = 0xFC + reportCommandReq = 0xF2 + reportCommandResp = 0xF1 + reportFRSWriteReq = 0xF7 + reportFRSWriteData = 0xF6 + reportFRSReadReq = 0xF4 + reportFRSReadResp = 0xF3 + reportBaseTimestamp = 0xFB + reportTimestampReuse = 0xFA + reportForceFlush = 0xF0 + reportFlushCompleted = 0xEF + reportResetReq = 0xF1 + reportResetResp = 0xF0 +) + +// SH-2 commands +const ( + cmdErrors = 0x01 + cmdCounts = 0x02 + cmdTare = 0x03 + cmdInitialize = 0x04 + cmdFRS = 0x05 + cmdDCD = 0x06 + cmdMECal = 0x07 + cmdProdIDReq = 0x07 + cmdDCDSave = 0x09 + cmdGetOscType = 0x0A + cmdClearDCDReset = 0x0B + cmdCal = 0x0C + cmdBootloader = 0x0D + cmdInteractiveZRO = 0x0E + + // Command parameters + initSystem = 0x01 + initUnsolicited = 0x80 + + countsClearCounts = 0x01 + countsGetCounts = 0x00 + + tareTareNow = 0x00 + tarePersist = 0x01 + tareSetReorientation = 0x02 + + calStart = 0x00 + calFinish = 0x01 + + commandParamCount = 9 + responseValueCount = 11 +) + +// Feature report flags +const ( + featChangeSensitivityRelative = 0x01 + featChangeSensitivityEnabled = 0x02 + featWakeEnabled = 0x04 + featAlwaysOnEnabled = 0x08 +) + +// Scaling factors for sensor data +// These are derived from the Q-point encoding in the SH-2 specification +const ( + scaleQuat = 1.0 / 16384.0 // Q14 + scaleAccel = 1.0 / 256.0 // Q8 + scaleGyro = 1.0 / 512.0 // Q9 + scaleMag = 1.0 / 16.0 // Q4 + scaleAccuracy = 1.0 / 4096.0 // Q12 + scalePressure = 1.0 / 1048576.0 // Q20 + scaleLight = 1.0 / 256.0 // Q8 + scaleHumidity = 1.0 / 256.0 // Q8 + scaleProximity = 1.0 / 16.0 // Q4 + scaleTemperature = 1.0 / 128.0 // Q7 + scaleAngle = 1.0 / 16.0 // Q4 + scaleHeartRate = 1.0 / 16.0 // Q4 +) + +// Activity classifier codes (extended beyond standard SH-2) +const ( + ActivityUnknown = 0 + ActivityInVehicle = 1 + ActivityOnBicycle = 2 + ActivityOnFoot = 3 + ActivityStill = 4 + ActivityTilting = 5 + ActivityWalking = 6 + ActivityRunning = 7 + ActivityOnStairs = 8 + ActivityOptionCount = 9 +) + +// Stability classifier values +const ( + StabilityUnknown = 0 + StabilityOnTable = 1 + StabilityStationary = 2 + StabilityStable = 3 + StabilityMotion = 4 +) + +// Tap detector flags +const ( + TapX = 0x01 // 1 - X axis tapped + TapXPos = 0x02 // 2 - X positive direction + TapY = 0x04 // 4 - Y axis tapped + TapYPos = 0x08 // 8 - Y positive direction + TapZ = 0x10 // 16 - Z axis tapped + TapZPos = 0x20 // 32 - Z positive direction + TapDouble = 0x40 // 64 - Double tap occurred +) + +// GUID values for SHTP +const ( + guidSHTP = 0 + guidExecutable = 1 + guidSensorHub = 2 +) + +// Advertisement tags +const ( + tagNull = 0 + tagGUID = 1 + tagMaxCargoHeaderWrite = 2 + tagMaxCargoHeaderRead = 3 + tagMaxTransferWrite = 4 + tagMaxTransferRead = 5 + tagNormalChannel = 6 + tagWakeChannel = 7 + tagAppName = 8 + tagChannelName = 9 + tagAdvCount = 10 + tagAppSpecific = 0x80 + tagSH2Version = 0x80 + tagSH2ReportLengths = 0x81 +) + +// Timeouts +const ( + advertTimeout = 200000 // microseconds + commandTimeout = 300000 // microseconds +) + +// Executable device commands +const ( + execDeviceCmdReset = 1 + execDeviceCmdOn = 2 + execDeviceCmdSleep = 3 +) + +// Executable device responses +const ( + execDeviceRespResetComplete = 1 +) diff --git a/bno08x/decode.go b/bno08x/decode.go new file mode 100644 index 000000000..0ec2a15a5 --- /dev/null +++ b/bno08x/decode.go @@ -0,0 +1,316 @@ +package bno08x + +import "encoding/binary" + +// decodeSensor decodes a sensor report payload into a SensorValue. +func decodeSensor(payload []byte, timestamp uint32) (SensorValue, bool) { + if len(payload) < 4 { + return SensorValue{}, false + } + + value := SensorValue{ + id: SensorID(payload[0]), + sequence: payload[1], + status: payload[2] & 0x03, + delay: payload[3], + timestamp: uint64(timestamp), + } + + data := payload[4:] + + switch value.id { + case SensorRawAccelerometer: + if len(data) >= 10 { + value.rawAccelerometer = RawVector3{ + X: int16(binary.LittleEndian.Uint16(data[0:])), + Y: int16(binary.LittleEndian.Uint16(data[2:])), + Z: int16(binary.LittleEndian.Uint16(data[4:])), + Timestamp: binary.LittleEndian.Uint32(data[6:]), + } + } + + case SensorAccelerometer: + if len(data) >= 6 { + value.accelerometer = Vector3{ + X: qToFloat(data[0:], scaleAccel), + Y: qToFloat(data[2:], scaleAccel), + Z: qToFloat(data[4:], scaleAccel), + } + } + + case SensorLinearAcceleration: + if len(data) >= 6 { + value.linearAcceleration = Vector3{ + X: qToFloat(data[0:], scaleAccel), + Y: qToFloat(data[2:], scaleAccel), + Z: qToFloat(data[4:], scaleAccel), + } + } + + case SensorGravity: + if len(data) >= 6 { + value.gravity = Vector3{ + X: qToFloat(data[0:], scaleAccel), + Y: qToFloat(data[2:], scaleAccel), + Z: qToFloat(data[4:], scaleAccel), + } + } + + case SensorRawGyroscope: + if len(data) >= 12 { + value.rawGyroscope = RawGyroscope{ + X: int16(binary.LittleEndian.Uint16(data[0:])), + Y: int16(binary.LittleEndian.Uint16(data[2:])), + Z: int16(binary.LittleEndian.Uint16(data[4:])), + Temperature: int16(binary.LittleEndian.Uint16(data[6:])), + Timestamp: binary.LittleEndian.Uint32(data[8:]), + } + } + + case SensorGyroscope: + if len(data) >= 6 { + value.gyroscope = Vector3{ + X: qToFloat(data[0:], scaleGyro), + Y: qToFloat(data[2:], scaleGyro), + Z: qToFloat(data[4:], scaleGyro), + } + } + + case SensorGyroscopeUncalibrated: + if len(data) >= 12 { + value.gyroscopeUncal = GyroscopeUncalibrated{ + X: qToFloat(data[0:], scaleGyro), + Y: qToFloat(data[2:], scaleGyro), + Z: qToFloat(data[4:], scaleGyro), + BiasX: qToFloat(data[6:], scaleGyro), + BiasY: qToFloat(data[8:], scaleGyro), + BiasZ: qToFloat(data[10:], scaleGyro), + } + } + + case SensorRawMagnetometer: + if len(data) >= 10 { + value.rawMagnetometer = RawVector3{ + X: int16(binary.LittleEndian.Uint16(data[0:])), + Y: int16(binary.LittleEndian.Uint16(data[2:])), + Z: int16(binary.LittleEndian.Uint16(data[4:])), + Timestamp: binary.LittleEndian.Uint32(data[6:]), + } + } + + case SensorMagneticField: + if len(data) >= 6 { + value.magneticField = Vector3{ + X: qToFloat(data[0:], scaleMag), + Y: qToFloat(data[2:], scaleMag), + Z: qToFloat(data[4:], scaleMag), + } + } + + case SensorMagneticFieldUncalibrated: + if len(data) >= 12 { + value.magneticFieldUncal = MagneticFieldUncalibrated{ + X: qToFloat(data[0:], scaleMag), + Y: qToFloat(data[2:], scaleMag), + Z: qToFloat(data[4:], scaleMag), + BiasX: qToFloat(data[6:], scaleMag), + BiasY: qToFloat(data[8:], scaleMag), + BiasZ: qToFloat(data[10:], scaleMag), + } + } + + case SensorRotationVector: + if len(data) >= 10 { + value.quaternion = Quaternion{ + I: qToFloat(data[0:], scaleQuat), + J: qToFloat(data[2:], scaleQuat), + K: qToFloat(data[4:], scaleQuat), + Real: qToFloat(data[6:], scaleQuat), + } + value.quaternionAccuracy = qToFloat(data[8:], scaleAccuracy) + } + + case SensorGameRotationVector: + if len(data) >= 8 { + value.quaternion = Quaternion{ + I: qToFloat(data[0:], scaleQuat), + J: qToFloat(data[2:], scaleQuat), + K: qToFloat(data[4:], scaleQuat), + Real: qToFloat(data[6:], scaleQuat), + } + } + + case SensorGeomagneticRotationVector: + if len(data) >= 10 { + value.quaternion = Quaternion{ + I: qToFloat(data[0:], scaleQuat), + J: qToFloat(data[2:], scaleQuat), + K: qToFloat(data[4:], scaleQuat), + Real: qToFloat(data[6:], scaleQuat), + } + value.quaternionAccuracy = qToFloat(data[8:], scaleAccuracy) + } + + case SensorARVRStabilizedRV: + if len(data) >= 10 { + value.quaternion = Quaternion{ + I: qToFloat(data[0:], scaleQuat), + J: qToFloat(data[2:], scaleQuat), + K: qToFloat(data[4:], scaleQuat), + Real: qToFloat(data[6:], scaleQuat), + } + value.quaternionAccuracy = qToFloat(data[8:], scaleAccuracy) + } + + case SensorARVRStabilizedGRV: + if len(data) >= 8 { + value.quaternion = Quaternion{ + I: qToFloat(data[0:], scaleQuat), + J: qToFloat(data[2:], scaleQuat), + K: qToFloat(data[4:], scaleQuat), + Real: qToFloat(data[6:], scaleQuat), + } + } + + case SensorGyroIntegratedRV: + if len(data) >= 10 { + value.quaternion = Quaternion{ + I: qToFloat(data[0:], scaleQuat), + J: qToFloat(data[2:], scaleQuat), + K: qToFloat(data[4:], scaleQuat), + Real: qToFloat(data[6:], scaleQuat), + } + // Angular velocity X at data[8:10] + } + + case SensorPressure: + if len(data) >= 4 { + value.pressure = float32(int32(binary.LittleEndian.Uint32(data[0:]))) * scalePressure + } + + case SensorAmbientLight: + if len(data) >= 4 { + value.ambientLight = float32(int32(binary.LittleEndian.Uint32(data[0:]))) * scaleLight + } + + case SensorHumidity: + if len(data) >= 2 { + value.humidity = qToFloat(data[0:], scaleHumidity) + } + + case SensorProximity: + if len(data) >= 2 { + value.proximity = qToFloat(data[0:], scaleProximity) + } + + case SensorTemperature: + if len(data) >= 2 { + value.temperature = qToFloat(data[0:], scaleTemperature) + } + + case SensorTapDetector: + if len(data) >= 1 { + value.tapDetector = TapDetector{ + Flags: data[0], + } + } + + case SensorStepDetector: + if len(data) >= 4 { + value.stepDetector = StepDetector{ + Latency: binary.LittleEndian.Uint32(data[0:]), + } + } + + case SensorStepCounter: + if len(data) >= 8 { + value.stepCounter = StepCounter{ + Count: uint16(binary.LittleEndian.Uint32(data[4:8])), + Latency: binary.LittleEndian.Uint32(data[0:4]), + } + } + + case SensorSignificantMotion: + if len(data) >= 2 { + value.significantMotion = SignificantMotion{ + Motion: binary.LittleEndian.Uint16(data[0:]), + } + } + + case SensorStabilityClassifier: + if len(data) >= 1 { + value.stabilityClassifier = StabilityClassifier{ + Classification: data[0], + } + } + + case SensorStabilityDetector: + if len(data) >= 1 { + value.stabilityDetector = data[0] + } + + case SensorShakeDetector: + if len(data) >= 2 { + value.shakeDetector = ShakeDetector{ + Shake: binary.LittleEndian.Uint16(data[0:]), + } + } + + case SensorFlipDetector: + if len(data) >= 2 { + value.flipDetector = binary.LittleEndian.Uint16(data[0:2]) + } + + case SensorPickupDetector: + if len(data) >= 2 { + // Pickup detected at data[0:2] + } + + case SensorPersonalActivityClassifier: + if len(data) >= 16 { + value.personalActivityClassifier = PersonalActivityClassifier{ + Page: data[0], + MostLikelyState: data[1], + EndOfPage: data[15], + } + for i := 0; i < 10 && i+2 < len(data); i++ { + value.personalActivityClassifier.Confidence[i] = data[2+i] + } + } + + case SensorSleepDetector: + if len(data) >= 1 { + value.sleepDetector = data[0] + } + + case SensorTiltDetector: + if len(data) >= 1 { + value.tiltDetector = data[0] + } + + case SensorPocketDetector: + if len(data) >= 1 { + value.pocketDetector = data[0] + } + + case SensorCircleDetector: + if len(data) >= 1 { + value.circleDetector = data[0] + } + + case SensorHeartRateMonitor: + if len(data) >= 2 { + value.heartRateMonitor = binary.LittleEndian.Uint16(data[0:]) + } + } + + return value, true +} + +// qToFloat converts a Q-point fixed-point value to float32. +func qToFloat(data []byte, scale float32) float32 { + if len(data) < 2 { + return 0 + } + return float32(int16(binary.LittleEndian.Uint16(data))) * scale +} diff --git a/bno08x/hal.go b/bno08x/hal.go new file mode 100644 index 000000000..34c6cd5a1 --- /dev/null +++ b/bno08x/hal.go @@ -0,0 +1,130 @@ +package bno08x + +import ( + "encoding/binary" + "time" +) + +// halI2C implements the hardware abstraction layer for I2C communication. +type halI2C struct { + device *Device + chunkSize int + scratch []byte + header [shtpHeaderLength]byte // Reusable header buffer +} + +func newHAL(dev *Device) *halI2C { + chunk := dev.readChunk + if chunk < shtpHeaderLength { + chunk = shtpHeaderLength + } + return &halI2C{ + device: dev, + chunkSize: chunk, + scratch: make([]byte, chunk), + } +} + +func (h *halI2C) open() error { + // HAL is now open and ready for communication + // Soft reset will be sent after handlers are registered + return nil +} + +func (h *halI2C) close() {} + +func (h *halI2C) read(target []byte) (int, uint32, error) { + // Read SHTP header (4 bytes) to get packet length + // Use pre-allocated header buffer to avoid allocations + err := h.device.bus.Tx(h.device.address, nil, h.header[:]) + if err != nil { + return 0, 0, err + } + + // Parse packet length from header + packetLen := binary.LittleEndian.Uint16(h.header[0:2]) + + // Check if continuation bit is set (0x8000) + // This means no data is available yet + if packetLen&continueMask != 0 { + return 0, 0, nil + } + + // No continuation bit, check for actual data + if packetLen == 0 { + return 0, 0, nil + } + + if int(packetLen) > len(target) { + return 0, 0, errBufferTooSmall + } + + // Now read the full packet in chunks, re-reading the header in first chunk + // This follows Arduino's approach: initial header read is just to get size, + // actual packet data (including header) is read in the loop + cargoRemaining := int(packetLen) + offset := 0 + firstRead := true + + for cargoRemaining > 0 { + var request int + if firstRead { + // First read: get the full packet including header (up to chunkSize) + request = h.chunkSize + if request > cargoRemaining { + request = cargoRemaining + } + } else { + // Subsequent reads: each chunk has a 4-byte header we need to skip + request = h.chunkSize + if request > cargoRemaining+shtpHeaderLength { + request = cargoRemaining + shtpHeaderLength + } + } + + // Ensure scratch buffer is large enough + if request > len(h.scratch) { + h.scratch = make([]byte, request) + } + buf := h.scratch[:request] + + // Read chunk + err = h.device.bus.Tx(h.device.address, nil, buf) + if err != nil { + return 0, 0, err + } + + var cargoRead int + if firstRead { + // First read: copy everything including header + cargoRead = request + copy(target[offset:], buf[:cargoRead]) + firstRead = false + } else { + // Subsequent reads: skip the 4-byte header + cargoRead = request - shtpHeaderLength + copy(target[offset:], buf[shtpHeaderLength:shtpHeaderLength+cargoRead]) + } + + offset += cargoRead + cargoRemaining -= cargoRead + } + + timestamp := uint32(time.Now().UnixNano() / 1000) + return int(packetLen), timestamp, nil +} + +func (h *halI2C) write(frame []byte) (int, error) { + if len(frame) > h.chunkSize { + return 0, errFrameTooLarge + } + err := h.device.bus.Tx(h.device.address, frame, nil) + if err != nil { + return 0, err + } + return len(frame), nil +} + +func (h *halI2C) getTimeUs() uint32 { + return uint32(time.Now().UnixNano() / 1000) +} diff --git a/bno08x/sh2.go b/bno08x/sh2.go new file mode 100644 index 000000000..d75610766 --- /dev/null +++ b/bno08x/sh2.go @@ -0,0 +1,387 @@ +// SH-2 specification found at https://www.ceva-ip.com/wp-content/uploads/SH-2-Reference-Manual.pdf + +package bno08x + +import ( + "encoding/binary" + "time" +) + +// getReportLen returns the length in bytes of a sensor report given its ID. +// Returns 0 for unknown report IDs. +func getReportLen(reportID byte) int { + switch reportID { + case 0xF1: // FLUSH_COMPLETED + return 6 + case 0xFA: // TIMESTAMP_REBASE + return 5 + case 0xFB: // BASE_TIMESTAMP_REF + return 5 + case 0xFC: // GET_FEATURE_RESP + return 17 + case 0x01: // Accelerometer (calibrated) + return 10 + case 0x02: // Gyroscope (calibrated) + return 10 + case 0x03: // Magnetic field (calibrated) + return 10 + case 0x04: // Linear acceleration + return 10 + case 0x05: // Rotation vector + return 14 + case 0x06: // Gravity + return 10 + case 0x07: // Gyroscope uncalibrated + return 16 + case 0x08: // Game rotation vector + return 12 + case 0x09: // Geomagnetic rotation vector + return 14 + case 0x0A: // Pressure + return 10 + case 0x0B: // Ambient light + return 10 + case 0x0C: // Humidity + return 10 + case 0x0D: // Proximity + return 10 + case 0x0E: // Temperature + return 10 + case 0x0F: // Magnetic field uncalibrated + return 16 + case 0x10: // Tap detector + return 5 + case 0x11: // Step counter + return 12 + case 0x12: // Significant motion + return 6 + case 0x13: // Stability classifier + return 5 + case 0x14: // Raw accelerometer + return 16 + case 0x15: // Raw gyroscope + return 16 + case 0x16: // Raw magnetometer + return 16 + case 0x18: // Step detector + return 8 + case 0x19: // Shake detector + return 6 + case 0x1A: // Flip detector + return 6 + case 0x1B: // Pickup detector + return 6 + case 0x1C: // Stability detector + return 6 + case 0x1E: // Personal activity classifier + return 16 + default: + // For most sensor reports, they are typically 10-16 bytes + // If we don't know the exact length, return a safe default + // that covers most cases (the handler will bounds-check) + if reportID < 0xF0 { + return 10 // Most sensor reports are at least this long + } + return 0 + } +} + +// sh2Protocol implements the Sensor Hub 2 (SH-2) application protocol. +type sh2Protocol struct { + device *Device + transport *shtp + cmdSeq uint8 + waiting bool + lastCmd uint8 + pendingConfigRequest bool + pendingConfigSensor SensorID + receivedConfig SensorConfig + configReady bool + configBuf [17]byte // Reusable buffer for setSensorConfig + commandBuf [3 + commandParamCount]byte // Reusable buffer for sendCommand +} + +func newSH2Protocol(device *Device) *sh2Protocol { + proto := &sh2Protocol{ + device: device, + transport: device.shtp, + } + + // Register handlers for each channel + device.shtp.register(channelControl, proto.handleControl) + device.shtp.register(channelSensorReport, proto.handleSensor) + device.shtp.register(channelWakeReport, proto.handleSensor) + device.shtp.register(channelGyroRV, proto.handleSensor) + device.shtp.register(channelExecutable, proto.handleExecutable) + + return proto +} + +// softReset sends a software reset command to the sensor. +func (s *sh2Protocol) softReset() error { + payload := []byte{execDeviceCmdReset} + return s.transport.send(channelExecutable, payload) +} + +// initialize sends the initialize command to the sensor. +func (s *sh2Protocol) initialize() error { + return s.sendCommand(cmdInitialize, []byte{initSystem}) +} + +// requestProductIDs requests product identification information. +func (s *sh2Protocol) requestProductIDs() error { + payload := []byte{reportProdIDReq, 0x00} + return s.transport.send(channelControl, payload) +} + +// enableReport enables a sensor report at the specified interval. +func (s *sh2Protocol) enableReport(id SensorID, intervalUs uint32) error { + config := SensorConfig{ + ReportInterval: intervalUs, + } + return s.setSensorConfig(id, config) +} + +// getSensorConfig retrieves the configuration for a sensor. +// This method sends a GET_FEATURE request and waits for the response +// by polling the device. It will timeout after approximately 1 second. +func (s *sh2Protocol) getSensorConfig(id SensorID) (SensorConfig, error) { + // Mark that we're waiting for a config response + s.pendingConfigRequest = true + s.pendingConfigSensor = id + s.configReady = false + + payload := []byte{reportGetFeature, byte(id)} + err := s.transport.send(channelControl, payload) + if err != nil { + s.pendingConfigRequest = false + return SensorConfig{}, err + } + + // Poll for response with timeout + maxAttempts := 100 // ~1 second with 10ms delays + for i := 0; i < maxAttempts; i++ { + // Service the device to process incoming messages + s.device.shtp.poll() + + if s.configReady { + s.pendingConfigRequest = false + s.configReady = false + return s.receivedConfig, nil + } + + // Small delay between polls + time.Sleep(10 * time.Millisecond) + } + + s.pendingConfigRequest = false + return SensorConfig{}, errTimeout +} + +// setSensorConfig configures a sensor. +func (s *sh2Protocol) setSensorConfig(id SensorID, config SensorConfig) error { + // Use pre-allocated buffer to avoid allocations + payload := s.configBuf[:] + payload[0] = reportSetFeature + payload[1] = byte(id) + + // Build feature flags + var flags uint8 + if config.ChangeSensitivityEnabled { + flags |= featChangeSensitivityEnabled + } + if config.ChangeSensitivityRelative { + flags |= featChangeSensitivityRelative + } + if config.WakeupEnabled { + flags |= featWakeEnabled + } + if config.AlwaysOnEnabled { + flags |= featAlwaysOnEnabled + } + payload[2] = flags + + binary.LittleEndian.PutUint16(payload[3:5], config.ChangeSensitivity) + binary.LittleEndian.PutUint32(payload[5:9], config.ReportInterval) + binary.LittleEndian.PutUint32(payload[9:13], config.BatchInterval) + binary.LittleEndian.PutUint32(payload[13:17], config.SensorSpecific) + + return s.transport.send(channelControl, payload) +} + +// sendCommand sends a command with parameters to the sensor. +func (s *sh2Protocol) sendCommand(command byte, params []byte) error { + // Use pre-allocated buffer to avoid allocations + payload := s.commandBuf[:] + payload[0] = reportCommandReq + payload[1] = s.cmdSeq + payload[2] = command + s.cmdSeq++ + s.lastCmd = command + s.waiting = true + + for i := 0; i < commandParamCount && i < len(params); i++ { + payload[3+i] = params[i] + } + + return s.transport.send(channelControl, payload[:3+commandParamCount]) +} + +// handleControl processes control channel messages. +func (s *sh2Protocol) handleControl(payload []byte, timestamp uint32) { + if len(payload) == 0 { + return + } + + reportID := payload[0] + + switch reportID { + case reportProdIDResp: + s.handleProdID(payload, timestamp) + case reportCommandResp: + s.handleCommandResp(payload, timestamp) + case reportGetFeatureResp: + s.handleGetFeatureResp(payload, timestamp) + case reportFRSReadResp: + // FRS (Flash Record System) read response + // Not implemented in basic version + } +} + +// handleProdID processes product ID responses. +func (s *sh2Protocol) handleProdID(payload []byte, timestamp uint32) { + if len(payload) < 16 { + return + } + + entry := ProductID{ + ResetCause: payload[1], + VersionMajor: payload[2], + VersionMinor: payload[3], + PartNumber: binary.LittleEndian.Uint32(payload[4:8]), + BuildNumber: binary.LittleEndian.Uint32(payload[8:12]), + VersionPatch: binary.LittleEndian.Uint16(payload[12:14]), + Reserved0: payload[14], + Reserved1: payload[15], + } + + // Store in first slot + s.device.productIDs.Entries[0] = entry + s.device.productIDs.NumEntries = 1 +} + +// handleCommandResp processes command responses. +func (s *sh2Protocol) handleCommandResp(payload []byte, timestamp uint32) { + if len(payload) < 16 { + return + } + + // seq := payload[1] + command := payload[2] + // commandSeq := payload[3] + // respSeq := payload[4] + + // Check if this response is for our command + if s.waiting && command == s.lastCmd { + s.waiting = false + // Status is in payload[6] + // For now, we just acknowledge receipt + } +} + +// handleGetFeatureResp processes get feature responses. +func (s *sh2Protocol) handleGetFeatureResp(payload []byte, timestamp uint32) { + if len(payload) < 17 { + return + } + + // Parse the response + sensorID := SensorID(payload[1]) + flags := payload[2] + changeSensitivity := binary.LittleEndian.Uint16(payload[3:5]) + reportInterval := binary.LittleEndian.Uint32(payload[5:9]) + batchInterval := binary.LittleEndian.Uint32(payload[9:13]) + sensorSpecific := binary.LittleEndian.Uint32(payload[13:17]) + + // If we're waiting for this sensor's config, store it + if s.pendingConfigRequest && s.pendingConfigSensor == sensorID { + s.receivedConfig = SensorConfig{ + ChangeSensitivityEnabled: flags&featChangeSensitivityEnabled != 0, + ChangeSensitivityRelative: flags&featChangeSensitivityRelative != 0, + WakeupEnabled: flags&featWakeEnabled != 0, + AlwaysOnEnabled: flags&featAlwaysOnEnabled != 0, + ChangeSensitivity: changeSensitivity, + ReportInterval: reportInterval, + BatchInterval: batchInterval, + SensorSpecific: sensorSpecific, + } + s.configReady = true + } +} + +// handleSensor processes sensor report messages. +// The payload can contain multiple sensor reports batched together. +func (s *sh2Protocol) handleSensor(payload []byte, timestamp uint32) { + cursor := 0 + var referenceDelta uint32 + + for cursor < len(payload) { + if cursor >= len(payload) { + break + } + + reportID := payload[cursor] + reportLen := getReportLen(reportID) + + if reportLen == 0 { + // Unknown report ID + break + } + + if cursor+reportLen > len(payload) { + // Not enough data for this report + break + } + + // Handle special report types + switch reportID { + case 0xFB: // SENSORHUB_BASE_TIMESTAMP_REF + if reportLen >= 5 { + // Extract timebase (little-endian uint32) + timebase := binary.LittleEndian.Uint32(payload[cursor+1 : cursor+5]) + referenceDelta = -timebase // Store negative for delta calculation + } + + case 0xFA: // SENSORHUB_TIMESTAMP_REBASE + if reportLen >= 5 { + timebase := binary.LittleEndian.Uint32(payload[cursor+1 : cursor+5]) + referenceDelta += timebase + } + + case 0xF1: // SENSORHUB_FLUSH_COMPLETED + // Route to control handler + s.handleControl(payload[cursor:cursor+reportLen], timestamp) + + default: + // Regular sensor report + value, ok := decodeSensor(payload[cursor:cursor+reportLen], timestamp) + if ok { + s.device.enqueue(value) + } + } + + cursor += reportLen + } +} // handleExecutable processes executable channel messages. +func (s *sh2Protocol) handleExecutable(payload []byte, timestamp uint32) { + if len(payload) == 0 { + return + } + + reportID := payload[0] + + switch reportID { + case execDeviceRespResetComplete: + s.device.lastReset = true + } +} diff --git a/bno08x/shtp.go b/bno08x/shtp.go new file mode 100644 index 000000000..6771288ee --- /dev/null +++ b/bno08x/shtp.go @@ -0,0 +1,83 @@ +// SHTP specification found at https://www.ceva-ip.com/wp-content/uploads/SH-2-SHTP-Reference-Manual.pdf + +package bno08x + +import "encoding/binary" + +// shtpHandler is a callback for handling SHTP channel data. +type shtpHandler func(payload []byte, timestamp uint32) + +// shtp implements the Sensor Hub Transport Protocol layer. +type shtp struct { + hal *halI2C + handlers map[uint8]shtpHandler + seq [8]uint8 + rx [maxTransferIn]byte + tx [maxTransferOut]byte // Reusable transmit buffer +} + +func newSHTP(hal *halI2C) *shtp { + return &shtp{ + hal: hal, + handlers: make(map[uint8]shtpHandler), + } +} + +// register registers a handler for a specific SHTP channel. +func (s *shtp) register(channel uint8, handler shtpHandler) { + if handler == nil { + delete(s.handlers, channel) + return + } + s.handlers[channel] = handler +} + +// send transmits a payload on the specified channel. +func (s *shtp) send(channel uint8, payload []byte) error { + total := len(payload) + shtpHeaderLength + if total > maxTransferOut { + return errFrameTooLarge + } + + // Use pre-allocated transmit buffer to avoid allocations + frame := s.tx[:total] + binary.LittleEndian.PutUint16(frame[0:2], uint16(total)) + frame[2] = channel + frame[3] = s.seq[channel] + s.seq[channel]++ + copy(frame[shtpHeaderLength:], payload) + + _, err := s.hal.write(frame) + return err +} + +// poll checks for and processes incoming SHTP packets. +// Returns true if a packet was processed, false if no data available. +func (s *shtp) poll() (bool, error) { + n, timestamp, err := s.hal.read(s.rx[:]) + if err != nil { + return false, err + } + if n == 0 { + return false, nil + } + + packet := s.rx[:n] + length := int(binary.LittleEndian.Uint16(packet[0:2]) & ^uint16(continueMask)) + if length > n { + length = n + } + if length < shtpHeaderLength { + return false, nil + } + + channel := packet[2] + // seq := packet[3] // sequence number, not currently validated + payload := packet[shtpHeaderLength:length] + + if handler := s.handlers[channel]; handler != nil { + handler(payload, timestamp) + } + + return true, nil +} diff --git a/bno08x/types.go b/bno08x/types.go new file mode 100644 index 000000000..182774dea --- /dev/null +++ b/bno08x/types.go @@ -0,0 +1,572 @@ +package bno08x + +// SensorID identifies a specific sensor type. +type SensorID uint8 + +// Sensor IDs as defined in the SH-2 specification. +const ( + SensorRawAccelerometer SensorID = 0x14 + SensorAccelerometer SensorID = 0x01 + SensorLinearAcceleration SensorID = 0x04 + SensorGravity SensorID = 0x06 + SensorRawGyroscope SensorID = 0x15 + SensorGyroscope SensorID = 0x02 + SensorGyroscopeUncalibrated SensorID = 0x07 + SensorRawMagnetometer SensorID = 0x16 + SensorMagneticField SensorID = 0x03 + SensorMagneticFieldUncalibrated SensorID = 0x0F + SensorRotationVector SensorID = 0x05 + SensorGameRotationVector SensorID = 0x08 + SensorGeomagneticRotationVector SensorID = 0x09 + SensorPressure SensorID = 0x0A + SensorAmbientLight SensorID = 0x0B + SensorHumidity SensorID = 0x0C + SensorProximity SensorID = 0x0D + SensorTemperature SensorID = 0x0E + SensorReserved SensorID = 0x17 + SensorTapDetector SensorID = 0x10 + SensorStepDetector SensorID = 0x18 + SensorStepCounter SensorID = 0x11 + SensorSignificantMotion SensorID = 0x12 + SensorStabilityClassifier SensorID = 0x13 + SensorShakeDetector SensorID = 0x19 + SensorFlipDetector SensorID = 0x1A + SensorPickupDetector SensorID = 0x1B + SensorStabilityDetector SensorID = 0x1C + SensorPersonalActivityClassifier SensorID = 0x1E + SensorSleepDetector SensorID = 0x1F + SensorTiltDetector SensorID = 0x20 + SensorPocketDetector SensorID = 0x21 + SensorCircleDetector SensorID = 0x22 + SensorHeartRateMonitor SensorID = 0x23 + SensorARVRStabilizedRV SensorID = 0x28 + SensorARVRStabilizedGRV SensorID = 0x29 + SensorGyroIntegratedRV SensorID = 0x2A + SensorIZROMotionRequest SensorID = 0x2B + SensorMaxID SensorID = 0x2B +) + +// ProductID contains firmware information from the sensor. +type ProductID struct { + ResetCause uint8 + VersionMajor uint8 + VersionMinor uint8 + PartNumber uint32 + BuildNumber uint32 + VersionPatch uint16 + Reserved0 uint8 + Reserved1 uint8 +} + +// ProductIDs holds all product ID entries returned by the sensor. +type ProductIDs struct { + Entries [5]ProductID + NumEntries uint8 +} + +// Vector3 represents a 3D vector. +type Vector3 struct { + X float32 + Y float32 + Z float32 +} + +// Quaternion represents a quaternion in (real, i, j, k) format. +// Note: This maps to (w, x, y, z) convention where w=real, x=i, y=j, z=k. +type Quaternion struct { + Real float32 + I float32 + J float32 + K float32 +} + +// RawVector3 contains raw ADC counts with timestamp. +type RawVector3 struct { + X int16 + Y int16 + Z int16 + Timestamp uint32 +} + +// RawGyroscope contains raw gyro readings with temperature and timestamp. +type RawGyroscope struct { + X int16 + Y int16 + Z int16 + Temperature int16 + Timestamp uint32 +} + +// GyroscopeUncalibrated contains uncalibrated gyroscope data with bias. +type GyroscopeUncalibrated struct { + X float32 + Y float32 + Z float32 + BiasX float32 + BiasY float32 + BiasZ float32 +} + +// MagneticFieldUncalibrated contains uncalibrated magnetometer data with bias. +type MagneticFieldUncalibrated struct { + X float32 + Y float32 + Z float32 + BiasX float32 + BiasY float32 + BiasZ float32 +} + +// TapDetector contains tap/double-tap detection flags. +type TapDetector struct { + Flags uint8 +} + +// StepDetector contains step detection with latency. +type StepDetector struct { + Latency uint32 +} + +// StepCounter contains step count with latency. +type StepCounter struct { + Count uint16 + Latency uint32 +} + +// SignificantMotion indicates significant motion was detected. +type SignificantMotion struct { + Motion uint16 +} + +// ActivityClassification contains activity classification data. +type ActivityClassification struct { + Page uint8 + MostLikelyState uint8 + Classification [10]uint8 + EndOfPage uint8 +} + +// ShakeDetector contains shake detection data. +type ShakeDetector struct { + Shake uint16 +} + +// StabilityClassifier contains stability classification. +type StabilityClassifier struct { + Classification uint8 +} + +// PersonalActivityClassifier contains personal activity data. +type PersonalActivityClassifier struct { + Page uint8 + MostLikelyState uint8 + Confidence [10]uint8 + EndOfPage uint8 +} + +// SensorValue contains decoded sensor data for all sensor types. +type SensorValue struct { + id SensorID + status uint8 + sequence uint8 + delay uint8 + timestamp uint64 + + // Orientation data (quaternions) + quaternion Quaternion + quaternionAccuracy float32 + + // Linear measurements + accelerometer Vector3 + linearAcceleration Vector3 + gravity Vector3 + gyroscope Vector3 + gyroscopeUncal GyroscopeUncalibrated + magneticField Vector3 + magneticFieldUncal MagneticFieldUncalibrated + + // Raw sensor data + rawAccelerometer RawVector3 + rawGyroscope RawGyroscope + rawMagnetometer RawVector3 + + // Environmental sensors + pressure float32 // hPa + ambientLight float32 // lux + humidity float32 // % + proximity float32 // cm + temperature float32 // °C + + // Activity detection + tapDetector TapDetector + stepCounter StepCounter + stepDetector StepDetector + significantMotion SignificantMotion + shakeDetector ShakeDetector + flipDetector uint16 + stabilityClassifier StabilityClassifier + stabilityDetector uint8 + activityClassifier ActivityClassification + personalActivityClassifier PersonalActivityClassifier + sleepDetector uint8 + tiltDetector uint8 + pocketDetector uint8 + circleDetector uint8 + heartRateMonitor uint16 +} + +// SensorConfig holds configuration settings for a sensor. +type SensorConfig struct { + ChangeSensitivityEnabled bool + ChangeSensitivityRelative bool + WakeupEnabled bool + AlwaysOnEnabled bool + ChangeSensitivity uint16 + ReportInterval uint32 // microseconds + BatchInterval uint32 // microseconds + SensorSpecific uint32 +} + +// Error represents a driver error. +type Error string + +func (e Error) Error() string { return string(e) } + +// Error constants. +var ( + errBufferTooSmall = Error("bno08x: buffer too small") + errNoEvent = Error("bno08x: no sensor event available") + errTimeout = Error("bno08x: operation timed out") + errFrameTooLarge = Error("bno08x: frame exceeds maximum size") + errNoBus = Error("bno08x: I2C bus not configured") + errInvalidParam = Error("bno08x: invalid parameter") + errHubError = Error("bno08x: sensor hub error") + errIO = Error("bno08x: I/O error") +) + +// Metadata accessor methods (always available for any sensor type) + +// ID returns the sensor ID. +func (sv SensorValue) ID() SensorID { + return sv.id +} + +// Status returns the sensor status flags. +func (sv SensorValue) Status() uint8 { + return sv.status +} + +// Sequence returns the sequence number. +func (sv SensorValue) Sequence() uint8 { + return sv.sequence +} + +// Delay returns the sensor delay value. +func (sv SensorValue) Delay() uint8 { + return sv.delay +} + +// Timestamp returns the sensor timestamp. +func (sv SensorValue) Timestamp() uint64 { + return sv.timestamp +} + +// Orientation data accessor methods + +// Quaternion returns the quaternion value for rotation vector sensors. +// Panics if called on a sensor type that doesn't provide quaternion data. +func (sv SensorValue) Quaternion() Quaternion { + switch sv.id { + case SensorRotationVector, SensorGameRotationVector, SensorGeomagneticRotationVector, + SensorARVRStabilizedRV, SensorARVRStabilizedGRV, SensorGyroIntegratedRV: + return sv.quaternion + default: + panic("bno08x: Quaternion() called on non-rotation sensor type") + } +} + +// QuaternionAccuracy returns the quaternion accuracy estimate. +// Panics if called on a sensor type that doesn't provide quaternion accuracy. +func (sv SensorValue) QuaternionAccuracy() float32 { + switch sv.id { + case SensorRotationVector, SensorGeomagneticRotationVector, SensorARVRStabilizedRV: + return sv.quaternionAccuracy + default: + panic("bno08x: QuaternionAccuracy() called on sensor type without accuracy data") + } +} + +// Linear measurement accessor methods + +// Accelerometer returns the accelerometer vector. +// Panics if called on a sensor type other than SensorAccelerometer. +func (sv SensorValue) Accelerometer() Vector3 { + if sv.id != SensorAccelerometer { + panic("bno08x: Accelerometer() called on non-accelerometer sensor type") + } + return sv.accelerometer +} + +// LinearAcceleration returns the linear acceleration vector. +// Panics if called on a sensor type other than SensorLinearAcceleration. +func (sv SensorValue) LinearAcceleration() Vector3 { + if sv.id != SensorLinearAcceleration { + panic("bno08x: LinearAcceleration() called on wrong sensor type") + } + return sv.linearAcceleration +} + +// Gravity returns the gravity vector. +// Panics if called on a sensor type other than SensorGravity. +func (sv SensorValue) Gravity() Vector3 { + if sv.id != SensorGravity { + panic("bno08x: Gravity() called on non-gravity sensor type") + } + return sv.gravity +} + +// Gyroscope returns the gyroscope vector. +// Panics if called on a sensor type other than SensorGyroscope. +func (sv SensorValue) Gyroscope() Vector3 { + if sv.id != SensorGyroscope { + panic("bno08x: Gyroscope() called on non-gyroscope sensor type") + } + return sv.gyroscope +} + +// GyroscopeUncal returns the uncalibrated gyroscope data. +// Panics if called on a sensor type other than SensorGyroscopeUncalibrated. +func (sv SensorValue) GyroscopeUncal() GyroscopeUncalibrated { + if sv.id != SensorGyroscopeUncalibrated { + panic("bno08x: GyroscopeUncal() called on wrong sensor type") + } + return sv.gyroscopeUncal +} + +// MagneticField returns the magnetic field vector. +// Panics if called on a sensor type other than SensorMagneticField. +func (sv SensorValue) MagneticField() Vector3 { + if sv.id != SensorMagneticField { + panic("bno08x: MagneticField() called on wrong sensor type") + } + return sv.magneticField +} + +// MagneticFieldUncal returns the uncalibrated magnetic field data. +// Panics if called on a sensor type other than SensorMagneticFieldUncalibrated. +func (sv SensorValue) MagneticFieldUncal() MagneticFieldUncalibrated { + if sv.id != SensorMagneticFieldUncalibrated { + panic("bno08x: MagneticFieldUncal() called on wrong sensor type") + } + return sv.magneticFieldUncal +} + +// Raw sensor data accessor methods + +// RawAccelerometer returns the raw accelerometer data. +// Panics if called on a sensor type other than SensorRawAccelerometer. +func (sv SensorValue) RawAccelerometer() RawVector3 { + if sv.id != SensorRawAccelerometer { + panic("bno08x: RawAccelerometer() called on wrong sensor type") + } + return sv.rawAccelerometer +} + +// RawGyroscope returns the raw gyroscope data. +// Panics if called on a sensor type other than SensorRawGyroscope. +func (sv SensorValue) RawGyroscope() RawGyroscope { + if sv.id != SensorRawGyroscope { + panic("bno08x: RawGyroscope() called on wrong sensor type") + } + return sv.rawGyroscope +} + +// RawMagnetometer returns the raw magnetometer data. +// Panics if called on a sensor type other than SensorRawMagnetometer. +func (sv SensorValue) RawMagnetometer() RawVector3 { + if sv.id != SensorRawMagnetometer { + panic("bno08x: RawMagnetometer() called on wrong sensor type") + } + return sv.rawMagnetometer +} + +// Environmental sensor accessor methods + +// Pressure returns the pressure reading in hPa. +// Panics if called on a sensor type other than SensorPressure. +func (sv SensorValue) Pressure() float32 { + if sv.id != SensorPressure { + panic("bno08x: Pressure() called on non-pressure sensor type") + } + return sv.pressure +} + +// AmbientLight returns the ambient light reading in lux. +// Panics if called on a sensor type other than SensorAmbientLight. +func (sv SensorValue) AmbientLight() float32 { + if sv.id != SensorAmbientLight { + panic("bno08x: AmbientLight() called on wrong sensor type") + } + return sv.ambientLight +} + +// Humidity returns the humidity reading in percent. +// Panics if called on a sensor type other than SensorHumidity. +func (sv SensorValue) Humidity() float32 { + if sv.id != SensorHumidity { + panic("bno08x: Humidity() called on non-humidity sensor type") + } + return sv.humidity +} + +// Proximity returns the proximity reading in cm. +// Panics if called on a sensor type other than SensorProximity. +func (sv SensorValue) Proximity() float32 { + if sv.id != SensorProximity { + panic("bno08x: Proximity() called on non-proximity sensor type") + } + return sv.proximity +} + +// Temperature returns the temperature reading in °C. +// Panics if called on a sensor type other than SensorTemperature. +func (sv SensorValue) Temperature() float32 { + if sv.id != SensorTemperature { + panic("bno08x: Temperature() called on non-temperature sensor type") + } + return sv.temperature +} + +// Activity detection accessor methods + +// TapDetector returns the tap detector data. +// Panics if called on a sensor type other than SensorTapDetector. +func (sv SensorValue) TapDetector() TapDetector { + if sv.id != SensorTapDetector { + panic("bno08x: TapDetector() called on wrong sensor type") + } + return sv.tapDetector +} + +// StepCounter returns the step counter value. +// Panics if called on a sensor type other than SensorStepCounter. +func (sv SensorValue) StepCounter() StepCounter { + if sv.id != SensorStepCounter { + panic("bno08x: StepCounter() called on wrong sensor type") + } + return sv.stepCounter +} + +// StepDetector returns the step detector data. +// Panics if called on a sensor type other than SensorStepDetector. +func (sv SensorValue) StepDetector() StepDetector { + if sv.id != SensorStepDetector { + panic("bno08x: StepDetector() called on wrong sensor type") + } + return sv.stepDetector +} + +// SignificantMotion returns the significant motion data. +// Panics if called on a sensor type other than SensorSignificantMotion. +func (sv SensorValue) SignificantMotion() SignificantMotion { + if sv.id != SensorSignificantMotion { + panic("bno08x: SignificantMotion() called on wrong sensor type") + } + return sv.significantMotion +} + +// ShakeDetector returns the shake detector data. +// Panics if called on a sensor type other than SensorShakeDetector. +func (sv SensorValue) ShakeDetector() ShakeDetector { + if sv.id != SensorShakeDetector { + panic("bno08x: ShakeDetector() called on wrong sensor type") + } + return sv.shakeDetector +} + +// FlipDetector returns the flip detector data. +// Panics if called on a sensor type other than SensorFlipDetector. +func (sv SensorValue) FlipDetector() uint16 { + if sv.id != SensorFlipDetector { + panic("bno08x: FlipDetector() called on wrong sensor type") + } + return sv.flipDetector +} + +// StabilityClassifier returns the stability classifier data. +// Panics if called on a sensor type other than SensorStabilityClassifier. +func (sv SensorValue) StabilityClassifier() StabilityClassifier { + if sv.id != SensorStabilityClassifier { + panic("bno08x: StabilityClassifier() called on wrong sensor type") + } + return sv.stabilityClassifier +} + +// StabilityDetector returns the stability detector value. +// Panics if called on a sensor type other than SensorStabilityDetector. +func (sv SensorValue) StabilityDetector() uint8 { + if sv.id != SensorStabilityDetector { + panic("bno08x: StabilityDetector() called on wrong sensor type") + } + return sv.stabilityDetector +} + +// ActivityClassifier returns the activity classification data. +// Note: This field appears unused in decode.go, keeping for API compatibility. +func (sv SensorValue) ActivityClassifier() ActivityClassification { + return sv.activityClassifier +} + +// PersonalActivityClassifier returns the personal activity classifier data. +// Panics if called on a sensor type other than SensorPersonalActivityClassifier. +func (sv SensorValue) PersonalActivityClassifier() PersonalActivityClassifier { + if sv.id != SensorPersonalActivityClassifier { + panic("bno08x: PersonalActivityClassifier() called on wrong sensor type") + } + return sv.personalActivityClassifier +} + +// SleepDetector returns the sleep detector value. +// Panics if called on a sensor type other than SensorSleepDetector. +func (sv SensorValue) SleepDetector() uint8 { + if sv.id != SensorSleepDetector { + panic("bno08x: SleepDetector() called on wrong sensor type") + } + return sv.sleepDetector +} + +// TiltDetector returns the tilt detector value. +// Panics if called on a sensor type other than SensorTiltDetector. +func (sv SensorValue) TiltDetector() uint8 { + if sv.id != SensorTiltDetector { + panic("bno08x: TiltDetector() called on wrong sensor type") + } + return sv.tiltDetector +} + +// PocketDetector returns the pocket detector value. +// Panics if called on a sensor type other than SensorPocketDetector. +func (sv SensorValue) PocketDetector() uint8 { + if sv.id != SensorPocketDetector { + panic("bno08x: PocketDetector() called on wrong sensor type") + } + return sv.pocketDetector +} + +// CircleDetector returns the circle detector value. +// Panics if called on a sensor type other than SensorCircleDetector. +func (sv SensorValue) CircleDetector() uint8 { + if sv.id != SensorCircleDetector { + panic("bno08x: CircleDetector() called on wrong sensor type") + } + return sv.circleDetector +} + +// HeartRateMonitor returns the heart rate monitor value. +// Panics if called on a sensor type other than SensorHeartRateMonitor. +func (sv SensorValue) HeartRateMonitor() uint16 { + if sv.id != SensorHeartRateMonitor { + panic("bno08x: HeartRateMonitor() called on wrong sensor type") + } + return sv.heartRateMonitor +} diff --git a/examples/bno08x/main.go b/examples/bno08x/main.go new file mode 100644 index 000000000..21741350a --- /dev/null +++ b/examples/bno08x/main.go @@ -0,0 +1,66 @@ +// Package main provides a basic example of using the BNO08x driver +// to read rotation vector (quaternion) data from the sensor. +package main + +import ( + "machine" + "time" + + "tinygo.org/x/drivers/bno08x" +) + +func main() { + time.Sleep(2 * time.Second) // Wait for sensor to power up + // Initialize I2C bus + i2c := machine.I2C0 + err := i2c.Configure(machine.I2CConfig{ + Frequency: 400 * machine.KHz, + }) + if err != nil { + println("Failed to configure I2C:", err.Error()) + return + } + + println("Initializing BNO08x sensor...") + + // Create and configure sensor + sensor := bno08x.New(i2c) + err = sensor.Configure(bno08x.Config{}) + if err != nil { + println("Failed to configure sensor:", err.Error()) + return + } + + println("Sensor initialized successfully") + + // Enable Game Rotation Vector reports at 100Hz (10000 microseconds = 10ms interval) + // Using Game Rotation Vector (0x08) to match the working channel_debug test + err = sensor.EnableReport(bno08x.SensorGameRotationVector, 10000) + if err != nil { + println("Failed to enable game rotation vector:", err.Error()) + return + } + + println("Reading rotation vectors...") + println("Format: Real I J K Accuracy") + + // Add a delay after enabling reports (Arduino does this) + time.Sleep(100 * time.Millisecond) + + // Main loop - read and display quaternion data + for { + event, ok := sensor.GetSensorEvent() + if ok && (event.ID() == bno08x.SensorRotationVector || event.ID() == bno08x.SensorGameRotationVector) { + q := event.Quaternion() + if event.ID() == bno08x.SensorRotationVector { + println(q.Real, q.I, q.J, q.K, event.QuaternionAccuracy()) + } else { + // GameRotationVector doesn't have accuracy + println(q.Real, q.I, q.J, q.K) + } + } + + // Arduino uses 10ms delay in loop + time.Sleep(10 * time.Millisecond) + } +} diff --git a/smoketest.sh b/smoketest.sh index b0035988f..f1cbc0721 100755 --- a/smoketest.sh +++ b/smoketest.sh @@ -20,6 +20,7 @@ tinygo build -size short -o ./build/test.hex -target=itsybitsy-m0 ./examples/bmi tinygo build -size short -o ./build/test.hex -target=itsybitsy-m0 ./examples/bmp180/main.go tinygo build -size short -o ./build/test.hex -target=itsybitsy-m0 ./examples/bmp280/main.go tinygo build -size short -o ./build/test.hex -target=trinket-m0 ./examples/bmp388/main.go +tinygo build -size short -o ./build/test.hex -target=metro-rp2350 ./examples/bno08x/main.go tinygo build -size short -o ./build/test.hex -target=bluepill ./examples/ds1307/sram/main.go tinygo build -size short -o ./build/test.hex -target=bluepill ./examples/ds1307/time/main.go tinygo build -size short -o ./build/test.hex -target=itsybitsy-m0 ./examples/ds3231/main.go