Custom definition of phase difference to AoA mapping

dw3000: add PDoA to AoA lookup tables, refs UWB-1236
mac: use the AoA value computed the driver, refs UWB-1236

Bug: 172565633
Change-Id: I4137fc0d905a5d4cf67b799c6bafc6a7ba2e1829
Signed-off-by: Jean-Michel RIOT <jean-michel.riot@qorvo.com>

1 files changed, 203 insertions, 0 deletions

diff --git a/tools/calibrations/pdoa_lut_generation b/tools/calibrations/pdoa_lut_generation
new file mode 100755
index 0000000..dc850ee
--- /dev/null
+++ b/tools/calibrations/pdoa_lut_generation
@@ -0,0 +1,203 @@
+#!/usr/bin/env python3
+
+import os
+import sys
+import math
+import json
+from optparse import OptionParser
+
+# This defaulr value is sync with:
+# dw3000_calib.c:DW3000_CALIBRATION_PDOA_LUT_MAX
+DW3000_CALIBRATION_PDOA_LUT_MAX = 31
+
+usage="""%s [options]
+
+This script aims to dump a binary blob suitable for uwb-stack calibration file.
+It works in two modes :
+  1. Input JSON LUT
+  2. Antenna spacing and formula
+
+== Input JSON LUT ==
+It takes a JSON input on stdin and dumps the corresponding blob on stdout.
+The JSON input must be a list of either:
+  * lists containing 2 items of type float, 1st is assumed to be PDoA, 2nd AoA,
+  * maps containing 'aoa' and 'pdoa' keys, and corresponding values of type float.
+    float values are assumed to be in radian unit.
+
+== Antenna spacing and formula ==
+This mode uses the theorical math formula to compute AoA value from PDoA measurement.
+It also uses the antenna spacing parameter, please ensure to provide the right value.
+2 sub modes are possible:
+  * PDoA value are provided in list of float that matches the LUT size
+  * --no-json option is specified or empty input is provided, then the tool will
+    split the [-Pi, Pi] range in %d equal parts and use it as PDoA value list
+"""%(
+   os.path.basename(sys.argv[0]),
+   DW3000_CALIBRATION_PDOA_LUT_MAX,
+    )
+
+
+# ==== Fixed point / math utils ====
+
+Q = 11
+K = 2 ** Q
+S16_MAX = ((2 ** 15) - 1)
+S16_MIN = -(2 ** 15)
+
+speed_of_light_m_per_s = 299702547
+
+def freq_hz(chan):
+    if chan == 5:
+        return 6.5e9
+    elif chan == 9:
+        return 7.9872e9
+    else:
+        raise ValueError("Channel must be either 5 or 9")
+
+def sat_uf(x):
+    return min(S16_MAX, max(S16_MIN, x))
+
+def float_to_q11(x):
+    return sat_uf(round(x * K))
+
+pi_q = float_to_q11(math.pi)
+
+def pdoa_to_aoa_rad(x, chan, antenna_dist_m):
+    L_M = speed_of_light_m_per_s / freq_hz(chan)
+    phase_m = x * L_M / (2.0 * math.pi)
+    rad = phase_m / antenna_dist_m
+    rad = min(1.0, max(-1.0, rad))
+    return math.asin(rad)
+
+# ==================================
+
+def blob_item(pdoa, aoa, fmt, byteorder):
+    pdoa_q11 = float_to_q11(pdoa)
+    aoa_q11 = float_to_q11(aoa)
+    pdoa_bytes = pdoa_q11.to_bytes(2, byteorder=byteorder, signed=True)
+    aoa_bytes = aoa_q11.to_bytes(2, byteorder=byteorder, signed=True)
+    return fmt%(pdoa_bytes[0], pdoa_bytes[1], aoa_bytes[0], aoa_bytes[1])
+
+def input_json_lut(pdoa_lut, options):
+    print("Generating LUT using 'input JSON LUT' mode...")
+    print("LUT = %s"%str(pdoa_lut))
+    if not isinstance(pdoa_lut, list) \
+      or (options.size_check and (len(pdoa_lut) != options.lut_size)):
+        raise ValueError("input JSON must be list of size %d"
+                          %(str(options.lut_size)))
+
+    blob = ""
+    first_val = True
+    internal_pdoa_lut = []
+    for item in pdoa_lut:
+        if isinstance(item, list) and len(item) == 2:
+            pdoa = item[0]
+            aoa = item[1]
+        elif isinstance(item, dict):
+            # NB: exception raised if key not provided in input data
+            pdoa = item['pdoa']
+            aoa = item['aoa']
+        else:
+            raise ValueError("input JSON list item must of type list \
+[float, float] (PDoA then AoA) or map {'aoa':float, 'pdoa':float}")
+        internal_pdoa_lut.append((pdoa, aoa))
+
+    internal_pdoa_lut.sort(key=lambda x: x[0])
+
+    for item in internal_pdoa_lut:
+        pdoa = item[0]
+        aoa = item[1]
+        if first_val:
+            first_val = False
+        else:
+            blob += options.sep
+        blob += blob_item(pdoa, aoa, options.fmt, options.byteorder)
+
+    return blob
+
+def antenna_spacing_and_formula(pdoa_values, options):
+    print("Generating LUT using 'antenna spacing and formula' mode...")
+    print("PDoA values = %s"%(str(pdoa_values)))
+    print("Antenna spacing (mm) = %f"%(options.antenna_spacing / 1000.0))
+    if not isinstance(pdoa_values, list) \
+      or (options.size_check and (len(pdoa_values) != options.lut_size)):
+        raise ValueError("input JSON must be list of size %d"
+                          %(options.lut_size))
+    blob = ""
+    first_val = True
+
+    for value in pdoa_values:
+        pdoa = float(value) # would raise TypeError
+        aoa = pdoa_to_aoa_rad(pdoa, options.channel,
+                              options.antenna_spacing / 1000000.0)
+        print("\t%f\t=>\t%f"%(pdoa, aoa))
+        if first_val:
+            first_val = False
+        else:
+            blob += options.sep
+        blob += blob_item(pdoa, aoa, options.fmt, options.byteorder)
+    return blob
+
+def float_range(start, stop, step):
+    while start < stop:
+        yield float(start)
+        start += step
+
+if __name__ == "__main__":
+    parser = OptionParser(usage=usage)
+    parser.add_option("--channel", dest="channel", type="int", default=5,
+                      help="UWB Channel (5 or 9, default is 5)")
+    parser.add_option("--formula-mode", dest="formula_mode",
+                      action="store_true", default=False,
+                      help="selects formula mode")
+    parser.add_option("--antenna-spacing", dest="antenna_spacing",
+                      type="int", default=20800,
+                      help="sets the antenna pair spacing used in formula \
+                            mode, unit is micrometers (default value 20800 \
+                            matches Mona Lisa design).")
+    parser.add_option("--byteorder", dest="byteorder",
+                      type="string", default=sys.byteorder,
+                      help="Override byte order. Default is the system's \
+                            byteorder (%s)"%(sys.byteorder))
+    parser.add_option("--lut-size", dest="lut_size",
+                      type="int", default=DW3000_CALIBRATION_PDOA_LUT_MAX,
+                      help="Override default lut size (%d)"%(DW3000_CALIBRATION_PDOA_LUT_MAX))
+    parser.add_option("--no-size-check", dest="size_check",
+                      action="store_false", default=True,
+                      help="skips length of the input map.")
+    parser.add_option("--dump-c-array", dest="dump_c_array",
+                      action="store_true", default=False,
+                      help="Dumps a C array instead of calib blob")
+    parser.add_option("--no-input", dest="read_stdin",
+                      action="store_false", default=True,
+                      help="Skips reading input on stdin")
+    (options, args) = parser.parse_args()
+
+    if options.dump_c_array:
+        options.sep = ",\n"
+        options.fmt = "\t{ 0x%02x%02x, 0x%02x%02x }"
+        options.byteorder = 'big'
+    else:
+        options.sep = ":"
+        options.fmt = "%02x:%02x:%02x:%02x"
+
+    txt_input = ""
+    if options.read_stdin:
+        for line in sys.stdin:
+            txt_input += line
+
+    if options.formula_mode:
+        if txt_input:
+            pdoa_values = json.loads(txt_input)
+        else:
+            step = 2 * math.pi / (options.lut_size-1)
+            pdoa_values = list(float_range(-math.pi, math.pi, step))
+        blob = antenna_spacing_and_formula(pdoa_values, options)
+    else:
+        pdoa_lut = json.loads(txt_input)
+        blob = input_json_lut(pdoa_lut, options)
+
+    if options.dump_c_array:
+        print("const dw3000_pdoa_lut_t pdoa_lut = {\n" + blob + "\n};")
+    else:
+        print("antX.antY.chN.pdoa_lut " + blob)