initial commit

2025-02-23 20:40:38 +01:00
commit a429c81bcb
7 changed files with 293 additions and 0 deletions
--- a/main.go
+++ b/main.go
@@ -0,0 +1,173 @@
+package main
+
+import (
+	"encoding/binary"
+	"fmt"
+	"machine"
+	"time"
+
+	"tinygo.org/x/bluetooth"
+	"tinygo.org/x/drivers/lsm6ds3tr"
+)
+
+var adapter = bluetooth.DefaultAdapter
+
+var isBleConnected bool = false
+
+var isCapturing bool = false
+
+var (
+	LSM6DS3TRService = [16]byte{0x4C, 0x53, 0x4D, 0x36, 0x44, 0x53, 0x33, 0x54, 0x52, 0x53, 0x65, 0x72, 0x76, 0x69, 0x63, 0x65}
+	accelerationData = [16]byte{0x61, 0x63, 0x63, 0x65, 0x6C, 0x65, 0x72, 0x61, 0x74, 0x69, 0x6F, 0x6E, 0x44, 0x61, 0x74, 0x61}
+	unixTimeStampRst = [16]byte{0x75, 0x6E, 0x69, 0x78, 0x54, 0x69, 0x6D, 0x65, 0x53, 0x74, 0x61, 0x6D, 0x70, 0x52, 0x73, 0x74}
+	capturingService = [16]byte{0x63, 0x61, 0x70, 0x74, 0x75, 0x72, 0x69, 0x6E, 0x67, 0x53, 0x65, 0x72, 0x76, 0x69, 0x63, 0x65}
+	//tempSenseService = [16]byte{0x74, 0x65, 0x6D, 0x70, 0x53, 0x65, 0x6E, 0x73, 0x65, 0x53, 0x65, 0x72, 0x76, 0x69, 0x63, 0x65}
+	//temperatureSense = [16]byte{0x74, 0x65, 0x6D, 0x70, 0x65, 0x72, 0x61, 0x74, 0x75, 0x72, 0x65, 0x53, 0x65, 0x6E, 0x73, 0x65}
+)
+
+var currentTimeStamp time.Time = time.Now()
+var lastTimeStamp time.Time = time.Now()
+
+const sleepDuration time.Duration = time.Millisecond * 100
+
+func main() {
+	// Configure LSM6DS3TR
+	machine.I2C0.Configure(machine.I2CConfig{})
+
+	accel := lsm6ds3tr.New(machine.I2C0)
+	err := accel.Configure(lsm6ds3tr.Configuration{})
+	if err != nil {
+		for {
+			println("Failed to configure", err.Error())
+			time.Sleep(time.Second)
+		}
+	}
+
+	// Configure Bluetooth
+	must("enable BLE stack", adapter.Enable())
+	adv := adapter.DefaultAdvertisement()
+	must("config adv", adv.Configure(bluetooth.AdvertisementOptions{
+		LocalName: "Go Bluetooth",
+		ManufacturerData: []bluetooth.ManufacturerDataElement{
+			// 0xFFFF: Special Use/Default ID
+			// Bluetooth Company Identifiers:
+			// https://gist.github.com/angorb/f92f76108b98bb0d81c74f60671e9c67
+			{CompanyID: 0xffff, Data: []byte{0x01, 0x02}},
+		},
+	}))
+
+	adapter.SetConnectHandler(func(device bluetooth.Device, connected bool) {
+		if connected {
+			isBleConnected = true
+			adv.Stop()
+		} else {
+			isBleConnected = false
+			isCapturing = false
+			adv.Start()
+		}
+	})
+	//
+	// Start Bluetooth advertisment
+	must("start adv", adv.Start())
+
+	var senseCharacteristic bluetooth.Characteristic
+	must("add sense service", adapter.AddService(&bluetooth.Service{
+		UUID: bluetooth.NewUUID(LSM6DS3TRService),
+		Characteristics: []bluetooth.CharacteristicConfig{
+			{
+				Handle: &senseCharacteristic,
+				UUID:   bluetooth.NewUUID(accelerationData),
+				// can only send a max amount of 20 bytes in one packet
+				//Value: []byte{},
+				Flags: bluetooth.CharacteristicNotifyPermission | bluetooth.CharacteristicReadPermission | bluetooth.CharacteristicWritePermission | bluetooth.CharacteristicWriteWithoutResponsePermission,
+			},
+			{
+				UUID:  bluetooth.NewUUID(unixTimeStampRst),
+				Flags: bluetooth.CharacteristicNotifyPermission | bluetooth.CharacteristicReadPermission | bluetooth.CharacteristicWritePermission | bluetooth.CharacteristicWriteWithoutResponsePermission,
+				WriteEvent: func(client bluetooth.Connection, offset int, value []byte) {
+					if len(value) != 8 {
+						return
+					}
+					millisFromEpoch := binary.BigEndian.Uint64(value)
+					currentTimeStamp = time.Unix(0, int64(millisFromEpoch)*int64(time.Millisecond))
+				},
+			},
+			{
+				UUID:  bluetooth.NewUUID(capturingService),
+				Flags: bluetooth.CharacteristicNotifyPermission | bluetooth.CharacteristicReadPermission | bluetooth.CharacteristicWritePermission | bluetooth.CharacteristicWriteWithoutResponsePermission,
+				WriteEvent: func(client bluetooth.Connection, offset int, value []byte) {
+					if len(value) != 1 {
+						return
+					}
+					if value[0] == 1 {
+						isCapturing = true
+					} else {
+						isCapturing = false
+					}
+				},
+			},
+		},
+	}))
+
+	// var tempCharacteristic bluetooth.Characteristic
+	// must("add temperature service", adapter.AddService(&bluetooth.Service{
+	// 	UUID: bluetooth.NewUUID(tempSenseService),
+	// 	Characteristics: []bluetooth.CharacteristicConfig{
+	// 		{
+	// 			Handle: &tempCharacteristic,
+	// 			UUID:   bluetooth.NewUUID(temperatureSense),
+	// 			Value:  []byte(tempData),
+	// 			Flags:  bluetooth.CharacteristicNotifyPermission | bluetooth.CharacteristicReadPermission,
+	// 		},
+	// 	},
+	// }))
+
+	// Main Loop
+	for {
+		// Only read and update sensor data
+		// with an active bluetooth connection
+		// and when `isCapturing` is set to true
+		if isBleConnected && isCapturing {
+			X, Y, Z, _ := accel.ReadRotation()
+			x, y, z, _ := accel.ReadAcceleration()
+
+			arrRot := valuesToByteArray(X, Y, Z, int8(1))
+			arrAcc := valuesToByteArray(x, y, z, int8(2))
+
+			currentTimeStamp = currentTimeStamp.Add(time.Now().Sub(lastTimeStamp))
+			fmt.Println("TIME: ", time.Now().Sub(lastTimeStamp))
+			arrTime := timeStampToByteArray(currentTimeStamp.UnixMilli(), int8(3))
+
+			senseCharacteristic.Write(arrRot)
+			senseCharacteristic.Write(arrAcc)
+			senseCharacteristic.Write(arrTime)
+		}
+
+		if isCapturing {
+			lastTimeStamp = time.Now()
+		}
+		time.Sleep(sleepDuration)
+	}
+}
+
+func valuesToByteArray(x int32, y int32, z int32, p int8) []byte {
+	byteSlice := make([]byte, 13)
+	binary.LittleEndian.PutUint32(byteSlice, uint32(x))
+	binary.LittleEndian.PutUint32(byteSlice[4:], uint32(y))
+	binary.LittleEndian.PutUint32(byteSlice[8:], uint32(z))
+	byteSlice[12] = byte(p)
+	return byteSlice
+}
+
+func timeStampToByteArray(value int64, p int8) []byte {
+	byteSlice := make([]byte, 9)
+	binary.LittleEndian.PutUint64(byteSlice, uint64(value))
+	byteSlice[8] = byte(p)
+	return byteSlice
+}
+
+func must(action string, err error) {
+	if err != nil {
+		panic("failed to " + action + ": " + err.Error())
+	}
+}