diff --git a/README.md b/README.md index f97cb9c..336d9a2 100644 --- a/README.md +++ b/README.md @@ -109,8 +109,14 @@ First, you shall [flash UF2 bootloader to the board](./doc/Nano33BLE.md). You on Then connect the board to your computer, double-tap on button and copy `ht_nano-33-ble_xxx.uf2` file to NANO33BOOT usb drive. ## Use head tracker -Attach the board with flashed head tracker code to your FPV goggles. -Place your goggles on a solid surface and power the head tracker with 2 cell battery or 5v source. I use analog adaptor bay on my DJI goggles to source 5v. +Attach the head tracker to your FPV goggles. +Power the head tracker via USB, 2 cell battery or 5v source. +> Hint: I personally use analog adapter bay on my DJI V1 goggles to source 5v. + +### Bare +The head tracker is usable "bare", no extra accessories needed, not even a button. +To reset the orientation without a button, simply **double-tap** the head tracker. +> Hint: When the head tracker is mounted on your goggles directly, you can just double-tap your googles in any place to reset the orientation. ### LEDs On start, board shall blink continuously blue, red and green/orange leds. @@ -119,12 +125,11 @@ On start, board shall blink continuously blue, red and green/orange leds. - Green/orange led indicates health of the head tracker and shall slowly blink during normal operation. ### Buttons -The head tracker records initial orientation on power up, place your goggles accordingly. -Optionally, a **reset orientation** button can be wired to **D2** and **GND** pins. -Keep **reset orientation** button pressed on power up to discard calibration parameters stored in flash memory. +The head tracker records initial orientation on power up, place your goggles accordingly or reset orientation later by double-tapping the head tracker or by using a **reset orientation** button that can be wired to **D2** and **GND** pins. +Keep **reset orientation** button pressed on power up to **discard calibration parameters** stored in flash memory. ### Screen -If you have a LED 128x32 screen added you your board (via I2C), the board's bluetooth address is displayed on it. Blinking ":" symbols indicate bluetooth connection status, like blue led. Upon start, while gyroscope is calibrating, you shall see head tracker version briefly on the screen. The version is then replaced by 3 horisonal bars, one for each axis: roll, pitch and yaw. +If you have a LED `128x32` screen added you your board (via I2C), the board's bluetooth address is displayed on it. Blinking ":" symbols indicate bluetooth connection status, like blue led. Upon start, while gyroscope is calibrating, you shall see head tracker version briefly on the screen. The version is then replaced by 3 horisonal bars, one for each axis: roll, pitch and yaw. ## Connect to radio diff --git a/src/main.go b/src/main.go index 583f890..a4e3390 100644 --- a/src/main.go +++ b/src/main.go @@ -29,6 +29,7 @@ const flashStoreTreshold = 10_000 var ( d *display.Display t trainer.Trainer + i *orientation.IMU o *orientation.Orientation f *Flash ) @@ -40,7 +41,8 @@ func init() { initExtras() // Orientation - o = orientation.New() + i = orientation.NewIMU() + o = orientation.New(i) o.Configure(PERIOD * time.Millisecond) // Trainer (Bluetooth or PPM) @@ -149,9 +151,11 @@ func main() { iter = 0 for { - if !pinResetCenter.Get() { // Low means button pressed => shall reset center + if !pinResetCenter.Get() || (iter%400 == 0 && i.ReadTap()) { // Button pressed OR [double] tap registered (shall not read register more frequently than double tap duration) o.Reset() - continue + on(ledR) + } else { + off(ledR) } o.Update() diff --git a/src/orientation/imu_nano-33-ble.go b/src/orientation/imu_nano-33-ble.go index c132e33..69a237d 100644 --- a/src/orientation/imu_nano-33-ble.go +++ b/src/orientation/imu_nano-33-ble.go @@ -63,3 +63,7 @@ func (imu *IMU) Read() (gx, gy, gz, ax, ay, az float64, err error) { ax, ay, az = float64(-axi)/1000000, float64(ayi)/1000000, float64(azi)/1000000 return } + +func (imu *IMU) ReadTap() (tap bool) { + return false // TODO implemented tap detection on Nano 33 BLE +} diff --git a/src/orientation/imu_xiao-ble.go b/src/orientation/imu_xiao-ble.go index 275a23a..0ec42b7 100644 --- a/src/orientation/imu_xiao-ble.go +++ b/src/orientation/imu_xiao-ble.go @@ -9,6 +9,15 @@ import ( "tinygo.org/x/drivers/lsm6ds3tr" ) +const ( + TAP_SRC = 0x1C + TAP_CFG = 0x58 + TAP_THS_6D = 0x59 + INT_DUR2 = 0x5A + WAKE_UP_THS = 0x5B + MD1_CFG = 0x5E +) + type IMU struct { device *lsm6ds3tr.Device gyrCal *GyrCal @@ -34,12 +43,24 @@ func (imu *IMU) Configure() { // Configure IMU imu.device = lsm6ds3tr.New(machine.I2C1) imu.device.Configure(lsm6ds3tr.Configuration{ - AccelRange: lsm6ds3tr.ACCEL_4G, - AccelSampleRate: lsm6ds3tr.ACCEL_SR_833, - GyroRange: lsm6ds3tr.GYRO_500DPS, - GyroSampleRate: lsm6ds3tr.GYRO_SR_833, + AccelRange: lsm6ds3tr.ACCEL_4G, // 4g + AccelSampleRate: lsm6ds3tr.ACCEL_SR_833, // every ~1.2ms + GyroRange: lsm6ds3tr.GYRO_500DPS, // 500 deg/s + GyroSampleRate: lsm6ds3tr.GYRO_SR_833, // every ~1.2ms }) + tapConfig := map[byte]byte{ + TAP_CFG: 0x8F, // interrupts enable + tap all axes + latch (saves the state of the interrupt until register is read) + TAP_THS_6D: 0x01, // tap threshold + INT_DUR2: 0xFF, // tap sensing params: duration = 16*([7:4]+1)*1.2ms, quiet = 2*([3:2]+1)*1.2ms, shock = 4*([1:0]+1)*1.2ms => 0xFF = 307.2ms, 10.8ms, 4.8ms + WAKE_UP_THS: 0x80, // enable double tap events + MD1_CFG: 0x08, // route double tap events to INT1 (requited for the latch to work) + } + + for reg, val := range tapConfig { + machine.I2C1.WriteRegister(uint8(imu.device.Address), reg, []byte{val}) + } + } func (imu *IMU) Read() (gx, gy, gz, ax, ay, az float64, err error) { @@ -61,3 +82,9 @@ func (imu *IMU) Read() (gx, gy, gz, ax, ay, az float64, err error) { ax, ay, az = float64(-axi)/1000000, float64(ayi)/1000000, float64(azi)/1000000 return } + +func (imu *IMU) ReadTap() (tap bool) { + data := []byte{0x00} + machine.I2C1.ReadRegister(uint8(imu.device.Address), TAP_SRC, data) + return data[0]&0x10 != 0 +} diff --git a/src/orientation/orientation.go b/src/orientation/orientation.go index 96af147..24f4ba9 100644 --- a/src/orientation/orientation.go +++ b/src/orientation/orientation.go @@ -18,9 +18,9 @@ type Orientation struct { current mgl.Quat } -func New() *Orientation { +func New(imu *IMU) *Orientation { return &Orientation{ - imu: NewIMU(), + imu: imu, offset: mgl.QuatIdent(), } }