Fix #67, performance getCumulativePosition and getAngularSpeed (#71)

- fix #67, improve performance **getCumulativePosition()** and **getAngularSpeed()**
  - kudos to Chris-42
- add **AS5600_position_speed.ino**
- update readme.md

AS5600.cpp

@@ -1,7 +1,7 @@
 //
 //    FILE: AS56000.cpp
 //  AUTHOR: Rob Tillaart
-// VERSION: 0.6.3
+// VERSION: 0.6.4
 // PURPOSE: Arduino library for AS5600 magnetic rotation meter
 //    DATE: 2022-05-28
 //     URL: https://github.com/RobTillaart/AS5600
@@ -319,14 +319,20 @@ uint16_t AS5600::rawAngle()
 uint16_t AS5600::readAngle()
 {
   uint16_t value = readReg2(AS5600_ANGLE);
+  if (_error != AS5600_OK)
+  {
+    return _lastReadAngle;
+  }
   if (_offset > 0) value += _offset;
   value &= 0x0FFF;
 
   if ((_directionPin == AS5600_SW_DIRECTION_PIN) &&
       (_direction == AS5600_COUNTERCLOCK_WISE))
   {
+    //  mask needed for value == 0.
     value = (4096 - value) & 0x0FFF;
   }
+  _lastReadAngle = value;
   return value;
 }
 
@@ -426,11 +432,19 @@ bool AS5600::magnetTooWeak()
 //  }
 
 
-float AS5600::getAngularSpeed(uint8_t mode)
+float AS5600::getAngularSpeed(uint8_t mode, bool update)
 {
+  if (update)
+  {
+    _lastReadAngle = readAngle();
+    if (_error != AS5600_OK)
+    {
+      return NAN;
+    }
+  }
   //  default behaviour
   uint32_t now     = micros();
-  int      angle   = readAngle();
+  int      angle   = _lastReadAngle;
   uint32_t deltaT  = now - _lastMeasurement;
   int      deltaA  = angle - _lastAngle;
 
@@ -463,13 +477,17 @@ float AS5600::getAngularSpeed(uint8_t mode)
 //
 //  POSITION cumulative
 //
-int32_t AS5600::getCumulativePosition()
+int32_t AS5600::getCumulativePosition(bool update)
 {
-  int16_t value = readAngle();
-  if (_error != AS5600_OK)
+  if (update)
   {
-    return _position;
+    _lastReadAngle = readAngle();
+    if (_error != AS5600_OK)
+    {
+      return _position;  //  last known position.
+    }
   }
+  int16_t value = _lastReadAngle;
 
   //  whole rotation CW?
   //  less than half a circle

AS5600.h

@@ -2,7 +2,7 @@
 //
 //    FILE: AS5600.h
 //  AUTHOR: Rob Tillaart
-// VERSION: 0.6.3
+// VERSION: 0.6.4
 // PURPOSE: Arduino library for AS5600 magnetic rotation meter
 //    DATE: 2022-05-28
 //     URL: https://github.com/RobTillaart/AS5600
@@ -12,7 +12,7 @@
 #include "Wire.h"
 
 
-#define AS5600_LIB_VERSION              (F("0.6.3"))
+#define AS5600_LIB_VERSION              (F("0.6.4"))
 
 
 //  default addresses
@@ -105,7 +105,7 @@ class AS5600
   //  made virtual, see #66
   virtual bool isConnected();
 
-  //  address = fixed   0x36 for AS5600, 
+  //  address = fixed   0x36 for AS5600,
   //          = default 0x40 for AS5600L
   uint8_t  getAddress();
 
@@ -219,11 +219,12 @@ class AS5600
   //  approximation of the angular speed in rotations per second.
   //  mode == 1: radians /second
   //  mode == 0: degrees /second  (default)
-  float    getAngularSpeed(uint8_t mode = AS5600_MODE_DEGREES);
+  float    getAngularSpeed(uint8_t mode = AS5600_MODE_DEGREES,
+                           bool update = true);
 
   //  EXPERIMENTAL CUMULATIVE POSITION
   //  reads sensor and updates cumulative position
-  int32_t  getCumulativePosition();
+  int32_t  getCumulativePosition(bool update = true);
   //  converts last position to whole revolutions.
   int32_t  getRevolutions();
   //  resets position only (not the i)
@@ -254,6 +255,7 @@ class AS5600
   //  for getAngularSpeed()
   uint32_t _lastMeasurement = 0;
   int16_t  _lastAngle       = 0;
+  int16_t  _lastReadAngle   = 0;
 
   //  for readAngle() and rawAngle()
   uint16_t _offset          = 0;

CHANGELOG.md

@@ -5,6 +5,12 @@ The format is based on [Keep a Changelog](http://keepachangelog.com/)
 and this project adheres to [Semantic Versioning](http://semver.org/).
 
 
+## [0.6.4] - 2024-10-09
+- fix #67, improve performance **getCumulativePosition()** and **getAngularSpeed()**
+  - kudos to Chris-42
+- add **AS5600_position_speed.ino**
+- update readme.md
+
 ## [0.6.3] - 2024-10-04
 - fix #69, **resetCumulativePosition()**
 - minor edits

README.md

@@ -385,10 +385,17 @@ as.increaseOffset(-30);
 
 ### Angular Speed
 
-- **float getAngularSpeed(uint8_t mode = AS5600_MODE_DEGREES)** 
+- **float getAngularSpeed(uint8_t mode = AS5600_MODE_DEGREES, bool update = true)** 
 is an experimental function that returns 
 an approximation of the angular speed in rotations per second.
 
+If update is false, the function will use the last read value of **readAngle()**.
+This is also used by **getCumulativePosition()** and when used both these
+functions a substantial performance gain is made.
+See example **AS5600_position_speed.ino**.
+
+In case of a reading failure (when update == true), the function can return NAN.
+
 The function needs to be called at least **four** times per rotation
 or once per second to get a reasonably precision. 
 
@@ -433,8 +440,12 @@ The cumulative position (32 bits) consists of 3 parts
 
 Functions are:
 
-- **int32_t getCumulativePosition()** reads sensor and updates cumulative position.  
+- **int32_t getCumulativePosition(bool update = true)** reads sensor and updates cumulative position.  
 Updated in 0.6.2 to follow the setting of the **directionPin**.
+If update is false, the function will use the last read value of **readAngle()**.
+This is also used by **getCumulativePosition()** and when used both these
+functions a substantial performance gain is made.
+See example **AS5600_position_speed.ino**.
 - **int32_t getRevolutions()** converts last position to whole revolutions.
 Convenience function.
 Updated in 0.6.2 to return **zero** for the first negative revolution as this
@@ -456,6 +467,35 @@ int32_t getRevolutions();
 int32_t resetRevolutions();   //  replaces resetPosition();
 ```
 
+### Angular Speed + Cumulative position optimization.
+
+Since 0.6.4 it is possible to optimize the performance of getting both.
+- **getCumulativePosition()**
+- **getAngularSpeed()** 
+
+As both use to read the Angle, one can reuse this by setting the update flag to false.
+One must call **readAngle()** right before both of them
+In code:
+
+```cpp
+    ...
+    alpha = as5600.readAngle();
+    pos   = as5600.getCumulativePosition(false);
+    speed = as5600.getAngularSpeed(AS5600_MODE_DEGREES, false);
+
+    //  process the values...
+```
+
+Please note that the **mode** parameter for getAngularSpeed() becomes mandatory.
+
+The call to **readAngle()** caches the angle read and uses it in both functions.
+The savings are substantial, see **AS5600_position_speed.ino**
+
+The advantage is that both speed and pos are based upon the same reading.
+A disadvantage is that the latter of the two calls is not max up to date.
+
+Use with care.
+
 
 ### Status registers
 

examples/AS5600_position_speed/AS5600_position_speed.ino

@@ -0,0 +1,96 @@
+//
+//    FILE: AS5600_position_speed.ino
+//  AUTHOR: Rob Tillaart
+// PURPOSE: demo compare performance with update flag
+//     URL: https://github.com/RobTillaart/AS5600
+//
+//  Examples may use AS5600 or AS5600L devices.
+//  Check if your sensor matches the one used in the example.
+//  Optionally adjust the code.
+
+
+#include "AS5600.h"
+
+//  select right class.
+//  AS5600 as5600;   //  use default Wire
+AS5600L as5600;   //  use default Wire
+
+uint32_t start, stop;
+
+void setup()
+{
+  Serial.begin(115200);
+  Serial.println(__FILE__);
+  Serial.print("AS5600_LIB_VERSION: ");
+  Serial.println(AS5600_LIB_VERSION);
+
+  Wire.begin();
+
+  as5600.begin(4);  //  set direction pin.
+  as5600.setDirection(AS5600_CLOCK_WISE);  //  default, just be explicit.
+
+  Serial.println(as5600.getAddress());
+
+  //  as5600.setAddress(0x40);  //  AS5600L only
+
+  int b = as5600.isConnected();
+  Serial.print("Connect: ");
+  Serial.println(b);
+
+  for (uint32_t speed = 100000; speed <= 800000;  speed += 100000)
+  {
+    Wire.setClock(speed);
+    Serial.println(speed);
+    delay(100);
+
+    start = micros();
+    as5600.getCumulativePosition();
+    as5600.getAngularSpeed();
+    stop = micros();
+    Serial.print("update true: \t");
+    Serial.println(stop - start);
+    delay(100);
+
+    start = micros();
+    as5600.readAngle();
+    as5600.getCumulativePosition(false);
+    as5600.getAngularSpeed(AS5600_MODE_DEGREES, false);
+    stop = micros();
+    Serial.print("update false: \t");
+    Serial.println(stop - start);
+    Serial.println();
+    delay(100);
+  }
+
+  /*
+     about ~1% slower on AVR @100K
+    start = micros();
+    as5600.getCumulativePosition(true);
+    as5600.getAngularSpeed(AS5600_MODE_DEGREES, false);
+    stop = micros();
+    Serial.print("update false: \t");
+    Serial.println(stop - start);
+    Serial.println();
+    delay(100);
+  */
+
+  delay(2000);
+}
+
+
+void loop()
+{
+  static uint32_t lastTime = 0;
+
+  if (millis() - lastTime >= 100)
+  {
+    lastTime = millis();
+    as5600.readAngle();
+    Serial.print(as5600.getCumulativePosition(false));
+    Serial.print("\t");
+    Serial.println(as5600.getAngularSpeed(AS5600_MODE_DEGREES, false));
+  }
+}
+
+
+//  -- END OF FILE --

examples/AS5600_position_speed/output_0.6.4_UNO.txt

@@ -0,0 +1,42 @@
+
+BOARD:  UNO
+IDE:    1.8.19
+SKETCH: AS5600_position_speed.ino
+
+
+AS5600_LIB_VERSION: 0.6.4
+64
+Connect: 1
+100000
+update true: 	1208
+update false: 	640
+
+200000
+update true: 	724
+update false: 	368
+
+300000
+update true: 	552
+update false: 	316
+
+400000
+update true: 	440
+update false: 	276
+
+500000
+update true: 	432
+update false: 	224
+
+600000
+update true: 	356
+update false: 	236
+
+700000
+update true: 	372
+update false: 	192
+
+800000
+update true: 	356
+update false: 	220
+
+followed by lots of data

library.json

@@ -15,7 +15,7 @@
     "type": "git",
     "url": "https://github.com/RobTillaart/AS5600.git"
   },
-  "version": "0.6.3",
+  "version": "0.6.4",
   "license": "MIT",
   "frameworks": "*",
   "platforms": "*",

library.properties

@@ -1,5 +1,5 @@
 name=AS5600
-version=0.6.3
+version=0.6.4
 author=Rob Tillaart <[email protected]>
 maintainer=Rob Tillaart <[email protected]>
 sentence=Arduino library for AS5600 and AS5600L magnetic rotation meter.