Compare commits

..

3 Commits

Author SHA1 Message Date
nayan 164ab04547 models v26 2026-11-21 12:07:55 -05:00
nayan 0f4c20d430 emu more + models v26 2026-11-21 11:47:00 -05:00
nayan 13097515a6 bump opendbc 2026-11-21 11:09:16 -05:00
62 changed files with 2670 additions and 392 deletions
@@ -34,6 +34,11 @@ on:
required: false
type: string
default: 'qcom'
on_device:
description: 'Compile on the tici / chestnut runner instead of on a hosted runner with tinygrad''s qcom / amd mock'
required: false
type: boolean
default: false
hf_repo:
description: 'Hugging Face dataset repository (e.g. sunnypilot/sunnypilot_models_v1)'
required: false
@@ -107,6 +112,11 @@ on:
options:
- qcom
- chestnut
on_device:
description: 'Compile on the tici / chestnut runner instead of on a hosted runner with tinygrad''s qcom / amd mock'
required: false
type: boolean
default: false
hf_repo:
description: 'Hugging Face dataset repository'
required: false
@@ -130,6 +140,7 @@ jobs:
is_20hz: ${{ inputs.is_20hz }}
artifact_suffix: ${{ inputs.artifact_suffix }}
target_hardware: ${{ inputs.target_hardware }}
on_device: ${{ inputs.on_device || false }}
secrets: inherit
publish_model:
@@ -0,0 +1,61 @@
name: Chestnut mock build parity
description: >-
Convert one commaai precompiled chestnut model (LS2) on this runner under tinygrad's amd mock, as a hosted chestnut build does, and require the
exact bytes the chestnut runner built (the published artifact). Needs setup-amd-mock, numpy and zstandard; tinygrad_repo checked out.
runs:
using: composite
steps:
- name: Key this check by everything its result depends on
id: key
shell: bash
env:
SOURCES: ${{ hashFiles('tinygrad_repo/tinygrad/**/*.py', 'tinygrad_repo/test/mockgpu/**', 'openpilot/sunnypilot/modeld_v2/*.py', '.github/workflows/chestnut-build-parity/action.yml') }}
run: echo "key=chestnut-build-parity-$SOURCES-$(sha256sum "$LLVM_PATH" | cut -c1-16)-$(python3 -c 'import platform; print(platform.python_version())')" >> "$GITHUB_OUTPUT"
- name: Passed before with all of it identical?
id: passed
uses: actions/cache/restore@v4
with:
path: ~/.chestnut_build_parity.passed
key: ${{ steps.key.outputs.key }}
- name: Rebuild LS2 on the mock and require the chestnut runner's artifact byte for byte
if: steps.passed.outputs.cache-hit != 'true'
shell: bash
env:
REF: b92fc4828664f8767bce5e92f1630ed319cbe509 # commaai/openpilot: precompiled big_driving_tinygrad.pkl, pins tinygrad ec91b74b5b
PKL_OID: b4dead3ee091d66ab002a6b8e716c26f4f79e3c931e4be5f48e017ba92f6e960
PKL_SIZE: "767228907"
TG_SHA: ec91b74b5b6b0fc924b72d3f27689679ca2438b9
WANT: 680a8d629785f2b1d1a2b0e40d0dfbcd0f7291c21e20c51259f1d768a22ec217 # driving_light_speed_2_november_04_2026_tinygrad.pkl, recompiled46
run: |
set -euo pipefail
work="$RUNNER_TEMP/chestnut_parity" && rm -rf "$work" && mkdir -p "$work"
href=$(curl -fsSL --retry 3 -X POST -H "Accept: application/vnd.git-lfs+json" -H "Content-Type: application/vnd.git-lfs+json" \
https://huggingface.co/commaai/openpilot-lfs.git/info/lfs/objects/batch \
-d "{\"operation\":\"download\",\"transfer\":[\"basic\"],\"objects\":[{\"oid\":\"$PKL_OID\",\"size\":$PKL_SIZE}]}" | jq -r '.objects[0].actions.download.href')
curl -fsSL --retry 3 -o "$work/big_driving_tinygrad.pkl" "$href"
echo "$PKL_OID $work/big_driving_tinygrad.pkl" | sha256sum -c -
tg="$work/tinygrad" && git init -q "$tg" && git -C "$tg" fetch -q --depth 1 https://github.com/tinygrad/tinygrad "$TG_SHA" && git -C "$tg" checkout -q FETCH_HEAD
CHECK_OOB=0 GMMU=0 PYTHONPATH="$GITHUB_WORKSPACE/tinygrad_repo" python3 tinygrad_repo/test/mockgpu/usb_server.py \
env DEV=USB+AMD PYTHONPATH="$tg:$GITHUB_WORKSPACE" python3 openpilot/sunnypilot/modeld_v2/convert_upstream_pkl.py "$work/big_driving_tinygrad.pkl" "$work/LS2.pkl"
got=$(sha256sum "$work/LS2.pkl" | cut -d' ' -f1)
if [ "$got" != "$WANT" ]; then
echo "::error::the mock's LS2 build ($got) is not the chestnut runner's ($WANT): this runner's LLVM or the mock no longer builds what the chestnut does"
exit 1
fi
echo "LS2 rebuilt on the mock: byte-identical to the chestnut runner's build"
touch ~/.chestnut_build_parity.passed
- name: Remember the pass
if: steps.passed.outputs.cache-hit != 'true'
uses: actions/cache/save@v4
with:
path: ~/.chestnut_build_parity.passed
key: ${{ steps.key.outputs.key }}
- name: Already passed
if: steps.passed.outputs.cache-hit == 'true'
shell: bash
run: |
echo "LS2 rebuild skipped: it already matched the chestnut runner's build with this exact mock, converter, LLVM and python"
@@ -0,0 +1,60 @@
name: Set up tinygrad's amd mock
description: LLVM 20 and ROCm comgr, as tinygrad_repo/.github/actions/setup-tinygrad installs them for the mock gpu
inputs:
emu_cache:
description: >-
Keep tinygrad's cache db (the emulator's compiled kernels and gate results) between runs under this name, e.g. the model's file name.
Empty: no cache.
default: ''
runs:
using: composite
steps:
- name: Install LLVM 20 and ROCm comgr
shell: bash
run: |
. /etc/os-release
curl -fsSL https://apt.llvm.org/llvm-snapshot.gpg.key | sudo tee /etc/apt/trusted.gpg.d/apt.llvm.org.asc > /dev/null
echo "deb http://apt.llvm.org/$VERSION_CODENAME/ llvm-toolchain-$VERSION_CODENAME-20 main" | sudo tee /etc/apt/sources.list.d/llvm.list
sudo mkdir -p /etc/apt/keyrings
curl -fsSL https://repo.radeon.com/rocm/rocm.gpg.key | gpg --dearmor | sudo tee /etc/apt/keyrings/rocm.gpg > /dev/null
echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/rocm.gpg] https://repo.radeon.com/rocm/apt/7.1 $VERSION_CODENAME main" | sudo tee /etc/apt/sources.list.d/rocm.list
printf 'Package: *\nPin: release o=repo.radeon.com\nPin-Priority: 600\n' | sudo tee /etc/apt/preferences.d/rocm-pin-600
sudo apt-get -qq update && sudo apt-get -qq install -y --no-install-recommends libllvm20 clang-20 lld-20 comgr
printf '/opt/rocm/lib\n/opt/rocm/lib64\n' | sudo tee /etc/ld.so.conf.d/rocm.conf && sudo ldconfig
echo "/usr/lib/llvm-20/bin" >> "$GITHUB_PATH"
llvm=$(ls /usr/lib/llvm-20/lib/libLLVM*.so* /usr/lib/*-linux-gnu/libLLVM-20.so* 2>/dev/null | head -1 || true)
[ -n "$llvm" ] || { echo "::error::libLLVM 20 not found"; exit 1; }
echo "LLVM_PATH=$llvm" >> "$GITHUB_ENV"
- name: Cache the chestnut's firmware
uses: actions/cache@v4
with:
path: ~/.cache/tinygrad/downloads/fw
key: chestnut-fw-${{ hashFiles('tinygrad_repo/tinygrad/runtime/autogen/am/fw.py') }}
- name: Fetch the chestnut's firmware (gitlab times out now and then; the mock boots from it)
shell: bash
run: |
for i in 1 2 3 4 5; do
PYTHONPATH="$GITHUB_WORKSPACE/tinygrad_repo" python3 -c "
from tinygrad.helpers import fetch_fw
from tinygrad.runtime.autogen.am import fw
for n, h in fw.hashes.items():
if n.startswith(('psp_14_0_2_', 'smu_14_0_2.', 'sdma_7_0_0.', 'gc_12_0_0_')): fetch_fw('amdgpu', n, h)" && exit 0
sleep $((i * 10))
done
echo "::error::could not fetch the chestnut's firmware from gitlab"; exit 1
- name: runner cpu
if: inputs.emu_cache != ''
id: cpu
shell: bash
run: echo "cpu=$( (grep -m1 '^model name' /proc/cpuinfo; grep -m1 '^flags' /proc/cpuinfo) | sha256sum | cut -c1-16)" >> "$GITHUB_OUTPUT"
- name: Keep the emulator's compiled kernels between runs
if: inputs.emu_cache != ''
uses: actions/cache@v4
with:
path: ~/.cache/tinygrad/cache.db
key: emu-${{ steps.cpu.outputs.cpu }}-${{ hashFiles('tinygrad_repo/tinygrad/**/*.py', 'tinygrad_repo/test/mockgpu/**') }}-${{ inputs.emu_cache }}
restore-keys: emu-${{ steps.cpu.outputs.cpu }}-${{ hashFiles('tinygrad_repo/tinygrad/**/*.py', 'tinygrad_repo/test/mockgpu/**') }}-
+96 -14
View File
@@ -35,6 +35,11 @@ on:
required: false
type: string
default: 'qcom'
on_device:
description: 'Compile on the tici / chestnut runner instead of on a hosted runner with tinygrad''s qcom / amd mock'
required: false
type: boolean
default: false
workflow_dispatch:
inputs:
upstream_branch:
@@ -59,6 +64,11 @@ on:
- qcom
- chestnut
default: 'qcom'
on_device:
description: 'Compile on the tici / chestnut runner instead of on a hosted runner with tinygrad''s qcom / amd mock'
required: false
type: boolean
default: false
run-name: Build model [${{ inputs.custom_name || inputs.upstream_branch }}] from ref [${{ inputs.upstream_branch }}]
@@ -111,7 +121,12 @@ jobs:
grep -qs "^version https://git-lfs" "$f" || continue
OID=$(awk -F'sha256:' '/^oid/{print $2}' "$f"); SIZE=$(awk '/^size/{print $2}' "$f")
HREF=$(curl -fsSL --retry 3 -X POST -H "Accept: application/vnd.git-lfs+json" -H "Content-Type: application/vnd.git-lfs+json" "$LFS_URL/objects/batch" \
-d "{\"operation\":\"download\",\"transfer\":[\"basic\"],\"objects\":[{\"oid\":\"$OID\",\"size\":$SIZE}]}" | jq -r '.objects[0].actions.download.href')
-d "{\"operation\":\"download\",\"transfer\":[\"basic\"],\"objects\":[{\"oid\":\"$OID\",\"size\":$SIZE}]}" | jq -r '.objects[0].actions.download.href // empty')
if [ -z "$HREF" ]; then
HF_PATH=$(curl -fsSL --retry 3 "https://huggingface.co/api/models/commaai/openpilot_driving_models/tree/main?recursive=true" | jq -r --arg oid "$OID" 'map(select(.lfs.oid == $oid))[0].path // empty')
[ -n "$HF_PATH" ] || { echo "::error::$f ($OID) is in neither openpilot's LFS nor commaai/openpilot_driving_models"; exit 1; }
HREF="https://huggingface.co/commaai/openpilot_driving_models/resolve/main/$HF_PATH"
fi
aria2c -x 16 -s 16 -k 16M --max-tries=5 --retry-wait=5 --file-allocation=none --summary-interval=30 --allow-overwrite=true \
-d "$(dirname "$f")" -o "$(basename "$f")" "$HREF"
echo "$OID $f" | sha256sum -c -
@@ -119,8 +134,9 @@ jobs:
}
UPSTREAM_PKL=selfdrive/modeld/models/big_driving_tinygrad.pkl
if [ "${{ inputs.target_hardware }}" != "chestnut" ]; then
git lfs pull -X "**/selfdrive/modeld/models/big_*.onnx,**/selfdrive/modeld/models/dmonitoring_*.onnx,**/selfdrive/modeld/models/big_*.pkl,**/selfdrive/modeld/models/dmonitoring_*.pkl"
git lfs pull -X "**/selfdrive/modeld/models/big_*.onnx,**/selfdrive/modeld/models/dmonitoring_*.onnx,**/selfdrive/modeld/models/big_*.pkl,**/selfdrive/modeld/models/dmonitoring_*.pkl" || true
rm -f selfdrive/modeld/models/big_*.onnx selfdrive/modeld/models/dmonitoring_*.onnx selfdrive/modeld/models/big_*.pkl selfdrive/modeld/models/dmonitoring_*.pkl
fetch_lfs selfdrive/modeld/models/*.onnx
elif compgen -G "selfdrive/modeld/models/big_*.onnx" > /dev/null; then
fetch_lfs selfdrive/modeld/models/big_*.onnx selfdrive/modeld/models/big_*.pkl
find selfdrive/modeld/models -type f \( -name "*.onnx" -o -name "*.pkl" \) ! -name "big_*.onnx" ! -name "big_*.pkl" -delete
@@ -153,18 +169,67 @@ jobs:
if-no-files-found: error
build_model:
runs-on: [self-hosted, "${{ inputs.target_hardware == 'chestnut' && 'chestnut' || 'tici' }}"]
runs-on: ${{ inputs.on_device && fromJSON(inputs.target_hardware == 'chestnut' && '["self-hosted","chestnut"]' || '["self-hosted","tici"]') || 'ubuntu-24.04' }}
needs: get_model
env:
MODEL_NAME: ${{ inputs.custom_name || inputs.upstream_branch }} (${{ needs.get_model.outputs.model_date }})
REF: ${{ inputs.upstream_branch }}
MOCK_BUILD: ${{ !inputs.on_device }}
steps:
- uses: actions/checkout@v4
with:
fetch-depth: 1
submodules: recursive
- name: Set up the hosted mock build
if: env.MOCK_BUILD == 'true'
uses: actions/setup-python@v5
with:
python-version: '3.12'
- name: Install LLVM 20 and ROCm comgr for the amd mock
if: env.MOCK_BUILD == 'true' && inputs.target_hardware == 'chestnut'
uses: ./.github/workflows/setup-amd-mock
with:
emu_cache: build
- name: Install the amd mock build dependencies
if: env.MOCK_BUILD == 'true' && inputs.target_hardware == 'chestnut'
run: |
python3 -m pip install -q numpy zstandard
- name: Install the qcom mock build dependencies
if: env.MOCK_BUILD == 'true' && inputs.target_hardware != 'chestnut'
id: qcom_deps
run: |
sudo apt-get -qq update && sudo apt-get -qq install -y --no-install-recommends qemu-user-static aria2
python3 -m pip install -q numpy zstandard tinymesa==25.2.7.2
echo "qemu=$(dpkg-query -W -f='${Version}' qemu-user-static)" >> "$GITHUB_OUTPUT"
- name: Compiler check passed before with all of it identical?
if: env.MOCK_BUILD == 'true' && inputs.target_hardware != 'chestnut'
id: qcom_compiler
uses: actions/cache/restore@v4
with:
path: ~/.qcom_compiler_parity.passed
key: qcom-compiler-parity-${{ steps.qcom_deps.outputs.qemu }}-${{ hashFiles('tinygrad_repo/test/device/qcom/compiler_parity.py', 'tinygrad_repo/test/mockgpu/qcom/a630_compiled.pkl.xz', 'tinygrad_repo/tinygrad/runtime/support/compiler_qcom.py') }}
- name: Check the off-device qcom compiler builds the a630's bytes
if: env.MOCK_BUILD == 'true' && inputs.target_hardware != 'chestnut' && steps.qcom_compiler.outputs.cache-hit != 'true'
run: |
# every kernel an a630 has compiled must compile to the same bytes here before a model is built here
PYTHONPATH="$TINYGRAD_PATH" python3 "$TINYGRAD_PATH/test/device/qcom/compiler_parity.py" check "$TINYGRAD_PATH/test/mockgpu/qcom/a630_compiled.pkl.xz"
touch ~/.qcom_compiler_parity.passed
- name: Remember the compiler check passed
if: env.MOCK_BUILD == 'true' && inputs.target_hardware != 'chestnut' && steps.qcom_compiler.outputs.cache-hit != 'true'
uses: actions/cache/save@v4
with:
path: ~/.qcom_compiler_parity.passed
key: ${{ steps.qcom_compiler.outputs.cache-primary-key }}
- name: Set environment variables
if: env.MOCK_BUILD != 'true'
id: set-env
run: |
# Set up common environment
@@ -177,6 +242,7 @@ jobs:
printenv >> $GITHUB_ENV
- name: Setup build environment
if: env.MOCK_BUILD != 'true'
run: |
mkdir -p "${BUILD_DIR}/"
sudo find $BUILD_DIR/ -mindepth 1 -delete
@@ -198,21 +264,28 @@ jobs:
- name: Build Model
run: |
source /etc/profile
export UV_PROJECT_ENVIRONMENT=${HOME}/venv
export VIRTUAL_ENV=$UV_PROJECT_ENVIRONMENT
source ${UV_PROJECT_ENVIRONMENT}/bin/activate
if [ "$MOCK_BUILD" = "true" ]; then
TASKSET=""
export CHECK_OOB=0 GMMU=0 # as test/mockgpu runs the amd mock
else
source /etc/profile
export UV_PROJECT_ENVIRONMENT=${HOME}/venv
export VIRTUAL_ENV=$UV_PROJECT_ENVIRONMENT
source ${UV_PROJECT_ENVIRONMENT}/bin/activate
TASKSET="taskset -c 7"
fi
export PYTHONPATH="${PYTHONPATH}:${{ env.TINYGRAD_PATH }}:${{ github.workspace }}"
COMPILE_MODELD="${{ github.workspace }}/openpilot/sunnypilot/modeld_v2/compile_modeld.py"
MODEL_SIZE=$(python3 -c "from openpilot.common.transformations.model import MEDMODEL_INPUT_SIZE as s; print(f'{s[0]}x{s[1]}')")
CAMERA_RES=$(python3 -c "from openpilot.common.transformations.camera import _ar_ox_fisheye as a, _os_fisheye as o; print(f'{a.width}x{a.height} {o.width}x{o.height}')")
TG_FLAGS_QCOM="DEV=QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 OPENPILOT_HACKS=1"
MOCK=$([ "$MOCK_BUILD" = "true" ] && echo MOCK || true)
TG_FLAGS_QCOM="DEV=${MOCK:+MOCK+}QCOM IMAGE=1 FLOAT16=1 NOLOCALS=1 OPENPILOT_HACKS=1"
if [ "${{ inputs.target_hardware }}" == "chestnut" ]; then
echo "CHESTNUT build"
export CHESTNUT=1
TG_FLAGS="DEBUG=1 DEV=USB+AMD:LLVM FRAME_DEV=CPU FLOAT16=1 GMMU=0 TC_OPT=2 TC_MIN_GLOBALS=32"
TG_FLAGS="DEBUG=1 DEV=${MOCK}USB+AMD:LLVM FRAME_DEV=CPU FLOAT16=1 GMMU=0 TC_OPT=2 TC_MIN_GLOBALS=32"
OUTPUT_PKL="${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl"
else
echo "QCOM build"
@@ -227,14 +300,16 @@ jobs:
echo "Converting commaai's precompiled model with tinygrad $TG_SHA"
UPSTREAM_TG="${{ runner.temp }}/upstream_tinygrad"
rm -rf "$UPSTREAM_TG" && git init -q "$UPSTREAM_TG" && git -C "$UPSTREAM_TG" fetch -q --depth 1 "$TG_URL" "$TG_SHA" && git -C "$UPSTREAM_TG" checkout -q FETCH_HEAD
ON_MOCK=""
if [ "$MOCK_BUILD" = "true" ]; then ON_MOCK="env PYTHONPATH=$TINYGRAD_PATH python3 $TINYGRAD_PATH/test/mockgpu/usb_server.py"; fi
CONVERTED="${{ runner.temp }}/big_driving_tinygrad_plain.pkl"
env DEV=USB+AMD PYTHONPATH="$UPSTREAM_TG:${{ github.workspace }}" taskset -c 7 python3 openpilot/sunnypilot/modeld_v2/convert_upstream_pkl.py \
$ON_MOCK env DEV=USB+AMD PYTHONPATH="$UPSTREAM_TG:${{ github.workspace }}" $TASKSET python3 openpilot/sunnypilot/modeld_v2/convert_upstream_pkl.py \
"${{ env.MODELS_DIR }}/big_driving_tinygrad.pkl" "$CONVERTED"
mv "$CONVERTED" "$OUTPUT_PKL"
elif NEW_ARCH_ONNX=$(find "${{ env.MODELS_DIR }}" -maxdepth 1 -name "${{ inputs.target_hardware == 'chestnut' && 'big_' || '' }}*driving*.onnx" -print -quit) &&
[ -n "$NEW_ARCH_ONNX" ] && grep -qa new_img "$NEW_ARCH_ONNX"; then # the new architecture: one ONNX taking the warped frames as new_img
echo "Compiling new architecture ONNX: $NEW_ARCH_ONNX"
env ${TG_FLAGS} taskset -c 7 python3 openpilot/sunnypilot/modeld_v2/compile_modeld_v2.py \
env ${TG_FLAGS} $TASKSET python3 openpilot/sunnypilot/modeld_v2/compile_modeld_v2.py \
"$NEW_ARCH_ONNX" "$OUTPUT_PKL" --device-input "*" --benchmark-runs 1
else
echo "Detected onnx architecture. Routing to native compile_modeld.py."
@@ -284,7 +359,7 @@ jobs:
if [ -n "$MODEL_TYPE" ]; then
echo "Detected: $MODEL_TYPE -> $OUTPUT_PKL"
env ${TG_FLAGS} taskset -c 7 python3 "$COMPILE_MODELD" \
env ${TG_FLAGS} $TASKSET python3 "$COMPILE_MODELD" \
--model-type $MODEL_TYPE \
--model-size $MODEL_SIZE \
--camera-resolutions $CAMERA_RES \
@@ -293,6 +368,13 @@ jobs:
fi
fi
- name: Replay its check frame on the mock and require the result bit for bit
if: env.MOCK_BUILD == 'true'
env:
DEV: ${{ inputs.target_hardware == 'chestnut' && 'MOCKUSB+AMD' || 'MOCK+QCOM' }}
PYTHONPATH: ${{ env.TINYGRAD_PATH }}:${{ github.workspace }}
run: python3 -m openpilot.sunnypilot.modeld_v2.mock_parity check "${{ env.MODELS_DIR }}/${{ inputs.target_hardware == 'chestnut' && 'big_' || '' }}driving_tinygrad.pkl"
- name: Prepare Output
run: |
sudo rm -rf ${{ env.OUTPUT_DIR }}
@@ -304,7 +386,7 @@ jobs:
--include='/${{ inputs.target_hardware == 'chestnut' && 'big_' || '' }}driving_tinygrad.pkl' \
--exclude='*' \
--delete-excluded \
--chown=comma:comma \
$([ "$MOCK_BUILD" = "true" ] || echo --chown=comma:comma) \
${{ env.MODELS_DIR }}/ ${{ env.OUTPUT_DIR }}/
python3 "${{ github.workspace }}/release/ci/model_generator.py" \
@@ -333,7 +415,7 @@ jobs:
path: ${{ env.OUTPUT_DIR }}/artifact_name.txt
- name: Re-enable powersave
if: always()
if: always() && env.MOCK_BUILD != 'true'
run: |
source ${UV_PROJECT_ENVIRONMENT}/bin/activate
PYTHONPATH=$PYTHONPATH:${{ github.workspace }}/ ${{ github.workspace }}/scripts/manage-powersave.py --enable
@@ -168,7 +168,7 @@ jobs:
echo "Building locationd..."
# -j1: parallel rednose generators OOM the device
scons -j1 cache_dir="$SCONS_CACHE" --minimal \
openpilot/selfdrive/locationd
openpilot/selfdrive/locationd openpilot/sunnypilot/selfdrive/locationd
echo "Building rest of sunnypilot"
sudo rm -rf /tmp/* || true
mkdir -p "$BUILD_DIR/tmp"
+88 -13
View File
@@ -28,18 +28,7 @@ jobs:
env:
DEV: CPU
run: tools/op.sh test -v openpilot/sunnypilot/modeld_v2/tests/test_plain_artifact.py
- name: Install LLVM 20 and ROCm comgr for the amd mock, as tinygrad_repo/.github/actions/setup-tinygrad does
run: |
. /etc/os-release
curl -fsSL https://apt.llvm.org/llvm-snapshot.gpg.key | sudo tee /etc/apt/trusted.gpg.d/apt.llvm.org.asc > /dev/null
echo "deb http://apt.llvm.org/$VERSION_CODENAME/ llvm-toolchain-$VERSION_CODENAME-20 main" | sudo tee /etc/apt/sources.list.d/llvm.list
sudo mkdir -p /etc/apt/keyrings
curl -fsSL https://repo.radeon.com/rocm/rocm.gpg.key | gpg --dearmor | sudo tee /etc/apt/keyrings/rocm.gpg > /dev/null
echo "deb [arch=amd64 signed-by=/etc/apt/keyrings/rocm.gpg] https://repo.radeon.com/rocm/apt/7.1 $VERSION_CODENAME main" | sudo tee /etc/apt/sources.list.d/rocm.list
printf 'Package: *\nPin: release o=repo.radeon.com\nPin-Priority: 600\n' | sudo tee /etc/apt/preferences.d/rocm-pin-600
sudo apt-get -qq update && sudo apt-get -qq install -y --no-install-recommends libllvm20 clang-20 lld-20 comgr
printf '/opt/rocm/lib\n/opt/rocm/lib64\n' | sudo tee /etc/ld.so.conf.d/rocm.conf && sudo ldconfig
echo "/usr/lib/llvm-20/bin" >> "$GITHUB_PATH"
- uses: ./.github/workflows/setup-amd-mock
- name: Convert a pickled USB+AMD jit and replay it under tinygrad's amd mock
env:
DEV: MOCKUSB+AMD
@@ -80,6 +69,8 @@ jobs:
runs-on: ubuntu-latest
outputs:
models: ${{ steps.set-matrix.outputs.models }}
chestnut: ${{ steps.set-matrix.outputs.chestnut }}
qcom: ${{ steps.set-matrix.outputs.qcom }}
steps:
- uses: actions/checkout@v4
- name: Fetch and Parse json
@@ -91,16 +82,22 @@ jobs:
with open("openpilot/sunnypilot/models/fetcher.py", "r") as f:
urls = re.findall(r"MODEL_URL(?:_CHESTNUT)?\s*=\s*[\"'"'"']([^\"'"'"']+)[\"'"'"']", f.read())
artifacts = []
artifacts, chestnut, qcom = [], [], []
for url in urls:
data = json.loads(urllib.request.urlopen(url).read())
for bundle in data.get("bundles", []):
for model in bundle.get("models", []):
if "artifact" in model:
artifacts.append(model["artifact"])
if "chestnut" in url:
chestnut.append(model["artifact"])
elif "tinygrad" in model["artifact"]["file_name"]:
qcom.append(model["artifact"])
with open(os.environ["GITHUB_OUTPUT"], "a") as f:
f.write(f"models={json.dumps(artifacts)}\n")
f.write(f"chestnut={json.dumps(chestnut)}\n")
f.write(f"qcom={json.dumps(qcom)}\n")
'
test-model:
@@ -137,3 +134,81 @@ jobs:
export PYTHONPATH="$GITHUB_WORKSPACE/tinygrad_repo:$GITHUB_WORKSPACE:$PYTHONPATH"
python3 -m pip uninstall -y tinygrad || uv pip uninstall -y tinygrad --system || true
python3 -m pytest openpilot/sunnypilot/modeld_v2/tests/test_models.py
chestnut-mock-parity:
name: Mock parity ${{ matrix.artifact.file_name }}
needs: generate-matrix
runs-on: ubuntu-24.04
strategy:
fail-fast: false
matrix:
artifact: ${{ fromJson(needs.generate-matrix.outputs.chestnut) }}
steps:
- uses: actions/checkout@v4
with:
submodules: true
- uses: ./.github/workflows/setup-amd-mock
with:
emu_cache: ${{ matrix.artifact.file_name }}
- run: python3 -m pip install -q numpy
- name: Download model
env:
HF_TOKEN: ${{ secrets.HF_TOKEN }}
MODEL_URL: ${{ matrix.artifact.download_uri.url }}
MODEL_SHA: ${{ matrix.artifact.download_uri.sha256 }}
shell: bash
run: |
AUTH=()
if [ -n "$HF_TOKEN" ]; then AUTH=(-H "Authorization: Bearer $HF_TOKEN"); fi
curl --retry 3 --retry-all-errors -s -S -f -L "${AUTH[@]}" -o "$RUNNER_TEMP/model.pkl" "$MODEL_URL"
if [ -n "$MODEL_SHA" ]; then echo "$MODEL_SHA $RUNNER_TEMP/model.pkl" | sha256sum -c -; fi
- name: Replay its check frame on the amd mock and require the chestnut's result bit for bit
env:
PYTHONPATH: ${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}
run: python3 -m openpilot.sunnypilot.modeld_v2.mock_parity check "$RUNNER_TEMP/model.pkl"
qcom-mock-parity:
name: QCOM mock parity ${{ matrix.artifact.file_name }}
needs: generate-matrix
runs-on: ubuntu-24.04
strategy:
fail-fast: false
matrix:
artifact: ${{ fromJson(needs.generate-matrix.outputs.qcom) }}
steps:
- uses: actions/checkout@v4
with:
submodules: true
- uses: actions/setup-python@v5
with:
python-version: '3.12'
- run: python3 -m pip install -q numpy tinymesa==25.2.7.2
- name: Download model
env:
HF_TOKEN: ${{ secrets.HF_TOKEN }}
MODEL_URL: ${{ matrix.artifact.download_uri.url }}
MODEL_SHA: ${{ matrix.artifact.download_uri.sha256 }}
shell: bash
run: |
AUTH=()
if [ -n "$HF_TOKEN" ]; then AUTH=(-H "Authorization: Bearer $HF_TOKEN"); fi
curl --retry 3 --retry-all-errors -s -S -f -L "${AUTH[@]}" -o "$RUNNER_TEMP/model.pkl" "$MODEL_URL"
if [ -n "$MODEL_SHA" ]; then echo "$MODEL_SHA $RUNNER_TEMP/model.pkl" | sha256sum -c -; fi
- name: Replay its check frame on the qcom mock and require the comma device's result bit for bit
env:
DEV: MOCK+QCOM
PYTHONPATH: ${{ github.workspace }}/tinygrad_repo:${{ github.workspace }}
run: python3 -m openpilot.sunnypilot.modeld_v2.mock_parity check "$RUNNER_TEMP/model.pkl"
chestnut-mock-build-parity:
name: Chestnut build on the amd mock = chestnut runner build
runs-on: ubuntu-24.04
steps:
- uses: actions/checkout@v4
with:
submodules: true
- uses: ./.github/workflows/setup-amd-mock
with:
emu_cache: chestnut-build-parity
- run: python3 -m pip install -q numpy zstandard
- uses: ./.github/workflows/chestnut-build-parity
+2
View File
@@ -293,6 +293,8 @@ SConscript([
'openpilot/selfdrive/ui/SConscript',
])
SConscript(['openpilot/sunnypilot/SConscript'])
# Build desktop-only tools
if GetOption('extras') and arch != "comma_arm64":
SConscript([
+2 -3
View File
@@ -4,7 +4,7 @@
A supported vehicle is one that just works when you install a comma device. All supported cars provide a better experience than any stock system. Supported vehicles reference the US market unless otherwise specified.
# 358 Supported Cars
# 357 Supported Cars
|Make|Model|Supported Package|ACC|No ACC accel below|No ALC below|Steering Torque|Resume from stop|<a href="##"><img width=2000></a>Hardware Needed<br>&nbsp;|Video|Setup Video|
|---|---|---|:---:|:---:|:---:|:---:|:---:|:---:|:---:|:---:|
@@ -144,7 +144,6 @@ A supported vehicle is one that just works when you install a comma device. All
|Hyundai|Kona Electric 2022-23|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai O connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Electric 2022-23">Buy Here</a></sub></details>|||
|Hyundai|Kona Electric (with HDA II, Korea only) 2023|Smart Cruise Control (SCC)|Stock|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai R connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Electric (with HDA II, Korea only) 2023">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=U2fOCmcQ8hw" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|Hyundai|Kona Hybrid 2020|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai I connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Hybrid 2020">Buy Here</a></sub></details>|||
|Hyundai|Kona Hybrid (without HDA II) 2024-26|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Kona Hybrid (without HDA II) 2024-26">Buy Here</a></sub></details>|||
|Hyundai|Nexo 2021|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Nexo 2021">Buy Here</a></sub></details>|||
|Hyundai|Palisade 2020-22|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai H connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Palisade 2020-22">Buy Here</a></sub></details>|<a href="https://youtu.be/TAnDqjF4fDY?t=456" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|Hyundai|Santa Cruz 2022-24|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Santa Cruz 2022-24">Buy Here</a></sub></details>|||
@@ -164,7 +163,7 @@ A supported vehicle is one that just works when you install a comma device. All
|Hyundai|Tucson Diesel 2019|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai L connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Diesel 2019">Buy Here</a></sub></details>|||
|Hyundai|Tucson Hybrid 2022-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Hybrid 2022-24">Buy Here</a></sub></details>|||
|Hyundai|Tucson Hybrid (without HDA II) 2025-26|Smart Cruise Control (SCC)|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Hybrid (without HDA II) 2025-26">Buy Here</a></sub></details>|||
|Hyundai|Tucson Plug-in Hybrid 2023-24|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Plug-in Hybrid 2023-24">Buy Here</a></sub></details>|||
|Hyundai|Tucson Plug-in Hybrid 2024|All|openpilot available[<sup>1</sup>](#footnotes)|0 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai N connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Tucson Plug-in Hybrid 2024">Buy Here</a></sub></details>|||
|Hyundai|Veloster 2019-20|Smart Cruise Control (SCC)|Stock|5 mph|0 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-empty.svg)](##)|<details><summary>Parts</summary><sub>- 1 Hyundai E connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Hyundai Veloster 2019-20">Buy Here</a></sub></details>|||
|Jeep|Grand Cherokee 2016-18|Adaptive Cruise Control (ACC)|Stock|0 mph|9 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Jeep Grand Cherokee 2016-18">Buy Here</a></sub></details>|<a href="https://www.youtube.com/watch?v=eLR9o2JkuRk" target="_blank"><img height="18px" src="assets/icon-youtube.svg" /></a>||
|Jeep|Grand Cherokee 2019-21|Adaptive Cruise Control (ACC)|Stock|0 mph|39 mph|[![star](assets/icon-star-full.svg)](##)|[![star](assets/icon-star-full.svg)](##)|<details><summary>Parts</summary><sub>- 1 FCA connector<br>- 1 OBD-C cable (2 ft)<br>- 1 comma four<br>- 1 comma power v3<br>- 1 harness box<br>- 1 mount<br><a href="https://comma.ai/shop/comma-3x?harness=Jeep Grand Cherokee 2019-21">Buy Here</a></sub></details>|||
+1 -10
View File
@@ -483,16 +483,7 @@ struct ModelDataV2SP @0xa1680744031fdb2d {
}
}
struct GpsLocationSP @0xcb9fd56c7057593a {
latitude @0 :Float64;
longitude @1 :Float64;
speed @2 :Float32;
bearingDeg @3 :Float32;
horizontalAccuracy @4 :Float32; # m
bearingAccuracyDeg @5 :Float32;
hasFix @6 :Bool;
gpsOK @7 :Bool; # a raw fix arrived in the last 2 s
fused @8 :Bool; # false while passing the raw fix through
struct CustomReserved10 @0xcb9fd56c7057593a {
}
struct CustomReserved11 @0xc2243c65e0340384 {
+2 -2
View File
@@ -2648,7 +2648,7 @@ struct Event {
carStateSP @114 :Custom.CarStateSP;
liveMapDataSP @115 :Custom.LiveMapDataSP;
modelDataV2SP @116 :Custom.ModelDataV2SP;
gpsLocationSP @136 :Custom.GpsLocationSP;
customReserved10 @136 :Custom.CustomReserved10;
customReserved11 @137 :Custom.CustomReserved11;
customReserved12 @138 :Custom.CustomReserved12;
customReserved13 @139 :Custom.CustomReserved13;
@@ -2701,7 +2701,7 @@ struct Event {
lateralPlanDEPRECATED @64 :LateralPlan;
navModelDEPRECATED @104 :Deprecated.NavModelData;
uiPlanDEPRECATED @106 :UiPlan;
liveLocationKalmanDEPRECATED @72 :LiveLocationKalman;
liveLocationKalman @72 :LiveLocationKalman;
liveTracksDEPRECATED @16 :List(Deprecated.LiveTracksDEPRECATED);
onroadEventsDEPRECATED @68: List(Car.OnroadEventDEPRECATED);
gyroscope2DEPRECATED @100 :SensorEventData;
+1 -1
View File
@@ -95,7 +95,7 @@ _services: dict[str, tuple] = {
"carStateSP": (True, 100., 10),
"liveMapDataSP": (True, 1., 1),
"modelDataV2SP": (True, 20., 10, QueueSize.BIG),
"gpsLocationSP": (True, 20., 20),
"liveLocationKalman": (True, 20.),
# debug
"uiDebug": (True, 0., 1),
+2 -1
View File
@@ -172,13 +172,14 @@ inline static std::unordered_map<std::string, ParamKeyAttributes> keys = {
{"EnableCopyparty", {PERSISTENT | BACKUP, BOOL}},
{"EnableGithubRunner", {PERSISTENT | BACKUP, BOOL}},
{"GreenLightAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
{"GithubRunnerSufficientVoltage", {CLEAR_ON_MANAGER_START , BOOL}},
{"HasAcceptedTermsSP", {PERSISTENT, STRING, "0"}},
{"HideVEgoUI", {PERSISTENT | BACKUP, BOOL, "0"}},
{"IntelligentCruiseButtonManagement", {PERSISTENT | BACKUP , BOOL}},
{"InteractivityTimeout", {PERSISTENT | BACKUP, INT, "0"}},
{"IsDevelopmentBranch", {CLEAR_ON_MANAGER_START, BOOL}},
{"IsReleaseSpBranch", {CLEAR_ON_MANAGER_START, BOOL}},
{"LastGPSPositionFused", {PERSISTENT, STRING}},
{"LastGPSPositionLLK", {PERSISTENT, STRING}},
{"LeadDepartAlert", {PERSISTENT | BACKUP, BOOL, "0"}},
{"MaxTimeOffroad", {PERSISTENT | BACKUP, INT, "1800"}},
{"ModelRunnerTypeCache", {CLEAR_ON_ONROAD_TRANSITION, INT}},
@@ -28,6 +28,8 @@ class DesireHelper:
self.alc = AutoLaneChangeController(self)
self.lane_turn_controller = LaneTurnController(self)
self.lane_turn_direction = TurnDirection.none
self.turn_frames = 0
self.turn_repulse = 98
@staticmethod
def get_lane_change_direction(CS):
@@ -95,8 +97,10 @@ class DesireHelper:
self.prev_one_blinker = one_blinker and lateral_active
if self.lane_turn_direction != TurnDirection.none:
self.desire = TURN_DESIRES[self.lane_turn_direction]
self.turn_frames += 1
self.desire = TURN_DESIRES[self.lane_turn_direction] if self.turn_frames % self.turn_repulse else log.Desire.none
else:
self.turn_frames = 0
self.desire = log.Desire.none
if self.lane_change_state == LaneChangeState.laneChangeStarting:
if self.lane_change_direction == LaneChangeDirection.left:
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:65a08adc31d5c456219687d99b7bf5e44d61dae2d49ea67850e76105c7248cce
size 60918562
oid sha256:416c99bbd5563b01518be6de91b856986f66ffa03451ea9c451736dcc99896fd
size 83976077
+2
View File
@@ -0,0 +1,2 @@
SConscript(['common/transformations/SConscript'])
SConscript(['selfdrive/locationd/SConscript'])
@@ -0,0 +1,4 @@
Import('env')
transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc'])
Export('transformations')
@@ -0,0 +1,100 @@
#define _USE_MATH_DEFINES
#include "sunnypilot/common/transformations/coordinates.hpp"
#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Dense>
double a = 6378137; // lgtm [cpp/short-global-name]
double b = 6356752.3142; // lgtm [cpp/short-global-name]
double esq = 6.69437999014 * 0.001; // lgtm [cpp/short-global-name]
double e1sq = 6.73949674228 * 0.001;
static Geodetic to_degrees(Geodetic geodetic){
geodetic.lat = RAD2DEG(geodetic.lat);
geodetic.lon = RAD2DEG(geodetic.lon);
return geodetic;
}
static Geodetic to_radians(Geodetic geodetic){
geodetic.lat = DEG2RAD(geodetic.lat);
geodetic.lon = DEG2RAD(geodetic.lon);
return geodetic;
}
ECEF geodetic2ecef(const Geodetic &geodetic) {
auto g = to_radians(geodetic);
double xi = sqrt(1.0 - esq * pow(sin(g.lat), 2));
double x = (a / xi + g.alt) * cos(g.lat) * cos(g.lon);
double y = (a / xi + g.alt) * cos(g.lat) * sin(g.lon);
double z = (a / xi * (1.0 - esq) + g.alt) * sin(g.lat);
return {x, y, z};
}
Geodetic ecef2geodetic(const ECEF &e) {
// Convert from ECEF to geodetic using Ferrari's methods
// https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution
double x = e.x;
double y = e.y;
double z = e.z;
double r = sqrt(x * x + y * y);
double Esq = a * a - b * b;
double F = 54 * b * b * z * z;
double G = r * r + (1 - esq) * z * z - esq * Esq;
double C = (esq * esq * F * r * r) / (pow(G, 3));
double S = cbrt(1 + C + sqrt(C * C + 2 * C));
double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
double Q = sqrt(1 + 2 * esq * esq * P);
double r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
double U = sqrt(pow((r - esq * r_0), 2) + z * z);
double V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z);
double Z_0 = b * b * z / (a * V);
double h = U * (1 - b * b / (a * V));
double lat = atan((z + e1sq * Z_0) / r);
double lon = atan2(y, x);
return to_degrees({lat, lon, h});
}
LocalCoord::LocalCoord(const Geodetic &geodetic, const ECEF &e) {
init_ecef << e.x, e.y, e.z;
auto g = to_radians(geodetic);
ned2ecef_matrix <<
-sin(g.lat)*cos(g.lon), -sin(g.lon), -cos(g.lat)*cos(g.lon),
-sin(g.lat)*sin(g.lon), cos(g.lon), -cos(g.lat)*sin(g.lon),
cos(g.lat), 0, -sin(g.lat);
ecef2ned_matrix = ned2ecef_matrix.transpose();
}
NED LocalCoord::ecef2ned(const ECEF &e) {
Eigen::Vector3d ecef;
ecef << e.x, e.y, e.z;
Eigen::Vector3d ned = (ecef2ned_matrix * (ecef - init_ecef));
return {ned[0], ned[1], ned[2]};
}
ECEF LocalCoord::ned2ecef(const NED &n) {
Eigen::Vector3d ned;
ned << n.n, n.e, n.d;
Eigen::Vector3d ecef = (ned2ecef_matrix * ned) + init_ecef;
return {ecef[0], ecef[1], ecef[2]};
}
NED LocalCoord::geodetic2ned(const Geodetic &g) {
ECEF e = ::geodetic2ecef(g);
return ecef2ned(e);
}
Geodetic LocalCoord::ned2geodetic(const NED &n) {
ECEF e = ned2ecef(n);
return ::ecef2geodetic(e);
}
@@ -0,0 +1,43 @@
#pragma once
#include <eigen3/Eigen/Dense>
#define DEG2RAD(x) ((x) * M_PI / 180.0)
#define RAD2DEG(x) ((x) * 180.0 / M_PI)
struct ECEF {
double x, y, z;
Eigen::Vector3d to_vector() const {
return Eigen::Vector3d(x, y, z);
}
};
struct NED {
double n, e, d;
Eigen::Vector3d to_vector() const {
return Eigen::Vector3d(n, e, d);
}
};
struct Geodetic {
double lat, lon, alt;
bool radians=false;
};
ECEF geodetic2ecef(const Geodetic &g);
Geodetic ecef2geodetic(const ECEF &e);
class LocalCoord {
public:
Eigen::Matrix3d ned2ecef_matrix;
Eigen::Matrix3d ecef2ned_matrix;
Eigen::Vector3d init_ecef;
LocalCoord(const Geodetic &g, const ECEF &e);
LocalCoord(const Geodetic &g) : LocalCoord(g, ::geodetic2ecef(g)) {}
LocalCoord(const ECEF &e) : LocalCoord(::ecef2geodetic(e), e) {}
NED ecef2ned(const ECEF &e);
ECEF ned2ecef(const NED &n);
NED geodetic2ned(const Geodetic &g);
Geodetic ned2geodetic(const NED &n);
};
@@ -0,0 +1,143 @@
#define _USE_MATH_DEFINES
#include <iostream>
#include <cmath>
#include <eigen3/Eigen/Dense>
#include "sunnypilot/common/transformations/orientation.hpp"
#include "sunnypilot/common/transformations/coordinates.hpp"
Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat) {
if (quat.w() > 0){
return quat;
} else {
return Eigen::Quaterniond(-quat.w(), -quat.x(), -quat.y(), -quat.z());
}
}
Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler) {
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ())
* Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY())
* Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX());
return ensure_unique(q);
}
Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat) {
// TODO: switch to eigen implementation if the range of the Euler angles doesn't matter anymore
// Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
// return {euler(2), euler(1), euler(0)};
double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y()));
double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0);
double theta = asin(asin_arg_clipped);
double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z()));
return {gamma, theta, psi};
}
Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat) {
return quat.toRotationMatrix();
}
Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot) {
return ensure_unique(Eigen::Quaterniond(rot));
}
Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler) {
return quat2rot(euler2quat(euler));
}
Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot) {
return quat2euler(rot2quat(rot));
}
Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw) {
return euler2rot({roll, pitch, yaw});
}
Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle) {
Eigen::Quaterniond q;
q = Eigen::AngleAxisd(angle, axis);
return q.toRotationMatrix();
}
Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose) {
/*
Using Rotations to Build Aerospace Coordinate Systems
Don Koks
https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
*/
LocalCoord converter = LocalCoord(ecef_init);
Eigen::Vector3d zero = ecef_init.to_vector();
Eigen::Vector3d x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
Eigen::Vector3d y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
Eigen::Vector3d z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
Eigen::Vector3d x1 = rot(z0, ned_pose(2)) * x0;
Eigen::Vector3d y1 = rot(z0, ned_pose(2)) * y0;
Eigen::Vector3d z1 = rot(z0, ned_pose(2)) * z0;
Eigen::Vector3d x2 = rot(y1, ned_pose(1)) * x1;
Eigen::Vector3d y2 = rot(y1, ned_pose(1)) * y1;
Eigen::Vector3d z2 = rot(y1, ned_pose(1)) * z1;
Eigen::Vector3d x3 = rot(x2, ned_pose(0)) * x2;
Eigen::Vector3d y3 = rot(x2, ned_pose(0)) * y2;
x0 = Eigen::Vector3d(1, 0, 0);
y0 = Eigen::Vector3d(0, 1, 0);
z0 = Eigen::Vector3d(0, 0, 1);
double psi = atan2(x3.dot(y0), x3.dot(x0));
double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
y2 = rot(z0, psi) * y0;
z2 = rot(y2, theta) * z0;
double phi = atan2(y3.dot(z2), y3.dot(y2));
return {phi, theta, psi};
}
Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose) {
/*
Using Rotations to Build Aerospace Coordinate Systems
Don Koks
https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
*/
LocalCoord converter = LocalCoord(ecef_init);
Eigen::Vector3d x0 = Eigen::Vector3d(1, 0, 0);
Eigen::Vector3d y0 = Eigen::Vector3d(0, 1, 0);
Eigen::Vector3d z0 = Eigen::Vector3d(0, 0, 1);
Eigen::Vector3d x1 = rot(z0, ecef_pose(2)) * x0;
Eigen::Vector3d y1 = rot(z0, ecef_pose(2)) * y0;
Eigen::Vector3d z1 = rot(z0, ecef_pose(2)) * z0;
Eigen::Vector3d x2 = rot(y1, ecef_pose(1)) * x1;
Eigen::Vector3d y2 = rot(y1, ecef_pose(1)) * y1;
Eigen::Vector3d z2 = rot(y1, ecef_pose(1)) * z1;
Eigen::Vector3d x3 = rot(x2, ecef_pose(0)) * x2;
Eigen::Vector3d y3 = rot(x2, ecef_pose(0)) * y2;
Eigen::Vector3d zero = ecef_init.to_vector();
x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
double psi = atan2(x3.dot(y0), x3.dot(x0));
double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
y2 = rot(z0, psi) * y0;
z2 = rot(y2, theta) * z0;
double phi = atan2(y3.dot(z2), y3.dot(y2));
return {phi, theta, psi};
}
@@ -0,0 +1,17 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include "sunnypilot/common/transformations/coordinates.hpp"
Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat);
Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler);
Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat);
Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat);
Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot);
Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler);
Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot);
Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw);
Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle);
Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose);
Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose);
+1 -1
View File
@@ -139,7 +139,7 @@ class ModularAssistiveDrivingSystem:
self.transition_paused_state()
if self.steering_mode_on_brake == MadsSteeringModeOnBrake.PAUSE:
if CS.brakePressed or CS.regenBraking or self.pedal_pressed_non_gas_pressed(CS):
if self.pedal_pressed_non_gas_pressed(CS):
self.transition_paused_state()
self.events.remove(EventName.preEnableStandstill)
@@ -107,22 +107,6 @@ class TestShouldSilentLkasEnable(OpenpilotTestCase):
# pause
class TestPauseMode(OpenpilotTestCase):
def test_pauses_at_standstill_brake_already_held(self, mocker):
mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
mads.state_machine.state = State.enabled
mads.enabled = True
mads.active = True
sd.CS_prev = make_car_state(brake_pressed=True, standstill=True)
sd.sm['pandaStates'] = [make_panda_state(mocker, False)]
cs = make_car_state(brake_pressed=True, standstill=True)
run_frames(mads, sd, cs, n=199)
mads.update(cs)
assert not sd.events_sp.has(EventNameSP.controlsMismatchLateral)
assert mads.state_machine.state == State.paused
assert mads.lateral_mismatch_counter == 0
def test_stays_paused_at_standstill_brake_held(self, mocker):
mads, sd = make_mads(mocker, MadsSteeringModeOnBrake.PAUSE)
mads.state_machine.state = State.enabled
@@ -19,12 +19,12 @@ class BaseMapData(ABC):
def __init__(self):
self.params = Params()
self.sm = messaging.SubMaster(['gpsLocationSP'])
self.sm = messaging.SubMaster(['liveLocationKalman'])
self.pm = messaging.PubMaster(['liveMapDataSP'])
self.localizer_valid = False
self.last_bearing = None
self.last_position = coordinate_from_param("LastGPSPositionFused", self.params)
self.last_position = coordinate_from_param("LastGPSPositionLLK", self.params)
@abstractmethod
def update_location(self) -> None:
@@ -47,7 +47,7 @@ class BaseMapData(ABC):
next_speed_limit, next_speed_limit_distance = self.get_next_speed_limit_and_distance()
mapd_sp_send = messaging.new_message('liveMapDataSP')
mapd_sp_send.valid = self.sm['gpsLocationSP'].gpsOK
mapd_sp_send.valid = self.sm['liveLocationKalman'].gpsOK
live_map_data = mapd_sp_send.liveMapDataSP
live_map_data.speedLimitValid = bool(MAX_SPEED_LIMIT > speed_limit > 0)
@@ -5,8 +5,10 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import platform
from openpilot.cereal import log
from openpilot.common.params import Params
from openpilot.sunnypilot.mapd.live_map_data.base_map_data import BaseMapData
from openpilot.sunnypilot.navd.helpers import Coordinate
@@ -18,12 +20,12 @@ class OsmMapData(BaseMapData):
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else self.params
def update_location(self) -> None:
location = self.sm['gpsLocationSP']
self.localizer_valid = self.sm.valid['gpsLocationSP'] and location.hasFix
location = self.sm['liveLocationKalman']
self.localizer_valid = (location.status == log.LiveLocationKalman.Status.valid) and location.positionGeodetic.valid
if self.localizer_valid:
self.last_bearing = location.bearingDeg
self.last_position = Coordinate(location.latitude, location.longitude)
self.last_bearing = math.degrees(location.calibratedOrientationNED.value[2])
self.last_position = Coordinate(location.positionGeodetic.value[0], location.positionGeodetic.value[1])
if self.last_position is None:
return
@@ -5,9 +5,11 @@ This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import platform
import openpilot.cereal.messaging as messaging
from openpilot.cereal import log
from openpilot.common.params import Params
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.mapd.live_map_data.osm_map_data import OsmMapData
@@ -18,26 +20,28 @@ class TestOsmMapData(OpenpilotTestCase):
LATITUDE = 32.875134
LONGITUDE = -117.209777
ALTITUDE = 42.0
BEARING = 90.0
def setup_method(self):
self.mem_params = Params("/dev/shm/params") if platform.system() != "Darwin" else Params()
self.mem_params.remove("LastGPSPosition")
self.map_data = OsmMapData()
self.pm = messaging.PubMaster(['gpsLocationSP'])
self.pm = messaging.PubMaster(['liveLocationKalman'])
def location_message(self, has_fix: bool):
msg = messaging.new_message('gpsLocationSP', valid=has_fix)
location = msg.gpsLocationSP
location.hasFix = has_fix
location.latitude, location.longitude, location.bearingDeg = self.LATITUDE, self.LONGITUDE, self.BEARING
def location_message(self, status: log.LiveLocationKalman.Status, position_valid: bool):
msg = messaging.new_message('liveLocationKalman', valid=True)
location = msg.liveLocationKalman
location.status = status
location.positionGeodetic = {'value': [self.LATITUDE, self.LONGITUDE, self.ALTITUDE], 'valid': position_valid}
location.calibratedOrientationNED = {'value': [0.0, 0.0, math.radians(self.BEARING)], 'valid': True}
return msg
def send_location(self, has_fix: bool) -> None:
def send_location(self, status: log.LiveLocationKalman.Status, position_valid: bool) -> None:
# the first exchange synchronizes the PUB/SUB connection, the second one is actually received
self.pm.send('gpsLocationSP', self.location_message(has_fix))
self.pm.send('liveLocationKalman', self.location_message(status, position_valid))
self.map_data.sm.update(1000)
self.pm.send('gpsLocationSP', self.location_message(has_fix))
self.pm.send('liveLocationKalman', self.location_message(status, position_valid))
self.map_data.sm.update(1000)
def read_position(self) -> dict:
@@ -46,7 +50,7 @@ class TestOsmMapData(OpenpilotTestCase):
return json.loads(value)
def test_writes_position_for_mapd(self):
self.send_location(True)
self.send_location(log.LiveLocationKalman.Status.valid, True)
self.map_data.tick()
position = self.read_position()
@@ -55,10 +59,10 @@ class TestOsmMapData(OpenpilotTestCase):
self.assertAlmostEqual(position['bearing'], self.BEARING, delta=1e-6)
def test_invalid_localizer_keeps_last_known_position(self):
self.send_location(True)
self.send_location(log.LiveLocationKalman.Status.valid, True)
self.map_data.tick()
self.send_location(False)
self.send_location(log.LiveLocationKalman.Status.uninitialized, False)
self.map_data.tick()
position = self.read_position()
+28 -5
View File
@@ -8,10 +8,10 @@ import numpy as np
from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import DT_MDL
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
LANE_WIDTH_DEFAULT = 3.6 # m
LANE_WIDTH_CLIP = (2.8, 4.5) # m
LANE_WIDTH_CLIP = (2.4, 4.5) # m
LOOKAHEAD_TIME = 1.5 # s
MIN_LOOKAHEAD = 10.0 # m
MIN_SPEED = 5.0 # m/s
@@ -19,6 +19,11 @@ LINE_STD = (0.2, 0.6)
POINT_STD = 0.1
YIELD = (0.2, 0.45)
X_IDXS = np.array(ModelConstants.X_IDXS)
JUMP = 0.5
JUMP_TAU = 0.5
JUMP_ACCEPT = 3.0
WIDEN = 0.4
YAW_RATE = Plan.ORIENTATION_RATE.start + 2 # matched .991 correlation to actual car
class Blendful:
@@ -36,7 +41,8 @@ class Blendful:
self.l_prob = 0.0
self.r_prob = 0.0
self.model_curvature = 0.0
self.smooth = 0.0 # smooths the correction target below .53 lat accel
self.ref = None
self.jumped = np.zeros(2)
self.curvature = 0.0
def update(self, model_output: dict[str, np.ndarray], v_ego: float, active: bool, model_curvature: float, enabled: bool = True) -> float:
@@ -52,8 +58,8 @@ class Blendful:
self.usual_offset.initialized = False
self.lane_width.initialized = False
self.going_around = False
self.ref = None
target = 0.0 if fading else self._target(model_output, v_ego, model_curvature)
target = self.smooth = self.smooth + (target - self.smooth) * DT_MDL / (DT_MDL + np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [0.5, 0.0]))
build = 0.5 / max(v_ego, 1.0) ** 2 * DT_MDL # jerk over 1 second
release = (0.5 if fading else 2.0) / max(v_ego, 1.0) ** 2 * DT_MDL # lets go fast, but past zero it builds like any other pull
k = self.curvature
@@ -63,6 +69,10 @@ class Blendful:
def _target(self, model_output: dict[str, np.ndarray], v_ego: float, model_curvature: float) -> float:
lookahead = max(v_ego * LOOKAHEAD_TIME, MIN_LOOKAHEAD)
lines = model_output['lane_lines'][0, :, :, 0]
plan = model_output['plan'][0]
self._jumps(lines[1:3], v_ego, plan[0, YAW_RATE] if plan.shape[-1] > YAW_RATE else model_curvature * v_ego,
np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [JUMP_TAU, 0.0]), lookahead)
held = bool(np.any((self.jumped > 0) & (self.jumped < JUMP_ACCEPT)))
stds = model_output['lane_lines_stds'][0, :, :, 0]
lone_weight = np.interp(abs(model_curvature) * v_ego ** 2, [0.18, 0.53], [0.5, 1.0]) # 0.5 on straights, 1.0 in curves
@@ -83,6 +93,8 @@ class Blendful:
lane_trust = min(l_prob, r_prob)
else:
lane_trust = l_prob + r_prob - l_prob * r_prob
if held or (l_prob * r_prob > 0.5 and np.interp(lookahead, X_IDXS, lines[2] - lines[1]) > self.lane_width.x + WIDEN):
lane_trust = 0.0
if lone_weight > 0.5 and lane_trust < self.line_trust:
lane_trust = self.line_trust + (lane_trust - self.line_trust) * DT_MDL / (lone_weight - 0.5 + DT_MDL)
self.line_trust = lane_trust
@@ -96,7 +108,7 @@ class Blendful:
win = (X_IDXS >= 0.5 * lookahead) & (X_IDXS <= 1.5 * lookahead)
x = X_IDXS[win]
half_width = self.lane_width.x / 2
if l_prob * r_prob > 0.5:
if l_prob * r_prob > 0.5 and not held:
width = np.interp(lookahead, X_IDXS, lines[2]) - np.interp(lookahead, X_IDXS, lines[1])
self.lane_width.update(float(np.clip(width, *LANE_WIDTH_CLIP)))
half_width = np.clip(lines[2, win] - lines[1, win], *LANE_WIDTH_CLIP) / 2
@@ -143,3 +155,14 @@ class Blendful:
self.trust = lane_trust if lane_trust < self.trust else self.trust + (lane_trust - self.trust) * DT_MDL / (1.5 + DT_MDL)
self.dk = dk
return float(self.trust * dk)
def _jumps(self, raw: np.ndarray, v_ego: float, yaw_rate: float, tau: float, lookahead: float) -> None:
if self.ref is None:
self.ref, self.jumped = raw.copy(), np.zeros(2)
return
ref = np.array([np.interp(X_IDXS + v_ego * DT_MDL, X_IDXS, line) for line in self.ref]) - yaw_rate * DT_MDL * X_IDXS
off = np.abs([np.interp(lookahead, X_IDXS, d) for d in raw - ref]) > JUMP
self.jumped = np.where(off & ~off[::-1], self.jumped + DT_MDL, 0.0)
self.ref = ref + (raw - ref) * DT_MDL / (DT_MDL + tau)
accept = (self.jumped >= JUMP_ACCEPT) | off.all()
self.ref[accept] = raw[accept]
@@ -371,7 +371,7 @@ def _compile_warp_resolution_worker(cam_w, cam_h, model_w, model_h, benchmark_ru
if __name__ == "__main__":
if 'USB' in os.getenv('DEV', '') or os.getenv('CHESTNUT'):
if ('USB' in os.getenv('DEV', '') or os.getenv('CHESTNUT')) and not os.getenv('DEV', '').startswith('MOCK'):
from openpilot.system.hardware.chestnut.flash import link_up
for _ in range(10):
if link_up():
+10 -7
View File
@@ -17,6 +17,7 @@ def main():
parser.add_argument("--frame", required=True, help="width,height,stride,y_height,uv_height,buffer_size")
parser.add_argument("--warp-to", required=True, help="model input WxH")
parser.add_argument("--output", required=True)
parser.add_argument("--light", action="store_true")
parser.add_argument("--split-frames", action="store_true", help="one input per camera (frame, big_frame), bound to the camera buffers by pointer")
args = parser.parse_args()
@@ -24,18 +25,20 @@ def main():
from tinygrad import Device, Tensor, TinyJit
frame = parse_frame(args.frame)
warp = make_frame_prepare(frame, *parse_size(args.warp_to))
n = 1 if args.light else 2
cams = list(range(n))
rng = np.random.default_rng(42)
frames = rng.integers(0, 256, (2, frame.size), dtype=np.uint8)
m_inv = Tensor((rng.standard_normal((2, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()
m_inv = Tensor((rng.standard_normal((n, 3, 3)) * 8).astype(np.float32), device=Device.DEFAULT).realize()
if args.split_frames:
run = TinyJit(lambda frame, big_frame, M_inv: Tensor.stack(warp(frame, M_inv[0]), warp(big_frame, M_inv[1])), prune=True)
inputs = {"frame": Tensor(frames[0], device=Device.DEFAULT).realize(), "big_frame": Tensor(frames[1], device=Device.DEFAULT).realize(), "M_inv": m_inv}
specs = {"frame": ((frame.size,), "|u1", Device.DEFAULT), "big_frame": ((frame.size,), "|u1", Device.DEFAULT),
"M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
names = [("frame", "big_frame")[c] for c in cams]
run = TinyJit(lambda M_inv, **f: Tensor.stack(*(warp(f[nm], M_inv[i]) for i, nm in enumerate(names))), prune=True)
inputs = {nm: Tensor(frames[c], device=Device.DEFAULT).realize() for nm, c in zip(names, cams, strict=True)} | {"M_inv": m_inv}
specs = {nm: ((frame.size,), "|u1", Device.DEFAULT) for nm in names} | {"M_inv": ((n, 3, 3), "<f4", Device.DEFAULT)} # noqa: C420
else:
run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[i], M_inv[i]) for i in range(2))), prune=True)
run = TinyJit(lambda input_frame, M_inv: Tensor.stack(*(warp(input_frame[c], M_inv[i]) for i, c in enumerate(cams))), prune=True)
inputs = {"input_frame": Tensor(frames, device=Device.DEFAULT).realize(), "M_inv": m_inv}
specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((2, 3, 3), "<f4", Device.DEFAULT)}
specs = {"input_frame": ((2, frame.size), "|u1", Device.DEFAULT), "M_inv": ((n, 3, 3), "<f4", Device.DEFAULT)}
with record_kernel_linear() as recorded:
run(**inputs)
run(**inputs)
@@ -10,6 +10,7 @@ import dataclasses
import inspect
import itertools
import mmap
import os
import re
import struct
import tempfile
@@ -194,7 +195,8 @@ def convert(jit, kwargs: dict, frame: dict, target) -> tuple[dict, memoryview]:
base, at, nbytes = _address(argument_words[slot])
args.append({"buffer": buffers.setdefault(base, len(buffers)), "offset": at, "nbytes": nbytes or base.max_numel() * base.dtype.itemsize, "dtype": u8})
outs, ins = kernel[6]
calls.append({"kind": "program", "name": re.sub(r"\x1b\[[0-9;]*m", "", kernel[1]), "lib": programs[image_alloc], "target": dataclasses.asdict(target),
device_target = dataclasses.asdict(dataclasses.replace(target, interface=target.interface.removeprefix("MOCK")))
calls.append({"kind": "program", "name": re.sub(r"\x1b\[[0-9;]*m", "", kernel[1]), "lib": programs[image_alloc], "target": device_target,
"signature": [[f"data{i}", i, u8, [a.get("nbytes", 1)]] for i, a in enumerate(args)], "global_size": grid, "local_size": local,
"globals": list(range(len(args))), "outs": list(outs), "ins": list(ins), "args": args})
needs_init |= {args[i]["buffer"] for i in ins if "buffer" in args[i] and args[i]["buffer"] not in written}
@@ -235,14 +237,16 @@ def main():
frame, _ = capture_frame(artifact["run"], kwargs, "output_buffers")
part = convert(artifact["run"], kwargs, frame, Device[Device.DEFAULT].renderer.target)
plain = PlainJit(*part) # replays the frame, within tolerance
replay = {path[0]: Tensor(array, device=device).realize() for path, array, device in frame["inputs"] if len(path) == 1}
replay["output_buffers"] = {path[1]: Tensor(array, device=device).realize() for path, array, device in frame["inputs"] if len(path) == 2}
flatten(plain(**replay))
for index, (got, expected) in enumerate(zip([t.numpy() for t in replay["output_buffers"].values()], frame["outputs"], strict=True)):
assert np.array_equal(got, expected), f"output {index} of the plain artifact differs from the pickle"
if not os.getenv("MOCKUSB_SOCKET"):
plain = PlainJit(*part)
replay = {path[0]: Tensor(array, device=device).realize() for path, array, device in frame["inputs"] if len(path) == 1}
replay["output_buffers"] = {path[1]: Tensor(array, device=device).realize() for path, array, device in frame["inputs"] if len(path) == 2}
flatten(plain(**replay))
for index, (got, expected) in enumerate(zip([t.numpy() for t in replay["output_buffers"].values()], frame["outputs"], strict=True)):
assert np.array_equal(got, expected), f"output {index} of the plain artifact differs from the pickle"
write_bundle(args.output, {"run": part}, {key: artifact[key] for key in ("metadata", "input_specs", "output_specs")})
print(f"converted {len(part[0]['calls'])} kernels, bit-exact with the pickle: {args.output}")
checked = "left to mock_parity" if os.getenv("MOCKUSB_SOCKET") else "bit-exact with the pickle"
print(f"converted {len(part[0]['calls'])} kernels, {checked}: {args.output}")
if __name__ == "__main__":
+38 -15
View File
@@ -12,8 +12,6 @@ from functools import partial
from openpilot.common.basedir import BASEDIR
from openpilot.sunnypilot.modeld_v2.helpers import _dtype, read_parts
SPLIT_FRAMES = ("big_frame", "frame")
def pack_layout(shapes: dict[str, tuple], frame_size: int) -> tuple[dict[str, int], int]:
offsets, offset = {}, 0
@@ -21,6 +19,13 @@ def pack_layout(shapes: dict[str, tuple], frame_size: int) -> tuple[dict[str, in
offsets[name], offset = offset, offset + -(-math.prod(shape) * 4 // 128) * 128
return offsets | {"frames": offset}, offset + 2 * frame_size
EXTRA_WARPS = {"light_img": "light_tfm"}
SPLIT_FRAMES = ("big_frame", "frame")
def host_shapes(input_shapes: dict[str, tuple], states) -> dict[str, tuple]:
shapes = {name: shape for name, shape in input_shapes.items() if name not in states and name != "new_img" and name not in EXTRA_WARPS}
return shapes | {tfm: (1, 3, 3) for img, tfm in EXTRA_WARPS.items() if img in input_shapes}
class _Blobs: # the model's and the warp's blobs as one address space, for a part whose ranges span both files
def __init__(self, *blobs):
@@ -32,31 +37,46 @@ class _Blobs: # the model's and the warp's blobs as one address space, for a pa
def fused_with_warp(cam_w: int, cam_h: int, chestnut: bool): # load_model's extend: a native model runs upload, warp and model as one submit
return partial(frame_part, f"{BASEDIR}/openpilot/sunnypilot/modeld_v2/models/{'big_' if chestnut else ''}driving_warp_{cam_w}x{cam_h}_tinygrad.pkl")
models, big = f"{BASEDIR}/openpilot/sunnypilot/modeld_v2/models", 'big_' if chestnut else ''
return partial(frame_part, f"{models}/{big}driving_warp_{cam_w}x{cam_h}_tinygrad.pkl",
light_warp_path=f"{models}/{big}driving_lightwarp_{cam_w}x{cam_h}_tinygrad.pkl")
def frame_part(warp_path: str, parts: dict, extras: dict) -> dict:
def frame_part(warp_path: str, parts: dict, extras: dict, light_warp_path: str | None = None) -> dict:
if "input_specs" not in extras: # a legacy model warps in its own parts
return {}
(model, model_blob), (warp, warp_blob) = parts["run"], read_parts(warp_path)[0]["run"]
states = sorted(name for name in extras["input_specs"] if f"next_{name}" in extras["metadata"]["output_shapes"])
host = {name: shape for name, (shape, _, _) in extras["input_specs"].items() if name not in states and name != "new_img"}
frame_specs = {s["name"]: s for s in warp["inputs"] if s["name"] in SPLIT_FRAMES}
offsets, packed = pack_layout(host, 0 if frame_specs else next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2)
shift, upload, u8 = len(model_blob), len(model["buffers"]) + len(warp["buffers"]), repr(_dtype("dtypes.uint8"))
host = host_shapes({name: shape for name, (shape, _, _) in extras["input_specs"].items()}, states)
paths = {"light_img": light_warp_path}
extra = [(img, tfm, *read_parts(paths[img])[0]["run"]) for img, tfm in EXTRA_WARPS.items() if img in extras["input_specs"]]
split = any(s["name"] in SPLIT_FRAMES for s in warp["inputs"])
assert all(any(s["name"] in SPLIT_FRAMES for s in part["inputs"]) == split for _, _, part, _ in extra), "camera and extra warps must share a frame layout"
offsets, packed = pack_layout(host, 0 if split else next(s["numel"] for s in warp["inputs"] if s["name"] == "input_frame") // 2)
shift = len(model_blob)
buf_base, blob_base = [len(model["buffers"]) + len(warp["buffers"])], [len(model_blob) + len(warp_blob)]
for _, _, part, blob in extra:
buf_base.append(buf_base[-1] + len(part["buffers"])); blob_base.append(blob_base[-1] + len(blob))
upload, u8 = buf_base[-1], repr(_dtype("dtypes.uint8"))
extra_at = {img: (part, buf_base[i], blob_base[i], tfm) for i, (img, tfm, part, _) in enumerate(extra)}
def view(name: str, nbytes: int, dtype: str) -> dict:
return {"buffer": upload, "offset": offsets[name], "nbytes": nbytes, "dtype": dtype}
outputs = sorted(s["slot"] for s in model["inputs"] if s["name"] is None)
frame_specs = {s["name"]: s for _, _, part, _ in [(None, None, warp, None), *extra] for s in part["inputs"] if s["name"] in SPLIT_FRAMES}
names = sorted(["host", *states, *frame_specs])
slots = {s["slot"]: names.index(s["name"]) for s in model["inputs"] if s["name"] in states} | \
{slot: len(names) + i for i, slot in enumerate(outputs)}
returned = warp["returns"]["tensors"][0]
def warped(part: dict, base: int) -> dict:
returned = part["returns"]["tensors"][0]
return {k: v for k, v in returned.items() if k != "shape"} | {"buffer": returned["buffer"] + base}
model_inputs = {s["slot"]: {"input": slots[s["slot"]]} if s["slot"] in slots else
{k: v for k, v in returned.items() if k != "shape"} | {"buffer": returned["buffer"] + len(model["buffers"])} if s["name"] == "new_img" else
warped(warp, len(model["buffers"])) if s["name"] == "new_img" else
warped(extra_at[s["name"]][0], extra_at[s["name"]][1]) if s["name"] in extra_at else
view(s["name"], s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in model["inputs"]}
warp_inputs = {s["slot"]: {"input": names.index(s["name"])} if s["name"] in frame_specs else
view("frames" if s["name"] == "input_frame" else "tfm", s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in warp["inputs"]}
def warp_inputs(part: dict, tfm: str) -> dict:
return {s["slot"]: {"input": names.index(s["name"])} if s["name"] in frame_specs else
view("frames" if s["name"] == "input_frame" else tfm, s["numel"] * _dtype(s["dtype"]).itemsize, s["dtype"]) for s in part["inputs"]}
def remap(calls: list, inputs: dict, buffers: int, lib: int) -> list:
return [call | ({"lib": [call["lib"][0] + lib, call["lib"][1] + lib]} if "lib" in call else {}) |
@@ -65,8 +85,11 @@ def frame_part(warp_path: str, parts: dict, extras: dict) -> dict:
"inputs": [{"slot": names.index("host"), "name": "host", "dtype": u8, "numel": packed, "device": "NPY"}] +
[s | {"slot": slots[s["slot"]]} for s in model["inputs"] if s["slot"] in slots] +
[s | {"slot": names.index(name)} for name, s in frame_specs.items()],
"buffers": model["buffers"] + [b | ({"init": [b["init"][0] + shift, b["init"][1] + shift]} if "init" in b else {}) for b in warp["buffers"]] +
"buffers": model["buffers"] + [b | ({"init": [b["init"][0] + d, b["init"][1] + d]} if "init" in b else {})
for part, d in ((warp, shift), *((p, bb) for p, _, bb, _ in extra_at.values())) for b in part["buffers"]] +
[{"nbytes": packed, "device": model["inputs"][0]["device"], "dtype": u8}],
"calls": [{"kind": "copy", "args": [{"buffer": upload, "offset": 0, "nbytes": packed, "dtype": u8}, {"input": names.index("host")}]}] +
remap(warp["calls"], warp_inputs, len(model["buffers"]), shift) + remap(model["calls"], model_inputs, 0, 0)}
return {"run": (header, _Blobs(model_blob, warp_blob))} # replaces the model's run: one set of buffers, no separate warp
remap(warp["calls"], warp_inputs(warp, "tfm"), len(model["buffers"]), shift) +
[c for p, b0, bb, tfm in extra_at.values() for c in remap(p["calls"], warp_inputs(p, tfm), b0, bb)] +
remap(model["calls"], model_inputs, 0, 0)}
return {"run": (header, _Blobs(model_blob, warp_blob, *(blob for *_, blob in extra)))}
@@ -0,0 +1,22 @@
"""
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import numpy as np
from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame
from openpilot.common.transformations.orientation import rot_from_euler
LIGHT_INPUT_SIZE = (512, 256)
light_intrinsics = np.array([
[1000.0, 0.0, 256.0],
[0.0, 1000.0, 200.0],
[0.0, 0.0, 1.0]])
calib_from_light = np.linalg.inv((light_intrinsics @ get_view_frame_from_calib_frame(0, 0, 0, 0))[:, :3])
def get_light_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray) -> np.ndarray:
camera_from_calib = intrinsics @ view_frame_from_device_frame @ rot_from_euler(device_from_calib_euler)
return (camera_from_calib @ calib_from_light).astype(np.float32)
@@ -0,0 +1,153 @@
import json
import os
import shutil
import struct
import sys
import tempfile
import time
os.environ.setdefault("DEV", "MOCKUSB+AMD")
QCOM = "QCOM" in os.environ["DEV"]
ON_MOCK = os.environ["DEV"].startswith("MOCK")
ARCH, HARDWARE = ("a630", "comma 3x/4 (a630)") if QCOM else ("gfx1200", "chestnut")
os.environ.setdefault("CHECK_OOB", "0")
os.environ.setdefault("GMMU", "0")
if sys.platform == "darwin" and os.environ["DEV"].startswith("MOCKUSB"): # the mock's VRAM is a memfd, which macOS lacks: an unlinked file is the same
from tinygrad.runtime.autogen import libc
def _memfd_create(name, flags):
fd, path = tempfile.mkstemp()
os.unlink(path)
return fd
libc.memfd_create = _memfd_create
import numpy as np
from openpilot.sunnypilot.modeld_v2.helpers import ALIGNMENT, MAGIC, PlainJit, _device, read_parts
class ParityError(RuntimeError):
pass
def _emulator():
try:
if QCOM:
from test.mockgpu.qcom import emu
else:
from test.mockgpu.amd import emu
except ImportError as e:
raise ParityError(f"tinygrad's {'qcom' if QCOM else 'amd'} mock is not importable: put tinygrad_repo on PYTHONPATH") from e
if not hasattr(emu, "hw_unverified"):
raise ParityError(f"this tinygrad's emulator has no {ARCH} rounding model, its check frames would not match a {HARDWARE}")
return emu
def unverified(header: dict, blob: memoryview) -> dict[str, int]:
emu, out = _emulator(), {}
for call in header["calls"]:
if call["kind"] != "program":
continue
if (arch := call["target"].get("arch", "")).split(",")[0] != ARCH: # IMAGE builds append ",IMAGE_PITCH_ALIGNMENT=64"
out[f"arch={arch}"] = out.get(f"arch={arch}", 0) + 1
continue
for sig, n in emu.hw_unverified(bytes(blob[slice(*call["lib"])])).items():
out[sig] = out.get(sig, 0) + n
return out
def _array(blob: memoryview, stored: dict) -> np.ndarray:
return np.frombuffer(blob[slice(*stored["data"])], dtype=np.dtype(stored["dtype"])).reshape(stored["shape"]).copy()
def replay(header: dict, blob: memoryview, shared: dict) -> list[np.ndarray]:
from tinygrad import Tensor
check = header["check"]
jit = PlainJit(header, blob, None, verify=False, shared=shared)
kwargs: dict = {}
for path, stored, device in check["inputs"]:
tensor = Tensor(_array(blob, stored), device=_device(device, None)).realize()
kwargs[path[0]] = tensor if len(path) == 1 else {**kwargs.get(path[0], {}), path[1]: tensor}
result = jit(**kwargs)
outputs = kwargs[check["output_container"]].values() if check["output_container"] else result
return [t.numpy() for t in (outputs if isinstance(outputs, (list, tuple)) or check["output_container"] else [outputs])]
def _parts_with_offsets(path: str) -> tuple[dict, dict]:
parts, _ = read_parts(path)
with open(path, "rb") as f:
head = f.read(len(MAGIC) + 8)
start = len(MAGIC) + 8 + struct.unpack_from("<Q", head, len(MAGIC))[0]
f.seek(len(MAGIC) + 8)
entries = json.loads(f.read(start - len(MAGIC) - 8))["parts"]
base = start + -start % ALIGNMENT
offsets = {tuple(e["key"]) if isinstance(e["key"], list) else e["key"]: base + e["blob"][0] for e in entries}
return parts, offsets
def _gate(parts: dict) -> None:
missing: dict[str, int] = {}
for header, blob in parts.values():
if "check" not in header:
raise ParityError("artifact has no check frame")
for sig, n in unverified(header, blob).items():
missing[sig] = missing.get(sig, 0) + n
if missing:
raise ParityError(f"kernels use instructions the emulator is not verified against a {HARDWARE} for: " +
", ".join(f"{s} ({n}x)" for s, n in sorted(missing.items(), key=lambda kv: -kv[1])))
def check(path: str) -> None:
parts, _ = read_parts(path)
if ON_MOCK:
_gate(parts)
shared: dict = {}
for key, (header, blob) in parts.items():
got = replay(header, blob, shared)
for i, (g, stored) in enumerate(zip(got, header["check"]["outputs"], strict=True)):
want = _array(blob, stored)
if g.shape != want.shape or not np.array_equal(g.view(np.uint8), want.view(np.uint8)):
n = int(np.count_nonzero(g != want)) if g.shape == want.shape else -1
where = "mock" if ON_MOCK else "device"
raise ParityError(f"part {key} output {i}: the {where}'s check frame differs from the stored one in {n} of {want.size} values")
def record(path: str, out: str) -> None:
parts, offsets = _parts_with_offsets(path)
_gate(parts)
shared, writes = {}, []
for key, (header, blob) in parts.items():
for g, stored in zip(replay(header, blob, shared), header["check"]["outputs"], strict=True):
want = np.dtype(stored["dtype"])
if tuple(g.shape) != tuple(stored["shape"]) or g.dtype != want:
raise ParityError(f"part {key}: replayed output {g.dtype}{g.shape} does not fit the stored {want}{tuple(stored['shape'])}")
writes.append((offsets[key] + stored["data"][0], np.ascontiguousarray(g).tobytes()))
shutil.copyfile(path, out)
with open(out, "r+b") as f:
for offset, data in writes:
f.seek(offset)
f.write(data)
def main(argv: list[str]) -> int:
if len(argv) < 2 or argv[0] not in ("check", "record") or (argv[0] == "record" and len(argv) != 3):
print("usage: mock_parity check ARTIFACT... | record ARTIFACT OUT", file=sys.stderr)
return 2
try:
if argv[0] == "record":
st = time.perf_counter()
record(argv[1], argv[2])
print(f"{argv[2]}: check frame recorded on the mock in {time.perf_counter() - st:.0f}s")
return 0
for path in argv[1:]:
st = time.perf_counter()
check(path)
print(f"{os.path.basename(path)}: bit-identical to its {HARDWARE} check frame on the {'mock' if ON_MOCK else 'device'} ({time.perf_counter() - st:.0f}s)")
return 0
except ParityError as e:
print(f"PARITY FAIL: {e}", file=sys.stderr)
return 1
if __name__ == "__main__":
sys.exit(main(sys.argv[1:]))
@@ -11,7 +11,7 @@ import pickle
import numpy as np
from openpilot.common.basedir import BASEDIR
from openpilot.sunnypilot.modeld_v2.frame_fusion import pack_layout, SPLIT_FRAMES
from openpilot.sunnypilot.modeld_v2.frame_fusion import host_shapes, pack_layout, SPLIT_FRAMES
from openpilot.sunnypilot.modeld_v2.helpers import load_model
from openpilot.sunnypilot.modeld_v2.compile_modeld import (POLICY_INPUTS, derive_frame_skip,
make_split_input_queues, make_supercombo_input_queues)
@@ -174,7 +174,7 @@ class NativeTinygradAdapter(BaseModelAdapter):
self.input_queues = {name: Tensor(np.zeros(shape, dtype=dtype), device=self.model_device).realize()
for name, (shape, dtype) in self.input_shapes.items() if name in self.state_pairs}
shapes = {name: shape for name, (shape, _) in self.input_shapes.items() if name not in self.state_pairs and name != 'new_img'}
shapes = host_shapes({name: shape for name, (shape, _) in self.input_shapes.items()}, self.state_pairs)
offsets, packed = pack_layout(shapes, 0 if self.split else self.warp_frame_size)
self.packed_input = np.zeros(packed, dtype=np.uint8)
self.host = Tensor(self.packed_input, device='NPY')
+16 -2
View File
@@ -11,6 +11,7 @@ from functools import partial
import gc
import os
os.environ['GMMU'] = '0'
os.environ['QCOM_PRIORITY'] = '1'
from tinygrad.device import Device
import numpy as np
import threading
@@ -44,6 +45,7 @@ from openpilot.sunnypilot.modeld_v2.blendful import Blendful
from openpilot.sunnypilot.modeld_v2.constants import ModelConstants, Plan
from openpilot.sunnypilot.modeld_v2.meta_helper import load_meta_constants
from openpilot.sunnypilot.modeld_v2.camera_offset_helper import CameraOffsetHelper
from openpilot.sunnypilot.modeld_v2.light_frame import get_light_warp_matrix
from openpilot.sunnypilot.livedelay.helpers import get_lat_delay
from openpilot.sunnypilot.modeld_v2.modeld_base import ModelStateBase
from openpilot.sunnypilot.modeld_v2.helpers import load_model
@@ -171,7 +173,7 @@ class ModelState(ModelStateBase):
np.where(inputs[desire_key] - self.prev_desire > .99, inputs[desire_key], 0)
self.prev_desire[:] = inputs[desire_key]
for key in ('traffic_convention', 'lateral_control_params', 'action_t'):
for key in ('traffic_convention', 'lateral_control_params', 'action_t', 'speed_limit'):
if key in self.numpy_inputs and key in inputs:
self.numpy_inputs[key][:] = inputs[key]
@@ -179,6 +181,8 @@ class ModelState(ModelStateBase):
for i, key in enumerate(self._vision_input_names):
if key in transforms:
self.numpy_inputs['tfm'][i] = transforms[key].reshape(3, 3)
if 'light_tfm' in self.numpy_inputs and 'light' in transforms:
self.numpy_inputs['light_tfm'][0] = transforms['light'].reshape(3, 3)
else:
self.numpy_inputs['tfm'][:, :] = transforms[self._road_key].reshape(3, 3)
self.numpy_inputs['big_tfm'][:, :] = transforms[self._wide_key].reshape(3, 3)
@@ -338,7 +342,7 @@ def main(demo=False):
pub_socks = ["modelV2", "drivingModelData", "cameraOdometry", "modelDataV2SP"] + (["chestnutGpuState"] if CHESTNUT else [])
pm = PubMaster(pub_socks)
sm = SubMaster(["deviceState", "carState", "narrowRoadCameraState", "extrinsicsCalibration", "driverMonitoringState",
"carControl", "controlsState", "lateralDelay"])
"carControl", "controlsState", "lateralDelay", "longitudinalPlanSP"])
publish_state = PublishState()
chestnut_state = ChestnutGpuState(pm, model.chestnut) if CHESTNUT else None
@@ -351,6 +355,7 @@ def main(demo=False):
model_transform_main = np.zeros((3, 3), dtype=np.float32)
model_transform_extra = np.zeros((3, 3), dtype=np.float32)
model_transform_light = np.zeros((3, 3), dtype=np.float32)
live_calib_seen = False
buf_main, buf_extra = None, None
meta_main = FrameMeta()
@@ -369,6 +374,9 @@ def main(demo=False):
prev_action = log.ModelDataV2.Action()
DH = DesireHelper()
desire_frames = [q.shape[0] for m in (model, small_model) if m is not None for name, q in m.adapter.input_queues.items() if 'desire' in name]
if desire_frames:
DH.turn_repulse = min(desire_frames) - 2
meta_constants = load_meta_constants()
RELC = RoadEdgeLaneChangeController()
blendful = Blendful()
@@ -425,6 +433,7 @@ def main(demo=False):
model_transform_main = get_warp_matrix(device_from_calib_euler, main_intrinsics, False).astype(np.float32)
model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.wide_road.intrinsics, True).astype(np.float32)
model_transform_main, model_transform_extra = camera_offset_helper.update(model_transform_main, model_transform_extra, sm, main_wide_camera)
model_transform_light = get_light_warp_matrix(device_from_calib_euler, main_intrinsics)
live_calib_seen = True
traffic_convention = np.zeros(2)
@@ -446,6 +455,7 @@ def main(demo=False):
bufs = {name: buf_extra if 'big' in name else buf_main for name in model.vision_input_names}
transforms = {name: model_transform_extra if 'big' in name else model_transform_main for name in model.vision_input_names}
transforms['light'] = model_transform_light
frame_delay = DT_MDL # compensate for time passed since the frame was captured: current_time - timestamp_eof is 50ms on average
action_delay = DT_MDL / 2 # middle of the interval between model output (current state) and next frame (expected state)
@@ -463,6 +473,10 @@ def main(demo=False):
if 'action_t' in model.numpy_inputs:
inputs['action_t'] = np.array([lat_action_t, long_action_t], dtype=np.float32)
if 'speed_limit' in model.numpy_inputs:
resolver = sm['longitudinalPlanSP'].speedLimit.resolver
inputs['speed_limit'] = np.array([resolver.speedLimitFinal if resolver.speedLimitValid else 0.0], dtype=np.float32)
mt1 = time.perf_counter()
try:
send_chestnut = (chestnut_state is not None and
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:1baac6f02902df5689e9e275633c122a9955b7c57de2681e0137e1724c1f32dd
size 3463278
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:0478837491c584b2212e31a91c494a908b8fb048205700f05a02a13ee7fc9b90
size 7690094
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:580fadf61ded34b4b19f0ba1f5e4b61c0ea489ca34a0952ccf2878593e3f68c4
size 1851239
@@ -0,0 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:44700f3533abb3e65232c8aca46b48384a59218545ed6bb505c5d4762204359b
size 3964263
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:d776c72125156b5dd3eb59768409401e28c8a52a3a619cd3b9f02c10a156fda6
oid sha256:1f5dd8855de07d0e8b9ed836934e1dcbe77aa27c3b5c77eee1f21888f974ba8b
size 3699072
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:a3f13ed12e087817a86afd48b2496c66c4f61a1925065b676b2945e81fabaacd
oid sha256:226d40112a292077b7eca26b97273c56ab3a84feee8ddf9b6d02d46c2a7b8403
size 7924864
@@ -80,7 +80,8 @@ def serialize_jit(jit, kernel_linear, check: dict | None = None) -> tuple[dict,
if ast.op is Ops.PROGRAM:
elf, info = ast.to_elf(), ast.arg
global_size, local_size = info.launch_dims({})
calls.append({"kind": "program", "name": elf.name, "lib": blob.add(bytes(elf.lib)), "target": dataclasses.asdict(elf.target),
device_target = dataclasses.asdict(dataclasses.replace(elf.target, interface=elf.target.interface.removeprefix("MOCK")))
calls.append({"kind": "program", "name": elf.name, "lib": blob.add(bytes(elf.lib)), "target": device_target,
"signature": kernel_signature(ast, elf),
"global_size": [int(x) for x in global_size], "local_size": [int(x) for x in local_size],
"globals": list(info.globals), "outs": list(info.outs), "ins": list(info.ins), "args": arguments})
@@ -17,4 +17,4 @@ class TestRegistryModels(unittest.TestCase):
self.skipTest("MODEL_BASE_NAME env var not set, skipping integration test.")
bundle = load_model(os.path.join(os.environ.get("MODEL_DIR", "/tmp/models"), base_name), {"QCOM": "CPU", "AMD": "CPU"}, verify=False, cache=False)
jits = [value for value in bundle.values() if isinstance(value, PlainJit)]
assert "metadata" in bundle and jits and all(len(jit.captured.linear.src) > 0 for jit in jits)
assert "metadata" in bundle and jits and all(len(jit.captured._linear.src) > 0 for jit in jits)
+1 -1
View File
@@ -112,7 +112,7 @@ class ModelCache:
class ModelFetcher:
"""Handles fetching and caching of model data from remote source"""
MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v25.json"
MODEL_URL = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_v26.json"
MODEL_URL_CHESTNUT = "https://raw.githubusercontent.com/sunnypilot/sunnypilot-models/refs/heads/gh-pages/docs/driving_models_chestnut_v28.json"
MODEL_SOURCES = {
@@ -0,0 +1,3 @@
params_learner
paramsd
locationd
@@ -0,0 +1,30 @@
Import('env', 'arch', 'common', 'messaging', 'rednose', 'transformations')
loc_libs = [messaging, common, 'pthread', 'dl']
# build ekf models
rednose_gen_dir = 'models/generated'
rednose_gen_deps = [
"models/constants.py",
]
live_ekf = env.RednoseCompileFilter(
target='live',
filter_gen_script='models/live_kf.py',
output_dir=rednose_gen_dir,
extra_gen_artifacts=['live_kf_constants.h'],
gen_script_deps=rednose_gen_deps,
)
# locationd build
locationd_sources = ["locationd.cc", "models/live_kf.cc"]
lenv = env.Clone()
# ekf filter libraries need to be linked, even if no symbols are used
if arch != "Darwin":
lenv["LINKFLAGS"] += ["-Wl,--no-as-needed"]
lenv["LIBPATH"].append(Dir(rednose_gen_dir).abspath)
lenv["RPATH"].append(Dir(rednose_gen_dir).abspath)
locationd = lenv.Program("locationd", locationd_sources, LIBS=["live", "ekf_sym"] + loc_libs + transformations)
lenv.Depends(locationd, rednose)
lenv.Depends(locationd, live_ekf)
@@ -1,181 +0,0 @@
"""
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import json
import math
import time
from collections import deque
import numpy as np
import openpilot.cereal.messaging as messaging
from openpilot.common.gps import get_gps_location_service
from openpilot.common.params import Params
from openpilot.common.transformations.orientation import rot_from_euler
EARTH_RADIUS = 6378137.0
GPS_LATENCY = {'gpsLocation': 0.630, 'gpsLocationExternal': 0.095} # s, empirical fix latency per receiver
RESET_DISTANCE = 100.0 # m
MAX_REJECTS = 5
MAX_POS_STD = 50.0 # m
MIN_BEARING_SPEED = 5.0 # m/s
MAX_BEARING_ACCURACY = 10.0 # deg
GPS_TIMEOUT = 2.0 # s
SEED_INTERVAL = 60.0 # s
def wrap(a: float) -> float:
return (a + math.pi) % (2 * math.pi) - math.pi
class GpsFuser:
def __init__(self):
self.t: float | None = None
self.lat = self.lon = self.heading = math.nan
self.pos_var = self.heading_var = math.inf
self.history: deque = deque(maxlen=40)
self.rejects = 0
@property
def initialized(self) -> bool:
return math.isfinite(self.lat)
@property
def valid(self) -> bool:
return self.initialized and math.isfinite(self.heading) and self.pos_var < MAX_POS_STD ** 2
def _move(self, north: float, east: float) -> None:
self.lat += math.degrees(north / EARTH_RADIUS)
self.lon += math.degrees(east / (EARTH_RADIUS * math.cos(math.radians(self.lat))))
@staticmethod
def _offset(lat: float, lon: float, ref_lat: float, ref_lon: float) -> tuple[float, float]:
north = math.radians(lat - ref_lat) * EARTH_RADIUS
east = math.radians(lon - ref_lon) * EARTH_RADIUS * math.cos(math.radians(ref_lat))
return north, east
def _state_at(self, t: float) -> tuple[float, float, float]:
for ht, lat, lon, heading in reversed(self.history):
if ht <= t:
return lat, lon, heading
return self.lat, self.lon, self.heading
def predict(self, t: float, speed: float, yaw_rate: float) -> None:
dt = 0.0 if self.t is None else t - self.t
self.t = t
if not 0.0 < dt < 1.0 or not self.initialized:
return
if math.isfinite(self.heading):
self.heading = (self.heading + yaw_rate * dt) % (2 * math.pi)
self._move(speed * dt * math.cos(self.heading), speed * dt * math.sin(self.heading))
# dead reckoning error grows with distance driven (speed scale and heading error), not as a random walk
drift = (0.03 + math.sqrt(min(self.heading_var, 1.0))) * abs(speed) * dt
self.pos_var = (math.sqrt(self.pos_var) + drift) ** 2 + 0.01 * dt
self.heading_var += 1e-5 * dt
self.history.append((t, self.lat, self.lon, self.heading))
def update_gps(self, t: float, gps, latency: float) -> None:
# qcom reports no horizontal accuracy
accuracy = gps.horizontalAccuracy if gps.horizontalAccuracy > 0 else math.sqrt(3) * gps.verticalAccuracy
if not gps.hasFix or not 0 < accuracy < 1500 or gps.speedAccuracy <= 0:
return
if abs(gps.latitude) > 90 or abs(gps.longitude) > 180:
return
then_lat, then_lon, then_heading = self._state_at(t - latency)
moved = self._offset(self.lat, self.lon, then_lat, then_lon) if self.initialized else (0.0, 0.0)
bearing = math.radians(gps.bearingDeg)
bearing_ok = gps.speed > MIN_BEARING_SPEED and 0 < gps.bearingAccuracyDeg < MAX_BEARING_ACCURACY
pos_r = accuracy ** 2
innov = self._offset(gps.latitude, gps.longitude, then_lat, then_lon) if self.initialized else (math.inf, math.inf)
dist = math.hypot(*innov)
if not self.initialized or dist > RESET_DISTANCE or self.rejects >= MAX_REJECTS:
self.lat, self.lon = gps.latitude, gps.longitude
self._move(*moved)
self.pos_var = pos_r
self.rejects = 0
self.history.clear()
if bearing_ok:
self.heading, self.heading_var = bearing, math.radians(gps.bearingAccuracyDeg) ** 2
return
s = self.pos_var + pos_r
if dist ** 2 > 9.0 * s and dist > 10.0:
self.rejects += 1
return
self.rejects = 0
k = self.pos_var / s
lat0, lon0 = self.lat, self.lon
self._move(k * innov[0], k * innov[1])
dlat, dlon = self.lat - lat0, self.lon - lon0
self.history = deque(((ht, lat + dlat, lon + dlon, h) for ht, lat, lon, h in self.history), maxlen=self.history.maxlen)
self.pos_var *= 1 - k
if bearing_ok:
head_r = math.radians(gps.bearingAccuracyDeg) ** 2
if not math.isfinite(self.heading):
self.heading, self.heading_var = bearing, head_r
else:
kh = self.heading_var / (self.heading_var + head_r)
self.heading = (self.heading + kh * wrap(bearing - then_heading)) % (2 * math.pi)
self.heading_var *= 1 - kh
def main():
params = Params()
gps_service = get_gps_location_service(params)
latency = GPS_LATENCY[gps_service]
sm = messaging.SubMaster(['deviceMotion', 'carState', 'extrinsicsCalibration', gps_service], poll='deviceMotion')
pm = messaging.PubMaster(['gpsLocationSP'])
fuser = GpsFuser()
calib_from_device = np.eye(3)
last_seed = 0.0
while True:
sm.update()
t = sm.logMonoTime['deviceMotion'] * 1e-9
rpy = list(sm['extrinsicsCalibration'].rpyCalib)
if sm.updated['extrinsicsCalibration'] and len(rpy) == 3:
calib_from_device = rot_from_euler(rpy).T
gps = sm[gps_service]
gps_t = sm.logMonoTime[gps_service] * 1e-9
if sm.updated[gps_service] and sm.valid[gps_service]:
fuser.update_gps(gps_t, gps, latency)
if sm.valid['deviceMotion']:
w = sm['deviceMotion'].angularVelocityDevice
yaw_rate = float((calib_from_device @ np.array([w.x, w.y, w.z]))[2])
fuser.predict(t, sm['carState'].vEgo, yaw_rate)
gps_ok = gps.hasFix and 0 <= t - gps_t < GPS_TIMEOUT
msg = messaging.new_message('gpsLocationSP', valid=fuser.valid or gps_ok)
out = msg.gpsLocationSP
out.gpsOK = gps_ok
out.fused = fuser.valid
if fuser.valid:
out.hasFix = True
out.latitude, out.longitude = fuser.lat, fuser.lon
out.bearingDeg = math.degrees(fuser.heading) % 360
out.bearingAccuracyDeg = math.degrees(math.sqrt(fuser.heading_var))
out.horizontalAccuracy = math.sqrt(fuser.pos_var)
out.speed = sm['carState'].vEgo
elif gps_ok:
out.hasFix = True
out.latitude, out.longitude = gps.latitude, gps.longitude
out.bearingDeg, out.bearingAccuracyDeg = gps.bearingDeg, gps.bearingAccuracyDeg
out.horizontalAccuracy, out.speed = gps.horizontalAccuracy, gps.speed
pm.send('gpsLocationSP', msg)
if fuser.valid and time.monotonic() - last_seed > SEED_INTERVAL:
params.put("LastGPSPositionFused", json.dumps({"latitude": fuser.lat, "longitude": fuser.lon}))
last_seed = time.monotonic()
if __name__ == "__main__":
main()
@@ -0,0 +1,750 @@
#include "sunnypilot/selfdrive/locationd/locationd.h"
#include <sys/time.h>
#include <sys/resource.h>
#include <algorithm>
#include <cmath>
#include <vector>
using namespace EKFS;
using namespace Eigen;
ExitHandler do_exit;
const double ACCEL_SANITY_CHECK = 100.0; // m/s^2
const double ROTATION_SANITY_CHECK = 10.0; // rad/s
const double TRANS_SANITY_CHECK = 200.0; // m/s
const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
const double ALTITUDE_SANITY_CHECK = 10000; // m
const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
const double VALID_TIME_SINCE_RESET = 1.0; // s
const double VALID_POS_STD = 50.0; // m
const double MAX_RESET_TRACKER = 5.0;
const double SANE_GPS_UNCERTAINTY = 1500.0; // m
const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker
const double RESET_TRACKER_DECAY = 0.99995;
const double DECAY = 0.9993; // ~10 secs to resume after a bad input
const double MAX_FILTER_REWIND_TIME = 0.8; // s
const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30;
// TODO: GPS sensor time offsets are empirically calculated
// They should be replaced with synced time from a real clock
const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
const float GPS_POS_STD_THRESHOLD = 50.0;
const float GPS_VEL_STD_THRESHOLD = 5.0;
const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
const float GPS_POS_STD_RESET_THRESHOLD = 2.0;
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5;
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3;
const bool DEBUG = getenv("DEBUG") != nullptr && std::string(getenv("DEBUG")) != "0";
static VectorXd floatlist2vector(const capnp::List<float, capnp::Kind::PRIMITIVE>::Reader& floatlist) {
VectorXd res(floatlist.size());
for (int i = 0; i < floatlist.size(); i++) {
res[i] = floatlist[i];
}
return res;
}
static Vector4d quat2vector(const Quaterniond& quat) {
return Vector4d(quat.w(), quat.x(), quat.y(), quat.z());
}
static Quaterniond vector2quat(const VectorXd& vec) {
return Quaterniond(vec(0), vec(1), vec(2), vec(3));
}
static void init_measurement(cereal::LiveLocationKalman::Measurement::Builder meas, const VectorXd& val, const VectorXd& std, bool valid) {
meas.setValue(kj::arrayPtr(val.data(), val.size()));
meas.setStd(kj::arrayPtr(std.data(), std.size()));
meas.setValid(valid);
}
static MatrixXdr rotate_cov(const MatrixXdr& rot_matrix, const MatrixXdr& cov_in) {
// To rotate a covariance matrix, the cov matrix needs to multiplied left and right by the transform matrix
return ((rot_matrix * cov_in) * rot_matrix.transpose());
}
static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in) {
// Stds cannot be rotated like values, only covariances can be rotated
return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
}
Localizer::Localizer(LocalizerGnssSource gnss_source) {
this->kf = std::make_unique<LiveKalman>();
this->reset_kalman();
this->calib = Vector3d(0.0, 0.0, 0.0);
this->device_from_calib = MatrixXdr::Identity(3, 3);
this->calib_from_device = MatrixXdr::Identity(3, 3);
for (int i = 0; i < POSENET_STD_HIST_HALF * 2; i++) {
this->posenet_stds.push_back(10.0);
}
VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
this->configure_gnss_source(gnss_source);
}
void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
VectorXd predicted_state = this->kf->get_x();
MatrixXdr predicted_cov = this->kf->get_P();
VectorXd predicted_std = predicted_cov.diagonal().array().sqrt();
VectorXd fix_ecef = predicted_state.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
VectorXd fix_ecef_std = predicted_std.segment<STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START);
VectorXd vel_ecef = predicted_state.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
VectorXd vel_ecef_std = predicted_std.segment<STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START);
VectorXd fix_pos_geo_vec = this->get_position_geodetic();
VectorXd orientation_ecef = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ecef_std = predicted_std.segment<STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr orientation_ecef_cov = predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
MatrixXdr device_from_ecef = euler2rot(orientation_ecef).transpose();
VectorXd calibrated_orientation_ecef = rot2euler((this->calib_from_device * device_from_ecef).transpose());
VectorXd acc_calib = this->calib_from_device * predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
MatrixXdr acc_calib_cov = predicted_cov.block<STATE_ACCELERATION_ERR_LEN, STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START, STATE_ACCELERATION_ERR_START);
VectorXd acc_calib_std = rotate_cov(this->calib_from_device, acc_calib_cov).diagonal().array().sqrt();
VectorXd ang_vel_calib = this->calib_from_device * predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
MatrixXdr vel_angular_cov = predicted_cov.block<STATE_ANGULAR_VELOCITY_ERR_LEN, STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START);
VectorXd ang_vel_calib_std = rotate_cov(this->calib_from_device, vel_angular_cov).diagonal().array().sqrt();
VectorXd vel_device = device_from_ecef * vel_ecef;
VectorXd device_from_ecef_eul = quat2euler(vector2quat(predicted_state.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START))).transpose();
MatrixXdr condensed_cov(STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
condensed_cov.topLeftCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
condensed_cov.topRightCorner<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomRightCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START);
condensed_cov.bottomLeftCorner<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>() =
predicted_cov.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_ORIENTATION_ERR_START);
VectorXd H_input(device_from_ecef_eul.size() + vel_ecef.size());
H_input << device_from_ecef_eul, vel_ecef;
MatrixXdr HH = this->kf->H(H_input);
MatrixXdr vel_device_cov = (HH * condensed_cov) * HH.transpose();
VectorXd vel_device_std = vel_device_cov.diagonal().array().sqrt();
VectorXd vel_calib = this->calib_from_device * vel_device;
VectorXd vel_calib_std = rotate_cov(this->calib_from_device, vel_device_cov).diagonal().array().sqrt();
VectorXd orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, orientation_ecef);
VectorXd orientation_ned_std = rotate_cov(this->converter->ecef2ned_matrix, orientation_ecef_cov).diagonal().array().sqrt();
VectorXd calibrated_orientation_ned = ned_euler_from_ecef(fix_ecef_ecef, calibrated_orientation_ecef);
VectorXd nextfix_ecef = fix_ecef + vel_ecef;
VectorXd ned_vel = this->converter->ecef2ned((ECEF) { .x = nextfix_ecef(0), .y = nextfix_ecef(1), .z = nextfix_ecef(2) }).to_vector() - converter->ecef2ned(fix_ecef_ecef).to_vector();
VectorXd accDevice = predicted_state.segment<STATE_ACCELERATION_LEN>(STATE_ACCELERATION_START);
VectorXd accDeviceErr = predicted_std.segment<STATE_ACCELERATION_ERR_LEN>(STATE_ACCELERATION_ERR_START);
VectorXd angVelocityDevice = predicted_state.segment<STATE_ANGULAR_VELOCITY_LEN>(STATE_ANGULAR_VELOCITY_START);
VectorXd angVelocityDeviceErr = predicted_std.segment<STATE_ANGULAR_VELOCITY_ERR_LEN>(STATE_ANGULAR_VELOCITY_ERR_START);
Vector3d nans = Vector3d(NAN, NAN, NAN);
// TODO fill in NED and Calibrated stds
// write measurements to msg
init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initOrientationNED(), orientation_ned, orientation_ned_std, this->gps_mode);
init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
init_measurement(fix.initAccelerationCalibrated(), acc_calib, acc_calib_std, this->calibrated);
if (DEBUG) {
init_measurement(fix.initFilterState(), predicted_state, predicted_std, true);
}
double old_mean = 0.0, new_mean = 0.0;
int i = 0;
for (double x : this->posenet_stds) {
if (i < POSENET_STD_HIST_HALF) {
old_mean += x;
} else {
new_mean += x;
}
i++;
}
old_mean /= POSENET_STD_HIST_HALF;
new_mean /= POSENET_STD_HIST_HALF;
// experimentally found these values, no false positives in 20k minutes of driving
bool std_spike = (new_mean / old_mean > 4.0 && new_mean > 7.0);
fix.setPosenetOK(!(std_spike && this->car_speed > 5.0));
fix.setDeviceStable(!this->device_fell);
fix.setExcessiveResets(this->reset_tracker > MAX_RESET_TRACKER);
fix.setTimeToFirstFix(std::isnan(this->ttff) ? -1. : this->ttff);
this->device_fell = false;
//fix.setGpsWeek(this->time.week);
//fix.setGpsTimeOfWeek(this->time.tow);
fix.setUnixTimestampMillis(this->unix_timestamp_millis);
double time_since_reset = this->kf->get_filter_time() - this->last_reset_time;
fix.setTimeSinceReset(time_since_reset);
if (fix_ecef_std.norm() < VALID_POS_STD && this->calibrated && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setStatus(cereal::LiveLocationKalman::Status::VALID);
} else if (fix_ecef_std.norm() < VALID_POS_STD && time_since_reset > VALID_TIME_SINCE_RESET) {
fix.setStatus(cereal::LiveLocationKalman::Status::UNCALIBRATED);
} else {
fix.setStatus(cereal::LiveLocationKalman::Status::UNINITIALIZED);
}
}
VectorXd Localizer::get_position_geodetic() {
VectorXd fix_ecef = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
ECEF fix_ecef_ecef = { .x = fix_ecef(0), .y = fix_ecef(1), .z = fix_ecef(2) };
Geodetic fix_pos_geo = ecef2geodetic(fix_ecef_ecef);
return Vector3d(fix_pos_geo.lat, fix_pos_geo.lon, fix_pos_geo.alt);
}
VectorXd Localizer::get_state() {
return this->kf->get_x();
}
VectorXd Localizer::get_stdev() {
return this->kf->get_P().diagonal().array().sqrt();
}
bool Localizer::are_inputs_ok() {
return this->critical_services_valid(this->observation_values_invalid) && !this->observation_timings_invalid;
}
void Localizer::observation_timings_invalid_reset(){
this->observation_timings_invalid = false;
}
void Localizer::handle_sensor(double current_time, const cereal::SensorEventData::Reader& log) {
// TODO does not yet account for double sensor readings in the log
// Ignore empty readings (e.g. in case the magnetometer had no data ready)
if (log.getTimestamp() == 0) {
return;
}
double sensor_time = 1e-9 * log.getTimestamp();
// sensor time and log time should be close
if (std::abs(current_time - sensor_time) > 0.1) {
LOGE("Sensor reading ignored, sensor timestamp more than 100ms off from log time");
this->observation_timings_invalid = true;
return;
} else if (!this->is_timestamp_valid(sensor_time)) {
this->observation_timings_invalid = true;
return;
}
// TODO: handle messages from two IMUs at the same time
if (log.getSource() == cereal::SensorEventData::SensorSource::BMX055) {
return;
}
// Gyro Uncalibrated
if (log.which() == cereal::SensorEventData::GYRO_UNCALIBRATED) {
auto v = log.getGyroUncalibrated().getV();
auto meas = Vector3d(-v[2], -v[1], -v[0]);
VectorXd gyro_bias = this->kf->get_x().segment<STATE_GYRO_BIAS_LEN>(STATE_GYRO_BIAS_START);
float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]);
float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1];
bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold;
if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas });
this->observation_values_invalid["gyroscope"] *= DECAY;
} else {
this->observation_values_invalid["gyroscope"] += 1.0;
}
}
// Accelerometer
if (log.which() == cereal::SensorEventData::ACCELERATION) {
auto v = log.getAcceleration().getV();
// TODO: reduce false positives and re-enable this check
// check if device fell, estimate 10 for g
// 40m/s**2 is a good filter for falling detection, no false positives in 20k minutes of driving
// this->device_fell |= (floatlist2vector(v) - Vector3d(10.0, 0.0, 0.0)).norm() > 40.0;
auto meas = Vector3d(-v[2], -v[1], -v[0]);
if (meas.norm() < ACCEL_SANITY_CHECK) {
this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_ACCEL, { meas });
this->observation_values_invalid["accelerometer"] *= DECAY;
} else {
this->observation_values_invalid["accelerometer"] += 1.0;
}
}
}
void Localizer::input_fake_gps_observations(double current_time) {
// This is done to make sure that the error estimate of the position does not blow up
// when the filter is in no-gps mode
// Steps : first predict -> observe current obs with reasonable STD
this->kf->predict(current_time);
VectorXd current_x = this->kf->get_x();
VectorXd ecef_pos = current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
VectorXd ecef_vel = current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START);
const MatrixXdr &ecef_pos_R = this->kf->get_fake_gps_pos_cov();
const MatrixXdr &ecef_vel_R = this->kf->get_fake_gps_vel_cov();
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset) {
bool gps_unreasonable = (Vector2d(log.getHorizontalAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
if (!log.getHasFix() || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane) {
//this->gps_valid = false;
this->determine_gps_mode(current_time);
return;
}
double sensor_time = current_time - sensor_time_offset;
// Process message
//this->gps_valid = true;
this->gps_mode = true;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique<LocalCoord>(geodetic);
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getHorizontalAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
this->unix_timestamp_millis = log.getUnixTimestampMillis();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
} else if (gps_est_error > 100.0) {
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
}
this->last_gps_msg = sensor_time;
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
this->determine_gps_mode(current_time);
return;
}
double sensor_time = log.getMeasTime() * 1e-9;
sensor_time -= this->gps_time_offset;
auto ecef_pos_v = log.getPositionECEF().getValue();
VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_pos_std = log.getPositionECEF().getStd()[0];
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
auto ecef_vel_v = log.getVelocityECEF().getValue();
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
// indexed at 0 cause all std values are the same MAE
auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos[0], ecef_pos[1], ecef_pos[2] }, orientation_ecef);
LocalCoord convs((ECEF){ .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
ECEF next_ecef = {.x = ecef_pos[0] + ecef_vel[0], .y = ecef_pos[1] + ecef_vel[1], .z = ecef_pos[2] + ecef_vel[2]};
VectorXd ned_vel = convs.ecef2ned(next_ecef).to_vector();
double bearing_rad = atan2(ned_vel[1], ned_vel[0]);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, bearing_rad);
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
this->determine_gps_mode(current_time);
return;
}
// prevent jumping gnss measurements (covered lots, standstill...)
bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
orientation_reset &= !this->standstill;
if (orientation_reset) {
this->orientation_reset_count++;
} else {
this->orientation_reset_count = 0;
}
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
// always reset on first gps message and if the location is off but the accuracy is high
LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
} else if (orientation_reset_count > GPS_ORIENTATION_ERROR_RESET_CNT) {
LOGE("Locationd vs gnssMeasurement orientation difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
this->orientation_reset_count = 0;
}
this->gps_mode = true;
this->last_gps_msg = sensor_time;
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
}
void Localizer::handle_car_state(double current_time, const cereal::CarState::Reader& log) {
this->car_speed = std::abs(log.getVEgo());
this->standstill = log.getStandstill();
if (this->standstill) {
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ROT, { Vector3d(0.0, 0.0, 0.0) });
this->kf->predict_and_observe(current_time, OBSERVATION_NO_ACCEL, { Vector3d(0.0, 0.0, 0.0) });
}
}
void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log) {
VectorXd rot_device = this->device_from_calib * floatlist2vector(log.getRot());
VectorXd trans_device = this->device_from_calib * floatlist2vector(log.getTrans());
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if ((rot_device.norm() > ROTATION_SANITY_CHECK) || (trans_device.norm() > TRANS_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
VectorXd rot_calib_std = floatlist2vector(log.getRotStd());
VectorXd trans_calib_std = floatlist2vector(log.getTransStd());
if ((rot_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK) || (trans_calib_std.minCoeff() <= MIN_STD_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
if ((rot_calib_std.norm() > 10 * ROTATION_SANITY_CHECK) || (trans_calib_std.norm() > 10 * TRANS_SANITY_CHECK)) {
this->observation_values_invalid["cameraOdometry"] += 1.0;
return;
}
this->posenet_stds.pop_front();
this->posenet_stds.push_back(trans_calib_std[0]);
// Multiply by 10 to avoid to high certainty in kalman filter because of temporally correlated noise
trans_calib_std *= 10.0;
rot_calib_std *= 10.0;
MatrixXdr rot_device_cov = rotate_std(this->device_from_calib, rot_calib_std).array().square().matrix().asDiagonal();
MatrixXdr trans_device_cov = rotate_std(this->device_from_calib, trans_calib_std).array().square().matrix().asDiagonal();
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_ROTATION,
{ rot_device }, { rot_device_cov });
this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION,
{ trans_device }, { trans_device_cov });
this->observation_values_invalid["cameraOdometry"] *= DECAY;
this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]);
}
void Localizer::handle_live_calib(double current_time, const cereal::ExtrinsicsCalibration::Reader& log) {
if (!this->is_timestamp_valid(current_time)) {
this->observation_timings_invalid = true;
return;
}
if (log.getRpyCalib().size() > 0) {
auto live_calib = floatlist2vector(log.getRpyCalib());
if ((live_calib.minCoeff() < -CALIB_RPY_SANITY_CHECK) || (live_calib.maxCoeff() > CALIB_RPY_SANITY_CHECK)) {
this->observation_values_invalid["extrinsicsCalibration"] += 1.0;
return;
}
this->calib = live_calib;
this->device_from_calib = euler2rot(this->calib);
this->calib_from_device = this->device_from_calib.transpose();
this->calibrated = log.getCalStatus() == cereal::ExtrinsicsCalibration::Status::CALIBRATED;
this->observation_values_invalid["extrinsicsCalibration"] *= DECAY;
}
}
void Localizer::reset_kalman(double current_time) {
const VectorXd &init_x = this->kf->get_initial_x();
const MatrixXdr &init_P = this->kf->get_initial_P();
this->reset_kalman(current_time, init_x, init_P);
}
void Localizer::finite_check(double current_time) {
bool all_finite = this->kf->get_x().array().isFinite().all() or this->kf->get_P().array().isFinite().all();
if (!all_finite) {
LOGE("Non-finite values detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::time_check(double current_time) {
if (std::isnan(this->last_reset_time)) {
this->last_reset_time = current_time;
}
if (std::isnan(this->first_valid_log_time)) {
this->first_valid_log_time = current_time;
}
double filter_time = this->kf->get_filter_time();
bool big_time_gap = !std::isnan(filter_time) && (current_time - filter_time > 10);
if (big_time_gap) {
LOGE("Time gap of over 10s detected, kalman reset");
this->reset_kalman(current_time);
}
}
void Localizer::update_reset_tracker() {
// reset tracker is tuned to trigger when over 1reset/10s over 2min period
if (this->is_gps_ok()) {
this->reset_tracker *= RESET_TRACKER_DECAY;
} else {
this->reset_tracker = 0.0;
}
}
void Localizer::reset_kalman(double current_time, const VectorXd &init_orient, const VectorXd &init_pos, const VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R) {
// too nonlinear to init on completely wrong
VectorXd current_x = this->kf->get_x();
MatrixXdr current_P = this->kf->get_P();
MatrixXdr init_P = this->kf->get_initial_P();
const MatrixXdr &reset_orientation_P = this->kf->get_reset_orientation_P();
int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
current_x.segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START) = init_orient;
current_x.segment<STATE_ECEF_VELOCITY_LEN>(STATE_ECEF_VELOCITY_START) = init_vel;
current_x.segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) = init_pos;
init_P.block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
init_P.block<STATE_ECEF_ORIENTATION_ERR_LEN, STATE_ECEF_ORIENTATION_ERR_LEN>(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
init_P.block<STATE_ECEF_VELOCITY_ERR_LEN, STATE_ECEF_VELOCITY_ERR_LEN>(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START,
STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
this->reset_kalman(current_time, current_x, init_P);
}
void Localizer::reset_kalman(double current_time, const VectorXd &init_x, const MatrixXdr &init_P) {
this->kf->init_state(init_x, init_P, current_time);
this->last_reset_time = current_time;
this->reset_tracker += 1.0;
}
void Localizer::handle_msg_bytes(const char *data, const size_t size) {
AlignedBuffer aligned_buf;
capnp::FlatArrayMessageReader cmsg(aligned_buf.align(data, size));
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
this->handle_msg(event);
}
void Localizer::handle_msg(const cereal::Event::Reader& log) {
double t = log.getLogMonoTime() * 1e-9;
this->time_check(t);
if (log.isAccelerometer()) {
this->handle_sensor(t, log.getAccelerometer());
} else if (log.isGyroscope()) {
this->handle_sensor(t, log.getGyroscope());
} else if (log.isGpsLocation()) {
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
} else if (log.isGpsLocationExternal()) {
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
//} else if (log.isGnssMeasurements()) {
// this->handle_gnss(t, log.getGnssMeasurements());
} else if (log.isCarState()) {
this->handle_car_state(t, log.getCarState());
} else if (log.isCameraOdometry()) {
this->handle_cam_odo(t, log.getCameraOdometry());
} else if (log.isExtrinsicsCalibration()) {
this->handle_live_calib(t, log.getExtrinsicsCalibration());
}
this->finite_check();
this->update_reset_tracker();
}
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, bool inputsOK,
bool sensorsOK, bool gpsOK, bool msgValid) {
cereal::Event::Builder evt = msg_builder.initEvent();
evt.setValid(msgValid);
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
this->build_live_location(liveLoc);
liveLoc.setSensorsOK(sensorsOK);
liveLoc.setGpsOK(gpsOK);
liveLoc.setInputsOK(inputsOK);
return msg_builder.toBytes();
}
bool Localizer::is_gps_ok() {
return (this->kf->get_filter_time() - this->last_gps_msg) < 2.0;
}
bool Localizer::critical_services_valid(const std::map<std::string, double> &critical_services) {
for (auto &kv : critical_services){
if (kv.second >= INPUT_INVALID_THRESHOLD){
return false;
}
}
return true;
}
bool Localizer::is_timestamp_valid(double current_time) {
double filter_time = this->kf->get_filter_time();
if (!std::isnan(filter_time) && ((filter_time - current_time) > MAX_FILTER_REWIND_TIME)) {
LOGE("Observation timestamp is older than the max rewind threshold of the filter");
return false;
}
return true;
}
void Localizer::determine_gps_mode(double current_time) {
// 1. If the pos_std is greater than what's not acceptable and localizer is in gps-mode, reset to no-gps-mode
// 2. If the pos_std is greater than what's not acceptable and localizer is in no-gps-mode, fake obs
// 3. If the pos_std is smaller than what's not acceptable, let gps-mode be whatever it is
VectorXd current_pos_std = this->kf->get_P().block<STATE_ECEF_POS_ERR_LEN, STATE_ECEF_POS_ERR_LEN>(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
if (this->gps_mode){
this->gps_mode = false;
this->reset_kalman(current_time);
} else {
this->input_fake_gps_observations(current_time);
}
}
}
void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
this->gnss_source = source;
if (source == LocalizerGnssSource::UBLOX) {
this->gps_std_factor = 10.0;
this->gps_variance_factor = 1.0;
this->gps_vertical_variance_factor = 1.0;
this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
} else {
this->gps_std_factor = 2.0;
this->gps_variance_factor = 0.0;
this->gps_vertical_variance_factor = 3.0;
this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
}
}
int Localizer::locationd_thread() {
Params params;
LocalizerGnssSource source;
const char* gps_location_socket;
if (params.getBool("UbloxAvailable")) {
source = LocalizerGnssSource::UBLOX;
gps_location_socket = "gpsLocationExternal";
} else {
source = LocalizerGnssSource::QCOM;
gps_location_socket = "gpsLocation";
}
this->configure_gnss_source(source);
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "extrinsicsCalibration",
"carState", "accelerometer", "gyroscope"};
SubMaster sm(service_list, {}, nullptr, {gps_location_socket});
PubMaster pm({"liveLocationKalman"});
uint64_t cnt = 0;
bool filterInitialized = false;
const std::vector<std::string> critical_input_services = {"cameraOdometry", "extrinsicsCalibration", "accelerometer", "gyroscope"};
for (std::string service : critical_input_services) {
this->observation_values_invalid.insert({service, 0.0});
}
while (!do_exit) {
sm.update();
if (filterInitialized){
this->observation_timings_invalid_reset();
for (const char* service : service_list) {
if (sm.updated(service) && sm.valid(service)){
const cereal::Event::Reader log = sm[service];
this->handle_msg(log);
}
}
} else {
filterInitialized = sm.allAliveAndValid();
}
const char* trigger_msg = "cameraOdometry";
if (sm.updated(trigger_msg)) {
bool inputsOK = sm.allValid() && this->are_inputs_ok();
bool gpsOK = this->is_gps_ok();
bool sensorsOK = sm.allAliveAndValid({"accelerometer", "gyroscope"});
// Log time to first fix
if (gpsOK && std::isnan(this->ttff) && !std::isnan(this->first_valid_log_time)) {
this->ttff = std::max(1e-3, (sm[trigger_msg].getLogMonoTime() * 1e-9) - this->first_valid_log_time);
}
MessageBuilder msg_builder;
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, inputsOK, sensorsOK, gpsOK, filterInitialized);
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
if (cnt % 1200 == 0 && gpsOK) { // once a minute
VectorXd posGeo = this->get_position_geodetic();
std::string lastGPSPosJSON = util::string_format(
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
params.putNonBlocking("LastGPSPositionLLK", lastGPSPosJSON);
}
cnt++;
}
}
return 0;
}
int main() {
util::set_realtime_priority(5);
Localizer localizer;
return localizer.locationd_thread();
}
@@ -0,0 +1,100 @@
#pragma once
#include <eigen3/Eigen/Dense>
#include <deque>
#include <fstream>
#include <memory>
#include <map>
#include <string>
#include "cereal/messaging/messaging.h"
#include "common/params.h"
#include "common/swaglog.h"
#include "common/timing.h"
#include "common/util.h"
#include "sunnypilot/common/transformations/coordinates.hpp"
#include "sunnypilot/common/transformations/orientation.hpp"
#include "sunnypilot/system/sensord/sensors/constants.h"
#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
#define VISION_DECIMATION 2
#define SENSOR_DECIMATION 10
#define POSENET_STD_HIST_HALF 20
enum LocalizerGnssSource {
UBLOX, QCOM
};
class Localizer {
public:
Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
int locationd_thread();
void reset_kalman(double current_time = NAN);
void reset_kalman(double current_time, const Eigen::VectorXd &init_orient, const Eigen::VectorXd &init_pos, const Eigen::VectorXd &init_vel, const MatrixXdr &init_pos_R, const MatrixXdr &init_vel_R);
void reset_kalman(double current_time, const Eigen::VectorXd &init_x, const MatrixXdr &init_P);
void finite_check(double current_time = NAN);
void time_check(double current_time = NAN);
void update_reset_tracker();
bool is_gps_ok();
bool critical_services_valid(const std::map<std::string, double> &critical_services);
bool is_timestamp_valid(double current_time);
void determine_gps_mode(double current_time);
bool are_inputs_ok();
void observation_timings_invalid_reset();
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder,
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
Eigen::VectorXd get_position_geodetic();
Eigen::VectorXd get_state();
Eigen::VectorXd get_stdev();
void handle_msg_bytes(const char *data, const size_t size);
void handle_msg(const cereal::Event::Reader& log);
void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log);
void handle_car_state(double current_time, const cereal::CarState::Reader& log);
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
void handle_live_calib(double current_time, const cereal::ExtrinsicsCalibration::Reader& log);
void input_fake_gps_observations(double current_time);
private:
std::unique_ptr<LiveKalman> kf;
Eigen::VectorXd calib;
MatrixXdr device_from_calib;
MatrixXdr calib_from_device;
bool calibrated = false;
double car_speed = 0.0;
double last_reset_time = NAN;
std::deque<double> posenet_stds;
std::unique_ptr<LocalCoord> converter;
int64_t unix_timestamp_millis = 0;
double reset_tracker = 0.0;
bool device_fell = false;
bool gps_mode = false;
double first_valid_log_time = NAN;
double ttff = NAN;
double last_gps_msg = 0;
LocalizerGnssSource gnss_source;
bool observation_timings_invalid = false;
std::map<std::string, double> observation_values_invalid;
bool standstill = true;
int32_t orientation_reset_count = 0;
float gps_std_factor;
float gps_variance_factor;
float gps_vertical_variance_factor;
double gps_time_offset;
Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std
void configure_gnss_source(const LocalizerGnssSource &source);
};
@@ -0,0 +1 @@
generated/
+180
View File
@@ -0,0 +1,180 @@
#!/usr/bin/env python3
import math
import sys
from typing import Any
import numpy as np
from openpilot.common.constants import ACCELERATION_DUE_TO_GRAVITY
from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
from openpilot.common.swaglog import cloudlog
from rednose.helpers.kalmanfilter import KalmanFilter
if __name__ == '__main__': # Generating sympy
import sympy as sp
from rednose.helpers.ekf_sym import gen_code
else:
from rednose.helpers.ekf_sym_pyx import EKF_sym_pyx
i = 0
def _slice(n):
global i
s = slice(i, i + n)
i += n
return s
class States:
# Vehicle model params
STIFFNESS = _slice(1) # [-]
STEER_RATIO = _slice(1) # [-]
ANGLE_OFFSET = _slice(1) # [rad]
ANGLE_OFFSET_FAST = _slice(1) # [rad]
VELOCITY = _slice(2) # (x, y) [m/s]
YAW_RATE = _slice(1) # [rad/s]
STEER_ANGLE = _slice(1) # [rad]
ROAD_ROLL = _slice(1) # [rad]
class CarKalman(KalmanFilter):
name = 'car'
initial_x = np.array([
1.0,
15.0,
0.0,
0.0,
10.0, 0.0,
0.0,
0.0,
0.0
])
# process noise
Q = np.diag([
(.05 / 100)**2,
.01**2,
math.radians(0.02)**2,
math.radians(0.25)**2,
.1**2, .01**2,
math.radians(0.1)**2,
math.radians(0.1)**2,
math.radians(1)**2,
])
P_initial = Q.copy()
obs_noise: dict[int, Any] = {
ObservationKind.STEER_ANGLE: np.atleast_2d(math.radians(0.05)**2),
ObservationKind.ANGLE_OFFSET_FAST: np.atleast_2d(math.radians(10.0)**2),
ObservationKind.ROAD_ROLL: np.atleast_2d(math.radians(1.0)**2),
ObservationKind.STEER_RATIO: np.atleast_2d(5.0**2),
ObservationKind.STIFFNESS: np.atleast_2d(0.5**2),
ObservationKind.ROAD_FRAME_X_SPEED: np.atleast_2d(0.1**2),
}
global_vars = [
'mass',
'rotational_inertia',
'center_to_front',
'center_to_rear',
'stiffness_front',
'stiffness_rear',
]
@staticmethod
def generate_code(generated_dir):
dim_state = CarKalman.initial_x.shape[0]
name = CarKalman.name
# Linearized single-track lateral dynamics, equations 7.211-7.213
# Massimo Guiggiani, The Science of Vehicle Dynamics: Handling, Braking, and Ride of Road and Race Cars
# Springer Cham, 2023. doi: https://doi.org/10.1007/978-3-031-06461-6
# globals
global_vars = [sp.Symbol(name) for name in CarKalman.global_vars]
m, j, aF, aR, cF_orig, cR_orig = global_vars
# make functions and jacobians with sympy
# state variables
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
# Vehicle model constants
sf = state[States.STIFFNESS, :][0, 0]
cF, cR = sf * cF_orig, sf * cR_orig
angle_offset = state[States.ANGLE_OFFSET, :][0, 0]
angle_offset_fast = state[States.ANGLE_OFFSET_FAST, :][0, 0]
theta = state[States.ROAD_ROLL, :][0, 0]
sa = state[States.STEER_ANGLE, :][0, 0]
sR = state[States.STEER_RATIO, :][0, 0]
u, v = state[States.VELOCITY, :]
r = state[States.YAW_RATE, :][0, 0]
A = sp.Matrix(np.zeros((2, 2)))
A[0, 0] = -(cF + cR) / (m * u)
A[0, 1] = -(cF * aF - cR * aR) / (m * u) - u
A[1, 0] = -(cF * aF - cR * aR) / (j * u)
A[1, 1] = -(cF * aF**2 + cR * aR**2) / (j * u)
B = sp.Matrix(np.zeros((2, 1)))
B[0, 0] = cF / m / sR
B[1, 0] = (cF * aF) / j / sR
C = sp.Matrix(np.zeros((2, 1)))
C[0, 0] = ACCELERATION_DUE_TO_GRAVITY
C[1, 0] = 0
x = sp.Matrix([v, r]) # lateral velocity, yaw rate
x_dot = A * x + B * (sa - angle_offset - angle_offset_fast) - C * theta
dt = sp.Symbol('dt')
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.VELOCITY.start + 1, 0] = x_dot[0]
state_dot[States.YAW_RATE.start, 0] = x_dot[1]
# Basic descretization, 1st order integrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
#
# Observation functions
#
obs_eqs = [
[sp.Matrix([r]), ObservationKind.ROAD_FRAME_YAW_RATE, None],
[sp.Matrix([u, v]), ObservationKind.ROAD_FRAME_XY_SPEED, None],
[sp.Matrix([u]), ObservationKind.ROAD_FRAME_X_SPEED, None],
[sp.Matrix([sa]), ObservationKind.STEER_ANGLE, None],
[sp.Matrix([angle_offset_fast]), ObservationKind.ANGLE_OFFSET_FAST, None],
[sp.Matrix([sR]), ObservationKind.STEER_RATIO, None],
[sp.Matrix([sf]), ObservationKind.STIFFNESS, None],
[sp.Matrix([theta]), ObservationKind.ROAD_ROLL, None],
]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state, global_vars=global_vars)
def __init__(self, generated_dir):
dim_state, dim_state_err = CarKalman.initial_x.shape[0], CarKalman.P_initial.shape[0]
self.filter = EKF_sym_pyx(generated_dir, CarKalman.name, CarKalman.Q, CarKalman.initial_x, CarKalman.P_initial,
dim_state, dim_state_err, global_vars=CarKalman.global_vars, logger=cloudlog)
def set_globals(self, mass, rotational_inertia, center_to_front, center_to_rear, stiffness_front, stiffness_rear):
self.filter.set_global("mass", mass)
self.filter.set_global("rotational_inertia", rotational_inertia)
self.filter.set_global("center_to_front", center_to_front)
self.filter.set_global("center_to_rear", center_to_rear)
self.filter.set_global("stiffness_front", stiffness_front)
self.filter.set_global("stiffness_rear", stiffness_rear)
if __name__ == "__main__":
generated_dir = sys.argv[2]
CarKalman.generate_code(generated_dir)
@@ -0,0 +1,92 @@
import os
GENERATED_DIR = os.path.abspath(os.path.join(os.path.dirname(__file__), 'generated'))
class ObservationKind:
UNKNOWN = 0
NO_OBSERVATION = 1
GPS_NED = 2
ODOMETRIC_SPEED = 3
PHONE_GYRO = 4
GPS_VEL = 5
PSEUDORANGE_GPS = 6
PSEUDORANGE_RATE_GPS = 7
SPEED = 8
NO_ROT = 9
PHONE_ACCEL = 10
ORB_POINT = 11
ECEF_POS = 12
CAMERA_ODO_TRANSLATION = 13
CAMERA_ODO_ROTATION = 14
ORB_FEATURES = 15
MSCKF_TEST = 16
FEATURE_TRACK_TEST = 17
LANE_PT = 18
IMU_FRAME = 19
PSEUDORANGE_GLONASS = 20
PSEUDORANGE_RATE_GLONASS = 21
PSEUDORANGE = 22
PSEUDORANGE_RATE = 23
ECEF_VEL = 35
ECEF_ORIENTATION_FROM_GPS = 32
NO_ACCEL = 33
ORB_FEATURES_WIDE = 34
ROAD_FRAME_XY_SPEED = 24 # (x, y) [m/s]
ROAD_FRAME_YAW_RATE = 25 # [rad/s]
STEER_ANGLE = 26 # [rad]
ANGLE_OFFSET_FAST = 27 # [rad]
STIFFNESS = 28 # [-]
STEER_RATIO = 29 # [-]
ROAD_FRAME_X_SPEED = 30 # (x) [m/s]
ROAD_ROLL = 31 # [rad]
names = [
'Unknown',
'No observation',
'GPS NED',
'Odometric speed',
'Phone gyro',
'GPS velocity',
'GPS pseudorange',
'GPS pseudorange rate',
'Speed',
'No rotation',
'Phone acceleration',
'ORB point',
'ECEF pos',
'camera odometric translation',
'camera odometric rotation',
'ORB features',
'MSCKF test',
'Feature track test',
'Lane ecef point',
'imu frame eulers',
'GLONASS pseudorange',
'GLONASS pseudorange rate',
'pseudorange',
'pseudorange rate',
'Road Frame x,y speed',
'Road Frame yaw rate',
'Steer Angle',
'Fast Angle Offset',
'Stiffness',
'Steer Ratio',
'Road Frame x speed',
'Road Roll',
'ECEF orientation from GPS',
'NO accel',
'ORB features wide camera',
'ECEF_VEL',
]
@classmethod
def to_string(cls, kind):
return cls.names[kind]
SAT_OBS = [ObservationKind.PSEUDORANGE_GPS,
ObservationKind.PSEUDORANGE_RATE_GPS,
ObservationKind.PSEUDORANGE_GLONASS,
ObservationKind.PSEUDORANGE_RATE_GLONASS]
@@ -0,0 +1,122 @@
#include "sunnypilot/selfdrive/locationd/models/live_kf.h"
using namespace EKFS;
using namespace Eigen;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec) {
return Eigen::Map<Eigen::VectorXd>((double*)vec.data(), vec.rows(), vec.cols());
}
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat) {
return Eigen::Map<MatrixXdr>((double*)mat.data(), mat.rows(), mat.cols());
}
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec) {
std::vector<Eigen::Map<Eigen::VectorXd>> res;
for (const Eigen::VectorXd &vec : vec_vec) {
res.push_back(get_mapvec(vec));
}
return res;
}
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec) {
std::vector<Eigen::Map<MatrixXdr>> res;
for (const MatrixXdr &mat : mat_vec) {
res.push_back(get_mapmat(mat));
}
return res;
}
LiveKalman::LiveKalman() {
this->dim_state = live_initial_x.rows();
this->dim_state_err = live_initial_P_diag.rows();
this->initial_x = live_initial_x;
this->initial_P = live_initial_P_diag.asDiagonal();
this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
this->Q = live_Q_diag.asDiagonal();
for (auto& pair : live_obs_noise_diag) {
this->obs_noise[pair.first] = pair.second.asDiagonal();
}
// init filter
this->filter = std::make_shared<EKFSym>(this->name, get_mapmat(this->Q), get_mapvec(this->initial_x),
get_mapmat(initial_P), this->dim_state, this->dim_state_err, 0, 0, 0, std::vector<int>(),
std::vector<int>{3}, std::vector<std::string>(), 0.8);
}
void LiveKalman::init_state(const VectorXd &state, const VectorXd &covs_diag, double filter_time) {
MatrixXdr covs = covs_diag.asDiagonal();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(const VectorXd &state, const MatrixXdr &covs, double filter_time) {
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
void LiveKalman::init_state(const VectorXd &state, double filter_time) {
MatrixXdr covs = this->filter->covs();
this->filter->init_state(get_mapvec(state), get_mapmat(covs), filter_time);
}
VectorXd LiveKalman::get_x() {
return this->filter->state();
}
MatrixXdr LiveKalman::get_P() {
return this->filter->covs();
}
double LiveKalman::get_filter_time() {
return this->filter->get_filter_time();
}
std::vector<MatrixXdr> LiveKalman::get_R(int kind, int n) {
std::vector<MatrixXdr> R;
for (int i = 0; i < n; i++) {
R.push_back(this->obs_noise[kind]);
}
return R;
}
std::optional<Estimate> LiveKalman::predict_and_observe(double t, int kind, const std::vector<VectorXd> &meas, std::vector<MatrixXdr> R) {
std::optional<Estimate> r;
if (R.size() == 0) {
R = this->get_R(kind, meas.size());
}
r = this->filter->predict_and_update_batch(t, kind, get_vec_mapvec(meas), get_vec_mapmat(R));
return r;
}
void LiveKalman::predict(double t) {
this->filter->predict(t);
}
const Eigen::VectorXd &LiveKalman::get_initial_x() {
return this->initial_x;
}
const MatrixXdr &LiveKalman::get_initial_P() {
return this->initial_P;
}
const MatrixXdr &LiveKalman::get_fake_gps_pos_cov() {
return this->fake_gps_pos_cov;
}
const MatrixXdr &LiveKalman::get_fake_gps_vel_cov() {
return this->fake_gps_vel_cov;
}
const MatrixXdr &LiveKalman::get_reset_orientation_P() {
return this->reset_orientation_P;
}
MatrixXdr LiveKalman::H(const VectorXd &in) {
assert(in.size() == 6);
Matrix<double, 3, 6, Eigen::RowMajor> res;
this->filter->get_extra_routine("H")((double*)in.data(), res.data());
return res;
}
@@ -0,0 +1,66 @@
#pragma once
#include <string>
#include <cmath>
#include <memory>
#include <unordered_map>
#include <vector>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include "generated/live_kf_constants.h"
#include "rednose/helpers/ekf_sym.h"
#define EARTH_GM 3.986005e14 // m^3/s^2 (gravitational constant * mass of earth)
using namespace EKFS;
Eigen::Map<Eigen::VectorXd> get_mapvec(const Eigen::VectorXd &vec);
Eigen::Map<MatrixXdr> get_mapmat(const MatrixXdr &mat);
std::vector<Eigen::Map<Eigen::VectorXd>> get_vec_mapvec(const std::vector<Eigen::VectorXd> &vec_vec);
std::vector<Eigen::Map<MatrixXdr>> get_vec_mapmat(const std::vector<MatrixXdr> &mat_vec);
class LiveKalman {
public:
LiveKalman();
void init_state(const Eigen::VectorXd &state, const Eigen::VectorXd &covs_diag, double filter_time);
void init_state(const Eigen::VectorXd &state, const MatrixXdr &covs, double filter_time);
void init_state(const Eigen::VectorXd &state, double filter_time);
Eigen::VectorXd get_x();
MatrixXdr get_P();
double get_filter_time();
std::vector<MatrixXdr> get_R(int kind, int n);
std::optional<Estimate> predict_and_observe(double t, int kind, const std::vector<Eigen::VectorXd> &meas, std::vector<MatrixXdr> R = {});
std::optional<Estimate> predict_and_update_odo_speed(std::vector<Eigen::VectorXd> speed, double t, int kind);
std::optional<Estimate> predict_and_update_odo_trans(std::vector<Eigen::VectorXd> trans, double t, int kind);
std::optional<Estimate> predict_and_update_odo_rot(std::vector<Eigen::VectorXd> rot, double t, int kind);
void predict(double t);
const Eigen::VectorXd &get_initial_x();
const MatrixXdr &get_initial_P();
const MatrixXdr &get_fake_gps_pos_cov();
const MatrixXdr &get_fake_gps_vel_cov();
const MatrixXdr &get_reset_orientation_P();
MatrixXdr H(const Eigen::VectorXd &in);
private:
std::string name = "live";
std::shared_ptr<EKFSym> filter;
int dim_state;
int dim_state_err;
Eigen::VectorXd initial_x;
MatrixXdr initial_P;
MatrixXdr fake_gps_pos_cov;
MatrixXdr fake_gps_vel_cov;
MatrixXdr reset_orientation_P;
MatrixXdr Q; // process noise
std::unordered_map<int, MatrixXdr> obs_noise;
};
+242
View File
@@ -0,0 +1,242 @@
#!/usr/bin/env python3
import sys
import os
import numpy as np
from openpilot.sunnypilot.selfdrive.locationd.models.constants import ObservationKind
import sympy as sp
import inspect
from rednose.helpers.sympy_helpers import euler_rotate, quat_matrix_r, quat_rotate
from rednose.helpers.ekf_sym import gen_code
EARTH_GM = 3.986005e14 # m^3/s^2 (gravitational constant * mass of earth)
def numpy2eigenstring(arr):
assert(len(arr.shape) == 1)
arr_str = np.array2string(arr, precision=20, separator=',')[1:-1].replace(' ', '').replace('\n', '')
return f"(Eigen::VectorXd({len(arr)}) << {arr_str}).finished()"
class States:
ECEF_POS = slice(0, 3) # x, y and z in ECEF in meters
ECEF_ORIENTATION = slice(3, 7) # quat for pose of phone in ecef
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
ECEF_ORIENTATION_ERR = slice(3, 6) # euler angles for orientation error
ECEF_VELOCITY_ERR = slice(6, 9)
ANGULAR_VELOCITY_ERR = slice(9, 12)
GYRO_BIAS_ERR = slice(12, 15)
ACCELERATION_ERR = slice(15, 18)
ACC_BIAS_ERR = slice(18, 21)
class LiveKalman:
name = 'live'
initial_x = np.array([3.88e6, -3.37e6, 3.76e6,
0.42254641, -0.31238054, -0.83602975, -0.15788347, # NED [0,0,0] -> ECEF Quat
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0,
0, 0, 0])
# state covariance
initial_P_diag = np.array([10**2, 10**2, 10**2,
0.01**2, 0.01**2, 0.01**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
100**2, 100**2, 100**2,
0.01**2, 0.01**2, 0.01**2])
# state covariance when resetting midway in a segment
reset_orientation_diag = np.array([1**2, 1**2, 1**2])
# fake observation covariance, to ensure the uncertainty estimate of the filter is under control
fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
# process noise
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
0.001**2, 0.001**2, 0.001**2,
0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
3**2, 3**2, 3**2,
0.005**2, 0.005**2, 0.005**2])
obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
ObservationKind.ECEF_VEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.ECEF_ORIENTATION_FROM_GPS: np.array([.2**2, .2**2, .2**2, .2**2])}
@staticmethod
def generate_code(generated_dir):
name = LiveKalman.name
dim_state = LiveKalman.initial_x.shape[0]
dim_state_err = LiveKalman.initial_P_diag.shape[0]
state_sym = sp.MatrixSymbol('state', dim_state, 1)
state = sp.Matrix(state_sym)
x, y, z = state[States.ECEF_POS, :]
q = state[States.ECEF_ORIENTATION, :]
v = state[States.ECEF_VELOCITY, :]
vx, vy, vz = v
omega = state[States.ANGULAR_VELOCITY, :]
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
acc_bias = state[States.ACC_BIAS, :]
dt = sp.Symbol('dt')
# calibration and attitude rotation matrices
quat_rot = quat_rotate(*q)
# Got the quat predict equations from here
# A New Quaternion-Based Kalman Filter for
# Real-Time Attitude Estimation Using the Two-Step
# Geometrically-Intuitive Correction Algorithm
A = 0.5 * sp.Matrix([[0, -vroll, -vpitch, -vyaw],
[vroll, 0, vyaw, -vpitch],
[vpitch, -vyaw, 0, vroll],
[vyaw, vpitch, -vroll, 0]])
q_dot = A * q
# Time derivative of the state as a function of state
state_dot = sp.Matrix(np.zeros((dim_state, 1)))
state_dot[States.ECEF_POS, :] = v
state_dot[States.ECEF_ORIENTATION, :] = q_dot
state_dot[States.ECEF_VELOCITY, 0] = quat_rot * acceleration
# Basic descretization, 1st order intergrator
# Can be pretty bad if dt is big
f_sym = state + dt * state_dot
state_err_sym = sp.MatrixSymbol('state_err', dim_state_err, 1)
state_err = sp.Matrix(state_err_sym)
quat_err = state_err[States.ECEF_ORIENTATION_ERR, :]
v_err = state_err[States.ECEF_VELOCITY_ERR, :]
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
state_err_dot = sp.Matrix(np.zeros((dim_state_err, 1)))
state_err_dot[States.ECEF_POS_ERR, :] = v_err
state_err_dot[States.ECEF_ORIENTATION_ERR, :] = q_err_dot
state_err_dot[States.ECEF_VELOCITY_ERR, :] = quat_err_matrix * quat_rot * (acceleration + acceleration_err)
f_err_sym = state_err + dt * state_err_dot
# Observation matrix modifier
H_mod_sym = sp.Matrix(np.zeros((dim_state, dim_state_err)))
H_mod_sym[States.ECEF_POS, States.ECEF_POS_ERR] = np.eye(States.ECEF_POS.stop - States.ECEF_POS.start)
H_mod_sym[States.ECEF_ORIENTATION, States.ECEF_ORIENTATION_ERR] = 0.5 * quat_matrix_r(state[3:7])[:, 1:]
H_mod_sym[States.ECEF_ORIENTATION.stop:, States.ECEF_ORIENTATION_ERR.stop:] = np.eye(dim_state - States.ECEF_ORIENTATION.stop)
# these error functions are defined so that say there
# is a nominal x and true x:
# true x = err_function(nominal x, delta x)
# delta x = inv_err_function(nominal x, true x)
nom_x = sp.MatrixSymbol('nom_x', dim_state, 1)
true_x = sp.MatrixSymbol('true_x', dim_state, 1)
delta_x = sp.MatrixSymbol('delta_x', dim_state_err, 1)
err_function_sym = sp.Matrix(np.zeros((dim_state, 1)))
delta_quat = sp.Matrix(np.ones(4))
delta_quat[1:, :] = sp.Matrix(0.5 * delta_x[States.ECEF_ORIENTATION_ERR, :])
err_function_sym[States.ECEF_POS, :] = sp.Matrix(nom_x[States.ECEF_POS, :] + delta_x[States.ECEF_POS_ERR, :])
err_function_sym[States.ECEF_ORIENTATION, 0] = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]) * delta_quat
err_function_sym[States.ECEF_ORIENTATION.stop:, :] = sp.Matrix(nom_x[States.ECEF_ORIENTATION.stop:, :] + delta_x[States.ECEF_ORIENTATION_ERR.stop:, :])
inv_err_function_sym = sp.Matrix(np.zeros((dim_state_err, 1)))
inv_err_function_sym[States.ECEF_POS_ERR, 0] = sp.Matrix(-nom_x[States.ECEF_POS, 0] + true_x[States.ECEF_POS, 0])
delta_quat = quat_matrix_r(nom_x[States.ECEF_ORIENTATION, 0]).T * true_x[States.ECEF_ORIENTATION, 0]
inv_err_function_sym[States.ECEF_ORIENTATION_ERR, 0] = sp.Matrix(2 * delta_quat[1:])
inv_err_function_sym[States.ECEF_ORIENTATION_ERR.stop:, 0] = sp.Matrix(-nom_x[States.ECEF_ORIENTATION.stop:, 0] + true_x[States.ECEF_ORIENTATION.stop:, 0])
eskf_params = [[err_function_sym, nom_x, delta_x],
[inv_err_function_sym, nom_x, true_x],
H_mod_sym, f_err_sym, state_err_sym]
#
# Observation functions
#
h_gyro_sym = sp.Matrix([
vroll + roll_bias,
vpitch + pitch_bias,
vyaw + yaw_bias])
pos = sp.Matrix([x, y, z])
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2)**(3.0 / 2.0))) * pos)
h_acc_sym = (gravity + acceleration + acc_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_orientation_sym = q
h_relative_motion = sp.Matrix(quat_rot.T * v)
obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
[h_vel_sym, ObservationKind.ECEF_VEL, None],
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
# this returns a sympy routine for the jacobian of the observation function of the local vel
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
# write constants to extra header file for use in cpp
live_kf_header = "#pragma once\n\n"
live_kf_header += "#include <unordered_map>\n"
live_kf_header += "#include <eigen3/Eigen/Dense>\n\n"
for state, slc in inspect.getmembers(States, lambda x: isinstance(x, slice)):
assert(slc.step is None) # unsupported
live_kf_header += f'#define STATE_{state}_START {slc.start}\n'
live_kf_header += f'#define STATE_{state}_END {slc.stop}\n'
live_kf_header += f'#define STATE_{state}_LEN {slc.stop - slc.start}\n'
live_kf_header += "\n"
for kind, val in inspect.getmembers(ObservationKind, lambda x: isinstance(x, int)):
live_kf_header += f'#define OBSERVATION_{kind} {val}\n'
live_kf_header += "\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
live_kf_header += "static const std::unordered_map<int, Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor>> live_obs_noise_diag = {\n"
for kind, noise in LiveKalman.obs_noise_diag.items():
live_kf_header += f" {{ {kind}, {numpy2eigenstring(noise)} }},\n"
live_kf_header += "};\n\n"
open(os.path.join(generated_dir, "live_kf_constants.h"), 'w').write(live_kf_header)
if __name__ == "__main__":
generated_dir = sys.argv[2]
LiveKalman.generate_code(generated_dir)
@@ -0,0 +1 @@
out/
@@ -1,73 +0,0 @@
"""
Copyright (c) 2021-, sunnypilot, and a number of other contributors.
This file is part of sunnypilot and is licensed under the MIT License.
See the LICENSE.md file in the root directory for more details.
"""
import math
from types import SimpleNamespace
from openpilot.common.test import OpenpilotTestCase
from openpilot.sunnypilot.selfdrive.locationd.gps_fuser import EARTH_RADIUS, GPS_LATENCY, GpsFuser
LAT0, LON0 = 39.0, -77.0
LATENCY = GPS_LATENCY['gpsLocation']
def fix(north, east, speed=30.0, bearing=90.0, h_acc=0.0, v_acc=1.5):
return SimpleNamespace(latitude=LAT0 + math.degrees(north / EARTH_RADIUS),
longitude=LON0 + math.degrees(east / (EARTH_RADIUS * math.cos(math.radians(LAT0)))),
speed=speed, bearingDeg=bearing, horizontalAccuracy=h_acc, verticalAccuracy=v_acc,
speedAccuracy=0.3, bearingAccuracyDeg=0.5, hasFix=True)
def position(f: GpsFuser) -> tuple[float, float]:
north = math.radians(f.lat - LAT0) * EARTH_RADIUS
east = math.radians(f.lon - LON0) * EARTH_RADIUS * math.cos(math.radians(LAT0))
return north, east
class TestGpsFuser(OpenpilotTestCase):
def drive(self, f, seconds, speed=30.0, start=0.0, gps=lambda t, true_east: fix(0.0, true_east)):
t = start
while t < start + seconds:
t = round(t + 0.05, 2)
f.predict(t, speed, 0.0)
if abs(t - round(t)) < 1e-6:
f.update_gps(t, gps(t, speed * (t - LATENCY)), LATENCY)
return t
def test_compensates_latency(self):
f = GpsFuser()
t = self.drive(f, 30)
self.assertTrue(f.valid)
north, east = position(f)
self.assertAlmostEqual(east, 30.0 * t, delta=3.0)
self.assertAlmostEqual(north, 0.0, delta=1.0)
def test_rejects_single_jump(self):
f = GpsFuser()
t = self.drive(f, 20)
f.predict(t + 0.05, 30.0, 0.0)
f.update_gps(t + 0.05, fix(40.0, 30.0 * (t + 0.05 - LATENCY)), LATENCY)
self.assertAlmostEqual(position(f)[0], 0.0, delta=2.0)
def test_follows_persistent_offset(self):
f = GpsFuser()
t = self.drive(f, 20)
self.drive(f, 10, start=t, gps=lambda _, e: fix(150.0, e))
self.assertAlmostEqual(position(f)[0], 150.0, delta=3.0)
def test_ignores_bearing_when_slow(self):
f = GpsFuser()
t = self.drive(f, 20)
self.drive(f, 5, speed=1.0, start=t, gps=lambda _, e: fix(0.0, e, speed=1.0, bearing=270.0))
self.assertAlmostEqual(math.degrees(f.heading), 90.0, delta=1.0)
def test_invalid_after_long_outage(self):
f = GpsFuser()
t = self.drive(f, 20)
self.drive(f, 120, start=t, gps=lambda _, e: SimpleNamespace(hasFix=False, horizontalAccuracy=0.0, verticalAccuracy=0.0,
speedAccuracy=0.0, latitude=0.0, longitude=0.0))
self.assertFalse(f.valid)
@@ -0,0 +1,96 @@
import platform
import unittest
import json
import random
import time
import capnp
import openpilot.cereal.messaging as messaging
from openpilot.cereal.services import SERVICE_LIST
from openpilot.common.params import Params
from openpilot.common.transformations.coordinates import ecef2geodetic
from openpilot.system.manager.process_config import managed_processes
from openpilot.common.test import OpenpilotTestCase
@unittest.skipIf(platform.system() == 'Darwin', "msgq unsupported on macOS")
class TestLocationdProc(OpenpilotTestCase):
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'extrinsicsCalibration',
'accelerometer', 'gyroscope']
def setup_method(self):
self.pm = messaging.PubMaster(self.LLD_MSGS)
self.params = Params()
self.params.put_bool("UbloxAvailable", True)
managed_processes['locationd_llk'].start()
def teardown_method(self):
managed_processes['locationd_llk'].stop()
def get_msg(self, name, t):
try:
msg = messaging.new_message(name)
except capnp.lib.capnp.KjException:
msg = messaging.new_message(name, 0)
if name == "gpsLocationExternal":
msg.gpsLocationExternal.flags = 1
msg.gpsLocationExternal.hasFix = True
msg.gpsLocationExternal.verticalAccuracy = 1.0
msg.gpsLocationExternal.speedAccuracy = 1.0
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
msg.gpsLocationExternal.latitude = float(self.lat)
msg.gpsLocationExternal.longitude = float(self.lon)
msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
msg.gpsLocationExternal.altitude = float(self.alt)
#if name == "gnssMeasurements":
# msg.gnssMeasurements.measTime = t
# msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
# msg.gnssMeasurements.positionECEF.std = [0,0,0]
# msg.gnssMeasurements.positionECEF.valid = True
# msg.gnssMeasurements.velocityECEF.value = []
# msg.gnssMeasurements.velocityECEF.std = [0,0,0]
# msg.gnssMeasurements.velocityECEF.valid = True
elif name == 'cameraOdometry':
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]
msg.cameraOdometry.trans = [0.0, 0.0, 0.0]
msg.cameraOdometry.transStd = [0.0, 0.0, 0.0]
msg.logMonoTime = t
msg.valid = True
return msg
def test_params_gps(self):
random.seed(123489234)
self.params.remove('LastGPSPositionLLK')
self.x = -2710700 + (random.random() * 1e5)
self.y = -4280600 + (random.random() * 1e5)
self.z = 3850300 + (random.random() * 1e5)
self.lat, self.lon, self.alt = ecef2geodetic([self.x, self.y, self.z])
# get fake messages at the correct frequency, listed in services.py
msgs = []
for sec in range(65):
for name in self.LLD_MSGS:
for j in range(int(SERVICE_LIST[name].frequency)):
msgs.append(self.get_msg(name, int((sec + j / SERVICE_LIST[name].frequency) * 1e9)))
for msg in sorted(msgs, key=lambda x: x.logMonoTime):
self.pm.send(msg.which(), msg)
if msg.which() == "cameraOdometry":
self.pm.wait_for_readers_to_update(msg.which(), timeout=1, dt=0.005)
for _ in range(50):
val = self.params.get('LastGPSPositionLLK')
if val is not None:
break
time.sleep(0.1)
self.assertIsNotNone(val, "LastGPSPositionLLK not written within 5s")
lastGPS = json.loads(val)
self.assertAlmostEqual(lastGPS['latitude'], self.lat, delta=0.001)
self.assertAlmostEqual(lastGPS['longitude'], self.lon, delta=0.001)
self.assertAlmostEqual(lastGPS['altitude'], self.alt, delta=0.001)
+14 -6
View File
@@ -253,12 +253,6 @@ def hardware_thread(end_event, hw_queue) -> None:
chestnut_status = ChestnutStatus()
branch = get_short_branch()
build_metadata = get_build_metadata()
is_unsupported_combo = COMMA_HARDWARE and HARDWARE.get_device_type() == "tici" and build_metadata.channel_type != "tici"
startup_conditions["not_tici"] = not is_unsupported_combo
onroad_conditions["not_tici"] = not is_unsupported_combo
set_offroad_alert_if_changed("Offroad_TiciSupport", is_unsupported_combo, extra_text=build_metadata.channel)
while not end_event.is_set():
sm.update(PANDA_STATES_TIMEOUT)
@@ -381,6 +375,16 @@ def hardware_thread(end_event, hw_queue) -> None:
startup_conditions["not_always_offroad"] = not offroad_mode
onroad_conditions["not_always_offroad"] = not offroad_mode
# if an unsupported device and branch is detected, going onroad is blocked
# only allow going onroad when:
# - TIZI, or
# - TICI and channel_type is "tici"
build_metadata = get_build_metadata()
is_unsupported_combo = COMMA_HARDWARE and HARDWARE.get_device_type() == "tici" and build_metadata.channel_type != "tici"
startup_conditions["not_tici"] = not is_unsupported_combo
onroad_conditions["not_tici"] = not is_unsupported_combo
set_offroad_alert("Offroad_TiciSupport", is_unsupported_combo, extra_text=build_metadata.channel)
# if the temperature enters the danger zone, go offroad to cool down
onroad_conditions["device_temp_good"] = thermal_status < ThermalStatus.critical
extra_text = f"{offroad_comp_temp:.1f}C"
@@ -439,6 +443,10 @@ def hardware_thread(end_event, hw_queue) -> None:
# Offroad power monitoring
voltage = None if peripheralState.pandaType == log.PandaState.PandaType.unknown else peripheralState.voltage
# GitHub runner auto off: 9V is used as the threshold because most desktop runners
# will rarely exceed 5V so 9V is set as our buffer between desk use and car use.
params.put_bool("GithubRunnerSufficientVoltage", ((voltage or 0) and voltage > 9000))
power_monitor.calculate(voltage, onroad_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
+2 -1
View File
@@ -184,7 +184,8 @@ procs += [
NativeProcess("mapd", Paths.mapd_root(), ["bash", "-c", f"{MAPD_PATH} > /dev/null 2>&1"], mapd_ready),
PythonProcess("mapd_manager", "openpilot.sunnypilot.mapd.mapd_manager", always_run),
PythonProcess("gps_fuser", "openpilot.sunnypilot.selfdrive.locationd.gps_fuser", only_onroad),
# locationd
NativeProcess("locationd_llk", "openpilot/sunnypilot/selfdrive/locationd", ["./locationd"], only_onroad),
]
if os.path.exists("../../sunnypilot/sunnylink/uploader.py"):