fix: adjust IMU sample rate and use 10ms ticker in main loop

Also collect garbage often to avoid long unpredicted pauses.
This all makes sensor fusion algorithm works much better.

src/main.go

@@ -2,6 +2,7 @@ package main
 
 import (
 	"math"
+	"runtime"
 	"strconv"
 	"time"
 
@@ -13,7 +14,7 @@ import (
 var Version string
 
 const (
-	PERIOD           = 20
+	PERIOD           = 10
 	BLINK_MAIN_COUNT = 500
 	BLINK_WARM_COUNT = 125
 	BLINK_PARA_COUNT = 250
@@ -34,6 +35,10 @@ var (
 	f *Flash
 )
 
+var (
+	tickPeriod *time.Ticker
+)
+
 func init() {
 
 	initLeds()
@@ -59,6 +64,8 @@ func init() {
 
 	f = &Flash{}
 
+	tickPeriod = time.NewTicker(PERIOD * time.Millisecond)
+
 }
 
 func main() {
@@ -74,7 +81,7 @@ func main() {
 	// warm up IMU (1 sec)
 	for i := 0; i < 50; i++ {
 		o.Calibrate()
-		time.Sleep(PERIOD * time.Millisecond)
+		<-tickPeriod.C
 	}
 
 	flashLoad()
@@ -170,8 +177,11 @@ func main() {
 		statePara(iter)
 		trace(iter)
 
+		// memory
+		runtime.GC() // run garbage collector often to avoid long pauses
+
 		// wait
-		time.Sleep(PERIOD * time.Millisecond)
+		<-tickPeriod.C
 		iter += PERIOD
 		iter %= 10_000
 	}
@@ -263,11 +273,17 @@ func statePara(iter uint16) {
 	}
 }
 
+var ms = runtime.MemStats{}
+var traceTime = time.Now()
+
 func trace(iter uint16) {
 	if iter%TRACE_COUNT == 0 { // print out state
+		timeDiff := time.Since(traceTime)
+		traceTime = time.Now()
+		runtime.ReadMemStats(&ms)
 		channels := t.Channels()
 		r, p, y := channels[0], channels[1], channels[2]
 		rc, pc, yc := o.Offsets()
-		println(time.Now().Unix(), ": ", t.Address(), " | ", Version, " [", r, ",", p, ",", y, "] (", rc, ",", pc, ",", yc, ")")
+		println(time.Now().Unix(), "|", t.Address(), "|", Version, "| [", r, ",", p, ",", y, "] (", rc, ",", pc, ",", yc, ") |", ms.HeapInuse, "/", timeDiff.Milliseconds()-TRACE_COUNT, "ms")
 	}
 }

src/orientation/imu_xiao-ble.go

@@ -44,9 +44,9 @@ func (imu *IMU) Configure() {
 	imu.device = lsm6ds3tr.New(machine.I2C1)
 	imu.device.Configure(lsm6ds3tr.Configuration{
 		AccelRange:      lsm6ds3tr.ACCEL_4G,     // 4g
-		AccelSampleRate: lsm6ds3tr.ACCEL_SR_833, // every ~1.2ms
+		AccelSampleRate: lsm6ds3tr.ACCEL_SR_104, // every ~9.6ms
 		GyroRange:       lsm6ds3tr.GYRO_500DPS,  // 500 deg/s
-		GyroSampleRate:  lsm6ds3tr.GYRO_SR_833,  // every ~1.2ms
+		GyroSampleRate:  lsm6ds3tr.GYRO_SR_104,  // every ~9.6ms
 	})
 
 	tapConfig := map[byte]byte{

src/orientation/orientation.go

@@ -11,6 +11,10 @@ import (
 const radToDeg = 180 / math.Pi // 57.29578
 const degToRad = 1 / radToDeg  // 0.0174533
 
+// Rule of thumb: increasing beta leads to (a) faster bias corrections, (b) higher sensitiveness to lateral accelerations.
+// https://stackoverflow.com/questions/47589230/what-is-the-best-beta-value-in-madgwick-filter
+const madgwickBeta = 0.025
+
 type Orientation struct {
 	imu     *IMU
 	fusion  ahrs.Madgwick
@@ -27,7 +31,7 @@ func New(imu *IMU) *Orientation {
 
 func (o *Orientation) Configure(period time.Duration) {
 	o.imu.Configure()
-	o.fusion = ahrs.NewMadgwick(0.025, float64(time.Second/period))
+	o.fusion = ahrs.NewMadgwick(madgwickBeta, float64(time.Second/period))
 }
 
 // Reset orientation for sensor fusion algoritm