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/ |
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