Diff: FIZ_readers/FIZReader.cpp

Revision:

85:0cc5931bb9ef

Parent:

80:0b7f1b85b626

--- a/FIZ_readers/FIZReader.cpp	Thu Dec 15 05:53:28 2022 +0000
+++ b/FIZ_readers/FIZReader.cpp	Thu Dec 15 06:05:30 2022 +0000
@@ -1,6 +1,11 @@
 #include "FIZReader.h"
 #include "LTCApp.h"
+#include "cc.h"
 #include "mbed.h"
+#include <cstdint>
+#include <vector>
+#include <algorithm>
+
 FIZReader::FIZReader(const PinName Tx, const PinName Rx) : _port(Tx,Rx)
 {
     _focus = 0;
@@ -19,12 +24,72 @@
     return wasNew;
 }
 
-    int FIZReader::hexValue(char ascii) {
-        if (((ascii-'0') >= 0) && ((ascii-'0') <= 9))
-            return ascii-'0';
-        if (((ascii-'A') >= 0) && ((ascii-'A') <= 5))
-            return ascii-'A'+10;
-        if (((ascii-'a') >= 0) && ((ascii-'a') <= 5))
-            return ascii-'a'+10;
-        return 0;
-    }   
\ No newline at end of file
+int FIZReader::hexValue(char ascii) {
+    if (((ascii-'0') >= 0) && ((ascii-'0') <= 9))
+        return ascii-'0';
+    if (((ascii-'A') >= 0) && ((ascii-'A') <= 5))
+        return ascii-'A'+10;
+    if (((ascii-'a') >= 0) && ((ascii-'a') <= 5))
+        return ascii-'a'+10;
+    return 0;
+}
+
+unsigned int FIZReader::getAbsoluteValue(unsigned int encoder_pos, vector<unsigned int> &encoder_range, vector<unsigned int> &absolute_range) {
+    unsigned int result;
+    if( encoder_pos <= encoder_range.front() )   return absolute_range.front();
+    if( encoder_pos >= encoder_range.back())  return absolute_range.back();
+
+    int lower = encoder_range.size() -2;
+    int upper = encoder_range.size() -1;
+    for (int pos_pointer = 0; pos_pointer < encoder_range.size(); pos_pointer++) {
+        if (encoder_pos < encoder_range[pos_pointer]) {
+            lower = pos_pointer - 1;
+            upper = pos_pointer;
+            break;
+        }
+    }
+
+    // pos_upper = encoder_range[upper];
+    // pos_lower = encoder_range[lower];
+    // pos_value = encoder_pos;
+
+    result = absolute_range[lower]+(encoder_pos-encoder_range[lower])*(absolute_range[upper]-absolute_range[lower])/(encoder_range[upper]-encoder_range[lower]);
+
+    return result;
+
+}
+
+    // vector<int>::iterator lb = lower_bound(encoder_range.begin(), encoder_range.end(), encoder_pos);
+    // auto upper = lb++;
+    
+    // if (lb == encoder_range.end())
+    //     --upper;  // extrapolating above
+    // else if (*lb == encoder_pos)
+    //     return absolute_range[*lb];
+
+    // int lower = upper? upper - 1: 1; // if upper is not zero, get value below, else flip and get the one above
+
+    // if(encoder_range[upper]<encoder_range[lower]) 
+    //     result = absolute_range[upper]+(encoder_pos-encoder_range[upper])*(absolute_range[lower]-absolute_range[upper])/(encoder_range[lower]-encoder_range[upper]);
+    // else 
+uint32_t FIZReader::getAbsoluteFocus(uint32_t encoder_pos) {
+    if (UserSettings.focus_encoder_map.size())
+        return (uint32_t) getAbsoluteValue(encoder_pos, UserSettings.focus_encoder_map, UserSettings.focus_absolute_map);
+    else 
+        return encoder_pos;
+}
+uint16_t FIZReader::getAbsoluteIris(uint16_t encoder_pos) {
+    if (UserSettings.iris_encoder_map.size())
+        return (uint16_t) getAbsoluteValue(encoder_pos, UserSettings.iris_encoder_map, UserSettings.iris_absolute_map);
+    else 
+        return encoder_pos;
+}
+uint16_t FIZReader::getAbsoluteZoom(uint16_t encoder_pos) {
+    if (UserSettings.zoom_encoder_map.size()){
+        return (uint16_t) getAbsoluteValue(encoder_pos, UserSettings.zoom_encoder_map, UserSettings.zoom_absolute_map);
+    }
+    else 
+        return encoder_pos;
+}
+
+