Add support for CEVA BNO08x 9DoF sensor (#809)

* Add support for CEVA BNO08x 9DoF sensor. Also includes implementation of CEVA SH-2 and SHTP protocols. * Replace machine.I2C with drivers.I2C interface to remove dependency on machine package * Replace Pin functionality with that provided by tinygo.org/x/drivers/internal/pin * Unexport fields on SensorValue and replace with accessor methods. Add check for correct SensorID validation * Add example to smoketest.sh * Change build target for smoketest to match development environment.. Probably not important, but matches reality. * Fix decoding of some sensor data: Step Counter, Tap Detector, Flip Detector. These are experimental. * Refactor to allow SPI/UART etc. SPI is currently under development, but is omitted from this commit. Example code has been moved to i2c subdirectory. This commit introduces some major refactoring changes. It introduces a "Buser" interface and tries to remove any I2C specific code from the core. It still retains a couple of I2C specific fields in the "Config" struct ("Address" and "ReadChunk") but they are ignored in the as yet uncommited SPI code. * Fix CRLF -> LF for gofmt * Update smoketest to point to new example file
2026-08-25 08:59:00 +00:00 · 2025-12-11 05:07:04 +11:00
parent 9ed648f4a5
commit 936a255df9
10 changed files with 2076 additions and 0 deletions
@@ -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
+	}
+}