InfiniTime.git

diff --git a/src/components/heartrate/Ppg.cpp b/src/components/heartrate/Ppg.cpp
index 3a6988ae8f5ffb884a43dad608bd1e86d9ba1fd3..efbed852f5692aacae0b11f28227c1b44294e741 100644
--- a/src/components/heartrate/Ppg.cpp
+++ b/src/components/heartrate/Ppg.cpp
@@ -142,7 +142,7 @@   dataAverage.fill(0.0f);
   spectrum.fill(0.0f);
 }
 
-int8_t Ppg::Preprocess(uint32_t hrs, uint32_t als) {
+int8_t Ppg::Preprocess(uint16_t hrs, uint16_t als) {
   if (dataIndex < dataLength) {
     dataHRS[dataIndex++] = hrs;
   }




diff --git a/src/components/heartrate/Ppg.h b/src/components/heartrate/Ppg.h
index 4492b2c29b93a9fb6fb5b78656153acbd0e0529d..373e7985c565f41befaaa21236e5cdd1f958c15c 100644
--- a/src/components/heartrate/Ppg.h
+++ b/src/components/heartrate/Ppg.h
@@ -14,7 +14,7 @@   namespace Controllers {
     class Ppg {
     public:
       Ppg();
-      int8_t Preprocess(uint32_t hrs, uint32_t als);
+      int8_t Preprocess(uint16_t hrs, uint16_t als);
       int HeartRate();
       void Reset(bool resetDaqBuffer);
       static constexpr int deltaTms = 100;




diff --git a/src/drivers/Hrs3300.cpp b/src/drivers/Hrs3300.cpp
index 33889b6fc4a03350f7904341830d312ee77ce81f..9c77975e83298e64cfc1e7dc1e54cf5b5652bee0 100644
--- a/src/drivers/Hrs3300.cpp
+++ b/src/drivers/Hrs3300.cpp
@@ -67,40 +67,37 @@
   WriteRegister(static_cast<uint8_t>(Registers::PDriver), 0);
 }
 
-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);
-}
+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;
 
-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);
-}
-
-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");
   }
-
-  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));
+  // 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);
 
-  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) {




diff --git a/src/drivers/Hrs3300.h b/src/drivers/Hrs3300.h
index 8bbdc69a40e7906ffb478f6fcdc449eb4b52521f..6f721448477deebc3b5c3fdca335b6ce76ca90e5 100644
--- a/src/drivers/Hrs3300.h
+++ b/src/drivers/Hrs3300.h
@@ -21,6 +21,11 @@         Res = 0x16,
         Hgain = 0x17
       };
 
+      struct PackedHrsAls {
+        uint16_t hrs;
+        uint16_t als;
+      };
+
       Hrs3300(TwiMaster& twiMaster, uint8_t twiAddress);
       Hrs3300(const Hrs3300&) = delete;
       Hrs3300& operator=(const Hrs3300&) = delete;
@@ -30,10 +35,7 @@
       void Init();
       void Enable();
       void Disable();
-      uint32_t ReadHrs();
-      uint32_t ReadAls();
-      void SetGain(uint8_t gain);
-      void SetDrive(uint8_t drive);
+      PackedHrsAls ReadHrsAls();
 
     private:
       TwiMaster& twiMaster;




diff --git a/src/heartratetask/HeartRateTask.cpp b/src/heartratetask/HeartRateTask.cpp
index 9d82d11e0dab738c33335bcb979afdf6b5863b0f..8a5a871b41845c962be89ce9ec15399006286708 100644
--- a/src/heartratetask/HeartRateTask.cpp
+++ b/src/heartratetask/HeartRateTask.cpp
@@ -70,7 +70,8 @@       }
     }
 
     if (measurementStarted) {
-      int8_t ambient = ppg.Preprocess(heartRateSensor.ReadHrs(), heartRateSensor.ReadAls());
+      auto sensorData = heartRateSensor.ReadHrsAls();
+      int8_t ambient = ppg.Preprocess(sensorData.hrs, sensorData.als);
       int bpm = ppg.HeartRate();
 
       // If ambient light detected or a reset requested (bpm < 0)