1 files changed, 50 insertions, 40 deletions

diff --git a/src/drivers/Hrs3300.cpp b/src/drivers/Hrs3300.cpp
index 6c47ae28..a4b72479 100644
--- a/src/drivers/Hrs3300.cpp
+++ b/src/drivers/Hrs3300.cpp
@@ -6,6 +6,7 @@
 
 #include "drivers/Hrs3300.h"
 #include <algorithm>
+#include <iterator>
 #include <nrf_gpio.h>
 
 #include <FreeRTOS.h>
@@ -14,8 +15,14 @@
 
 using namespace Pinetime::Drivers;
 
+namespace {
+  static constexpr uint8_t ledDriveCurrentValue = 0x2f;
+}
+
 /** Driver for the HRS3300 heart rate sensor.
- * Original implementation from wasp-os : https://github.com/daniel-thompson/wasp-os/blob/master/wasp/drivers/hrs3300.py
+ * Original implementation from wasp-os : https://github.com/wasp-os/wasp-os/blob/master/wasp/drivers/hrs3300.py
+ *
+ * Experimentaly derived changes to improve signal/noise (see comments below) - Ceimour
  */
 Hrs3300::Hrs3300(TwiMaster& twiMaster, uint8_t twiAddress) : twiMaster {twiMaster}, twiAddress {twiAddress} {
 }
@@ -26,19 +33,21 @@ void Hrs3300::Init() {
   Disable();
   vTaskDelay(100);
 
-  // HRS disabled, 12.5 ms wait time between cycles, (partly) 20mA drive
-  WriteRegister(static_cast<uint8_t>(Registers::Enable), 0x60);
+  // HRS disabled, 50ms wait time between ADC conversion period, current 12.5mA
+  WriteRegister(static_cast<uint8_t>(Registers::Enable), 0x50);
 
-  // (partly) 20mA drive, power on, "magic" (datasheet says both
-  // "reserved" and "set low nibble to 8" but 0xe gives better results
-  // and is used by at least two other HRS3300 drivers
-  WriteRegister(static_cast<uint8_t>(Registers::PDriver), 0x6E);
+  // Current 12.5mA and low nibble 0xF.
+  // Note: Setting low nibble to 0x8 per the datasheet results in
+  // modulated LED driver output. Setting to 0xF results in clean,
+  // steady output during the ADC conversion period.
+  WriteRegister(static_cast<uint8_t>(Registers::PDriver), ledDriveCurrentValue);
 
-  // HRS and ALS both in 16-bit mode
-  WriteRegister(static_cast<uint8_t>(Registers::Res), 0x88);
+  // HRS and ALS both in 15-bit mode results in ~50ms LED drive period
+  // and presumably ~50ms ADC conversion period.
+  WriteRegister(static_cast<uint8_t>(Registers::Res), 0x77);
 
-  // 8x gain, non default, reduced value for better readings
-  WriteRegister(static_cast<uint8_t>(Registers::Hgain), 0xc);
+  // Gain set to 1x
+  WriteRegister(static_cast<uint8_t>(Registers::Hgain), 0x00);
 }
 
 void Hrs3300::Enable() {
@@ -46,6 +55,8 @@ void Hrs3300::Enable() {
   auto value = ReadRegister(static_cast<uint8_t>(Registers::Enable));
   value |= 0x80;
   WriteRegister(static_cast<uint8_t>(Registers::Enable), value);
+
+  WriteRegister(static_cast<uint8_t>(Registers::PDriver), ledDriveCurrentValue);
 }
 
 void Hrs3300::Disable() {
@@ -53,42 +64,41 @@ void Hrs3300::Disable() {
   auto value = ReadRegister(static_cast<uint8_t>(Registers::Enable));
   value &= ~0x80;
   WriteRegister(static_cast<uint8_t>(Registers::Enable), value);
-}
 
-uint32_t Hrs3300::ReadHrs() {
-  auto m = ReadRegister(static_cast<uint8_t>(Registers::C0DataM));
-  auto h = ReadRegister(static_cast<uint8_t>(Registers::C0DataH));
-  auto l = ReadRegister(static_cast<uint8_t>(Registers::C0dataL));
-  return ((l & 0x30) << 12) | (m << 8) | ((h & 0x0f) << 4) | (l & 0x0f);
+  WriteRegister(static_cast<uint8_t>(Registers::PDriver), 0);
 }
 
-uint32_t Hrs3300::ReadAls() {
-  auto m = ReadRegister(static_cast<uint8_t>(Registers::C1dataM));
-  auto h = ReadRegister(static_cast<uint8_t>(Registers::C1dataH));
-  auto l = ReadRegister(static_cast<uint8_t>(Registers::C1dataL));
-  return ((h & 0x3f) << 11) | (m << 3) | (l & 0x07);
-}
+Hrs3300::PackedHrsAls Hrs3300::ReadHrsAls() {
+  constexpr Registers dataRegisters[] =
+    {Registers::C1dataM, Registers::C0DataM, Registers::C0DataH, Registers::C1dataH, Registers::C1dataL, Registers::C0dataL};
+  // Calculate smallest register address
+  constexpr uint8_t baseOffset = static_cast<uint8_t>(*std::min_element(std::begin(dataRegisters), std::end(dataRegisters)));
+  // Calculate largest address to determine length of read needed
+  // Add one to largest relative index to find the length
+  constexpr uint8_t length = static_cast<uint8_t>(*std::max_element(std::begin(dataRegisters), std::end(dataRegisters))) - baseOffset + 1;
 
-void Hrs3300::SetGain(uint8_t gain) {
-  constexpr uint8_t maxGain = 64U;
-  gain = std::min(gain, maxGain);
-  uint8_t hgain = 0;
-  while ((1 << hgain) < gain) {
-    ++hgain;
+  Hrs3300::PackedHrsAls res;
+  uint8_t buf[length];
+  auto ret = twiMaster.Read(twiAddress, baseOffset, buf, length);
+  if (ret != TwiMaster::ErrorCodes::NoError) {
+    NRF_LOG_INFO("READ ERROR");
   }
+  // hrs
+  uint8_t m = static_cast<uint8_t>(Registers::C0DataM) - baseOffset;
+  uint8_t h = static_cast<uint8_t>(Registers::C0DataH) - baseOffset;
+  uint8_t l = static_cast<uint8_t>(Registers::C0dataL) - baseOffset;
+  // There are two extra bits (17 and 18) but they are not read here
+  // as resolutions >16bit aren't practically useful (too slow) and
+  // all hrs values throughout InfiniTime are 16bit
+  res.hrs = (buf[m] << 8) | ((buf[h] & 0x0f) << 4) | (buf[l] & 0x0f);
 
-  WriteRegister(static_cast<uint8_t>(Registers::Hgain), hgain << 2);
-}
-
-void Hrs3300::SetDrive(uint8_t drive) {
-  auto en = ReadRegister(static_cast<uint8_t>(Registers::Enable));
-  auto pd = ReadRegister(static_cast<uint8_t>(Registers::PDriver));
-
-  en = (en & 0xf7) | ((drive & 2) << 2);
-  pd = (pd & 0xbf) | ((drive & 1) << 6);
+  // als
+  m = static_cast<uint8_t>(Registers::C1dataM) - baseOffset;
+  h = static_cast<uint8_t>(Registers::C1dataH) - baseOffset;
+  l = static_cast<uint8_t>(Registers::C1dataL) - baseOffset;
+  res.als = ((buf[h] & 0x3f) << 11) | (buf[m] << 3) | (buf[l] & 0x07);
 
-  WriteRegister(static_cast<uint8_t>(Registers::Enable), en);
-  WriteRegister(static_cast<uint8_t>(Registers::PDriver), pd);
+  return res;
 }
 
 void Hrs3300::WriteRegister(uint8_t reg, uint8_t data) {