diff --git a/src/pgaSensor.cpp b/src/pgaSensor.cpp index 81bce5f..4e1214a 100644 --- a/src/pgaSensor.cpp +++ b/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); diff --git a/src/pgaSensor.h b/src/pgaSensor.h index 09ff14f..3a51ed8 100644 --- a/src/pgaSensor.h +++ b/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); diff --git a/tools/plot_pga_dump.py b/tools/plot_pga_dump.py new file mode 100644 index 0000000..07f22bf --- /dev/null +++ b/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()