Shane Smiskol
3 years ago
commit
6d32dfc72a
47 changed files with 737 additions and 254 deletions
@ -1 +1 @@ |
||||
Subproject commit 5935572cee86f202e2524d5f388f9475f92cd649 |
||||
Subproject commit c7d3a0acbae267ef93d30044e1941e060dac9e48 |
||||
@ -1 +1 @@ |
||||
Subproject commit 226adc655e1488474468a97ab4a7705aad7e5837 |
||||
Subproject commit be1a213a5ffa3cafe2b4f2d53f6df5d2452ad910 |
||||
@ -1 +1 @@ |
||||
Subproject commit e19ba095c3ee288d629284e24ec7e0deaf645f3f |
||||
Subproject commit 919154efe2bd07c4dd124d7e6a11a4afc8685f9d |
||||
@ -0,0 +1,5 @@ |
||||
#!/usr/bin/bash |
||||
|
||||
nmcli connection modify --temporary lte ipv4.route-metric 1 |
||||
nmcli connection modify --temporary lte ipv6.route-metric 1 |
||||
nmcli con up lte |
||||
Binary file not shown.
@ -1,2 +0,0 @@ |
||||
#!/usr/bin/bash |
||||
echo "restart" > /sys/kernel/debug/msm_subsys/modem |
||||
@ -1,16 +0,0 @@ |
||||
#!/data/data/com.termux/files/usr/bin/bash |
||||
watch -n1 ' |
||||
cat /sys/kernel/debug/clk/pwrcl_clk/measure |
||||
cat /sys/kernel/debug/clk/perfcl_clk/measure |
||||
cat /sys/devices/system/cpu/cpu*/cpufreq/scaling_cur_freq |
||||
cat /sys/class/kgsl/kgsl-3d0/gpuclk |
||||
echo |
||||
echo -n "CPU0 " ; cat /sys/devices/virtual/thermal/thermal_zone5/temp |
||||
echo -n "CPU1 " ; cat /sys/devices/virtual/thermal/thermal_zone7/temp |
||||
echo -n "CPU2 " ; cat /sys/devices/virtual/thermal/thermal_zone10/temp |
||||
echo -n "CPU3 " ; cat /sys/devices/virtual/thermal/thermal_zone12/temp |
||||
echo -n "MEM " ; cat /sys/devices/virtual/thermal/thermal_zone2/temp |
||||
echo -n "GPU " ; cat /sys/devices/virtual/thermal/thermal_zone16/temp |
||||
echo -n "BAT " ; cat /sys/devices/virtual/thermal/thermal_zone29/temp |
||||
' |
||||
|
||||
Binary file not shown.
@ -1,19 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
# flake8: noqa |
||||
|
||||
# put 2 fingeprints and print the diffs |
||||
f1 = { |
||||
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 520: 8, 528: 8, 532: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8, 1537: 8, 1538: 8, 1562: 8 |
||||
} |
||||
|
||||
f2 = { |
||||
168: 8, 257: 5, 258: 8, 264: 8, 268: 8, 270: 8, 274: 2, 280: 8, 284: 8, 288: 7, 290: 6, 291: 8, 292: 8, 294: 8, 300: 8, 308: 8, 320: 8, 324: 8, 331: 8, 332: 8, 344: 8, 368: 8, 376: 3, 384: 8, 388: 4, 448: 6, 456: 4, 464: 8, 469: 8, 480: 8, 500: 8, 501: 8, 512: 8, 514: 8, 515: 7, 516: 7, 517: 7, 518: 7, 520: 8, 528: 8, 532: 8, 542: 8, 544: 8, 557: 8, 559: 8, 560: 8, 564: 8, 571: 3, 579: 8, 584: 8, 608: 8, 624: 8, 625: 8, 632: 8, 639: 8, 653: 8, 654: 8, 655: 8, 658: 6, 660: 8, 669: 3, 671: 8, 672: 8, 678: 8, 680: 8, 701: 8, 703: 8, 704: 8, 705: 8, 706: 8, 709: 8, 710: 8, 719: 8, 720: 6, 729: 5, 736: 8, 737: 8, 746: 5, 752: 2, 754: 8, 760: 8, 764: 8, 766: 8, 770: 8, 773: 8, 779: 8, 782: 8, 784: 8, 792: 8, 799: 8, 800: 8, 804: 8, 816: 8, 817: 8, 820: 8, 825: 2, 826: 8, 832: 8, 838: 2, 848: 8, 853: 8, 856: 4, 860: 6, 863: 8, 878: 8, 882: 8, 897: 8, 906: 8, 908: 8, 924: 8, 926: 3, 929: 8, 937: 8, 938: 8, 939: 8, 940: 8, 941: 8, 942: 8, 943: 8, 947: 8, 948: 8, 958: 8, 959: 8, 962: 8, 969: 4, 973: 8, 974: 5, 979: 8, 980: 8, 981: 8, 982: 8, 983: 8, 984: 8, 992: 8, 993: 7, 995: 8, 996: 8, 1000: 8, 1001: 8, 1002: 8, 1003: 8, 1008: 8, 1009: 8, 1010: 8, 1011: 8, 1012: 8, 1013: 8, 1014: 8, 1015: 8, 1024: 8, 1025: 8, 1026: 8, 1031: 8, 1033: 8, 1050: 8, 1059: 8, 1082: 8, 1083: 8, 1098: 8, 1100: 8 |
||||
} |
||||
|
||||
for k in f1: |
||||
if k not in f2 or f1[k] != f2[k]: |
||||
print(k, "not in f2") |
||||
|
||||
for k in f2: |
||||
if k not in f1 or f2[k] != f1[k]: |
||||
print(k, "not in f1") |
||||
@ -1,23 +0,0 @@ |
||||
#!/usr/bin/env python3 |
||||
import os |
||||
import sys |
||||
import time |
||||
|
||||
print("starting at") |
||||
os.system("cat /sys/kernel/debug/regulator/pm8994_s11/voltage") |
||||
print("volts") |
||||
|
||||
os.system("echo 99e8000.cpr3-ctrl > /sys/devices/soc/spm-regulator-10/regulator/regulator.56/99e8000.cpr3-ctrl-vdd/driver/unbind") |
||||
os.system("echo 1 > /sys/kernel/debug/regulator/pm8994_s11/enable") |
||||
|
||||
if len(sys.argv) > 1: |
||||
i = int(sys.argv[1]) |
||||
os.system("echo %d %d > /sys/kernel/debug/regulator/pm8994_s11/voltage" % (i,i)) |
||||
os.system("cat /sys/kernel/debug/regulator/pm8994_s11/voltage") |
||||
else: |
||||
for i in range(900000, 465000, -10000): |
||||
print("setting voltage to",i) |
||||
os.system("echo %d %d > /sys/kernel/debug/regulator/pm8994_s11/voltage" % (i,i)) |
||||
os.system("cat /sys/kernel/debug/regulator/pm8994_s11/voltage") |
||||
time.sleep(1) |
||||
|
||||
@ -1,67 +0,0 @@ |
||||
#!/usr/bin/python3 |
||||
import sys |
||||
import os |
||||
import stat |
||||
import subprocess |
||||
import json |
||||
from common.text_window import TextWindow |
||||
import time |
||||
|
||||
# Required for sensord not to bus-error on startup |
||||
# commaai/cereal#22 |
||||
try: |
||||
os.mkdir("/dev/shm") |
||||
except FileExistsError: |
||||
pass |
||||
except PermissionError: |
||||
print("WARNING: failed to make /dev/shm") |
||||
|
||||
try: |
||||
with open('/tmp/sensor-test-results.json') as infile: |
||||
data = json.load(infile) |
||||
except Exception: |
||||
data = {'sensor-pass': 0, 'sensor-fail': 0} |
||||
|
||||
STARTUP_SCRIPT = "/data/data/com.termux/files/continue.sh" |
||||
try: |
||||
with open(STARTUP_SCRIPT, 'w') as startup_script: |
||||
startup_script.write("#!/usr/bin/bash\n\n/data/openpilot/selfdrive/debug/internal/sensor_test_bootloop.py\n") |
||||
os.chmod(STARTUP_SCRIPT, stat.S_IRWXU) |
||||
except Exception: |
||||
print("Failed to install new startup script -- aborting") |
||||
sys.exit(-1) |
||||
|
||||
sensord_env = {**os.environ, 'SENSOR_TEST': '1'} |
||||
process = subprocess.run("./sensord", cwd="/data/openpilot/selfdrive/sensord", env=sensord_env) # pylint: disable=subprocess-run-check |
||||
|
||||
if process.returncode == 40: |
||||
text = "Current run: SUCCESS\n" |
||||
data['sensor-pass'] += 1 |
||||
else: |
||||
text = "Current run: FAIL\n" |
||||
data['sensor-fail'] += 1 |
||||
|
||||
timestr = str(int(time.time())) |
||||
with open('/tmp/dmesg-' + timestr + '.log', 'w') as dmesg_out: |
||||
subprocess.call('dmesg', stdout=dmesg_out, shell=False) |
||||
with open("/tmp/logcat-" + timestr + '.log', 'w') as logcat_out: |
||||
subprocess.call(['logcat', '-d'], stdout=logcat_out, shell=False) |
||||
|
||||
text += "Sensor pass history: " + str(data['sensor-pass']) + "\n" |
||||
text += "Sensor fail history: " + str(data['sensor-fail']) + "\n" |
||||
|
||||
print(text) |
||||
|
||||
with open('/tmp/sensor-test-results.json', 'w') as outfile: |
||||
json.dump(data, outfile, indent=4) |
||||
|
||||
with TextWindow(text) as status: |
||||
for _ in range(100): |
||||
if status.get_status() == 1: |
||||
with open(STARTUP_SCRIPT, 'w') as startup_script: |
||||
startup_script.write("#!/usr/bin/bash\n\ncd /data/openpilot\nexec ./launch_openpilot.sh\n") |
||||
os.chmod(STARTUP_SCRIPT, stat.S_IRWXU) |
||||
break |
||||
time.sleep(0.1) |
||||
|
||||
subprocess.Popen("reboot") |
||||
@ -0,0 +1,13 @@ |
||||
#!/usr/bin/bash |
||||
|
||||
nmcli connection modify --temporary lte gsm.auto-config yes |
||||
nmcli connection modify --temporary lte gsm.home-only yes |
||||
nmcli connection modify --temporary lte connection.autoconnect-retries 20 |
||||
|
||||
# restart modem |
||||
sudo systemctl stop ModemManager |
||||
/usr/comma/lte/lte.sh stop_blocking |
||||
sudo systemctl restart lte |
||||
|
||||
#sudo systemctl restart ModemManager |
||||
sudo ModemManager --debug |
||||
@ -0,0 +1,80 @@ |
||||
#!/usr/bin/env python3 |
||||
from typing import List |
||||
|
||||
from cereal import log, messaging |
||||
from laika import AstroDog |
||||
from laika.helpers import ConstellationId |
||||
from laika.raw_gnss import GNSSMeasurement, calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox |
||||
|
||||
|
||||
def correct_and_pos_fix(processed_measurements: List[GNSSMeasurement], dog: AstroDog): |
||||
# pos fix needs more than 5 processed_measurements |
||||
pos_fix = calc_pos_fix(processed_measurements) |
||||
|
||||
if len(pos_fix) == 0: |
||||
return [], [] |
||||
est_pos = pos_fix[0][:3] |
||||
corrected = correct_measurements(processed_measurements, est_pos, dog) |
||||
return calc_pos_fix(corrected), corrected |
||||
|
||||
|
||||
def process_ublox_msg(ublox_msg, dog, ublox_mono_time: int): |
||||
if ublox_msg.which == 'measurementReport': |
||||
report = ublox_msg.measurementReport |
||||
if len(report.measurements) == 0: |
||||
return None |
||||
new_meas = read_raw_ublox(report) |
||||
processed_measurements = process_measurements(new_meas, dog) |
||||
|
||||
corrected = correct_and_pos_fix(processed_measurements, dog) |
||||
pos_fix, _ = corrected |
||||
# todo send corrected messages instead of processed_measurements. Need fix for when having less than 6 measurements |
||||
correct_meas_msgs = [create_measurement_msg(m) for m in processed_measurements] |
||||
# pos fix can be an empty list if not enough correct measurements are available |
||||
if len(pos_fix) > 0: |
||||
corrected_pos = pos_fix[0][:3].tolist() |
||||
else: |
||||
corrected_pos = [0., 0., 0.] |
||||
dat = messaging.new_message('gnssMeasurements') |
||||
dat.gnssMeasurements = { |
||||
"position": corrected_pos, |
||||
"ubloxMonoTime": ublox_mono_time, |
||||
"correctedMeasurements": correct_meas_msgs |
||||
} |
||||
return dat |
||||
|
||||
|
||||
def create_measurement_msg(meas: GNSSMeasurement): |
||||
c = log.GnssMeasurements.CorrectedMeasurement.new_message() |
||||
c.constellationId = meas.constellation_id.value |
||||
c.svId = int(meas.prn[1:]) |
||||
c.glonassFrequency = meas.glonass_freq if meas.constellation_id == ConstellationId.GLONASS else 0 |
||||
c.pseudorange = float(meas.observables['C1C']) # todo should be observables_final when using corrected measurements |
||||
c.pseudorangeStd = float(meas.observables_std['C1C']) |
||||
c.pseudorangeRate = float(meas.observables['D1C']) # todo should be observables_final when using corrected measurements |
||||
c.pseudorangeRateStd = float(meas.observables_std['D1C']) |
||||
c.satPos = meas.sat_pos_final.tolist() |
||||
c.satVel = meas.sat_vel.tolist() |
||||
return c |
||||
|
||||
|
||||
def main(): |
||||
dog = AstroDog() |
||||
sm = messaging.SubMaster(['ubloxGnss']) |
||||
pm = messaging.PubMaster(['gnssMeasurements']) |
||||
|
||||
while True: |
||||
sm.update() |
||||
|
||||
# Todo if no internet available use latest ephemeris |
||||
if sm.updated['ubloxGnss']: |
||||
ublox_msg = sm['ubloxGnss'] |
||||
msg = process_ublox_msg(ublox_msg, dog, sm.logMonoTime['ubloxGnss']) |
||||
if msg is None: |
||||
msg = messaging.new_message('gnssMeasurements') |
||||
pm.send('gnssMeasurements', msg) |
||||
|
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
main() |
||||
@ -0,0 +1,23 @@ |
||||
#!/usr/bin/env python3 |
||||
import unittest |
||||
from datetime import datetime |
||||
|
||||
from laika.helpers import ConstellationId |
||||
|
||||
from laika.gps_time import GPSTime |
||||
from laika.raw_gnss import GNSSMeasurement |
||||
from selfdrive.locationd.laikad import create_measurement_msg |
||||
|
||||
|
||||
class TestLaikad(unittest.TestCase): |
||||
|
||||
def test_create_msg_without_errors(self): |
||||
gpstime = GPSTime.from_datetime(datetime.now()) |
||||
meas = GNSSMeasurement(ConstellationId.GPS, 1, gpstime.week, gpstime.tow, {'C1C': 0., 'D1C': 0.}, {'C1C': 0., 'D1C': 0.}) |
||||
msg = create_measurement_msg(meas) |
||||
|
||||
self.assertEqual(msg.constellationId, 'gps') |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -0,0 +1,80 @@ |
||||
import unittest |
||||
|
||||
import numpy as np |
||||
|
||||
from laika import AstroDog |
||||
from laika.helpers import ConstellationId |
||||
from laika.raw_gnss import calc_pos_fix, correct_measurements, process_measurements, read_raw_ublox |
||||
from selfdrive.test.openpilotci import get_url |
||||
from tools.lib.logreader import LogReader |
||||
|
||||
|
||||
def get_gnss_measurements(log_reader): |
||||
gnss_measurements = [] |
||||
for msg in log_reader: |
||||
if msg.which() == "ubloxGnss": |
||||
ublox_msg = msg.ubloxGnss |
||||
if ublox_msg.which == 'measurementReport': |
||||
report = ublox_msg.measurementReport |
||||
if len(report.measurements) > 0: |
||||
gnss_measurements.append(read_raw_ublox(report)) |
||||
return gnss_measurements |
||||
|
||||
|
||||
class TestUbloxProcessing(unittest.TestCase): |
||||
NUM_TEST_PROCESS_MEAS = 10 |
||||
|
||||
@classmethod |
||||
def setUpClass(cls): |
||||
lr = LogReader(get_url("4cf7a6ad03080c90|2021-09-29--13-46-36", 0)) |
||||
cls.gnss_measurements = get_gnss_measurements(lr) |
||||
|
||||
def test_read_ublox_raw(self): |
||||
count_gps = 0 |
||||
count_glonass = 0 |
||||
for measurements in self.gnss_measurements: |
||||
for m in measurements: |
||||
if m.constellation_id == ConstellationId.GPS: |
||||
count_gps += 1 |
||||
elif m.constellation_id == ConstellationId.GLONASS: |
||||
count_glonass += 1 |
||||
|
||||
self.assertEqual(count_gps, 5036) |
||||
self.assertEqual(count_glonass, 3651) |
||||
|
||||
def test_get_fix(self): |
||||
dog = AstroDog() |
||||
position_fix_found = 0 |
||||
count_processed_measurements = 0 |
||||
count_corrected_measurements = 0 |
||||
position_fix_found_after_correcting = 0 |
||||
|
||||
pos_ests = [] |
||||
for measurements in self.gnss_measurements[:self.NUM_TEST_PROCESS_MEAS]: |
||||
processed_meas = process_measurements(measurements, dog) |
||||
count_processed_measurements += len(processed_meas) |
||||
pos_fix = calc_pos_fix(processed_meas) |
||||
if len(pos_fix) > 0 and all(pos_fix[0] != 0): |
||||
position_fix_found += 1 |
||||
|
||||
corrected_meas = correct_measurements(processed_meas, pos_fix[0][:3], dog) |
||||
count_corrected_measurements += len(corrected_meas) |
||||
|
||||
pos_fix = calc_pos_fix(corrected_meas) |
||||
if len(pos_fix) > 0 and all(pos_fix[0] != 0): |
||||
pos_ests.append(pos_fix[0]) |
||||
position_fix_found_after_correcting += 1 |
||||
|
||||
mean_fix = np.mean(np.array(pos_ests)[:, :3], axis=0) |
||||
np.testing.assert_allclose(mean_fix, [-2452306.662377, -4778343.136806, 3428550.090557], rtol=0, atol=1) |
||||
|
||||
# Note that can happen that there are less corrected measurements compared to processed when they are invalid. |
||||
# However, not for the current segment |
||||
self.assertEqual(position_fix_found, self.NUM_TEST_PROCESS_MEAS) |
||||
self.assertEqual(position_fix_found_after_correcting, self.NUM_TEST_PROCESS_MEAS) |
||||
self.assertEqual(count_processed_measurements, 69) |
||||
self.assertEqual(count_corrected_measurements, 69) |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
unittest.main() |
||||
@ -1 +1,2 @@ |
||||
fakedata/ |
||||
debayer_diff.txt |
||||
|
||||
@ -0,0 +1 @@ |
||||
14f411de8085d1cc9e467592c90bcaf95447a467 |
||||
@ -1 +1 @@ |
||||
70d79fbcc2b9ab0af867a7d6f138b58bcaaa3aa8 |
||||
10b766fa845934f0258c52cdf2103d0e1a9496c9 |
||||
@ -0,0 +1,215 @@ |
||||
#!/usr/bin/env python3 |
||||
import os |
||||
import sys |
||||
import bz2 |
||||
import numpy as np |
||||
|
||||
import pyopencl as cl # install with `PYOPENCL_CL_PRETEND_VERSION=2.0 pip install pyopencl` |
||||
|
||||
from selfdrive.hardware import PC, TICI |
||||
from common.basedir import BASEDIR |
||||
from selfdrive.test.openpilotci import BASE_URL, get_url |
||||
from selfdrive.version import get_commit |
||||
from tools.lib.logreader import LogReader |
||||
from tools.lib.filereader import FileReader |
||||
|
||||
TEST_ROUTE = "8345e3b82948d454|2022-05-04--13-45-33" |
||||
SEGMENT = 0 |
||||
|
||||
FRAME_WIDTH = 1928 |
||||
FRAME_HEIGHT = 1208 |
||||
FRAME_STRIDE = 2896 |
||||
|
||||
UV_WIDTH = FRAME_WIDTH // 2 |
||||
UV_HEIGHT = FRAME_HEIGHT // 2 |
||||
UV_SIZE = UV_WIDTH * UV_HEIGHT |
||||
|
||||
|
||||
def get_frame_fn(ref_commit, test_route, tici=True): |
||||
return f"{test_route}_debayer{'_tici' if tici else ''}_{ref_commit}.bz2" |
||||
|
||||
|
||||
def bzip_frames(frames): |
||||
data = bytes() |
||||
for y, u, v in frames: |
||||
data += y.tobytes() |
||||
data += u.tobytes() |
||||
data += v.tobytes() |
||||
return bz2.compress(data) |
||||
|
||||
|
||||
def unbzip_frames(url): |
||||
with FileReader(url) as f: |
||||
dat = f.read() |
||||
|
||||
data = bz2.decompress(dat) |
||||
|
||||
res = [] |
||||
for y_start in range(0, len(data), FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2): |
||||
u_start = y_start + FRAME_WIDTH * FRAME_HEIGHT |
||||
v_start = u_start + UV_SIZE |
||||
|
||||
y = np.frombuffer(data[y_start: u_start], dtype=np.uint8).reshape((FRAME_HEIGHT, FRAME_WIDTH)) |
||||
u = np.frombuffer(data[u_start: v_start], dtype=np.uint8).reshape((UV_HEIGHT, UV_WIDTH)) |
||||
v = np.frombuffer(data[v_start: v_start + UV_SIZE], dtype=np.uint8).reshape((UV_HEIGHT, UV_WIDTH)) |
||||
|
||||
res.append((y, u, v)) |
||||
|
||||
return res |
||||
|
||||
|
||||
def init_kernels(frame_offset=0): |
||||
ctx = cl.create_some_context(interactive=False) |
||||
|
||||
with open(os.path.join(BASEDIR, 'selfdrive/camerad/cameras/real_debayer.cl')) as f: |
||||
build_args = ' -cl-fast-relaxed-math -cl-denorms-are-zero -cl-single-precision-constant' + \ |
||||
f' -DFRAME_STRIDE={FRAME_STRIDE} -DRGB_WIDTH={FRAME_WIDTH} -DRGB_HEIGHT={FRAME_HEIGHT} -DFRAME_OFFSET={frame_offset} -DCAM_NUM=0' |
||||
if PC: |
||||
build_args += ' -DHALF_AS_FLOAT=1 -cl-std=CL2.0' |
||||
debayer_prg = cl.Program(ctx, f.read()).build(options=build_args) |
||||
|
||||
with open(os.path.join(BASEDIR, 'selfdrive/camerad/transforms/rgb_to_yuv.cl')) as f: |
||||
build_args = f' -cl-fast-relaxed-math -cl-denorms-are-zero -DWIDTH={FRAME_WIDTH} -DHEIGHT={FRAME_HEIGHT}' + \ |
||||
f' -DUV_WIDTH={UV_WIDTH} -DUV_HEIGHT={UV_HEIGHT} -DRGB_STRIDE={FRAME_WIDTH*3}' + \ |
||||
f' -DRGB_SIZE={FRAME_WIDTH*FRAME_HEIGHT}' |
||||
rgb_to_yuv_prg = cl.Program(ctx, f.read()).build(options=build_args) |
||||
|
||||
return ctx, debayer_prg, rgb_to_yuv_prg |
||||
|
||||
|
||||
def debayer_frame(ctx, debayer_prg, rgb_to_yuv_prg, data, rgb=False): |
||||
q = cl.CommandQueue(ctx) |
||||
|
||||
rgb_buff = np.empty(FRAME_WIDTH * FRAME_HEIGHT * 3, dtype=np.uint8) |
||||
yuv_buff = np.empty(FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2, dtype=np.uint8) |
||||
|
||||
cam_g = cl.Buffer(ctx, cl.mem_flags.READ_ONLY | cl.mem_flags.COPY_HOST_PTR, hostbuf=data) |
||||
rgb_wg = cl.Buffer(ctx, cl.mem_flags.WRITE_ONLY, FRAME_WIDTH * FRAME_HEIGHT * 3) |
||||
yuv_g = cl.Buffer(ctx, cl.mem_flags.WRITE_ONLY, FRAME_WIDTH * FRAME_HEIGHT + UV_SIZE * 2) |
||||
|
||||
local_worksize = (16, 16) if TICI else (8, 8) |
||||
local_mem = cl.LocalMemory(648 if TICI else 400) |
||||
ev1 = debayer_prg.debayer10(q, (FRAME_WIDTH, FRAME_HEIGHT), local_worksize, cam_g, rgb_wg, local_mem, np.float32(42)) |
||||
cl.enqueue_copy(q, rgb_buff, rgb_wg, wait_for=[ev1]).wait() |
||||
cl.enqueue_barrier(q) |
||||
|
||||
rgb_rg = cl.Buffer(ctx, cl.mem_flags.READ_ONLY | cl.mem_flags.COPY_HOST_PTR, hostbuf=rgb_buff) |
||||
cl.enqueue_barrier(q) |
||||
|
||||
ev3 = rgb_to_yuv_prg.rgb_to_yuv(q, (FRAME_WIDTH // 4, FRAME_HEIGHT // 4), None, rgb_rg, yuv_g) |
||||
cl.enqueue_barrier(q) |
||||
|
||||
cl.enqueue_copy(q, yuv_buff, yuv_g, wait_for=[ev3]).wait() |
||||
cl.enqueue_barrier(q) |
||||
|
||||
y = yuv_buff[:FRAME_WIDTH*FRAME_HEIGHT].reshape((FRAME_HEIGHT, FRAME_WIDTH)) |
||||
u = yuv_buff[FRAME_WIDTH*FRAME_HEIGHT:FRAME_WIDTH*FRAME_HEIGHT+UV_SIZE].reshape((UV_HEIGHT, UV_WIDTH)) |
||||
v = yuv_buff[FRAME_WIDTH*FRAME_HEIGHT+UV_SIZE:].reshape((UV_HEIGHT, UV_WIDTH)) |
||||
|
||||
if rgb: |
||||
return rgb_buff.reshape((FRAME_HEIGHT, FRAME_WIDTH, 3))[:, :, (2, 1, 0)] |
||||
else: |
||||
return y, u, v |
||||
|
||||
|
||||
def debayer_replay(lr): |
||||
ctx, debayer_prg, rgb_to_yuv_prg = init_kernels() |
||||
|
||||
frames = [] |
||||
for m in lr: |
||||
if m.which() == 'roadCameraState': |
||||
cs = m.roadCameraState |
||||
if cs.image: |
||||
data = np.frombuffer(cs.image, dtype=np.uint8) |
||||
img = debayer_frame(ctx, debayer_prg, rgb_to_yuv_prg, data) |
||||
|
||||
frames.append(img) |
||||
|
||||
return frames |
||||
|
||||
|
||||
if __name__ == "__main__": |
||||
update = "--update" in sys.argv |
||||
replay_dir = os.path.dirname(os.path.abspath(__file__)) |
||||
ref_commit_fn = os.path.join(replay_dir, "debayer_replay_ref_commit") |
||||
|
||||
# load logs |
||||
lr = list(LogReader(get_url(TEST_ROUTE, SEGMENT))) |
||||
|
||||
# run replay |
||||
frames = debayer_replay(lr) |
||||
|
||||
# get diff |
||||
failed = False |
||||
diff = '' |
||||
yuv_i = ['y', 'u', 'v'] |
||||
if not update: |
||||
with open(ref_commit_fn) as f: |
||||
ref_commit = f.read().strip() |
||||
frame_fn = get_frame_fn(ref_commit, TEST_ROUTE, tici=TICI) |
||||
|
||||
try: |
||||
cmp_frames = unbzip_frames(BASE_URL + frame_fn) |
||||
|
||||
if len(frames) != len(cmp_frames): |
||||
failed = True |
||||
diff += 'amount of frames not equal\n' |
||||
|
||||
for i, (frame, cmp_frame) in enumerate(zip(frames, cmp_frames)): |
||||
for j in range(3): |
||||
fr = frame[j] |
||||
cmp_f = cmp_frame[j] |
||||
if fr.shape != cmp_f.shape: |
||||
failed = True |
||||
diff += f'frame shapes not equal for ({i}, {yuv_i[j]})\n' |
||||
diff += f'{ref_commit}: {cmp_f.shape}\n' |
||||
diff += f'HEAD: {fr.shape}\n' |
||||
elif not np.array_equal(fr, cmp_f): |
||||
failed = True |
||||
if np.allclose(fr, cmp_f, atol=1): |
||||
diff += f'frames not equal for ({i}, {yuv_i[j]}), but are all close\n' |
||||
else: |
||||
diff += f'frames not equal for ({i}, {yuv_i[j]})\n' |
||||
|
||||
frame_diff = np.abs(np.subtract(fr, cmp_f)) |
||||
diff_len = len(np.nonzero(frame_diff)[0]) |
||||
if diff_len > 1000: |
||||
diff += f'different at a large amount of pixels ({diff_len})\n' |
||||
else: |
||||
diff += 'different at (frame, yuv, pixel, ref, HEAD):\n' |
||||
for k in zip(*np.nonzero(frame_diff)): |
||||
diff += f'{i}, {yuv_i[j]}, {k}, {cmp_f[k]}, {fr[k]}\n' |
||||
|
||||
if failed: |
||||
print(diff) |
||||
with open("debayer_diff.txt", "w") as f: |
||||
f.write(diff) |
||||
except Exception as e: |
||||
print(str(e)) |
||||
failed = True |
||||
|
||||
# upload new refs |
||||
if update or (failed and TICI): |
||||
from selfdrive.test.openpilotci import upload_file |
||||
|
||||
print("Uploading new refs") |
||||
|
||||
frames_bzip = bzip_frames(frames) |
||||
|
||||
new_commit = get_commit() |
||||
frame_fn = os.path.join(replay_dir, get_frame_fn(new_commit, TEST_ROUTE, tici=TICI)) |
||||
with open(frame_fn, "wb") as f2: |
||||
f2.write(frames_bzip) |
||||
|
||||
try: |
||||
upload_file(frame_fn, os.path.basename(frame_fn)) |
||||
except Exception as e: |
||||
print("failed to upload", e) |
||||
|
||||
if update: |
||||
with open(ref_commit_fn, 'w') as f: |
||||
f.write(str(new_commit)) |
||||
|
||||
print("\nNew ref commit: ", new_commit) |
||||
|
||||
sys.exit(int(failed)) |
||||
Loading…
Reference in new issue