Add TVGAIN support and fine-tune the gain/threshold values (#390)

* Add TVGAIN support and fine-tune the gain/threshold values
* Add tool to debug the gain, timing and threshold values for the PGA460 sensor

Applied this one to the community pro before sending it out

Author: FabianSchwartau

src/pgaSensor.cpp

@@ -153,25 +153,44 @@ void PGASensorManager::setupSensor(int sensorId)
   spiRegWrite(sensorId, PGA_REG_CURR_LIM_P1, PGA_DIS_CL(0) | PGA_CURR_LIM1(0));  // CURR_LIM1*7mA + 50mA
   spiRegWrite(sensorId, PGA_REG_CURR_LIM_P2, PGA_LPF_CO(0) | PGA_CURR_LIM2(0));  // CURR_LIM1*7mA + 50mA
   spiRegWrite(sensorId, PGA_REG_REC_LENGTH, PGA_P1_REC(8) | PGA_P2_REC(0));  // Record time = 4.096 × (Px_REC + 1) [ms], 8 = 36,9ms = 6m range
-  spiRegWrite(sensorId, PGA_REG_DECPL_TEMP, PGA_AFE_GAIN_RNG(1) | PGA_LPM_EN(0) | PGA_DECPL_TEMP_SEL(0) | PGA_DECPL_T(0));  // Time = 4096 × (DECPL_T + 1) [μs], 0 = 4ms = 0,66m?!?
+  spiRegWrite(sensorId, PGA_REG_DECPL_TEMP, PGA_AFE_GAIN_RNG(3) | PGA_LPM_EN(0) | PGA_DECPL_TEMP_SEL(0) | PGA_DECPL_T(0));  // Time = 4096 × (DECPL_T + 1) [μs], 0 = 4ms = 0,66m?!?
   spiRegWrite(sensorId, PGA_REG_EE_CNTRL, PGA_DATADUMP_EN(0));  // Disable data dump
 
+  // Gain map
+  // Gain = 0.5 × (TVGAIN+1) + value(AFE_GAIN_RNG) [dB]
+  // TVGAIN is 0..63, time is any TH_TIME_DELTA_*
+  // The gain map is applied for both profiles.
+  // Times are given in deltas to previous time, except the first one
+  // So there is a constant gain of g0 from 0 to t0, then a linear interpolation from g0 to g1 between t0 and t1,
+  // then another linear interpolation from g1 to g2 between t1 and t2, and so on. After t6, the gain is constant at g6.
+  // I think the INIT_GAIN value is ignored, if TVGAIN is used, but AFE_GAIN_RNG still applies.
+  PGATVGain gains(
+    TH_TIME_DELTA_2000US, 0,
+    TH_TIME_DELTA_5200US, 40,
+    TH_TIME_DELTA_5200US, 45,
+    TH_TIME_DELTA_5200US, 50,
+    TH_TIME_DELTA_5200US, 55,
+    TH_TIME_DELTA_5200US, 60
+  );
+  spiRegWriteGains(sensorId, gains);
+
   // Threshold map, then check DEV_STAT0.THR_CRC_ERR
-  // th_t = np.array([2000, 5200, 2400, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000])
-  // th_l = np.array([200, 80, 16, 16, 16, 24, 24, 24, 24, 24, 24, 24])
+  // The first 8 values are only 5 bit, so the 0..255 value has to be devided by 8.
+  // th_t = np.array([1000, 2000, 2400, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000])
+  // th_l = np.array([255, 80, 30, 30, 30, 25, 25, 20, 20, 20, 20, 20])
   PGAThresholds thresholds(
-    TH_TIME_DELTA_2000US, 255/8,
-    TH_TIME_DELTA_4000US, 80/8,
-    TH_TIME_DELTA_2400US, 24/8,
-    TH_TIME_DELTA_8000US, 24/8,
-    TH_TIME_DELTA_8000US, 24/8,
-    TH_TIME_DELTA_8000US, 32/8,
-    TH_TIME_DELTA_8000US, 32/8,
-    TH_TIME_DELTA_8000US, 32/8,
-    TH_TIME_DELTA_8000US, 32,
-    TH_TIME_DELTA_8000US, 32,
-    TH_TIME_DELTA_8000US, 32,
-    TH_TIME_DELTA_8000US, 32
+    TH_TIME_DELTA_1000US, 240/8,
+    TH_TIME_DELTA_2000US, 80/8,
+    TH_TIME_DELTA_2400US, 30/8,
+    TH_TIME_DELTA_8000US, 30/8,
+    TH_TIME_DELTA_8000US, 30/8,
+    TH_TIME_DELTA_8000US, 25/8,
+    TH_TIME_DELTA_8000US, 25/8,
+    TH_TIME_DELTA_8000US, 20/8,
+    TH_TIME_DELTA_8000US, 20,
+    TH_TIME_DELTA_8000US, 20,
+    TH_TIME_DELTA_8000US, 20,
+    TH_TIME_DELTA_8000US, 20
   );
   spiRegWriteThesholds(sensorId, 1, thresholds);
   spiRegWriteThesholds(sensorId, 2, thresholds);
@@ -267,6 +286,20 @@ void PGASensorManager::spiRegWrite(uint8_t sensorId, uint8_t reg_addr, uint8_t v
     safe_usleep(5);
 }
 
+void PGASensorManager::spiRegWriteGains(uint8_t sensorId, PGATVGain &gains)
+{
+  assert(sensorId >= 0 && sensorId <= 1);
+
+  // The order of the time/gain values is a bit messy, so we have to do some bit shifting to write them to the registers
+  spiRegWrite(sensorId, PGA_REG_TVGAIN0, gains.t0<<4 | gains.t1);
+  spiRegWrite(sensorId, PGA_REG_TVGAIN1, gains.t2<<4 | gains.t3);
+  spiRegWrite(sensorId, PGA_REG_TVGAIN2, gains.t4<<4 | gains.t5);
+  spiRegWrite(sensorId, PGA_REG_TVGAIN3, gains.g1<<2 | gains.g2>>4);
+  spiRegWrite(sensorId, PGA_REG_TVGAIN4, gains.g2<<4 | gains.g3>>2);
+  spiRegWrite(sensorId, PGA_REG_TVGAIN5, gains.g3<<6 | gains.g4);
+  spiRegWrite(sensorId, PGA_REG_TVGAIN6, gains.g5<<2);
+}
+
 void PGASensorManager::spiRegWriteThesholds(uint8_t sensorId, uint8_t preset, PGAThresholds &thresholds)
 {
   assert(sensorId >= 0 && sensorId <= 1);

src/pgaSensor.h

@@ -260,6 +260,44 @@ struct PGAThresholds
   uint8_t l12;  // 8(!) bits
 };
 
+struct PGATVGain
+{
+  PGATVGain(
+    uint8_t t0 = TH_TIME_DELTA_100US, uint8_t g0 = 0,
+    uint8_t t1 = TH_TIME_DELTA_100US, uint8_t g1 = 0,
+    uint8_t t2 = TH_TIME_DELTA_100US, uint8_t g2 = 0,
+    uint8_t t3 = TH_TIME_DELTA_100US, uint8_t g3 = 0,
+    uint8_t t4 = TH_TIME_DELTA_100US, uint8_t g4 = 0,
+    uint8_t t5 = TH_TIME_DELTA_100US, uint8_t g5 = 0
+  )
+  {
+    this->t0 = t0 & 0x0f;
+    this->g0 = g0 & 0x3f;
+    this->t1 = t1 & 0x0f;
+    this->g1 = g1 & 0x3f;
+    this->t2 = t2 & 0x0f;
+    this->g2 = g2 & 0x3f;
+    this->t3 = t3 & 0x0f;
+    this->g3 = g3 & 0x3f;
+    this->t4 = t4 & 0x0f;
+    this->g4 = g4 & 0x3f;
+    this->t5 = t5 & 0x0f;
+    this->g5 = g5 & 0x3f;
+  }
+  uint8_t t0;
+  uint8_t g0;
+  uint8_t t1;
+  uint8_t g1;
+  uint8_t t2;
+  uint8_t g2;
+  uint8_t t3;
+  uint8_t g3;
+  uint8_t t4;
+  uint8_t g4;
+  uint8_t t5;
+  uint8_t g5;
+};
+
 class PGASensorManager
 {
 public:
@@ -302,6 +340,7 @@ class PGASensorManager
   uint8_t spiTransfer(uint8_t sensorId, uint8_t data_out);
   int spiRegRead(uint8_t sensorId, uint8_t reg_addr, uint8_t *pdiag = nullptr);
   void spiRegWrite(uint8_t sensorId, uint8_t reg_addr, uint8_t value);
+  void spiRegWriteGains(uint8_t sensorId, PGATVGain &gains);
   void spiRegWriteThesholds(uint8_t sensorId, uint8_t preset, PGAThresholds &thresholds);
   void spiBurstAndListen(uint8_t sensorId, uint8_t preset, uint8_t numberOfObjectsToDetect);
   bool spiUSResult(uint8_t sensorId, uint8_t numberOfObjectsToDetect, PGAResult *usResults);

tools/plot_pga_dump.py

@@ -0,0 +1,154 @@
+# This is a small script to debug the OBSPro ultrasonic sensors (based on the PGA460 chip).
+# Its focus is on optimizing the time-variable-gain (TVGAIN), threshold values and timings.
+# To use it you need to rebuild the firmware with the PGA_DUMP_ENABLE define set to 1.
+# You can find this value in the src/pgaSensor.h file.
+# Once enabled, the pgaSensor.cpp will print out the raw PGA values for each sensor in a
+# comma-separated format over the USB-serial line. The output will look like this:
+# dump,0,123,45,67,89,23,45,67,...
+# dump,1,234,56,78,90,34,56,78,...
+# meas,0,1234,345,56
+# meas,1,2345,678,67
+# First value after dump/meas is always the sensor ID (0 or 1).
+# The "dump" line contains the raw PGA values over time (128 values).
+# The "meas" line contains: time-of-flight [µs], peak width [µs], peak amplitude
+# Note that the measurement will probably never exactly match the dump values, because the
+# dump and measurement lines are taken from different measurements. Also the measurement
+# value is updated much more frequently than the dump values.
+# Requirements and setup:
+#   - python3 -m venv .venv
+#   - source .venv/bin/activate
+#   - pip install pyserial matplotlib numpy
+
+import time
+import matplotlib.pyplot as plt
+import numpy as np
+import serial
+
+PORT = "/dev/ttyUSB0"
+BAUD = 115200
+SAMPLE_US = 288.0
+
+
+def parse_line(line):
+    parts = [p.strip() for p in line.split(",")]
+    if len(parts) < 2:
+        return None
+    if parts[0] not in {"dump", "meas"}:
+        return None
+    try:
+        sensor_id = int(parts[1])
+    except ValueError:
+        return None
+    if sensor_id not in (0, 1):
+        return None
+    if parts[0] == "dump":
+        try:
+            return "dump", sensor_id, [float(v) for v in parts[2:]]
+        except ValueError:
+            return None
+    if len(parts) < 5:
+        return None
+    try:
+        return "meas", sensor_id, float(parts[2]), float(parts[4])
+    except ValueError:
+        return None
+
+
+def open_serial():
+    return serial.Serial(
+        port=PORT,
+        baudrate=BAUD,
+        bytesize=serial.EIGHTBITS,
+        parity=serial.PARITY_NONE,
+        stopbits=serial.STOPBITS_ONE,
+        timeout=0.0,
+        xonxoff=False,
+        rtscts=False,
+        dsrdtr=False,
+    )
+
+
+def update_plot(ax, sensor_id, state):
+    dump_values = state["dump"][sensor_id]
+    meas = state["meas"][sensor_id]
+    ax.clear()
+    if dump_values:
+        x = np.arange(len(dump_values), dtype=float) * SAMPLE_US
+        ax.plot(x, dump_values, label="dump")
+    if meas:
+        ax.plot([meas[0]], [meas[1]], "ro", label="meas")
+
+    # Plot thresholds
+    # These thresholds are defined in the firmware, so copy them from there!
+    th_t = np.array([1000, 1400, 2400, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000, 8000])
+    th_l = np.array([240, 50, 30, 30, 30, 25, 25, 20, 20, 20, 20, 20])
+    last_t = 0
+    last_l = th_l[0]
+    for i in range(12):
+        ax.plot([last_t, last_t + th_t[i]], [last_l, th_l[i]], "k-", label="threshold" if i == 0 else "")
+        last_t += th_t[i]
+        last_l = th_l[i]
+
+    # ax.set_xlim(0, 300)
+    ax.set_ylim(0, 256)
+    ax.set_title(f"Sensor {sensor_id}")
+    ax.set_xlabel("Time (us)")
+    ax.set_ylabel("Amplitude")
+    ax.grid(True)
+    ax.legend(loc="upper right")
+
+
+def main():
+    plt.ion()
+    fig, axes = plt.subplots(2, 1, figsize=(10, 8), constrained_layout=True)
+
+    state = {
+        "dump": {0: None, 1: None},
+        "meas": {0: None, 1: None},
+    }
+
+    ser = open_serial()
+    print(f"Connected to {PORT} @ {BAUD}")
+    rx_buffer = bytearray()
+
+    while plt.fignum_exists(fig.number):
+        changed = False
+
+        bytes_waiting = ser.in_waiting
+        if bytes_waiting > 0:
+            rx_buffer.extend(ser.read(bytes_waiting))
+
+        while True:
+            newline_index = rx_buffer.find(b"\n")
+            if newline_index < 0:
+                break
+            raw_line = bytes(rx_buffer[:newline_index])
+            del rx_buffer[: newline_index + 1]
+            line = raw_line.decode("ascii", errors="ignore").strip()
+            if not line:
+                continue
+            parsed = parse_line(line)
+            if parsed is not None:
+                if parsed[0] == "dump":
+                    state["dump"][parsed[1]] = parsed[2]
+                    changed = True
+                else:
+                    state["meas"][parsed[1]] = (parsed[2], parsed[3])
+                    changed = True
+
+        if changed:
+            for sensor_id, ax in enumerate(axes):
+                update_plot(ax, sensor_id, state)
+            fig.canvas.draw_idle()
+
+        plt.pause(0.001)
+
+    if ser is not None:
+        try:
+            ser.close()
+        except Exception:
+            pass
+
+
+if __name__ == "__main__":
+    main()