From 99b637c7ce0368a2edfb7765206b2a2f0bab8ef7 Mon Sep 17 00:00:00 2001 From: Vehicle Researcher Date: Fri, 13 Dec 2019 13:03:08 -0800 Subject: [PATCH] openpilot v0.7 release old-commit-hash: c025b96e8a15640ee4d6e4d513fada6ed101afe5 --- .github/ISSUE_TEMPLATE/bug_report.md | 2 +- .gitignore | 12 +- Dockerfile.openpilot | 10 +- Pipfile | 4 +- Pipfile.lock | 4 +- README.md | 193 +- README_chffrplus.md | 36 - RELEASES.md | 15 + SConstruct | 212 +++ apk/ai.comma.plus.frame.apk | 4 +- apk/ai.comma.plus.offroad.apk | 4 +- common/.gitignore | 1 + common/SConscript | 6 + common/android.py | 73 + common/apk.py | 98 + common/common_pyx_setup.py | 20 + common/dbc.py | 276 --- common/kalman/Makefile | 10 - common/kalman/SConscript | 6 + common/kalman/simple_kalman.py | 8 +- common/kalman/simple_kalman_setup.py | 2 +- common/params.py | 5 +- common/realtime.py | 10 +- common/spinner.py | 33 +- common/transformations/camera.py | 6 +- installer/updater/update.json | 8 +- launch_chffrplus.sh | 13 +- models/driving_model.dlc | 4 +- models/monitoring_model.dlc | 3 - models/monitoring_model_q.dlc | 3 + panda/board/obj/panda.bin.signed | Bin 28676 -> 31596 bytes phonelibs/SConscript | 3 + phonelibs/boringssl/LICENSE.boringssl | 3 + phonelibs/eigen/COPYING.BSD | 3 + phonelibs/eigen/COPYING.GPL | 3 + phonelibs/eigen/COPYING.LGPL | 3 + phonelibs/eigen/COPYING.MINPACK | 3 + phonelibs/eigen/COPYING.MPL2 | 3 + phonelibs/eigen/COPYING.README | 3 + phonelibs/eigen/Eigen/CMakeLists.txt | 3 + phonelibs/eigen/Eigen/Cholesky | 3 + phonelibs/eigen/Eigen/CholmodSupport | 3 + phonelibs/eigen/Eigen/Core | 3 + phonelibs/eigen/Eigen/Dense | 3 + phonelibs/eigen/Eigen/Eigen | 3 + phonelibs/eigen/Eigen/Eigenvalues | 3 + phonelibs/eigen/Eigen/Geometry | 3 + phonelibs/eigen/Eigen/Householder | 3 + phonelibs/eigen/Eigen/IterativeLinearSolvers | 3 + phonelibs/eigen/Eigen/Jacobi | 3 + phonelibs/eigen/Eigen/LU | 3 + phonelibs/eigen/Eigen/MetisSupport | 3 + phonelibs/eigen/Eigen/OrderingMethods | 3 + phonelibs/eigen/Eigen/PaStiXSupport | 3 + phonelibs/eigen/Eigen/PardisoSupport | 3 + phonelibs/eigen/Eigen/QR | 3 + phonelibs/eigen/Eigen/QtAlignedMalloc | 3 + phonelibs/eigen/Eigen/SPQRSupport | 3 + phonelibs/eigen/Eigen/SVD | 3 + phonelibs/eigen/Eigen/Sparse | 3 + phonelibs/eigen/Eigen/SparseCholesky | 3 + phonelibs/eigen/Eigen/SparseCore | 3 + phonelibs/eigen/Eigen/SparseLU | 3 + phonelibs/eigen/Eigen/SparseQR | 3 + phonelibs/eigen/Eigen/StdDeque | 3 + phonelibs/eigen/Eigen/StdList | 3 + phonelibs/eigen/Eigen/StdVector | 3 + phonelibs/eigen/Eigen/SuperLUSupport | 3 + phonelibs/eigen/Eigen/UmfPackSupport | 3 + phonelibs/eigen/Eigen/src/Cholesky/LDLT.h | 3 + phonelibs/eigen/Eigen/src/Cholesky/LLT.h | 3 + .../eigen/Eigen/src/Cholesky/LLT_LAPACKE.h | 3 + .../Eigen/src/CholmodSupport/CholmodSupport.h | 3 + phonelibs/eigen/Eigen/src/Core/Array.h | 3 + phonelibs/eigen/Eigen/src/Core/ArrayBase.h | 3 + phonelibs/eigen/Eigen/src/Core/ArrayWrapper.h | 3 + phonelibs/eigen/Eigen/src/Core/Assign.h | 3 + .../eigen/Eigen/src/Core/AssignEvaluator.h | 3 + phonelibs/eigen/Eigen/src/Core/Assign_MKL.h | 3 + phonelibs/eigen/Eigen/src/Core/BandMatrix.h | 3 + phonelibs/eigen/Eigen/src/Core/Block.h | 3 + phonelibs/eigen/Eigen/src/Core/BooleanRedux.h | 3 + .../eigen/Eigen/src/Core/CommaInitializer.h | 3 + .../eigen/Eigen/src/Core/ConditionEstimator.h | 3 + .../eigen/Eigen/src/Core/CoreEvaluators.h | 3 + .../eigen/Eigen/src/Core/CoreIterators.h | 3 + .../eigen/Eigen/src/Core/CwiseBinaryOp.h | 3 + .../eigen/Eigen/src/Core/CwiseNullaryOp.h | 3 + .../eigen/Eigen/src/Core/CwiseTernaryOp.h | 3 + phonelibs/eigen/Eigen/src/Core/CwiseUnaryOp.h | 3 + .../eigen/Eigen/src/Core/CwiseUnaryView.h | 3 + phonelibs/eigen/Eigen/src/Core/DenseBase.h | 3 + .../eigen/Eigen/src/Core/DenseCoeffsBase.h | 3 + phonelibs/eigen/Eigen/src/Core/DenseStorage.h | 3 + phonelibs/eigen/Eigen/src/Core/Diagonal.h | 3 + .../eigen/Eigen/src/Core/DiagonalMatrix.h | 3 + .../eigen/Eigen/src/Core/DiagonalProduct.h | 3 + phonelibs/eigen/Eigen/src/Core/Dot.h | 3 + phonelibs/eigen/Eigen/src/Core/EigenBase.h | 3 + .../eigen/Eigen/src/Core/ForceAlignedAccess.h | 3 + phonelibs/eigen/Eigen/src/Core/Fuzzy.h | 3 + .../eigen/Eigen/src/Core/GeneralProduct.h | 3 + .../eigen/Eigen/src/Core/GenericPacketMath.h | 3 + .../eigen/Eigen/src/Core/GlobalFunctions.h | 3 + phonelibs/eigen/Eigen/src/Core/IO.h | 3 + phonelibs/eigen/Eigen/src/Core/Inverse.h | 3 + phonelibs/eigen/Eigen/src/Core/Map.h | 3 + phonelibs/eigen/Eigen/src/Core/MapBase.h | 3 + .../eigen/Eigen/src/Core/MathFunctions.h | 3 + .../eigen/Eigen/src/Core/MathFunctionsImpl.h | 3 + phonelibs/eigen/Eigen/src/Core/Matrix.h | 3 + phonelibs/eigen/Eigen/src/Core/MatrixBase.h | 3 + phonelibs/eigen/Eigen/src/Core/NestByValue.h | 3 + phonelibs/eigen/Eigen/src/Core/NoAlias.h | 3 + phonelibs/eigen/Eigen/src/Core/NumTraits.h | 3 + .../eigen/Eigen/src/Core/PermutationMatrix.h | 3 + .../eigen/Eigen/src/Core/PlainObjectBase.h | 3 + phonelibs/eigen/Eigen/src/Core/Product.h | 3 + .../eigen/Eigen/src/Core/ProductEvaluators.h | 3 + phonelibs/eigen/Eigen/src/Core/Random.h | 3 + phonelibs/eigen/Eigen/src/Core/Redux.h | 3 + phonelibs/eigen/Eigen/src/Core/Ref.h | 3 + phonelibs/eigen/Eigen/src/Core/Replicate.h | 3 + .../eigen/Eigen/src/Core/ReturnByValue.h | 3 + phonelibs/eigen/Eigen/src/Core/Reverse.h | 3 + phonelibs/eigen/Eigen/src/Core/Select.h | 3 + .../eigen/Eigen/src/Core/SelfAdjointView.h | 3 + .../eigen/Eigen/src/Core/SelfCwiseBinaryOp.h | 3 + phonelibs/eigen/Eigen/src/Core/Solve.h | 3 + .../eigen/Eigen/src/Core/SolveTriangular.h | 3 + phonelibs/eigen/Eigen/src/Core/SolverBase.h | 3 + phonelibs/eigen/Eigen/src/Core/StableNorm.h | 3 + phonelibs/eigen/Eigen/src/Core/Stride.h | 3 + phonelibs/eigen/Eigen/src/Core/Swap.h | 3 + phonelibs/eigen/Eigen/src/Core/Transpose.h | 3 + .../eigen/Eigen/src/Core/Transpositions.h | 3 + .../eigen/Eigen/src/Core/TriangularMatrix.h | 3 + phonelibs/eigen/Eigen/src/Core/VectorBlock.h | 3 + phonelibs/eigen/Eigen/src/Core/VectorwiseOp.h | 3 + phonelibs/eigen/Eigen/src/Core/Visitor.h | 3 + .../eigen/Eigen/src/Core/arch/AVX/Complex.h | 3 + .../Eigen/src/Core/arch/AVX/MathFunctions.h | 3 + .../Eigen/src/Core/arch/AVX/PacketMath.h | 3 + .../Eigen/src/Core/arch/AVX/TypeCasting.h | 3 + .../src/Core/arch/AVX512/MathFunctions.h | 3 + .../Eigen/src/Core/arch/AVX512/PacketMath.h | 3 + .../Eigen/src/Core/arch/AltiVec/Complex.h | 3 + .../src/Core/arch/AltiVec/MathFunctions.h | 3 + .../Eigen/src/Core/arch/AltiVec/PacketMath.h | 3 + .../eigen/Eigen/src/Core/arch/CUDA/Complex.h | 3 + .../eigen/Eigen/src/Core/arch/CUDA/Half.h | 3 + .../Eigen/src/Core/arch/CUDA/MathFunctions.h | 3 + .../Eigen/src/Core/arch/CUDA/PacketMath.h | 3 + .../Eigen/src/Core/arch/CUDA/PacketMathHalf.h | 3 + .../Eigen/src/Core/arch/CUDA/TypeCasting.h | 3 + .../Eigen/src/Core/arch/Default/Settings.h | 3 + .../eigen/Eigen/src/Core/arch/NEON/Complex.h | 3 + .../Eigen/src/Core/arch/NEON/MathFunctions.h | 3 + .../Eigen/src/Core/arch/NEON/PacketMath.h | 3 + .../eigen/Eigen/src/Core/arch/SSE/Complex.h | 3 + .../Eigen/src/Core/arch/SSE/MathFunctions.h | 3 + .../Eigen/src/Core/arch/SSE/PacketMath.h | 3 + .../Eigen/src/Core/arch/SSE/TypeCasting.h | 3 + .../Eigen/src/Core/arch/ZVector/Complex.h | 3 + .../src/Core/arch/ZVector/MathFunctions.h | 3 + .../Eigen/src/Core/arch/ZVector/PacketMath.h | 3 + .../src/Core/functors/AssignmentFunctors.h | 3 + .../Eigen/src/Core/functors/BinaryFunctors.h | 3 + .../Eigen/src/Core/functors/NullaryFunctors.h | 3 + .../Eigen/src/Core/functors/StlFunctors.h | 3 + .../Eigen/src/Core/functors/TernaryFunctors.h | 3 + .../Eigen/src/Core/functors/UnaryFunctors.h | 3 + .../Core/products/GeneralBlockPanelKernel.h | 3 + .../src/Core/products/GeneralMatrixMatrix.h | 3 + .../products/GeneralMatrixMatrixTriangular.h | 3 + .../GeneralMatrixMatrixTriangular_BLAS.h | 3 + .../Core/products/GeneralMatrixMatrix_BLAS.h | 3 + .../src/Core/products/GeneralMatrixVector.h | 3 + .../Core/products/GeneralMatrixVector_BLAS.h | 3 + .../Eigen/src/Core/products/Parallelizer.h | 3 + .../Core/products/SelfadjointMatrixMatrix.h | 3 + .../products/SelfadjointMatrixMatrix_BLAS.h | 3 + .../Core/products/SelfadjointMatrixVector.h | 3 + .../products/SelfadjointMatrixVector_BLAS.h | 3 + .../src/Core/products/SelfadjointProduct.h | 3 + .../Core/products/SelfadjointRank2Update.h | 3 + .../Core/products/TriangularMatrixMatrix.h | 3 + .../products/TriangularMatrixMatrix_BLAS.h | 3 + .../Core/products/TriangularMatrixVector.h | 3 + .../products/TriangularMatrixVector_BLAS.h | 3 + .../Core/products/TriangularSolverMatrix.h | 3 + .../products/TriangularSolverMatrix_BLAS.h | 3 + .../Core/products/TriangularSolverVector.h | 3 + .../eigen/Eigen/src/Core/util/BlasUtil.h | 3 + .../eigen/Eigen/src/Core/util/Constants.h | 3 + .../src/Core/util/DisableStupidWarnings.h | 3 + .../Eigen/src/Core/util/ForwardDeclarations.h | 3 + .../eigen/Eigen/src/Core/util/MKL_support.h | 3 + phonelibs/eigen/Eigen/src/Core/util/Macros.h | 3 + phonelibs/eigen/Eigen/src/Core/util/Memory.h | 3 + phonelibs/eigen/Eigen/src/Core/util/Meta.h | 3 + phonelibs/eigen/Eigen/src/Core/util/NonMPL2.h | 3 + .../src/Core/util/ReenableStupidWarnings.h | 3 + .../eigen/Eigen/src/Core/util/StaticAssert.h | 3 + .../eigen/Eigen/src/Core/util/XprHelper.h | 3 + .../src/Eigenvalues/ComplexEigenSolver.h | 3 + .../Eigen/src/Eigenvalues/ComplexSchur.h | 3 + .../src/Eigenvalues/ComplexSchur_LAPACKE.h | 3 + .../eigen/Eigen/src/Eigenvalues/EigenSolver.h | 3 + .../src/Eigenvalues/GeneralizedEigenSolver.h | 3 + .../GeneralizedSelfAdjointEigenSolver.h | 3 + .../src/Eigenvalues/HessenbergDecomposition.h | 3 + .../src/Eigenvalues/MatrixBaseEigenvalues.h | 3 + .../eigen/Eigen/src/Eigenvalues/RealQZ.h | 3 + .../eigen/Eigen/src/Eigenvalues/RealSchur.h | 3 + .../Eigen/src/Eigenvalues/RealSchur_LAPACKE.h | 3 + .../src/Eigenvalues/SelfAdjointEigenSolver.h | 3 + .../SelfAdjointEigenSolver_LAPACKE.h | 3 + .../src/Eigenvalues/Tridiagonalization.h | 3 + .../eigen/Eigen/src/Geometry/AlignedBox.h | 3 + .../eigen/Eigen/src/Geometry/AngleAxis.h | 3 + .../eigen/Eigen/src/Geometry/EulerAngles.h | 3 + .../eigen/Eigen/src/Geometry/Homogeneous.h | 3 + .../eigen/Eigen/src/Geometry/Hyperplane.h | 3 + .../eigen/Eigen/src/Geometry/OrthoMethods.h | 3 + .../Eigen/src/Geometry/ParametrizedLine.h | 3 + .../eigen/Eigen/src/Geometry/Quaternion.h | 3 + .../eigen/Eigen/src/Geometry/Rotation2D.h | 3 + .../eigen/Eigen/src/Geometry/RotationBase.h | 3 + phonelibs/eigen/Eigen/src/Geometry/Scaling.h | 3 + .../eigen/Eigen/src/Geometry/Transform.h | 3 + .../eigen/Eigen/src/Geometry/Translation.h | 3 + phonelibs/eigen/Eigen/src/Geometry/Umeyama.h | 3 + .../Eigen/src/Geometry/arch/Geometry_SSE.h | 3 + .../Eigen/src/Householder/BlockHouseholder.h | 3 + .../eigen/Eigen/src/Householder/Householder.h | 3 + .../src/Householder/HouseholderSequence.h | 3 + .../BasicPreconditioners.h | 3 + .../src/IterativeLinearSolvers/BiCGSTAB.h | 3 + .../ConjugateGradient.h | 3 + .../IncompleteCholesky.h | 3 + .../IterativeLinearSolvers/IncompleteLUT.h | 3 + .../IterativeSolverBase.h | 3 + .../LeastSquareConjugateGradient.h | 3 + .../IterativeLinearSolvers/SolveWithGuess.h | 3 + phonelibs/eigen/Eigen/src/Jacobi/Jacobi.h | 3 + phonelibs/eigen/Eigen/src/LU/Determinant.h | 3 + phonelibs/eigen/Eigen/src/LU/FullPivLU.h | 3 + phonelibs/eigen/Eigen/src/LU/InverseImpl.h | 3 + phonelibs/eigen/Eigen/src/LU/PartialPivLU.h | 3 + .../eigen/Eigen/src/LU/PartialPivLU_LAPACKE.h | 3 + .../eigen/Eigen/src/LU/arch/Inverse_SSE.h | 3 + .../Eigen/src/MetisSupport/MetisSupport.h | 3 + .../eigen/Eigen/src/OrderingMethods/Amd.h | 3 + .../Eigen/src/OrderingMethods/Eigen_Colamd.h | 3 + .../Eigen/src/OrderingMethods/Ordering.h | 3 + .../Eigen/src/PaStiXSupport/PaStiXSupport.h | 3 + .../Eigen/src/PardisoSupport/PardisoSupport.h | 3 + .../eigen/Eigen/src/QR/ColPivHouseholderQR.h | 3 + .../src/QR/ColPivHouseholderQR_LAPACKE.h | 3 + .../src/QR/CompleteOrthogonalDecomposition.h | 3 + .../eigen/Eigen/src/QR/FullPivHouseholderQR.h | 3 + phonelibs/eigen/Eigen/src/QR/HouseholderQR.h | 3 + .../Eigen/src/QR/HouseholderQR_LAPACKE.h | 3 + .../src/SPQRSupport/SuiteSparseQRSupport.h | 3 + phonelibs/eigen/Eigen/src/SVD/BDCSVD.h | 3 + phonelibs/eigen/Eigen/src/SVD/JacobiSVD.h | 3 + .../eigen/Eigen/src/SVD/JacobiSVD_LAPACKE.h | 3 + phonelibs/eigen/Eigen/src/SVD/SVDBase.h | 3 + .../Eigen/src/SVD/UpperBidiagonalization.h | 3 + .../src/SparseCholesky/SimplicialCholesky.h | 3 + .../SparseCholesky/SimplicialCholesky_impl.h | 3 + .../eigen/Eigen/src/SparseCore/AmbiVector.h | 3 + .../Eigen/src/SparseCore/CompressedStorage.h | 3 + .../ConservativeSparseSparseProduct.h | 3 + .../Eigen/src/SparseCore/MappedSparseMatrix.h | 3 + .../eigen/Eigen/src/SparseCore/SparseAssign.h | 3 + .../eigen/Eigen/src/SparseCore/SparseBlock.h | 3 + .../Eigen/src/SparseCore/SparseColEtree.h | 3 + .../src/SparseCore/SparseCompressedBase.h | 3 + .../src/SparseCore/SparseCwiseBinaryOp.h | 3 + .../Eigen/src/SparseCore/SparseCwiseUnaryOp.h | 3 + .../Eigen/src/SparseCore/SparseDenseProduct.h | 3 + .../src/SparseCore/SparseDiagonalProduct.h | 3 + .../eigen/Eigen/src/SparseCore/SparseDot.h | 3 + .../eigen/Eigen/src/SparseCore/SparseFuzzy.h | 3 + .../eigen/Eigen/src/SparseCore/SparseMap.h | 3 + .../eigen/Eigen/src/SparseCore/SparseMatrix.h | 3 + .../Eigen/src/SparseCore/SparseMatrixBase.h | 3 + .../Eigen/src/SparseCore/SparsePermutation.h | 3 + .../Eigen/src/SparseCore/SparseProduct.h | 3 + .../eigen/Eigen/src/SparseCore/SparseRedux.h | 3 + .../eigen/Eigen/src/SparseCore/SparseRef.h | 3 + .../src/SparseCore/SparseSelfAdjointView.h | 3 + .../Eigen/src/SparseCore/SparseSolverBase.h | 3 + .../SparseSparseProductWithPruning.h | 3 + .../Eigen/src/SparseCore/SparseTranspose.h | 3 + .../src/SparseCore/SparseTriangularView.h | 3 + .../eigen/Eigen/src/SparseCore/SparseUtil.h | 3 + .../eigen/Eigen/src/SparseCore/SparseVector.h | 3 + .../eigen/Eigen/src/SparseCore/SparseView.h | 3 + .../Eigen/src/SparseCore/TriangularSolver.h | 3 + phonelibs/eigen/Eigen/src/SparseLU/SparseLU.h | 3 + .../eigen/Eigen/src/SparseLU/SparseLUImpl.h | 3 + .../Eigen/src/SparseLU/SparseLU_Memory.h | 3 + .../Eigen/src/SparseLU/SparseLU_Structs.h | 3 + .../src/SparseLU/SparseLU_SupernodalMatrix.h | 3 + .../eigen/Eigen/src/SparseLU/SparseLU_Utils.h | 3 + .../Eigen/src/SparseLU/SparseLU_column_bmod.h | 3 + .../Eigen/src/SparseLU/SparseLU_column_dfs.h | 3 + .../src/SparseLU/SparseLU_copy_to_ucol.h | 3 + .../Eigen/src/SparseLU/SparseLU_gemm_kernel.h | 3 + .../src/SparseLU/SparseLU_heap_relax_snode.h | 3 + .../Eigen/src/SparseLU/SparseLU_kernel_bmod.h | 3 + .../Eigen/src/SparseLU/SparseLU_panel_bmod.h | 3 + .../Eigen/src/SparseLU/SparseLU_panel_dfs.h | 3 + .../Eigen/src/SparseLU/SparseLU_pivotL.h | 3 + .../Eigen/src/SparseLU/SparseLU_pruneL.h | 3 + .../Eigen/src/SparseLU/SparseLU_relax_snode.h | 3 + phonelibs/eigen/Eigen/src/SparseQR/SparseQR.h | 3 + .../eigen/Eigen/src/StlSupport/StdDeque.h | 3 + .../eigen/Eigen/src/StlSupport/StdList.h | 3 + .../eigen/Eigen/src/StlSupport/StdVector.h | 3 + .../eigen/Eigen/src/StlSupport/details.h | 3 + .../Eigen/src/SuperLUSupport/SuperLUSupport.h | 3 + .../Eigen/src/UmfPackSupport/UmfPackSupport.h | 3 + phonelibs/eigen/Eigen/src/misc/Image.h | 3 + phonelibs/eigen/Eigen/src/misc/Kernel.h | 3 + phonelibs/eigen/Eigen/src/misc/RealSvd2x2.h | 3 + phonelibs/eigen/Eigen/src/misc/blas.h | 3 + phonelibs/eigen/Eigen/src/misc/lapack.h | 3 + phonelibs/eigen/Eigen/src/misc/lapacke.h | 3 + .../eigen/Eigen/src/misc/lapacke_mangling.h | 3 + .../Eigen/src/plugins/ArrayCwiseBinaryOps.h | 3 + .../Eigen/src/plugins/ArrayCwiseUnaryOps.h | 3 + .../eigen/Eigen/src/plugins/BlockMethods.h | 3 + .../Eigen/src/plugins/CommonCwiseBinaryOps.h | 3 + .../Eigen/src/plugins/CommonCwiseUnaryOps.h | 3 + .../Eigen/src/plugins/MatrixCwiseBinaryOps.h | 3 + .../Eigen/src/plugins/MatrixCwiseUnaryOps.h | 3 + phonelibs/eigen/INSTALL | 3 + phonelibs/eigen/README.md | 3 + phonelibs/eigen/build.sh | 3 + .../eigen/signature_of_eigen3_matrix_library | 3 + phonelibs/eigen/unsupported/CMakeLists.txt | 3 + .../eigen/unsupported/Eigen/AdolcForward | 3 + .../eigen/unsupported/Eigen/AlignedVector3 | 3 + .../eigen/unsupported/Eigen/ArpackSupport | 3 + phonelibs/eigen/unsupported/Eigen/AutoDiff | 3 + phonelibs/eigen/unsupported/Eigen/BVH | 3 + .../eigen/unsupported/Eigen/CMakeLists.txt | 3 + .../unsupported/Eigen/CXX11/CMakeLists.txt | 3 + .../eigen/unsupported/Eigen/CXX11/Tensor | 3 + .../unsupported/Eigen/CXX11/TensorSymmetry | 3 + .../eigen/unsupported/Eigen/CXX11/ThreadPool | 3 + .../Eigen/CXX11/src/Tensor/README.md | 3 + .../Eigen/CXX11/src/Tensor/Tensor.h | 3 + .../Eigen/CXX11/src/Tensor/TensorArgMax.h | 3 + .../Eigen/CXX11/src/Tensor/TensorAssign.h | 3 + .../Eigen/CXX11/src/Tensor/TensorBase.h | 3 + .../CXX11/src/Tensor/TensorBroadcasting.h | 3 + .../Eigen/CXX11/src/Tensor/TensorChipping.h | 3 + .../CXX11/src/Tensor/TensorConcatenation.h | 3 + .../CXX11/src/Tensor/TensorContraction.h | 3 + .../src/Tensor/TensorContractionBlocking.h | 3 + .../CXX11/src/Tensor/TensorContractionCuda.h | 3 + .../src/Tensor/TensorContractionMapper.h | 3 + .../src/Tensor/TensorContractionThreadPool.h | 3 + .../Eigen/CXX11/src/Tensor/TensorConversion.h | 3 + .../CXX11/src/Tensor/TensorConvolution.h | 3 + .../Eigen/CXX11/src/Tensor/TensorCostModel.h | 3 + .../Eigen/CXX11/src/Tensor/TensorCustomOp.h | 3 + .../Eigen/CXX11/src/Tensor/TensorDevice.h | 3 + .../Eigen/CXX11/src/Tensor/TensorDeviceCuda.h | 3 + .../CXX11/src/Tensor/TensorDeviceDefault.h | 3 + .../Eigen/CXX11/src/Tensor/TensorDeviceSycl.h | 3 + .../CXX11/src/Tensor/TensorDeviceThreadPool.h | 3 + .../CXX11/src/Tensor/TensorDimensionList.h | 3 + .../Eigen/CXX11/src/Tensor/TensorDimensions.h | 3 + .../Eigen/CXX11/src/Tensor/TensorEvalTo.h | 3 + .../Eigen/CXX11/src/Tensor/TensorEvaluator.h | 3 + .../Eigen/CXX11/src/Tensor/TensorExecutor.h | 3 + .../Eigen/CXX11/src/Tensor/TensorExpr.h | 3 + .../Eigen/CXX11/src/Tensor/TensorFFT.h | 3 + .../Eigen/CXX11/src/Tensor/TensorFixedSize.h | 3 + .../Eigen/CXX11/src/Tensor/TensorForcedEval.h | 3 + .../src/Tensor/TensorForwardDeclarations.h | 3 + .../Eigen/CXX11/src/Tensor/TensorFunctors.h | 3 + .../Eigen/CXX11/src/Tensor/TensorGenerator.h | 3 + .../CXX11/src/Tensor/TensorGlobalFunctions.h | 3 + .../Eigen/CXX11/src/Tensor/TensorIO.h | 3 + .../Eigen/CXX11/src/Tensor/TensorImagePatch.h | 3 + .../Eigen/CXX11/src/Tensor/TensorIndexList.h | 3 + .../Eigen/CXX11/src/Tensor/TensorInflation.h | 3 + .../CXX11/src/Tensor/TensorInitializer.h | 3 + .../Eigen/CXX11/src/Tensor/TensorIntDiv.h | 3 + .../Eigen/CXX11/src/Tensor/TensorLayoutSwap.h | 3 + .../Eigen/CXX11/src/Tensor/TensorMacros.h | 3 + .../Eigen/CXX11/src/Tensor/TensorMap.h | 3 + .../Eigen/CXX11/src/Tensor/TensorMeta.h | 3 + .../Eigen/CXX11/src/Tensor/TensorMorphing.h | 3 + .../Eigen/CXX11/src/Tensor/TensorPadding.h | 3 + .../Eigen/CXX11/src/Tensor/TensorPatch.h | 3 + .../Eigen/CXX11/src/Tensor/TensorRandom.h | 3 + .../Eigen/CXX11/src/Tensor/TensorReduction.h | 3 + .../CXX11/src/Tensor/TensorReductionCuda.h | 3 + .../CXX11/src/Tensor/TensorReductionSycl.h | 3 + .../Eigen/CXX11/src/Tensor/TensorRef.h | 3 + .../Eigen/CXX11/src/Tensor/TensorReverse.h | 3 + .../Eigen/CXX11/src/Tensor/TensorScan.h | 3 + .../Eigen/CXX11/src/Tensor/TensorShuffling.h | 3 + .../Eigen/CXX11/src/Tensor/TensorStorage.h | 3 + .../Eigen/CXX11/src/Tensor/TensorStriding.h | 3 + .../Eigen/CXX11/src/Tensor/TensorSycl.h | 3 + .../TensorSyclConvertToDeviceExpression.h | 3 + .../src/Tensor/TensorSyclExprConstructor.h | 3 + .../src/Tensor/TensorSyclExtractAccessor.h | 3 + .../src/Tensor/TensorSyclExtractFunctors.h | 3 + .../CXX11/src/Tensor/TensorSyclLeafCount.h | 3 + .../src/Tensor/TensorSyclPlaceHolderExpr.h | 3 + .../Eigen/CXX11/src/Tensor/TensorSyclRun.h | 3 + .../Eigen/CXX11/src/Tensor/TensorSyclTuple.h | 3 + .../Eigen/CXX11/src/Tensor/TensorTraits.h | 3 + .../Eigen/CXX11/src/Tensor/TensorUInt128.h | 3 + .../CXX11/src/Tensor/TensorVolumePatch.h | 3 + .../src/TensorSymmetry/DynamicSymmetry.h | 3 + .../CXX11/src/TensorSymmetry/StaticSymmetry.h | 3 + .../Eigen/CXX11/src/TensorSymmetry/Symmetry.h | 3 + .../TensorSymmetry/util/TemplateGroupTheory.h | 3 + .../Eigen/CXX11/src/ThreadPool/EventCount.h | 3 + .../src/ThreadPool/NonBlockingThreadPool.h | 3 + .../Eigen/CXX11/src/ThreadPool/RunQueue.h | 3 + .../CXX11/src/ThreadPool/SimpleThreadPool.h | 3 + .../CXX11/src/ThreadPool/ThreadEnvironment.h | 3 + .../Eigen/CXX11/src/ThreadPool/ThreadLocal.h | 3 + .../src/ThreadPool/ThreadPoolInterface.h | 3 + .../Eigen/CXX11/src/ThreadPool/ThreadYield.h | 3 + .../Eigen/CXX11/src/util/CXX11Meta.h | 3 + .../Eigen/CXX11/src/util/CXX11Workarounds.h | 3 + .../Eigen/CXX11/src/util/EmulateArray.h | 3 + .../Eigen/CXX11/src/util/EmulateCXX11Meta.h | 3 + .../Eigen/CXX11/src/util/MaxSizeVector.h | 3 + phonelibs/eigen/unsupported/Eigen/EulerAngles | 3 + phonelibs/eigen/unsupported/Eigen/FFT | 3 + .../eigen/unsupported/Eigen/IterativeSolvers | 3 + .../eigen/unsupported/Eigen/KroneckerProduct | 3 + .../unsupported/Eigen/LevenbergMarquardt | 3 + .../eigen/unsupported/Eigen/MPRealSupport | 3 + .../eigen/unsupported/Eigen/MatrixFunctions | 3 + .../eigen/unsupported/Eigen/MoreVectorization | 3 + .../unsupported/Eigen/NonLinearOptimization | 3 + .../eigen/unsupported/Eigen/NumericalDiff | 3 + .../eigen/unsupported/Eigen/OpenGLSupport | 3 + phonelibs/eigen/unsupported/Eigen/Polynomials | 3 + phonelibs/eigen/unsupported/Eigen/Skyline | 3 + phonelibs/eigen/unsupported/Eigen/SparseExtra | 3 + .../eigen/unsupported/Eigen/SpecialFunctions | 3 + phonelibs/eigen/unsupported/Eigen/Splines | 3 + .../Eigen/src/AutoDiff/AutoDiffJacobian.h | 3 + .../Eigen/src/AutoDiff/AutoDiffScalar.h | 3 + .../Eigen/src/AutoDiff/AutoDiffVector.h | 3 + .../unsupported/Eigen/src/BVH/BVAlgorithms.h | 3 + .../eigen/unsupported/Eigen/src/BVH/KdBVH.h | 3 + .../ArpackSelfAdjointEigenSolver.h | 3 + .../Eigen/src/EulerAngles/CMakeLists.txt | 3 + .../Eigen/src/EulerAngles/EulerAngles.h | 3 + .../Eigen/src/EulerAngles/EulerSystem.h | 3 + .../unsupported/Eigen/src/FFT/ei_fftw_impl.h | 3 + .../Eigen/src/FFT/ei_kissfft_impl.h | 3 + .../IterativeSolvers/ConstrainedConjGrad.h | 3 + .../Eigen/src/IterativeSolvers/DGMRES.h | 3 + .../Eigen/src/IterativeSolvers/GMRES.h | 3 + .../Eigen/src/IterativeSolvers/IncompleteLU.h | 3 + .../IterativeSolvers/IterationController.h | 3 + .../Eigen/src/IterativeSolvers/MINRES.h | 3 + .../Eigen/src/IterativeSolvers/Scaling.h | 3 + .../KroneckerProduct/KroneckerTensorProduct.h | 3 + .../LevenbergMarquardt/CopyrightMINPACK.txt | 3 + .../Eigen/src/LevenbergMarquardt/LMcovar.h | 3 + .../Eigen/src/LevenbergMarquardt/LMonestep.h | 3 + .../Eigen/src/LevenbergMarquardt/LMpar.h | 3 + .../Eigen/src/LevenbergMarquardt/LMqrsolv.h | 3 + .../LevenbergMarquardt/LevenbergMarquardt.h | 3 + .../src/MatrixFunctions/MatrixExponential.h | 3 + .../src/MatrixFunctions/MatrixFunction.h | 3 + .../src/MatrixFunctions/MatrixLogarithm.h | 3 + .../Eigen/src/MatrixFunctions/MatrixPower.h | 3 + .../src/MatrixFunctions/MatrixSquareRoot.h | 3 + .../Eigen/src/MatrixFunctions/StemFunction.h | 3 + .../src/MoreVectorization/MathFunctions.h | 3 + .../HybridNonLinearSolver.h | 3 + .../LevenbergMarquardt.h | 3 + .../Eigen/src/NonLinearOptimization/chkder.h | 3 + .../Eigen/src/NonLinearOptimization/covar.h | 3 + .../Eigen/src/NonLinearOptimization/dogleg.h | 3 + .../Eigen/src/NonLinearOptimization/fdjac1.h | 3 + .../Eigen/src/NonLinearOptimization/lmpar.h | 3 + .../Eigen/src/NonLinearOptimization/qrsolv.h | 3 + .../Eigen/src/NonLinearOptimization/r1mpyq.h | 3 + .../Eigen/src/NonLinearOptimization/r1updt.h | 3 + .../Eigen/src/NonLinearOptimization/rwupdt.h | 3 + .../Eigen/src/NumericalDiff/NumericalDiff.h | 3 + .../Eigen/src/Polynomials/Companion.h | 3 + .../Eigen/src/Polynomials/PolynomialSolver.h | 3 + .../Eigen/src/Polynomials/PolynomialUtils.h | 3 + .../Eigen/src/Skyline/SkylineInplaceLU.h | 3 + .../Eigen/src/Skyline/SkylineMatrix.h | 3 + .../Eigen/src/Skyline/SkylineMatrixBase.h | 3 + .../Eigen/src/Skyline/SkylineProduct.h | 3 + .../Eigen/src/Skyline/SkylineStorage.h | 3 + .../Eigen/src/Skyline/SkylineUtil.h | 3 + .../SparseExtra/BlockOfDynamicSparseMatrix.h | 3 + .../Eigen/src/SparseExtra/BlockSparseMatrix.h | 3 + .../src/SparseExtra/DynamicSparseMatrix.h | 3 + .../Eigen/src/SparseExtra/MarketIO.h | 3 + .../src/SparseExtra/MatrixMarketIterator.h | 3 + .../Eigen/src/SparseExtra/RandomSetter.h | 3 + .../SpecialFunctionsArrayAPI.h | 3 + .../SpecialFunctionsFunctors.h | 3 + .../SpecialFunctions/SpecialFunctionsHalf.h | 3 + .../SpecialFunctions/SpecialFunctionsImpl.h | 3 + .../SpecialFunctionsPacketMath.h | 3 + .../arch/CUDA/CudaSpecialFunctions.h | 3 + .../unsupported/Eigen/src/Splines/Spline.h | 3 + .../Eigen/src/Splines/SplineFitting.h | 3 + .../unsupported/Eigen/src/Splines/SplineFwd.h | 3 + phonelibs/eigen/unsupported/README.txt | 3 + .../eigen/unsupported/doc/CMakeLists.txt | 3 + phonelibs/eigen/unsupported/doc/Overview.dox | 3 + .../unsupported/doc/eigendoxy_layout.xml.in | 3 + .../unsupported/doc/examples/BVH_Example.cpp | 3 + .../unsupported/doc/examples/CMakeLists.txt | 3 + .../unsupported/doc/examples/EulerAngles.cpp | 3 + .../eigen/unsupported/doc/examples/FFT.cpp | 3 + .../doc/examples/MatrixExponential.cpp | 3 + .../doc/examples/MatrixFunction.cpp | 3 + .../doc/examples/MatrixLogarithm.cpp | 3 + .../unsupported/doc/examples/MatrixPower.cpp | 3 + .../doc/examples/MatrixPower_optimal.cpp | 3 + .../unsupported/doc/examples/MatrixSine.cpp | 3 + .../unsupported/doc/examples/MatrixSinh.cpp | 3 + .../doc/examples/MatrixSquareRoot.cpp | 3 + .../doc/examples/PolynomialSolver1.cpp | 3 + .../doc/examples/PolynomialUtils1.cpp | 3 + .../unsupported/doc/snippets/CMakeLists.txt | 3 + phonelibs/fastcv/aarch64/include/fastcv.h | 3 + phonelibs/fastcv/aarch64/libfastcv.a | 3 + .../fastcv/aarch64/libfastcvadsp_stub.so | 3 + phonelibs/fastcv/aarch64/libfastcvopt.so | 3 + phonelibs/fastcv/x64/include/fastcv.h | 3 + phonelibs/fastcv/x64/include/fastcv.inl | 3 + phonelibs/fastcv/x64/include/stdint_.h | 3 + phonelibs/fastcv/x64/lib/libfastcv.a | 3 + phonelibs/json11/json11.o | 3 - phonelibs/libyuv/LICENSE.libyuv | 3 + phonelibs/qpoases/SConscript | 3 + .../snpe/aarch64-android-clang3.8/libSNPE.so | 3 + .../libsnpe_dsp_skel.so | 0 .../libsymphony-cpu.so | 3 + .../libsymphonypower.so | 3 + phonelibs/zmq/aarch64/lib/libczmq.a | 3 + phonelibs/zmq/aarch64/lib/libczmq.la | 3 + phonelibs/zmq/aarch64/lib/libczmq.so | 3 + phonelibs/zmq/aarch64/lib/libgnustl_shared.so | 3 + phonelibs/zmq/aarch64/lib/libzmq.a | 3 + phonelibs/zmq/aarch64/lib/libzmq.la | 3 + phonelibs/zmq/aarch64/lib/libzmq.so | 3 + run_docker_tests.sh | 26 +- selfdrive/assets/sounds/warning_repeat.wav | 4 +- selfdrive/athena/athenad.py | 124 +- selfdrive/athena/manage_athenad.py | 2 +- selfdrive/athena/test.py | 194 ++ selfdrive/athena/test_helpers.py | 97 + selfdrive/boardd/.gitignore | 1 + selfdrive/boardd/Makefile | 110 -- selfdrive/boardd/SConscript | 9 + selfdrive/boardd/boardd.cc | 95 +- selfdrive/boardd/boardd.py | 7 +- selfdrive/boardd/boardd_setup.py | 10 +- selfdrive/boardd/tests/boardd_old.py | 2 +- selfdrive/boardd/tests/replay_many.py | 39 +- .../boardd/tests/test_boardd_loopback.py | 2 +- selfdrive/camerad/SConscript | 19 + selfdrive/{visiond => camerad}/bufs.h | 0 .../cameras/camera_common.h | 1 + .../cameras/camera_frame_stream.cc | 1 + .../cameras/camera_frame_stream.h | 0 .../cameras/camera_qcom.c | 22 +- .../cameras/camera_qcom.h | 0 .../{visiond => camerad}/cameras/debayer.cl | 0 .../{visiond => camerad}/cameras/sensor_i2c.h | 0 .../include/msm_cam_sensor.h | 0 .../include/msm_camsensor_sdk.h | 0 .../include/msmb_camera.h | 0 .../{visiond => camerad}/include/msmb_isp.h | 0 .../{visiond => camerad}/include/msmb_ispif.h | 0 .../{visiond/visiond.cc => camerad/main.cc} | 1614 +++++++---------- .../{can => camerad/snapshot}/__init__.py | 0 selfdrive/camerad/snapshot/snapshot.py | 72 + .../snapshot/visionipc.py | 17 +- .../transforms/rgb_to_yuv.c | 0 .../transforms/rgb_to_yuv.cl | 0 .../transforms/rgb_to_yuv.h | 0 .../transforms/rgb_to_yuv_test.cc | 0 selfdrive/can/Makefile | 97 - selfdrive/can/can_define.py | 39 - selfdrive/can/common.cc | 165 -- selfdrive/can/common.h | 59 - selfdrive/can/common.pxd | 73 - selfdrive/can/common_dbc.h | 82 - selfdrive/can/dbc.cc | 31 - selfdrive/can/dbc_out/.gitignore | 2 - selfdrive/can/dbc_out/.gitkeep | 0 selfdrive/can/dbc_template.cc | 81 - selfdrive/can/libdbc_py.py | 95 - selfdrive/can/packer.cc | 139 -- selfdrive/can/packer.py | 9 - selfdrive/can/packer_impl.pyx | 124 -- selfdrive/can/packer_setup.py | 9 - selfdrive/can/parser.cc | 239 --- selfdrive/can/parser.py | 9 - selfdrive/can/parser_pyx.pyx | 138 -- selfdrive/can/parser_pyx_setup.py | 35 - selfdrive/can/plant_can_parser.py | 131 -- selfdrive/can/process_dbc.py | 105 -- selfdrive/can/tests/test_packer_parser.py | 92 - selfdrive/car/__init__.py | 5 + selfdrive/car/car_helpers.py | 2 +- selfdrive/car/chrysler/carcontroller.py | 8 +- selfdrive/car/chrysler/carstate.py | 2 +- selfdrive/car/chrysler/chryslercan.py | 9 +- selfdrive/car/chrysler/interface.py | 4 +- selfdrive/car/chrysler/radar_interface.py | 2 +- selfdrive/car/chrysler/test_chryslercan.py | 2 +- selfdrive/car/ford/carcontroller.py | 38 +- selfdrive/car/ford/carstate.py | 3 +- selfdrive/car/ford/fordcan.py | 4 - selfdrive/car/ford/interface.py | 2 +- selfdrive/car/ford/radar_interface.py | 2 +- selfdrive/car/gm/carcontroller.py | 8 +- selfdrive/car/gm/carstate.py | 2 +- selfdrive/car/gm/gmcan.py | 18 +- selfdrive/car/gm/interface.py | 8 +- selfdrive/car/gm/radar_interface.py | 5 +- selfdrive/car/honda/carcontroller.py | 6 +- selfdrive/car/honda/carstate.py | 31 +- selfdrive/car/honda/hondacan.py | 29 +- selfdrive/car/honda/interface.py | 63 +- selfdrive/car/honda/radar_interface.py | 8 +- selfdrive/car/hyundai/carcontroller.py | 28 +- selfdrive/car/hyundai/carstate.py | 2 +- selfdrive/car/hyundai/hyundaican.py | 14 - selfdrive/car/hyundai/interface.py | 4 +- selfdrive/car/interfaces.py | 3 +- selfdrive/car/mock/interface.py | 2 +- selfdrive/car/subaru/carcontroller.py | 8 +- selfdrive/car/subaru/carstate.py | 2 +- selfdrive/car/subaru/interface.py | 3 +- selfdrive/car/toyota/carcontroller.py | 27 +- selfdrive/car/toyota/carstate.py | 10 +- selfdrive/car/toyota/interface.py | 34 +- selfdrive/car/toyota/radar_interface.py | 5 +- selfdrive/car/toyota/toyotacan.py | 24 - selfdrive/car/vin.py | 21 +- selfdrive/car/volkswagen/carcontroller.py | 17 +- selfdrive/car/volkswagen/carstate.py | 108 +- selfdrive/car/volkswagen/interface.py | 24 +- selfdrive/common/SConscript | 36 + selfdrive/common/cereal.mk | 38 - selfdrive/{visiond => common}/clutil.c | 0 selfdrive/{visiond => common}/clutil.h | 0 selfdrive/common/messaging.h | 2 +- selfdrive/common/modeldata.h | 3 + selfdrive/common/swaglog.h | 2 +- selfdrive/common/version.h | 2 +- selfdrive/common/visionbuf_cl.c | 33 +- selfdrive/common/visionipc.c | 5 +- selfdrive/controls/controlsd.py | 138 +- selfdrive/controls/lib/alerts.py | 93 +- selfdrive/controls/lib/alerts_offroad.json | 10 +- selfdrive/controls/lib/cluster/Makefile | 32 - selfdrive/controls/lib/cluster/SConscript | 8 + .../controls/lib/cluster/fastcluster_py.py | 3 - selfdrive/controls/lib/driver_monitor.py | 42 +- selfdrive/controls/lib/fcw.py | 7 +- selfdrive/controls/lib/gps_helpers.py | 1 + selfdrive/controls/lib/lane_planner.py | 12 +- selfdrive/controls/lib/latcontrol_indi.py | 23 +- selfdrive/controls/lib/latcontrol_lqr.py | 30 +- selfdrive/controls/lib/latcontrol_pid.py | 9 +- selfdrive/controls/lib/lateral_mpc/.gitignore | 2 + selfdrive/controls/lib/lateral_mpc/Makefile | 89 - selfdrive/controls/lib/lateral_mpc/SConscript | 29 + .../controls/lib/lateral_mpc/lateral_mpc.c | 32 +- .../controls/lib/lateral_mpc/lateral_mpc.d | 3 - .../controls/lib/lateral_mpc/lateral_mpc.o | 3 - .../acado_auxiliary_functions.d | 3 - .../acado_auxiliary_functions.o | 3 - .../lib_mpc_export/acado_integrator.d | 3 - .../lib_mpc_export/acado_integrator.o | 3 - .../lib_mpc_export/acado_qpoases_interface.d | 3 - .../lib_mpc_export/acado_qpoases_interface.o | 3 - .../lateral_mpc/lib_mpc_export/acado_solver.d | 3 - .../lateral_mpc/lib_mpc_export/acado_solver.o | 3 - selfdrive/controls/lib/lateral_mpc/libmpc.so | 3 - .../controls/lib/lateral_mpc/libmpc_py.py | 3 +- selfdrive/controls/lib/long_mpc.py | 2 +- .../controls/lib/longitudinal_mpc/.gitignore | 2 + .../controls/lib/longitudinal_mpc/Makefile | 91 - .../controls/lib/longitudinal_mpc/SConscript | 32 + .../acado_auxiliary_functions.d | 3 - .../acado_auxiliary_functions.o | 3 - .../lib_mpc_export/acado_integrator.d | 3 - .../lib_mpc_export/acado_integrator.o | 3 - .../lib_mpc_export/acado_qpoases_interface.d | 3 - .../lib_mpc_export/acado_qpoases_interface.o | 3 - .../lib_mpc_export/acado_solver.d | 3 - .../lib_mpc_export/acado_solver.o | 3 - .../controls/lib/longitudinal_mpc/libmpc1.so | 3 - .../lib/longitudinal_mpc/libmpc_py.py | 11 - .../lib/longitudinal_mpc/longitudinal_mpc.d | 3 - .../lib/longitudinal_mpc/longitudinal_mpc.o | 3 - selfdrive/controls/lib/pathplanner.py | 109 +- selfdrive/controls/lib/pid.py | 9 +- selfdrive/controls/lib/planner.py | 15 +- selfdrive/controls/lib/radar_helpers.py | 35 +- selfdrive/controls/plannerd.py | 2 +- selfdrive/controls/radard.py | 46 +- .../controls/tests/test_following_distance.py | 2 +- selfdrive/{can/tests => debug}/__init__.py | 0 selfdrive/debug/can_printer.py | 2 +- selfdrive/debug/check_freq.py | 43 + selfdrive/debug/cpu_usage_stat.py | 2 +- selfdrive/debug/dump.py | 11 +- selfdrive/debug/get_fingerprint.py | 2 +- selfdrive/debug/live_cpu_and_temp.py | 2 +- selfdrive/debug/mpc/live_lateral_mpc.py | 2 +- selfdrive/debug/mpc/live_longitudinal_mpc.py | 5 +- selfdrive/debug/show_matching_cars.py | 2 +- selfdrive/launcher.py | 27 + selfdrive/locationd/Makefile | 115 -- selfdrive/locationd/SConscript | 25 + selfdrive/locationd/calibrationd.py | 2 +- selfdrive/locationd/get_vp.c | 41 - selfdrive/locationd/paramsd.cc | 8 +- selfdrive/locationd/test/ci_test.py | 3 - .../locationd/test/test_params_learner.py | 2 +- selfdrive/locationd/test/ublox.py | 2 +- selfdrive/locationd/test/ubloxd.py | 2 +- selfdrive/locationd/test/ubloxd_easy.py | 2 +- selfdrive/locationd/ubloxd_main.cc | 5 + selfdrive/logcatd/Makefile | 59 - selfdrive/logcatd/SConscript | 2 + selfdrive/logcatd/logcatd.cc | 1 + selfdrive/loggerd/Makefile | 4 - selfdrive/loggerd/SConscript | 6 + selfdrive/loggerd/build_from_src.mk | 136 -- selfdrive/loggerd/loggerd.cc | 5 +- .../loggerd/tests/loggerd_tests_common.py | 5 +- selfdrive/loggerd/uploader.py | 20 +- selfdrive/logmessaged.py | 2 +- selfdrive/manager.py | 256 +-- selfdrive/messaging/.gitignore | 1 - selfdrive/messaging/Makefile | 63 - selfdrive/messaging/__init__.py | 214 --- selfdrive/messaging/demo.cc | 50 - selfdrive/messaging/demo.py | 30 - selfdrive/messaging/impl_zmq.cc | 173 -- selfdrive/messaging/impl_zmq.hpp | 63 - selfdrive/messaging/messaging.cc | 51 - selfdrive/messaging/messaging.hpp | 53 - selfdrive/messaging/messaging.pxd | 42 - selfdrive/messaging/messaging_pyx.pyx | 104 -- selfdrive/messaging/messaging_pyx_setup.py | 34 - selfdrive/modeld/SConscript | 25 + selfdrive/{visiond => modeld}/constants.py | 3 + selfdrive/modeld/modeld | 4 + selfdrive/modeld/modeld.cc | 281 +++ .../{visiond => modeld}/models/commonmodel.c | 0 .../{visiond => modeld}/models/commonmodel.h | 0 .../{visiond => modeld}/models/driving.cc | 26 +- .../{visiond => modeld}/models/driving.h | 0 selfdrive/modeld/models/monitoring.cc | 152 ++ .../{visiond => modeld}/models/monitoring.h | 13 +- .../{visiond => modeld}/models/posenet.cc | 0 .../{visiond => modeld}/models/posenet.h | 0 selfdrive/modeld/monitoringd | 5 + selfdrive/modeld/monitoringd.cc | 80 + selfdrive/{visiond => modeld}/runners/run.h | 0 .../{visiond => modeld}/runners/runmodel.h | 0 .../{visiond => modeld}/runners/snpemodel.cc | 0 .../{visiond => modeld}/runners/snpemodel.h | 0 .../{visiond => modeld}/transforms/loadyuv.c | 0 .../{visiond => modeld}/transforms/loadyuv.cl | 0 .../{visiond => modeld}/transforms/loadyuv.h | 0 .../transforms/transform.c | 0 .../transforms/transform.cl | 0 .../transforms/transform.h | 0 selfdrive/proclogd/Makefile | 58 - selfdrive/proclogd/SConscript | 2 + selfdrive/proclogd/proclogd.cc | 1 + selfdrive/registration.py | 50 +- selfdrive/sensord/Makefile | 4 - selfdrive/sensord/SConscript | 5 + selfdrive/sensord/build_from_src.mk | 90 - selfdrive/sensord/gpsd | 3 + selfdrive/sensord/gpsd.cc | 4 + selfdrive/sensord/rawgps.cc | 1 + selfdrive/sensord/sensord | 3 + selfdrive/sensord/sensors.cc | 1 + selfdrive/sensord/start_gpsd.py | 6 - selfdrive/sensord/start_sensord.py | 6 - selfdrive/service_list.yaml | 163 -- selfdrive/services.py | 20 - .../test/longitudinal_maneuvers/plant.py | 35 +- .../test/longitudinal_maneuvers/plant_ui.py | 2 +- .../test_longitudinal.py | 1 + selfdrive/test/openpilotci_upload.py | 2 +- selfdrive/test/process_replay/compare_logs.py | 8 +- .../test/process_replay/process_replay.py | 8 +- selfdrive/test/process_replay/ref_commit | 2 +- selfdrive/test/test_car_models.py | 12 +- selfdrive/test/test_fingerprints.py | 1 - selfdrive/test/test_openpilot.py | 172 +- selfdrive/thermald.py | 51 +- selfdrive/ui/Makefile | 127 -- selfdrive/ui/SConscript | 5 + selfdrive/ui/start.py | 7 - selfdrive/ui/ui | 4 + selfdrive/ui/ui.cc | 150 +- selfdrive/updated.py | 4 +- selfdrive/version.py | 20 +- selfdrive/visiond/LICENSE.boringssl | 192 -- selfdrive/visiond/LICENSE.libyuv | 29 - selfdrive/visiond/LICENSE.opencv | 41 - selfdrive/visiond/Makefile | 4 - selfdrive/visiond/README | 1 - selfdrive/visiond/__init__.py | 0 selfdrive/visiond/build_from_src.mk | 226 --- selfdrive/visiond/models/monitoring.cc | 108 -- selfdrive/visiond/snapshot/Makefile | 32 - selfdrive/visiond/snapshot/__init__.py | 0 selfdrive/visiond/snapshot/snapshot.py | 52 - selfdrive/visiond/start.py | 8 - 844 files changed, 5546 insertions(+), 7054 deletions(-) delete mode 100644 README_chffrplus.md create mode 100644 SConstruct create mode 100644 common/.gitignore create mode 100644 common/SConscript create mode 100644 common/android.py create mode 100644 common/apk.py create mode 100644 common/common_pyx_setup.py delete mode 100755 common/dbc.py delete mode 100644 common/kalman/Makefile create mode 100644 common/kalman/SConscript delete mode 100644 models/monitoring_model.dlc create mode 100644 models/monitoring_model_q.dlc create mode 100644 phonelibs/SConscript create mode 100644 phonelibs/boringssl/LICENSE.boringssl create mode 100644 phonelibs/eigen/COPYING.BSD create mode 100644 phonelibs/eigen/COPYING.GPL create mode 100644 phonelibs/eigen/COPYING.LGPL create mode 100644 phonelibs/eigen/COPYING.MINPACK create mode 100644 phonelibs/eigen/COPYING.MPL2 create mode 100644 phonelibs/eigen/COPYING.README create mode 100644 phonelibs/eigen/Eigen/CMakeLists.txt create mode 100644 phonelibs/eigen/Eigen/Cholesky create mode 100644 phonelibs/eigen/Eigen/CholmodSupport create mode 100644 phonelibs/eigen/Eigen/Core create mode 100644 phonelibs/eigen/Eigen/Dense create mode 100644 phonelibs/eigen/Eigen/Eigen create mode 100644 phonelibs/eigen/Eigen/Eigenvalues create mode 100644 phonelibs/eigen/Eigen/Geometry create mode 100644 phonelibs/eigen/Eigen/Householder create mode 100644 phonelibs/eigen/Eigen/IterativeLinearSolvers create mode 100644 phonelibs/eigen/Eigen/Jacobi create mode 100644 phonelibs/eigen/Eigen/LU create mode 100644 phonelibs/eigen/Eigen/MetisSupport create mode 100644 phonelibs/eigen/Eigen/OrderingMethods create mode 100644 phonelibs/eigen/Eigen/PaStiXSupport create mode 100755 phonelibs/eigen/Eigen/PardisoSupport create mode 100644 phonelibs/eigen/Eigen/QR create mode 100644 phonelibs/eigen/Eigen/QtAlignedMalloc create mode 100644 phonelibs/eigen/Eigen/SPQRSupport create mode 100644 phonelibs/eigen/Eigen/SVD create mode 100644 phonelibs/eigen/Eigen/Sparse create mode 100644 phonelibs/eigen/Eigen/SparseCholesky create mode 100644 phonelibs/eigen/Eigen/SparseCore create mode 100644 phonelibs/eigen/Eigen/SparseLU create mode 100644 phonelibs/eigen/Eigen/SparseQR create mode 100644 phonelibs/eigen/Eigen/StdDeque create mode 100644 phonelibs/eigen/Eigen/StdList create mode 100644 phonelibs/eigen/Eigen/StdVector create mode 100644 phonelibs/eigen/Eigen/SuperLUSupport create mode 100644 phonelibs/eigen/Eigen/UmfPackSupport create mode 100644 phonelibs/eigen/Eigen/src/Cholesky/LDLT.h create mode 100644 phonelibs/eigen/Eigen/src/Cholesky/LLT.h create mode 100644 phonelibs/eigen/Eigen/src/Cholesky/LLT_LAPACKE.h create mode 100644 phonelibs/eigen/Eigen/src/CholmodSupport/CholmodSupport.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Array.h create mode 100644 phonelibs/eigen/Eigen/src/Core/ArrayBase.h create mode 100644 phonelibs/eigen/Eigen/src/Core/ArrayWrapper.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Assign.h create mode 100644 phonelibs/eigen/Eigen/src/Core/AssignEvaluator.h create mode 100755 phonelibs/eigen/Eigen/src/Core/Assign_MKL.h create mode 100644 phonelibs/eigen/Eigen/src/Core/BandMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Block.h create mode 100644 phonelibs/eigen/Eigen/src/Core/BooleanRedux.h create mode 100644 phonelibs/eigen/Eigen/src/Core/CommaInitializer.h create mode 100644 phonelibs/eigen/Eigen/src/Core/ConditionEstimator.h create mode 100644 phonelibs/eigen/Eigen/src/Core/CoreEvaluators.h create mode 100644 phonelibs/eigen/Eigen/src/Core/CoreIterators.h create mode 100644 phonelibs/eigen/Eigen/src/Core/CwiseBinaryOp.h create mode 100644 phonelibs/eigen/Eigen/src/Core/CwiseNullaryOp.h create mode 100644 phonelibs/eigen/Eigen/src/Core/CwiseTernaryOp.h create mode 100644 phonelibs/eigen/Eigen/src/Core/CwiseUnaryOp.h create mode 100644 phonelibs/eigen/Eigen/src/Core/CwiseUnaryView.h create mode 100644 phonelibs/eigen/Eigen/src/Core/DenseBase.h create mode 100644 phonelibs/eigen/Eigen/src/Core/DenseCoeffsBase.h create mode 100644 phonelibs/eigen/Eigen/src/Core/DenseStorage.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Diagonal.h create mode 100644 phonelibs/eigen/Eigen/src/Core/DiagonalMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/Core/DiagonalProduct.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Dot.h create mode 100644 phonelibs/eigen/Eigen/src/Core/EigenBase.h create mode 100644 phonelibs/eigen/Eigen/src/Core/ForceAlignedAccess.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Fuzzy.h create mode 100644 phonelibs/eigen/Eigen/src/Core/GeneralProduct.h create mode 100644 phonelibs/eigen/Eigen/src/Core/GenericPacketMath.h create mode 100644 phonelibs/eigen/Eigen/src/Core/GlobalFunctions.h create mode 100644 phonelibs/eigen/Eigen/src/Core/IO.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Inverse.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Map.h create mode 100644 phonelibs/eigen/Eigen/src/Core/MapBase.h create mode 100644 phonelibs/eigen/Eigen/src/Core/MathFunctions.h create mode 100644 phonelibs/eigen/Eigen/src/Core/MathFunctionsImpl.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Matrix.h create mode 100644 phonelibs/eigen/Eigen/src/Core/MatrixBase.h create mode 100644 phonelibs/eigen/Eigen/src/Core/NestByValue.h create mode 100644 phonelibs/eigen/Eigen/src/Core/NoAlias.h create mode 100644 phonelibs/eigen/Eigen/src/Core/NumTraits.h create mode 100644 phonelibs/eigen/Eigen/src/Core/PermutationMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/Core/PlainObjectBase.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Product.h create mode 100644 phonelibs/eigen/Eigen/src/Core/ProductEvaluators.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Random.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Redux.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Ref.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Replicate.h create mode 100644 phonelibs/eigen/Eigen/src/Core/ReturnByValue.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Reverse.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Select.h create mode 100644 phonelibs/eigen/Eigen/src/Core/SelfAdjointView.h create mode 100644 phonelibs/eigen/Eigen/src/Core/SelfCwiseBinaryOp.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Solve.h create mode 100644 phonelibs/eigen/Eigen/src/Core/SolveTriangular.h create mode 100644 phonelibs/eigen/Eigen/src/Core/SolverBase.h create mode 100644 phonelibs/eigen/Eigen/src/Core/StableNorm.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Stride.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Swap.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Transpose.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Transpositions.h create mode 100644 phonelibs/eigen/Eigen/src/Core/TriangularMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/Core/VectorBlock.h create mode 100644 phonelibs/eigen/Eigen/src/Core/VectorwiseOp.h create mode 100644 phonelibs/eigen/Eigen/src/Core/Visitor.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/AVX/Complex.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/AVX/MathFunctions.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/AVX/PacketMath.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/AVX/TypeCasting.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/AVX512/MathFunctions.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/AVX512/PacketMath.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/AltiVec/Complex.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/AltiVec/MathFunctions.h create mode 100755 phonelibs/eigen/Eigen/src/Core/arch/AltiVec/PacketMath.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/CUDA/Complex.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/CUDA/Half.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/CUDA/MathFunctions.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/CUDA/PacketMath.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/CUDA/PacketMathHalf.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/CUDA/TypeCasting.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/Default/Settings.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/NEON/Complex.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/NEON/MathFunctions.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/NEON/PacketMath.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/SSE/Complex.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/SSE/MathFunctions.h create mode 100755 phonelibs/eigen/Eigen/src/Core/arch/SSE/PacketMath.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/SSE/TypeCasting.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/ZVector/Complex.h create mode 100644 phonelibs/eigen/Eigen/src/Core/arch/ZVector/MathFunctions.h create mode 100755 phonelibs/eigen/Eigen/src/Core/arch/ZVector/PacketMath.h create mode 100644 phonelibs/eigen/Eigen/src/Core/functors/AssignmentFunctors.h create mode 100644 phonelibs/eigen/Eigen/src/Core/functors/BinaryFunctors.h create mode 100644 phonelibs/eigen/Eigen/src/Core/functors/NullaryFunctors.h create mode 100644 phonelibs/eigen/Eigen/src/Core/functors/StlFunctors.h create mode 100644 phonelibs/eigen/Eigen/src/Core/functors/TernaryFunctors.h create mode 100644 phonelibs/eigen/Eigen/src/Core/functors/UnaryFunctors.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/GeneralBlockPanelKernel.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/GeneralMatrixMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/GeneralMatrixMatrixTriangular_BLAS.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/GeneralMatrixMatrix_BLAS.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/GeneralMatrixVector.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/GeneralMatrixVector_BLAS.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/Parallelizer.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/SelfadjointMatrixMatrix_BLAS.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/SelfadjointMatrixVector.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/SelfadjointMatrixVector_BLAS.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/SelfadjointProduct.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/SelfadjointRank2Update.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/TriangularMatrixMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/TriangularMatrixMatrix_BLAS.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/TriangularMatrixVector.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/TriangularMatrixVector_BLAS.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/TriangularSolverMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/TriangularSolverMatrix_BLAS.h create mode 100644 phonelibs/eigen/Eigen/src/Core/products/TriangularSolverVector.h create mode 100755 phonelibs/eigen/Eigen/src/Core/util/BlasUtil.h create mode 100644 phonelibs/eigen/Eigen/src/Core/util/Constants.h create mode 100755 phonelibs/eigen/Eigen/src/Core/util/DisableStupidWarnings.h create mode 100644 phonelibs/eigen/Eigen/src/Core/util/ForwardDeclarations.h create mode 100755 phonelibs/eigen/Eigen/src/Core/util/MKL_support.h create mode 100644 phonelibs/eigen/Eigen/src/Core/util/Macros.h create mode 100644 phonelibs/eigen/Eigen/src/Core/util/Memory.h create mode 100755 phonelibs/eigen/Eigen/src/Core/util/Meta.h create mode 100644 phonelibs/eigen/Eigen/src/Core/util/NonMPL2.h create mode 100644 phonelibs/eigen/Eigen/src/Core/util/ReenableStupidWarnings.h create mode 100644 phonelibs/eigen/Eigen/src/Core/util/StaticAssert.h create mode 100644 phonelibs/eigen/Eigen/src/Core/util/XprHelper.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/ComplexEigenSolver.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/ComplexSchur.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/ComplexSchur_LAPACKE.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/EigenSolver.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/HessenbergDecomposition.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/RealQZ.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/RealSchur.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/RealSchur_LAPACKE.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/SelfAdjointEigenSolver_LAPACKE.h create mode 100644 phonelibs/eigen/Eigen/src/Eigenvalues/Tridiagonalization.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/AlignedBox.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/AngleAxis.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/EulerAngles.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/Homogeneous.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/Hyperplane.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/OrthoMethods.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/ParametrizedLine.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/Quaternion.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/Rotation2D.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/RotationBase.h create mode 100755 phonelibs/eigen/Eigen/src/Geometry/Scaling.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/Transform.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/Translation.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/Umeyama.h create mode 100644 phonelibs/eigen/Eigen/src/Geometry/arch/Geometry_SSE.h create mode 100644 phonelibs/eigen/Eigen/src/Householder/BlockHouseholder.h create mode 100644 phonelibs/eigen/Eigen/src/Householder/Householder.h create mode 100644 phonelibs/eigen/Eigen/src/Householder/HouseholderSequence.h create mode 100644 phonelibs/eigen/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h create mode 100644 phonelibs/eigen/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h create mode 100644 phonelibs/eigen/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h create mode 100644 phonelibs/eigen/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h create mode 100644 phonelibs/eigen/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h create mode 100644 phonelibs/eigen/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h create mode 100644 phonelibs/eigen/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h create mode 100644 phonelibs/eigen/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h create mode 100644 phonelibs/eigen/Eigen/src/Jacobi/Jacobi.h create mode 100644 phonelibs/eigen/Eigen/src/LU/Determinant.h create mode 100644 phonelibs/eigen/Eigen/src/LU/FullPivLU.h create mode 100644 phonelibs/eigen/Eigen/src/LU/InverseImpl.h create mode 100644 phonelibs/eigen/Eigen/src/LU/PartialPivLU.h create mode 100644 phonelibs/eigen/Eigen/src/LU/PartialPivLU_LAPACKE.h create mode 100644 phonelibs/eigen/Eigen/src/LU/arch/Inverse_SSE.h create mode 100644 phonelibs/eigen/Eigen/src/MetisSupport/MetisSupport.h create mode 100644 phonelibs/eigen/Eigen/src/OrderingMethods/Amd.h create mode 100644 phonelibs/eigen/Eigen/src/OrderingMethods/Eigen_Colamd.h create mode 100644 phonelibs/eigen/Eigen/src/OrderingMethods/Ordering.h create mode 100644 phonelibs/eigen/Eigen/src/PaStiXSupport/PaStiXSupport.h create mode 100644 phonelibs/eigen/Eigen/src/PardisoSupport/PardisoSupport.h create mode 100644 phonelibs/eigen/Eigen/src/QR/ColPivHouseholderQR.h create mode 100644 phonelibs/eigen/Eigen/src/QR/ColPivHouseholderQR_LAPACKE.h create mode 100644 phonelibs/eigen/Eigen/src/QR/CompleteOrthogonalDecomposition.h create mode 100644 phonelibs/eigen/Eigen/src/QR/FullPivHouseholderQR.h create mode 100644 phonelibs/eigen/Eigen/src/QR/HouseholderQR.h create mode 100644 phonelibs/eigen/Eigen/src/QR/HouseholderQR_LAPACKE.h create mode 100644 phonelibs/eigen/Eigen/src/SPQRSupport/SuiteSparseQRSupport.h create mode 100644 phonelibs/eigen/Eigen/src/SVD/BDCSVD.h create mode 100644 phonelibs/eigen/Eigen/src/SVD/JacobiSVD.h create mode 100644 phonelibs/eigen/Eigen/src/SVD/JacobiSVD_LAPACKE.h create mode 100644 phonelibs/eigen/Eigen/src/SVD/SVDBase.h create mode 100644 phonelibs/eigen/Eigen/src/SVD/UpperBidiagonalization.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCholesky/SimplicialCholesky.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/AmbiVector.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/CompressedStorage.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/MappedSparseMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseAssign.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseBlock.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseColEtree.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseCompressedBase.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseCwiseBinaryOp.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseCwiseUnaryOp.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseDenseProduct.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseDiagonalProduct.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseDot.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseFuzzy.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseMap.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseMatrixBase.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparsePermutation.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseProduct.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseRedux.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseRef.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseSelfAdjointView.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseSolverBase.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseSparseProductWithPruning.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseTranspose.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseTriangularView.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseUtil.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseVector.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/SparseView.h create mode 100644 phonelibs/eigen/Eigen/src/SparseCore/TriangularSolver.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLUImpl.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_Memory.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_Structs.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_Utils.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_column_bmod.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_column_dfs.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_gemm_kernel.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_kernel_bmod.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_panel_bmod.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_panel_dfs.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_pivotL.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_pruneL.h create mode 100644 phonelibs/eigen/Eigen/src/SparseLU/SparseLU_relax_snode.h create mode 100644 phonelibs/eigen/Eigen/src/SparseQR/SparseQR.h create mode 100644 phonelibs/eigen/Eigen/src/StlSupport/StdDeque.h create mode 100644 phonelibs/eigen/Eigen/src/StlSupport/StdList.h create mode 100644 phonelibs/eigen/Eigen/src/StlSupport/StdVector.h create mode 100644 phonelibs/eigen/Eigen/src/StlSupport/details.h create mode 100644 phonelibs/eigen/Eigen/src/SuperLUSupport/SuperLUSupport.h create mode 100644 phonelibs/eigen/Eigen/src/UmfPackSupport/UmfPackSupport.h create mode 100644 phonelibs/eigen/Eigen/src/misc/Image.h create mode 100644 phonelibs/eigen/Eigen/src/misc/Kernel.h create mode 100644 phonelibs/eigen/Eigen/src/misc/RealSvd2x2.h create mode 100644 phonelibs/eigen/Eigen/src/misc/blas.h create mode 100644 phonelibs/eigen/Eigen/src/misc/lapack.h create mode 100755 phonelibs/eigen/Eigen/src/misc/lapacke.h create mode 100644 phonelibs/eigen/Eigen/src/misc/lapacke_mangling.h create mode 100644 phonelibs/eigen/Eigen/src/plugins/ArrayCwiseBinaryOps.h create mode 100644 phonelibs/eigen/Eigen/src/plugins/ArrayCwiseUnaryOps.h create mode 100644 phonelibs/eigen/Eigen/src/plugins/BlockMethods.h create mode 100644 phonelibs/eigen/Eigen/src/plugins/CommonCwiseBinaryOps.h create mode 100644 phonelibs/eigen/Eigen/src/plugins/CommonCwiseUnaryOps.h create mode 100644 phonelibs/eigen/Eigen/src/plugins/MatrixCwiseBinaryOps.h create mode 100644 phonelibs/eigen/Eigen/src/plugins/MatrixCwiseUnaryOps.h create mode 100644 phonelibs/eigen/INSTALL create mode 100644 phonelibs/eigen/README.md create mode 100755 phonelibs/eigen/build.sh create mode 100644 phonelibs/eigen/signature_of_eigen3_matrix_library create mode 100644 phonelibs/eigen/unsupported/CMakeLists.txt create mode 100644 phonelibs/eigen/unsupported/Eigen/AdolcForward create mode 100644 phonelibs/eigen/unsupported/Eigen/AlignedVector3 create mode 100644 phonelibs/eigen/unsupported/Eigen/ArpackSupport create mode 100644 phonelibs/eigen/unsupported/Eigen/AutoDiff create mode 100644 phonelibs/eigen/unsupported/Eigen/BVH create mode 100644 phonelibs/eigen/unsupported/Eigen/CMakeLists.txt create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/CMakeLists.txt create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/Tensor create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/TensorSymmetry create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/ThreadPool create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/README.md create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/Tensor.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorArgMax.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorAssign.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBase.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorBroadcasting.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorChipping.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConcatenation.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContraction.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionBlocking.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionCuda.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionMapper.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorContractionThreadPool.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConversion.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorConvolution.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorCostModel.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorCustomOp.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDevice.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceCuda.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceDefault.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceSycl.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDeviceThreadPool.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensionList.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorDimensions.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorEvalTo.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorEvaluator.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorExecutor.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorExpr.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFFT.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFixedSize.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForcedEval.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorForwardDeclarations.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorFunctors.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGenerator.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorGlobalFunctions.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIO.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorImagePatch.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIndexList.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorInflation.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorInitializer.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorIntDiv.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorLayoutSwap.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMacros.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMap.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMeta.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorMorphing.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorPadding.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorPatch.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorRandom.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorReduction.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorReductionCuda.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorReductionSycl.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorRef.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorReverse.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorScan.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorShuffling.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorStorage.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorStriding.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSycl.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclConvertToDeviceExpression.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExprConstructor.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExtractAccessor.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclExtractFunctors.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclLeafCount.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclPlaceHolderExpr.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclRun.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorSyclTuple.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorTraits.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorUInt128.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/Tensor/TensorVolumePatch.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/TensorSymmetry/DynamicSymmetry.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/TensorSymmetry/StaticSymmetry.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/TensorSymmetry/Symmetry.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/TensorSymmetry/util/TemplateGroupTheory.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/ThreadPool/EventCount.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/ThreadPool/NonBlockingThreadPool.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/ThreadPool/RunQueue.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/ThreadPool/SimpleThreadPool.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/ThreadPool/ThreadEnvironment.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/ThreadPool/ThreadLocal.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/ThreadPool/ThreadPoolInterface.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/ThreadPool/ThreadYield.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/util/CXX11Meta.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/util/CXX11Workarounds.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/util/EmulateArray.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/util/EmulateCXX11Meta.h create mode 100644 phonelibs/eigen/unsupported/Eigen/CXX11/src/util/MaxSizeVector.h create mode 100644 phonelibs/eigen/unsupported/Eigen/EulerAngles create mode 100644 phonelibs/eigen/unsupported/Eigen/FFT create mode 100644 phonelibs/eigen/unsupported/Eigen/IterativeSolvers create mode 100644 phonelibs/eigen/unsupported/Eigen/KroneckerProduct create mode 100644 phonelibs/eigen/unsupported/Eigen/LevenbergMarquardt create mode 100644 phonelibs/eigen/unsupported/Eigen/MPRealSupport create mode 100644 phonelibs/eigen/unsupported/Eigen/MatrixFunctions create mode 100644 phonelibs/eigen/unsupported/Eigen/MoreVectorization create mode 100644 phonelibs/eigen/unsupported/Eigen/NonLinearOptimization create mode 100644 phonelibs/eigen/unsupported/Eigen/NumericalDiff create mode 100644 phonelibs/eigen/unsupported/Eigen/OpenGLSupport create mode 100644 phonelibs/eigen/unsupported/Eigen/Polynomials create mode 100644 phonelibs/eigen/unsupported/Eigen/Skyline create mode 100644 phonelibs/eigen/unsupported/Eigen/SparseExtra create mode 100644 phonelibs/eigen/unsupported/Eigen/SpecialFunctions create mode 100644 phonelibs/eigen/unsupported/Eigen/Splines create mode 100644 phonelibs/eigen/unsupported/Eigen/src/AutoDiff/AutoDiffJacobian.h create mode 100755 phonelibs/eigen/unsupported/Eigen/src/AutoDiff/AutoDiffScalar.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/AutoDiff/AutoDiffVector.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/BVH/BVAlgorithms.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/BVH/KdBVH.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Eigenvalues/ArpackSelfAdjointEigenSolver.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/EulerAngles/CMakeLists.txt create mode 100644 phonelibs/eigen/unsupported/Eigen/src/EulerAngles/EulerAngles.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/EulerAngles/EulerSystem.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/FFT/ei_fftw_impl.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/FFT/ei_kissfft_impl.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/IterativeSolvers/ConstrainedConjGrad.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/IterativeSolvers/DGMRES.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/IterativeSolvers/GMRES.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/IterativeSolvers/IncompleteLU.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/IterativeSolvers/IterationController.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/IterativeSolvers/MINRES.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/IterativeSolvers/Scaling.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/KroneckerProduct/KroneckerTensorProduct.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/LevenbergMarquardt/CopyrightMINPACK.txt create mode 100644 phonelibs/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMcovar.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMonestep.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMpar.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/LevenbergMarquardt/LMqrsolv.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/LevenbergMarquardt/LevenbergMarquardt.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/MatrixFunctions/MatrixExponential.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/MatrixFunctions/MatrixFunction.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/MatrixFunctions/MatrixLogarithm.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/MatrixFunctions/MatrixPower.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/MatrixFunctions/MatrixSquareRoot.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/MatrixFunctions/StemFunction.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/MoreVectorization/MathFunctions.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/HybridNonLinearSolver.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/LevenbergMarquardt.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/chkder.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/covar.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/dogleg.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/fdjac1.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/lmpar.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/qrsolv.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/r1mpyq.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/r1updt.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NonLinearOptimization/rwupdt.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/NumericalDiff/NumericalDiff.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Polynomials/Companion.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Polynomials/PolynomialSolver.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Polynomials/PolynomialUtils.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Skyline/SkylineInplaceLU.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Skyline/SkylineMatrix.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Skyline/SkylineMatrixBase.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Skyline/SkylineProduct.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Skyline/SkylineStorage.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Skyline/SkylineUtil.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SparseExtra/BlockOfDynamicSparseMatrix.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SparseExtra/BlockSparseMatrix.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SparseExtra/DynamicSparseMatrix.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SparseExtra/MarketIO.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SparseExtra/MatrixMarketIterator.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SparseExtra/RandomSetter.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SpecialFunctions/SpecialFunctionsArrayAPI.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SpecialFunctions/SpecialFunctionsFunctors.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SpecialFunctions/SpecialFunctionsHalf.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SpecialFunctions/SpecialFunctionsImpl.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SpecialFunctions/SpecialFunctionsPacketMath.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/SpecialFunctions/arch/CUDA/CudaSpecialFunctions.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Splines/Spline.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Splines/SplineFitting.h create mode 100644 phonelibs/eigen/unsupported/Eigen/src/Splines/SplineFwd.h create mode 100644 phonelibs/eigen/unsupported/README.txt create mode 100644 phonelibs/eigen/unsupported/doc/CMakeLists.txt create mode 100644 phonelibs/eigen/unsupported/doc/Overview.dox create mode 100644 phonelibs/eigen/unsupported/doc/eigendoxy_layout.xml.in create mode 100644 phonelibs/eigen/unsupported/doc/examples/BVH_Example.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/CMakeLists.txt create mode 100644 phonelibs/eigen/unsupported/doc/examples/EulerAngles.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/FFT.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/MatrixExponential.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/MatrixFunction.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/MatrixLogarithm.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/MatrixPower.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/MatrixPower_optimal.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/MatrixSine.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/MatrixSinh.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/MatrixSquareRoot.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/PolynomialSolver1.cpp create mode 100644 phonelibs/eigen/unsupported/doc/examples/PolynomialUtils1.cpp create mode 100644 phonelibs/eigen/unsupported/doc/snippets/CMakeLists.txt create mode 100644 phonelibs/fastcv/aarch64/include/fastcv.h create mode 100644 phonelibs/fastcv/aarch64/libfastcv.a create mode 100644 phonelibs/fastcv/aarch64/libfastcvadsp_stub.so create mode 100644 phonelibs/fastcv/aarch64/libfastcvopt.so create mode 100755 phonelibs/fastcv/x64/include/fastcv.h create mode 100755 phonelibs/fastcv/x64/include/fastcv.inl create mode 100755 phonelibs/fastcv/x64/include/stdint_.h create mode 100755 phonelibs/fastcv/x64/lib/libfastcv.a delete mode 100644 phonelibs/json11/json11.o create mode 100644 phonelibs/libyuv/LICENSE.libyuv create mode 100644 phonelibs/qpoases/SConscript create mode 100644 phonelibs/snpe/aarch64-android-clang3.8/libSNPE.so rename {selfdrive/visiond/dsp => phonelibs/snpe/aarch64-android-clang3.8}/libsnpe_dsp_skel.so (100%) create mode 100755 phonelibs/snpe/aarch64-android-clang3.8/libsymphony-cpu.so create mode 100755 phonelibs/snpe/aarch64-android-clang3.8/libsymphonypower.so create mode 100644 phonelibs/zmq/aarch64/lib/libczmq.a create mode 100755 phonelibs/zmq/aarch64/lib/libczmq.la create mode 100755 phonelibs/zmq/aarch64/lib/libczmq.so create mode 100755 phonelibs/zmq/aarch64/lib/libgnustl_shared.so create mode 100644 phonelibs/zmq/aarch64/lib/libzmq.a create mode 100755 phonelibs/zmq/aarch64/lib/libzmq.la create mode 100755 phonelibs/zmq/aarch64/lib/libzmq.so mode change 100644 => 100755 selfdrive/athena/athenad.py mode change 100644 => 100755 selfdrive/athena/manage_athenad.py create mode 100755 selfdrive/athena/test.py create mode 100644 selfdrive/athena/test_helpers.py delete mode 100644 selfdrive/boardd/Makefile create mode 100644 selfdrive/boardd/SConscript create mode 100644 selfdrive/camerad/SConscript rename selfdrive/{visiond => camerad}/bufs.h (100%) rename selfdrive/{visiond => camerad}/cameras/camera_common.h (97%) rename selfdrive/{visiond => camerad}/cameras/camera_frame_stream.cc (99%) rename selfdrive/{visiond => camerad}/cameras/camera_frame_stream.h (100%) rename selfdrive/{visiond => camerad}/cameras/camera_qcom.c (99%) rename selfdrive/{visiond => camerad}/cameras/camera_qcom.h (100%) rename selfdrive/{visiond => camerad}/cameras/debayer.cl (100%) rename selfdrive/{visiond => camerad}/cameras/sensor_i2c.h (100%) rename selfdrive/{visiond => camerad}/include/msm_cam_sensor.h (100%) rename selfdrive/{visiond => camerad}/include/msm_camsensor_sdk.h (100%) rename selfdrive/{visiond => camerad}/include/msmb_camera.h (100%) rename selfdrive/{visiond => camerad}/include/msmb_isp.h (100%) rename selfdrive/{visiond => camerad}/include/msmb_ispif.h (100%) rename selfdrive/{visiond/visiond.cc => camerad/main.cc} (75%) rename selfdrive/{can => camerad/snapshot}/__init__.py (100%) create mode 100755 selfdrive/camerad/snapshot/snapshot.py rename selfdrive/{visiond => camerad}/snapshot/visionipc.py (90%) rename selfdrive/{visiond => camerad}/transforms/rgb_to_yuv.c (100%) rename selfdrive/{visiond => camerad}/transforms/rgb_to_yuv.cl (100%) rename selfdrive/{visiond => camerad}/transforms/rgb_to_yuv.h (100%) rename selfdrive/{visiond => camerad}/transforms/rgb_to_yuv_test.cc (100%) delete mode 100644 selfdrive/can/Makefile delete mode 100644 selfdrive/can/can_define.py delete mode 100644 selfdrive/can/common.cc delete mode 100644 selfdrive/can/common.h delete mode 100644 selfdrive/can/common.pxd delete mode 100644 selfdrive/can/common_dbc.h delete mode 100644 selfdrive/can/dbc.cc delete mode 100644 selfdrive/can/dbc_out/.gitignore delete mode 100644 selfdrive/can/dbc_out/.gitkeep delete mode 100644 selfdrive/can/dbc_template.cc delete mode 100644 selfdrive/can/libdbc_py.py delete mode 100644 selfdrive/can/packer.cc delete mode 100644 selfdrive/can/packer.py delete mode 100644 selfdrive/can/packer_impl.pyx delete mode 100644 selfdrive/can/packer_setup.py delete mode 100644 selfdrive/can/parser.cc delete mode 100644 selfdrive/can/parser.py delete mode 100644 selfdrive/can/parser_pyx.pyx delete mode 100644 selfdrive/can/parser_pyx_setup.py delete mode 100644 selfdrive/can/plant_can_parser.py delete mode 100755 selfdrive/can/process_dbc.py delete mode 100644 selfdrive/can/tests/test_packer_parser.py create mode 100644 selfdrive/common/SConscript delete mode 100644 selfdrive/common/cereal.mk rename selfdrive/{visiond => common}/clutil.c (100%) rename selfdrive/{visiond => common}/clutil.h (100%) delete mode 100644 selfdrive/controls/lib/cluster/Makefile create mode 100644 selfdrive/controls/lib/cluster/SConscript create mode 100644 selfdrive/controls/lib/lateral_mpc/.gitignore delete mode 100644 selfdrive/controls/lib/lateral_mpc/Makefile create mode 100644 selfdrive/controls/lib/lateral_mpc/SConscript delete mode 100644 selfdrive/controls/lib/lateral_mpc/lateral_mpc.d delete mode 100644 selfdrive/controls/lib/lateral_mpc/lateral_mpc.o delete mode 100644 selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d delete mode 100644 selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o delete mode 100644 selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d delete mode 100644 selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o delete mode 100644 selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d delete mode 100644 selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o delete mode 100644 selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d delete mode 100644 selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o delete mode 100755 selfdrive/controls/lib/lateral_mpc/libmpc.so create mode 100644 selfdrive/controls/lib/longitudinal_mpc/.gitignore delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/Makefile create mode 100644 selfdrive/controls/lib/longitudinal_mpc/SConscript delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.d delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o delete mode 100755 selfdrive/controls/lib/longitudinal_mpc/libmpc1.so delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.d delete mode 100644 selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o rename selfdrive/{can/tests => debug}/__init__.py (100%) create mode 100755 selfdrive/debug/check_freq.py create mode 100644 selfdrive/launcher.py delete mode 100644 selfdrive/locationd/Makefile create mode 100644 selfdrive/locationd/SConscript delete mode 100644 selfdrive/locationd/get_vp.c delete mode 100644 selfdrive/logcatd/Makefile create mode 100644 selfdrive/logcatd/SConscript delete mode 100644 selfdrive/loggerd/Makefile create mode 100644 selfdrive/loggerd/SConscript delete mode 100644 selfdrive/loggerd/build_from_src.mk delete mode 100644 selfdrive/messaging/.gitignore delete mode 100644 selfdrive/messaging/Makefile delete mode 100644 selfdrive/messaging/__init__.py delete mode 100644 selfdrive/messaging/demo.cc delete mode 100644 selfdrive/messaging/demo.py delete mode 100644 selfdrive/messaging/impl_zmq.cc delete mode 100644 selfdrive/messaging/impl_zmq.hpp delete mode 100644 selfdrive/messaging/messaging.cc delete mode 100644 selfdrive/messaging/messaging.hpp delete mode 100644 selfdrive/messaging/messaging.pxd delete mode 100644 selfdrive/messaging/messaging_pyx.pyx delete mode 100644 selfdrive/messaging/messaging_pyx_setup.py create mode 100644 selfdrive/modeld/SConscript rename selfdrive/{visiond => modeld}/constants.py (59%) create mode 100755 selfdrive/modeld/modeld create mode 100644 selfdrive/modeld/modeld.cc rename selfdrive/{visiond => modeld}/models/commonmodel.c (100%) rename selfdrive/{visiond => modeld}/models/commonmodel.h (100%) rename selfdrive/{visiond => modeld}/models/driving.cc (92%) rename selfdrive/{visiond => modeld}/models/driving.h (100%) create mode 100644 selfdrive/modeld/models/monitoring.cc rename selfdrive/{visiond => modeld}/models/monitoring.h (62%) rename selfdrive/{visiond => modeld}/models/posenet.cc (100%) rename selfdrive/{visiond => modeld}/models/posenet.h (100%) create mode 100755 selfdrive/modeld/monitoringd create mode 100644 selfdrive/modeld/monitoringd.cc rename selfdrive/{visiond => modeld}/runners/run.h (100%) rename selfdrive/{visiond => modeld}/runners/runmodel.h (100%) rename selfdrive/{visiond => modeld}/runners/snpemodel.cc (100%) rename selfdrive/{visiond => modeld}/runners/snpemodel.h (100%) rename selfdrive/{visiond => modeld}/transforms/loadyuv.c (100%) rename selfdrive/{visiond => modeld}/transforms/loadyuv.cl (100%) rename selfdrive/{visiond => modeld}/transforms/loadyuv.h (100%) rename selfdrive/{visiond => modeld}/transforms/transform.c (100%) rename selfdrive/{visiond => modeld}/transforms/transform.cl (100%) rename selfdrive/{visiond => modeld}/transforms/transform.h (100%) delete mode 100644 selfdrive/proclogd/Makefile create mode 100644 selfdrive/proclogd/SConscript delete mode 100644 selfdrive/sensord/Makefile create mode 100644 selfdrive/sensord/SConscript delete mode 100644 selfdrive/sensord/build_from_src.mk create mode 100755 selfdrive/sensord/gpsd create mode 100755 selfdrive/sensord/sensord delete mode 100755 selfdrive/sensord/start_gpsd.py delete mode 100755 selfdrive/sensord/start_sensord.py delete mode 100644 selfdrive/service_list.yaml delete mode 100644 selfdrive/services.py delete mode 100644 selfdrive/ui/Makefile create mode 100644 selfdrive/ui/SConscript delete mode 100755 selfdrive/ui/start.py create mode 100755 selfdrive/ui/ui delete mode 100644 selfdrive/visiond/LICENSE.boringssl delete mode 100644 selfdrive/visiond/LICENSE.libyuv delete mode 100644 selfdrive/visiond/LICENSE.opencv delete mode 100644 selfdrive/visiond/Makefile delete mode 100644 selfdrive/visiond/README delete mode 100644 selfdrive/visiond/__init__.py delete mode 100644 selfdrive/visiond/build_from_src.mk delete mode 100644 selfdrive/visiond/models/monitoring.cc delete mode 100644 selfdrive/visiond/snapshot/Makefile delete mode 100644 selfdrive/visiond/snapshot/__init__.py delete mode 100755 selfdrive/visiond/snapshot/snapshot.py delete mode 100755 selfdrive/visiond/start.py diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index 433b370d93..f01c556e8f 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -16,7 +16,7 @@ Steps to reproduce the behavior, or a explorer/cabana link to the exact drive an **Expected behavior** A clear and concise description of what you expected to happen. -** Device/Version information (please complete the following information):** +**Device/Version information (please complete the following information):** - Device: [e.g. EON/EON Gold] - Version: [e.g. 0.6.4], or commit hash when on devel - Car make/model [e.g. Toyota Prius 2016] diff --git a/.gitignore b/.gitignore index 09e8fbde7c..8ed8c2c0ca 100644 --- a/.gitignore +++ b/.gitignore @@ -15,7 +15,9 @@ a.out .*.swp .*.swo .*.un~ +*.tmp *.o +*.os *.so *.a *.clb @@ -30,18 +32,22 @@ selfdrive/boardd/boardd selfdrive/logcatd/logcatd selfdrive/mapd/default_speeds_by_region.json selfdrive/proclogd/proclogd -selfdrive/ui/ui +selfdrive/ui/_ui selfdrive/test/longitudinal_maneuvers/out selfdrive/visiond/visiond selfdrive/loggerd/loggerd -selfdrive/sensord/gpsd -selfdrive/sensord/sensord +selfdrive/sensord/_gpsd +selfdrive/sensord/_sensord +selfdrive/camerad/camerad +selfdrive/modeld/_modeld +selfdrive/modeld/_monitoringd /src/ one openpilot notebooks xx +panda_jungle .coverage* htmlcov diff --git a/Dockerfile.openpilot b/Dockerfile.openpilot index c49bd576b1..b5a140c5a8 100644 --- a/Dockerfile.openpilot +++ b/Dockerfile.openpilot @@ -16,6 +16,7 @@ RUN apt-get update && apt-get install -y \ libeigen3-dev \ libffi-dev \ libglew-dev \ + libgles2-mesa-dev \ libglib2.0-0 \ liblzma-dev \ libmysqlclient-dev \ @@ -23,7 +24,7 @@ RUN apt-get update && apt-get install -y \ libopencv-dev \ libssl-dev \ libtool \ - libusb-1.0-0 \ + libusb-1.0-0-dev \ libzmq5-dev \ locales \ ocl-icd-libopencl1 \ @@ -63,7 +64,7 @@ RUN pip install matplotlib==3.1.1 dictdiffer==0.8.0 fastcluster==1.1.25 aenum==2 COPY phonelibs/install_capnp.sh /tmp/install_capnp.sh RUN /tmp/install_capnp.sh -RUN git clone --branch v0.6.5 https://github.com/commaai/openpilot-tools.git /tmp/openpilot/tools +RUN git clone --branch v0.7 https://github.com/commaai/openpilot-tools.git /tmp/openpilot/tools ENV PYTHONPATH /tmp/openpilot:${PYTHONPATH} COPY ./.pylintrc /tmp/openpilot/.pylintrc @@ -75,6 +76,7 @@ COPY ./phonelibs /tmp/openpilot/phonelibs COPY ./pyextra /tmp/openpilot/pyextra COPY ./panda /tmp/openpilot/panda +COPY SConstruct /tmp/openpilot/SConstruct + RUN mkdir -p /tmp/openpilot/selfdrive/test/out -RUN make -C /tmp/openpilot/selfdrive/controls/lib/longitudinal_mpc clean -RUN make -C /tmp/openpilot/selfdrive/controls/lib/lateral_mpc clean +RUN cd /tmp/openpilot && scons -j$(nproc) diff --git a/Pipfile b/Pipfile index 5f39a196ed..598b072e07 100644 --- a/Pipfile +++ b/Pipfile @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:8c6eff576ba07fc45d0257241e024075ac4e3d6c3391ed75d496f140d146e72b -size 2535 +oid sha256:5e6997ef9a2f37fb6783d0b41c6d85b8c275e916f0e66dcbd8b1050461892852 +size 2599 diff --git a/Pipfile.lock b/Pipfile.lock index a88026ae33..309e3feb53 100644 --- a/Pipfile.lock +++ b/Pipfile.lock @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:eeb5d8af1db839a0bf1a594af50c52d0d79dc6e764569819dc8e78dbb039090c -size 159193 +oid sha256:67e035ae5f7a07977f9839dcff6ff49189f5742e2e2e92977b2cfc0e041189df +size 168833 diff --git a/README.md b/README.md index 7cf601bdb4..1c12443c2b 100644 --- a/README.md +++ b/README.md @@ -4,14 +4,14 @@ Table of Contents ======================= * [What is openpilot?](#what-is-openpilot) +* [Integration with Stock Features](#integration-with-stock-features) * [Supported Hardware](#supported-hardware) * [Supported Cars](#supported-cars) * [Community Maintained Cars and Features](#community-maintained-cars-and-features) * [Installation Instructions](#installation-instructions) -* [Limitations of openpilot ALC](#limitations-of-openpilot-alc) -* [Limitations of openpilot ACC](#limitations-of-openpilot-acc) +* [Limitations of openpilot ALC and LDW](#limitations-of-openpilot-alc-and-ldw) +* [Limitations of openpilot ACC and FCW](#limitations-of-openpilot-acc-and-fcw) * [Limitations of openpilot DM](#limitations-of-openpilot-dm) -* [Integration with Stock Features](#integration-with-stock-features) * [User Data and comma Account](#user-data-and-comma-account) * [Safety and Testing](#safety-and-testing) * [Testing on PC](#testing-on-pc) @@ -24,7 +24,7 @@ Table of Contents What is openpilot? ------ -[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, openpilot performs the functions of Adaptive Cruise Control (ACC) and Automated Lane Centering (ALC) for a growing variety of supported [car makes, models and model years](#supported-cars). In addition, while openpilot is engaged, a camera based Driver Monitoring (DM) feature alerts distracted or asleep drivers. +[openpilot](http://github.com/commaai/openpilot) is an open source driver assistance system. Currently, openpilot performs the functions of Adaptive Cruise Control (ACC), Automated Lane Centering (ALC), Forward Collision Warning (FCW) and Lane Departure Warning (LDW) for a growing variety of supported [car makes, models and model years](#supported-cars). In addition, while openpilot is engaged, a camera based Driver Monitoring (DM) feature alerts distracted and asleep drivers. @@ -41,6 +41,19 @@ What is openpilot?
+Integration with Stock Features +------ + +In all supported cars: +* Stock Lane Keep Assist (LKA) and stock ALC are replaced by openpilot ALC, which only functions when openpilot is engaged by the user. +* Stock LDW is replaced by openpilot LDW. + +Additionally, on specific supported cars (see ACC column in [supported cars](#supported-cars)): +* Stock ACC is replaced by openpilot ACC. +* openpilot FCW operates in addition to stock FCW. + +openpilot should preserve all other vehicle's stock features, including, but are not limited to: FCW, Automatic Emergency Braking (AEB), auto high-beam, blind spot warning, and side collision warning. + Supported Hardware ------ @@ -49,87 +62,87 @@ At the moment, openpilot supports the [EON DevKit](https://comma.ai/shop/product Supported Cars ------ -| Make | Model (US Market Reference) | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | -| ----------| -----------------------------------| ------------------| --------| -----------------| -----------------| ---------------| -| Acura | ILX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph6| 25mph | -| Acura | RDX 2016-18 | AcuraWatch Plus | Yes | Yes | 25mph6| 12mph | -| Chrysler | Pacifica 2017-181 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | -| Chrysler | Pacifica Hybrid 2017-181| Adaptive Cruise | Yes | Stock | 0mph | 9mph | -| Chrysler | Pacifica Hybrid 20191 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | -| Honda | Accord 2018-19 | All | Yes | Stock | 0mph | 3mph | -| Honda | Accord Hybrid 2018-19 | All | Yes | Stock | 0mph | 3mph | -| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | Yes | Yes | 0mph | 12mph | -| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Yes | Stock | 0mph | 2mph | -| Honda | Civic Hatchback 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | -| Honda | CR-V 2015-16 | Touring | Yes | Yes | 25mph6| 12mph | -| Honda | CR-V 2017-19 | Honda Sensing | Yes | Stock | 0mph | 12mph | -| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Yes | Stock | 0mph | 12mph | -| Honda | Fit 2018-19 | Honda Sensing | Yes | Yes | 25mph6| 12mph | -| Honda | Odyssey 2018-20 | Honda Sensing | Yes | Yes | 25mph6| 0mph | -| Honda | Passport 2019 | All | Yes | Yes | 25mph6| 12mph | -| Honda | Pilot 2016-18 | Honda Sensing | Yes | Yes | 25mph6| 12mph | -| Honda | Pilot 2019 | All | Yes | Yes | 25mph6| 12mph | -| Honda | Ridgeline 2017-19 | Honda Sensing | Yes | Yes | 25mph6| 12mph | -| Hyundai | Santa Fe 20192 | All | Yes | Stock | 0mph | 0mph | -| Hyundai | Elantra 2017-192 | SCC + LKAS | Yes | Stock | 19mph | 34mph | -| Hyundai | Genesis 20182 | All | Yes | Stock | 19mph | 34mph | -| Jeep | Grand Cherokee 2016-181 | Adaptive Cruise | Yes | Stock | 0mph | 9mph | -| Jeep | Grand Cherokee 20191 | Adaptive Cruise | Yes | Stock | 0mph | 39mph | -| Kia | Optima 20192 | SCC + LKAS | Yes | Stock | 0mph | 0mph | -| Kia | Sorento 20182 | All | Yes | Stock | 0mph | 0mph | -| Kia | Stinger 20182 | SCC + LKAS | Yes | Stock | 0mph | 0mph | -| Lexus | CT Hybrid 2017-18 | All | Yes | Stock5| 0mph | 0mph | -| Lexus | ES Hybrid 2019 | All | Yes | Yes | 0mph | 0mph | -| Lexus | RX Hybrid 2016-19 | All | Yes | Stock5| 0mph | 0mph | -| Lexus | IS 2017-2019 | All | Yes | Stock | 22mph | 0mph | -| Lexus | IS Hybrid 2017 | All | Yes | Stock | 0mph | 0mph | -| Subaru | Crosstrek 2018-19 | EyeSight | Yes | Stock | 0mph | 0mph | -| Subaru | Impreza 2019-20 | EyeSight | Yes | Stock | 0mph | 0mph | -| Toyota | Avalon 2016 | TSS-P | Yes | Stock5| 20mph6| 0mph | -| Toyota | Avalon 2017-18 | All | Yes | Stock5| 20mph6| 0mph | -| Toyota | Camry 2018-19 | All | Yes | Stock | 0mph3 | 0mph | -| Toyota | Camry Hybrid 2018-19 | All | Yes | Stock | 0mph3 | 0mph | -| Toyota | C-HR 2017-19 | All | Yes | Stock | 0mph | 0mph | -| Toyota | C-HR Hybrid 2017-19 | All | Yes | Stock | 0mph | 0mph | -| Toyota | Corolla 2017-19 | All | Yes | Stock5| 20mph6| 0mph | -| Toyota | Corolla 2020 | All | Yes | Yes | 0mph | 0mph | -| Toyota | Corolla Hatchback 2019 | All | Yes | Yes | 0mph | 0mph | -| Toyota | Corolla Hybrid 2020 | All | Yes | Yes | 0mph | 0mph | -| Toyota | Highlander 2017-19 | All | Yes | Stock5| 0mph | 0mph | -| Toyota | Highlander Hybrid 2017-19 | All | Yes | Stock5| 0mph | 0mph | -| Toyota | Prius 2016 | TSS-P | Yes | Stock5| 0mph | 0mph | -| Toyota | Prius 2017-19 | All | Yes | Stock5| 0mph | 0mph | -| Toyota | Prius Prime 2017-20 | All | Yes | Stock5| 0mph | 0mph | -| Toyota | Rav4 2016 | TSS-P | Yes | Stock5| 20mph6| 0mph | -| Toyota | Rav4 2017-18 | All | Yes | Stock5| 20mph6| 0mph | -| Toyota | Rav4 2019 | All | Yes | Yes | 0mph | 0mph | -| Toyota | Rav4 Hybrid 2016 | TSS-P | Yes | Stock5| 0mph | 0mph | -| Toyota | Rav4 Hybrid 2017-18 | All | Yes | Stock5| 0mph | 0mph | -| Toyota | Sienna 2018 | All | Yes | Stock5| 0mph | 0mph | -| Volkswagen| Golf 2016-194 | Driver Assistance | Yes | Stock | 0mph | 0mph | +| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | +| ----------| -----------------------------------| ------------------| -----------------| -------------------| -------------| +| Acura | ILX 2016-18 | AcuraWatch Plus | openpilot | 25mph6 | 25mph | +| Acura | RDX 2016-18 | AcuraWatch Plus | openpilot | 25mph6 | 12mph | +| Chrysler | Pacifica 2017-181 | Adaptive Cruise | Stock | 0mph | 9mph | +| Chrysler | Pacifica Hybrid 2017-181| Adaptive Cruise | Stock | 0mph | 9mph | +| Chrysler | Pacifica Hybrid 20191 | Adaptive Cruise | Stock | 0mph | 39mph | +| Honda | Accord 2018-19 | All | Stock | 0mph | 3mph | +| Honda | Accord Hybrid 2018-19 | All | Stock | 0mph | 3mph | +| Honda | Civic Sedan/Coupe 2016-18 | Honda Sensing | openpilot | 0mph | 12mph | +| Honda | Civic Sedan/Coupe 2019 | Honda Sensing | Stock | 0mph | 2mph | +| Honda | Civic Hatchback 2017-19 | Honda Sensing | Stock | 0mph | 12mph | +| Honda | CR-V 2015-16 | Touring | openpilot | 25mph6 | 12mph | +| Honda | CR-V 2017-19 | Honda Sensing | Stock | 0mph | 12mph | +| Honda | CR-V Hybrid 2017-2019 | Honda Sensing | Stock | 0mph | 12mph | +| Honda | Fit 2018-19 | Honda Sensing | openpilot | 25mph6 | 12mph | +| Honda | Odyssey 2018-20 | Honda Sensing | openpilot | 25mph6 | 0mph | +| Honda | Passport 2019 | All | openpilot | 25mph6 | 12mph | +| Honda | Pilot 2016-18 | Honda Sensing | openpilot | 25mph6 | 12mph | +| Honda | Pilot 2019 | All | openpilot | 25mph6 | 12mph | +| Honda | Ridgeline 2017-19 | Honda Sensing | openpilot | 25mph6 | 12mph | +| Hyundai | Santa Fe 20192 | All | Stock | 0mph | 0mph | +| Hyundai | Elantra 2017-192 | SCC + LKAS | Stock | 19mph | 34mph | +| Hyundai | Genesis 20182 | All | Stock | 19mph | 34mph | +| Jeep | Grand Cherokee 2016-181 | Adaptive Cruise | Stock | 0mph | 9mph | +| Jeep | Grand Cherokee 20191 | Adaptive Cruise | Stock | 0mph | 39mph | +| Kia | Optima 20192 | SCC + LKAS | Stock | 0mph | 0mph | +| Kia | Sorento 20182 | All | Stock | 0mph | 0mph | +| Kia | Stinger 20182 | SCC + LKAS | Stock | 0mph | 0mph | +| Lexus | CT Hybrid 2017-18 | All | Stock5| 0mph | 0mph | +| Lexus | ES Hybrid 2019 | All | openpilot | 0mph | 0mph | +| Lexus | RX Hybrid 2016-19 | All | Stock5| 0mph | 0mph | +| Lexus | IS 2017-2019 | All | Stock | 22mph | 0mph | +| Lexus | IS Hybrid 2017 | All | Stock | 0mph | 0mph | +| Subaru | Crosstrek 2018-19 | EyeSight | Stock | 0mph | 0mph | +| Subaru | Impreza 2019-20 | EyeSight | Stock | 0mph | 0mph | +| Toyota | Avalon 2016 | TSS-P | Stock5| 20mph6 | 0mph | +| Toyota | Avalon 2017-18 | All | Stock5| 20mph6 | 0mph | +| Toyota | Camry 2018-19 | All | Stock | 0mph3 | 0mph | +| Toyota | Camry Hybrid 2018-19 | All | Stock | 0mph3 | 0mph | +| Toyota | C-HR 2017-19 | All | Stock | 0mph | 0mph | +| Toyota | C-HR Hybrid 2017-19 | All | Stock | 0mph | 0mph | +| Toyota | Corolla 2017-19 | All | Stock5| 20mph6 | 0mph | +| Toyota | Corolla 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Corolla Hatchback 2019 | All | openpilot | 0mph | 0mph | +| Toyota | Corolla Hybrid 2020 | All | openpilot | 0mph | 0mph | +| Toyota | Highlander 2017-19 | All | Stock5| 0mph | 0mph | +| Toyota | Highlander Hybrid 2017-19 | All | Stock5| 0mph | 0mph | +| Toyota | Prius 2016 | TSS-P | Stock5| 0mph | 0mph | +| Toyota | Prius 2017-19 | All | Stock5| 0mph | 0mph | +| Toyota | Prius Prime 2017-20 | All | Stock5| 0mph | 0mph | +| Toyota | Rav4 2016 | TSS-P | Stock5| 20mph6 | 0mph | +| Toyota | Rav4 2017-18 | All | Stock5| 20mph6 | 0mph | +| Toyota | Rav4 2019 | All | openpilot | 0mph | 0mph | +| Toyota | Rav4 Hybrid 2016 | TSS-P | Stock5| 0mph | 0mph | +| Toyota | Rav4 Hybrid 2017-18 | All | Stock5| 0mph | 0mph | +| Toyota | Sienna 2018 | All | Stock5| 0mph | 0mph | +| Volkswagen| Golf 2016-194 | Driver Assistance | Stock | 0mph | 0mph | 1Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and [FCA giraffe](https://comma.ai/shop/products/giraffe)
-2Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and open sourced [Hyundai Giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai), designed for the 2019 Sante Fe; pinout may differ for other Hyundai and Kia models.
+2Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and open sourced [Hyundai giraffe](https://github.com/commaai/neo/tree/master/giraffe/hyundai), designed for the 2019 Sante Fe; pinout may differ for other Hyundai and Kia models.
328mph for Camry 4CYL L, 4CYL LE and 4CYL SE which don't have Full-Speed Range Dynamic Radar Cruise Control.
4Requires a [custom connector](https://community.comma.ai/wiki/index.php/Volkswagen#Integration_at_R242_Camera) for the [car harness](https://comma.ai/shop/products/car-harness)
Community Maintained Cars and Features ------ -| Make | Model (US Market Reference) | Supported Package | Lateral | Longitudinal | No Accel Below | No Steer Below | -| ----------| -----------------------------------| ------------------| --------| -----------------| -----------------| ---------------| -| Buick | Regal 20187 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | -| Chevrolet | Malibu 20177 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | -| Chevrolet | Volt 2017-187 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | -| Cadillac | ATS 20187 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | -| GMC | Acadia Denali 20187 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | -| Holden | Astra 20177 | Adaptive Cruise | Yes | Yes | 0mph | 7mph | +| Make | Model (US Market Reference) | Supported Package | ACC | No ACC accel below | No ALC below | +| ----------| -----------------------------------| ------------------| -----------------| -------------------| -------------| +| Buick | Regal 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Chevrolet | Malibu 20177 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Chevrolet | Volt 2017-187 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Cadillac | ATS 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | +| GMC | Acadia Denali 20187 | Adaptive Cruise | openpilot | 0mph | 7mph | +| Holden | Astra 20177 | Adaptive Cruise | openpilot | 0mph | 7mph | -5When disconnecting the Driver Support Unit (DSU), openpilot longitudinal control will replace stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).***
+5When disconnecting the Driver Support Unit (DSU), openpilot ACC will replace stock ACC. For DSU locations, see [Toyota Wiki page](https://community.comma.ai/wiki/index.php/Toyota). ***NOTE: disconnecting the DSU disables Automatic Emergency Braking (AEB).***
6[Comma Pedal](https://community.comma.ai/wiki/index.php/Comma_Pedal) is used to provide stop-and-go capability to some of the openpilot-supported cars that don't currently support stop-and-go. Here is how to [build a Comma Pedal](https://medium.com/@jfrux/comma-pedal-building-with-macrofab-6328bea791e8). ***NOTE: The Comma Pedal is not officially supported by [comma](https://comma.ai).***
7Requires a [panda](https://comma.ai/shop/products/panda-obd-ii-dongle) and [community built giraffe](https://zoneos.com/volt/). ***NOTE: disconnecting the ASCM disables Automatic Emergency Braking (AEB).***
-Community Maintained Cars and Features are not confirmed by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. +Community Maintained Cars and Features are not verified by comma to meet our [safety model](SAFETY.md). Be extra cautious using them. They are only available after enabling the toggle in `Settings->Developer->Enable Community Features`. Installation Instructions ------ @@ -138,14 +151,16 @@ Install openpilot on a EON by entering ``https://openpilot.comma.ai`` during the Follow this [video instructions](https://youtu.be/3nlkomHathI) to properly mount the EON on the windshield. Note: openpilot features an automatic pose calibration routine and openpilot performance should not be affected by small pitch and yaw misalignments caused by imprecise EON mounting. +Before placing the device on your windshield, check the state and local laws and ordinances where you drive. Some state laws prohibit or restrict the placement of objects on the windshield of a motor vehicle. + You will be able to engage openpilot after reviewing the onboarding screens and finishing the calibration procedure. -Limitations of openpilot ALC +Limitations of openpilot ALC and LDW ------ -openpilot Automated Lane Centering (ALC) does not automatically drive the vehicle or reduce the amount of attention that must be paid to operate your vehicle. The driver must always keep control of the steering wheel and be ready to correct the openpilot ALC action at all times. +openpilot ALC and openpilot LDW do not automatically drive the vehicle or reduce the amount of attention that must be paid to operate your vehicle. The driver must always keep control of the steering wheel and be ready to correct the openpilot ALC action at all times. -Many factors can impact the performance of openpilot ALC, causing it to be unable to function as intended. These include, but are not limited to: +Many factors can impact the performance of openpilot ALC and openpilot LDW, causing them to be unable to function as intended. These include, but are not limited to: * Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation. * The road facing camera is obstructed, covered or damaged by mud, ice, snow, etc. @@ -160,12 +175,12 @@ Many factors can impact the performance of openpilot ALC, causing it to be unabl The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times. -Limitations of openpilot ACC +Limitations of openpilot ACC and FCW ------ -openpilot Adaptive Cruise Control (ACC) is not a system that allows careless or inattentive driving. It is still necessary for the driver to pay close attention to the vehicle’s surroundings and to be ready to re-take control of the gas and the brake at all times. +openpilot ACC and openpilot FCW are not systems that allow careless or inattentive driving. It is still necessary for the driver to pay close attention to the vehicle’s surroundings and to be ready to re-take control of the gas and the brake at all times. -Many factors can impact the performance of openpilot ACC, causing it to be unable to function as intended. These include, but are not limited to: +Many factors can impact the performance of openpilot ACC and openpilot FCW, causing them to be unable to function as intended. These include, but are not limited to: * Poor visibility (heavy rain, snow, fog, etc.) or weather conditions that may interfere with sensor operation. * The road facing camera or radar are obstructed, covered, or damaged by mud, ice, snow, etc. @@ -185,37 +200,29 @@ Many factors can impact the performance of openpilot ACC, causing it to be unabl The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. It is the driver's responsibility to be in control of the vehicle at all times. -Limitation of openpilot DM +Limitations of openpilot DM ------ -openpilot Driver Monitoring (DM) should not be considered an exact measurements of the status of alertness of the driver. +openpilot DM should not be considered an exact measurements of the status of alertness of the driver. Many factors can impact the performance of openpilot DM, causing it to be unable to function as intended. These include, but are not limited to: * Low light conditions, such as driving at night or in dark tunnels. * Bright light (due to oncoming headlights, direct sunlight, etc.). -* The driver face is partially or completely outside field of view of the the driver facing camera. +* The driver face is partially or completely outside field of view of the driver facing camera. * Right hand driving vehicles. * The driver facing camera is obstructed, covered, or damaged. -The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. A fatigued or impared driver should not rely on openpilot DM to asses his level of attention. - -Integration with Stock Features ------- -Lane Departure Warning (LDW), Lane Keep Assist (LKAS), and Automated Lane Centering (ALC) are replaced by openpilot ALC, which only functions when openpilot is engaged. - -Adaptive Cruise Control (ACC) is replaced by openpilot ACC. - -openpilot preserves all other vehicle's stock features, including, but are not limited to: AEB, auto high-beam, blind spot warning, and side collision warning. +The list above does not represent an exhaustive list of situations that may interfere with proper operation of openpilot components. A driver should not rely on openpilot DM to assess their level of attention. User Data and comma Account ------ -By default, openpilot uploads the driving data to our servers. You can also access your data by pairing with the comma connect app ([iOS](https://apps.apple.com/us/app/comma-connect/id1456551889), [android](https://play.google.com/store/apps/details?id=ai.comma.connect&hl=en_US)). We use your data to train better models and improve openpilot for everyone. +By default, openpilot uploads the driving data to our servers. You can also access your data by pairing with the comma connect app ([iOS](https://apps.apple.com/us/app/comma-connect/id1456551889), [Android](https://play.google.com/store/apps/details?id=ai.comma.connect&hl=en_US)). We use your data to train better models and improve openpilot for everyone. openpilot is open source software: the user is free to disable data collection if they wish to do so. -It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs. +openpilot logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs. The driver facing camera is only logged if you explicitly opt-in in settings. The microphone is not recorded. By using openpilot, you agree to [our Privacy Policy](https://my.comma.ai/privacy). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma for the use of this data. diff --git a/README_chffrplus.md b/README_chffrplus.md deleted file mode 100644 index dc13e8c3f2..0000000000 --- a/README_chffrplus.md +++ /dev/null @@ -1,36 +0,0 @@ -Welcome to chffrplus -====== - -[chffrplus](https://github.com/commaai/chffrplus) is an open source dashcam. - -This is the shipping reference software for the comma EON Dashcam DevKit. It keeps many of the niceities of [openpilot](https://github.com/commaai/openpilot), like high quality sensors, great camera, and good autostart and stop. Though unlike openpilot, it cannot control your car. chffrplus can interface with your car through a [panda](https://shop.comma.ai/products/panda-obd-ii-dongle), but just like our dashcam app [chffr](https://getchffr.com/), it is read only. - -It integrates with the rest of the comma ecosystem, so you can view your drives on the [chffr](https://getchffr.com/) app for Android or iOS, and reverse engineer your car with [cabana](https://community.comma.ai/cabana/?demo=1). - - -Hardware ------- - -Right now chffrplus supports the [EON Dashcam DevKit](https://shop.comma.ai/products/eon-dashcam-devkit) for hardware to run on. - -Install chffrplus on a EON device by entering ``https://chffrplus.comma.ai`` during NEOS setup. - - -User Data / chffr Account / Crash Reporting ------- - -By default chffrplus creates an account and includes a client for chffr, our dashcam app. - -It's open source software, so you are free to disable it if you wish. - -It logs the road facing camera, CAN, GPS, IMU, magnetometer, thermal sensors, crashes, and operating system logs. -It does not log the user facing camera or the microphone. - -By using it, you agree to [our privacy policy](https://beta.comma.ai/privacy.html). You understand that use of this software or its related services will generate certain types of user data, which may be logged and stored at the sole discretion of comma.ai. By accepting this agreement, you grant an irrevocable, perpetual, worldwide right to comma.ai for the use of this data. - - -Licensing ------- - -chffrplus is released under the MIT license. - diff --git a/RELEASES.md b/RELEASES.md index 7d21b52260..2e99a28bc0 100644 --- a/RELEASES.md +++ b/RELEASES.md @@ -1,3 +1,18 @@ +Version 0.7 (2019-12-13) +======================== + * Move to SCons build system! + * Add Lane Departure Warning (LDW) for all supported vehicles! + * NEOS update: increase wifi speed thanks to jyoung8607! + * Adaptive driver monitoring based on scene + * New driving model trained end-to-end: improve lane lines and lead detection + * Smarter torque limit alerts for all cars + * Improve GM longitudinal control: proper computations for 15Hz radar + * Move GM port, Toyota with DSU removed, comma pedal in community features; toggle switch required + * Remove upload over cellular toggle: only upload qlog and qcamera files if not on wifi + * Refactor Panda code towards ISO26262 and SIL2 compliancy + * Forward stock FCW for Honda Nidec + * Volkswagen port now standard: comma Harness intercepts stock camera + Version 0.6.6 (2019-11-05) ======================== * Volkswagen support thanks to jyoung8607! diff --git a/SConstruct b/SConstruct new file mode 100644 index 0000000000..9de2ba53a1 --- /dev/null +++ b/SConstruct @@ -0,0 +1,212 @@ +import os +import subprocess +import sys + +AddOption('--test', + action='store_true', + help='build test files') + +AddOption('--asan', + action='store_true', + help='turn on ASAN') + +arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() + +if arch == "aarch64": + lenv = { + "LD_LIBRARY_PATH": '/data/data/com.termux/files/usr/lib', + "PATH": os.environ['PATH'], + "ANDROID_DATA": os.environ['ANDROID_DATA'], + "ANDROID_ROOT": os.environ['ANDROID_ROOT'], + } + + cpppath = [ + "#phonelibs/opencl/include", + ] + libpath = [ + "#phonelibs/snpe/aarch64-android-clang3.8", + "/usr/lib", + "/data/data/com.termux/files/usr/lib", + "/system/vendor/lib64", + "/system/comma/usr/lib", + "#phonelibs/yaml-cpp/lib", + "#phonelibs/nanovg", + "#phonelibs/libyuv/lib", + ] + + cflags = ["-DQCOM", "-mcpu=cortex-a57"] + cxxflags = ["-DQCOM", "-mcpu=cortex-a57"] + + rpath = ["/system/vendor/lib64"] +else: + lenv = { + "PATH": "#external/bin:" + os.environ['PATH'], + } + cpppath = [ + "#phonelibs/capnp-cpp/include", + "#phonelibs/capnp-c/include", + "#phonelibs/zmq/x64/include", + ] + libpath = [ + "#phonelibs/capnp-cpp/x64/lib", + "#phonelibs/capnp-c/x64/lib", + "#phonelibs/yaml-cpp/x64/lib", + "#phonelibs/snpe/x86_64-linux-clang", + "#phonelibs/zmq/x64/lib", + "#phonelibs/libyuv/x64/lib", + "#external/zmq/lib", + "#cereal", + "#selfdrive/common", + "/usr/lib", + "/usr/local/lib", + ] + + rpath = ["phonelibs/capnp-cpp/x64/lib", + "cereal", + "selfdrive/common"] + + # allows shared libraries to work globally + rpath = [os.path.join(os.getcwd(), x) for x in rpath] + + cflags = [] + cxxflags = [] + +ccflags_asan = ["-fsanitize=address", "-fno-omit-frame-pointer"] if GetOption('asan') else [] +ldflags_asan = ["-fsanitize=address"] if GetOption('asan') else [] + +# change pythonpath to this +lenv["PYTHONPATH"] = Dir("#").path + +env = Environment( + ENV=lenv, + CCFLAGS=[ + "-g", + "-fPIC", + "-O2", + "-Werror=implicit-function-declaration", + "-Werror=incompatible-pointer-types", + "-Werror=int-conversion", + "-Werror=return-type", + "-Werror=format-extra-args", + ] + cflags + ccflags_asan, + + CPPPATH=cpppath + [ + "#", + "#selfdrive", + "#phonelibs/bzip2", + "#phonelibs/libyuv/include", + "#phonelibs/yaml-cpp/include", + "#phonelibs/openmax/include", + "#phonelibs/json/src", + "#phonelibs/json11", + "#phonelibs/eigen", + "#phonelibs/curl/include", + "#phonelibs/opencv/include", + "#phonelibs/libgralloc/include", + "#phonelibs/android_frameworks_native/include", + "#phonelibs/android_hardware_libhardware/include", + "#phonelibs/android_system_core/include", + "#phonelibs/linux/include", + "#phonelibs/snpe/include", + "#phonelibs/nanovg", + "#selfdrive/common", + "#selfdrive/camerad", + "#selfdrive/camerad/include", + "#selfdrive/loggerd/include", + "#selfdrive/modeld", + "#cereal/messaging", + "#cereal", + "#opendbc/can", + ], + + CC='clang', + CXX='clang++', + LINKFLAGS=ldflags_asan, + + RPATH=rpath, + + CFLAGS=["-std=gnu11"] + cflags, + CXXFLAGS=["-std=c++14"] + cxxflags, + LIBPATH=libpath + + [ + "#cereal", + "#selfdrive/common", + "#phonelibs", + ] +) + +if os.environ.get('SCONS_CACHE'): + CacheDir('/tmp/scons_cache') + +node_interval = 5 +node_count = 0 +def progress_function(node): + global node_count + node_count += node_interval + sys.stderr.write("progress: %d\n" % node_count) + +if os.environ.get('SCONS_PROGRESS'): + Progress(progress_function, interval=node_interval) + +SHARED = False + +def abspath(x): + if arch == 'aarch64': + pth = os.path.join("/data/pythonpath", x[0].path) + env.Depends(pth, x) + return File(pth) + else: + # rpath works elsewhere + return x[0].path.rsplit("/", 1)[1][:-3] + +#zmq = 'zmq' +# still needed for apks +zmq = FindFile("libzmq.a", libpath) +Export('env', 'arch', 'zmq', 'SHARED') + +# cereal and messaging are shared with the system +SConscript(['cereal/SConscript']) +if SHARED: + cereal = abspath([File('cereal/libcereal_shared.so')]) + messaging = abspath([File('cereal/libmessaging_shared.so')]) +else: + cereal = [File('#cereal/libcereal.a')] + messaging = [File('#cereal/libmessaging.a')] +Export('cereal', 'messaging') + +SConscript(['selfdrive/common/SConscript']) +Import('_common', '_visionipc', '_gpucommon', '_gpu_libs') + +if SHARED: + common, visionipc, gpucommon = abspath(common), abspath(visionipc), abspath(gpucommon) +else: + common = [_common, 'json'] + visionipc = _visionipc + gpucommon = [_gpucommon] + _gpu_libs + +Export('common', 'visionipc', 'gpucommon') + +SConscript(['opendbc/can/SConscript']) + +SConscript(['common/SConscript']) +SConscript(['common/kalman/SConscript']) +SConscript(['phonelibs/SConscript']) + +SConscript(['selfdrive/modeld/SConscript']) +SConscript(['selfdrive/camerad/SConscript']) +SConscript(['selfdrive/controls/lib/cluster/SConscript']) +SConscript(['selfdrive/controls/lib/lateral_mpc/SConscript']) +SConscript(['selfdrive/controls/lib/longitudinal_mpc/SConscript']) + +SConscript(['selfdrive/boardd/SConscript']) +SConscript(['selfdrive/proclogd/SConscript']) + +if arch == "aarch64": + SConscript(['selfdrive/logcatd/SConscript']) + SConscript(['selfdrive/ui/SConscript']) + SConscript(['selfdrive/sensord/SConscript']) + SConscript(['selfdrive/loggerd/SConscript']) + +SConscript(['selfdrive/locationd/SConscript']) + +# TODO: finish cereal, dbcbuilder, MPC diff --git a/apk/ai.comma.plus.frame.apk b/apk/ai.comma.plus.frame.apk index f0dc46f2bb..19b7e4e57b 100644 --- a/apk/ai.comma.plus.frame.apk +++ b/apk/ai.comma.plus.frame.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:1d3e4d7e9a84cb5a507137f1d57046649ffc000f2ff0bbd047438846f07d04d5 -size 2616780 +oid sha256:8b20fb584bc1cc3637e5cb58b6688e9e250bb7f9eacb8b6e0d50a890e79d7797 +size 2848396 diff --git a/apk/ai.comma.plus.offroad.apk b/apk/ai.comma.plus.offroad.apk index c5f9e74332..56e758d566 100644 --- a/apk/ai.comma.plus.offroad.apk +++ b/apk/ai.comma.plus.offroad.apk @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:930595cab227f8288a5dafaa04d3e71c98166c55cf6399e25e46f51237f1ac98 -size 17796274 +oid sha256:7a35f3ee4353210c0cfe714653920c05c588fb40c6c62088f81cd02bfc7eb6d3 +size 16150762 diff --git a/common/.gitignore b/common/.gitignore new file mode 100644 index 0000000000..ce1da4c53c --- /dev/null +++ b/common/.gitignore @@ -0,0 +1 @@ +*.cpp diff --git a/common/SConscript b/common/SConscript new file mode 100644 index 0000000000..103d416f53 --- /dev/null +++ b/common/SConscript @@ -0,0 +1,6 @@ +Import('env') + +# parser +env.Command(['common_pyx.so'], + ['common_pyx_setup.py', 'clock.pyx'], + "cd common && python3 common_pyx_setup.py build_ext --inplace") diff --git a/common/android.py b/common/android.py new file mode 100644 index 0000000000..eda82385a8 --- /dev/null +++ b/common/android.py @@ -0,0 +1,73 @@ +import binascii +import itertools +import re +import struct +import subprocess + +def getprop(key): + return subprocess.check_output(["getprop", key], encoding='utf8').strip() + +def get_imei(): + ret = getprop("oem.device.imeicache") + if ret == "": + ret = "000000000000000" + return ret + +def get_serial(): + return getprop("ro.serialno") + +def get_subscriber_info(): + ret = parse_service_call_string(["iphonesubinfo", "7"]) + if ret is None or len(ret) < 8: + return "" + return ret + +def reboot(reason=None): + if reason is None: + reason_args = ["null"] + else: + reason_args = ["s16", reason] + + subprocess.check_output([ + "service", "call", "power", "16", # IPowerManager.reboot + "i32", "0", # no confirmation, + *reason_args, + "i32", "1" # wait + ]) + +def parse_service_call_unpack(call, fmt): + r = parse_service_call_bytes(call) + try: + return struct.unpack(fmt, r)[0] + except Exception: + return None + +def parse_service_call_string(call): + r = parse_service_call_bytes(call) + try: + r = r[8:] # Cut off length field + r = r.decode('utf_16_be') + + # All pairs of two characters seem to be swapped. Not sure why + result = "" + for a, b, in itertools.zip_longest(r[::2], r[1::2], fillvalue='\x00'): + result += b + a + + result = result.replace('\x00', '') + + return result + except Exception: + return None + +def parse_service_call_bytes(call): + ret = subprocess.check_output(["service", "call", *call], encoding='utf8').strip() + if 'Parcel' not in ret: + return None + + try: + r = b"" + for hex_part in re.findall(r'[ (]([0-9a-f]{8})', ret): + r += binascii.unhexlify(hex_part) + return r + except Exception: + return None diff --git a/common/apk.py b/common/apk.py new file mode 100644 index 0000000000..064696f10e --- /dev/null +++ b/common/apk.py @@ -0,0 +1,98 @@ +import os +import subprocess +import glob +import hashlib +import shutil +from common.basedir import BASEDIR +from selfdrive.swaglog import cloudlog + +android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame") + +def get_installed_apks(): + dat = subprocess.check_output(["pm", "list", "packages", "-f"], encoding='utf8').strip().split("\n") + ret = {} + for x in dat: + if x.startswith("package:"): + v,k = x.split("package:")[1].split("=") + ret[k] = v + return ret + +def install_apk(path): + # can only install from world readable path + install_path = "/sdcard/%s" % os.path.basename(path) + shutil.copyfile(path, install_path) + + ret = subprocess.call(["pm", "install", "-r", install_path]) + os.remove(install_path) + return ret == 0 + +def start_frame(): + set_package_permissions() + system("am start -n ai.comma.plus.frame/.MainActivity") + +def set_package_permissions(): + pm_grant("ai.comma.plus.offroad", "android.permission.ACCESS_FINE_LOCATION") + appops_set("ai.comma.plus.offroad", "SU", "allow") + appops_set("ai.comma.plus.offroad", "WIFI_SCAN", "allow") + appops_set("ai.comma.plus.offroad", "READ_EXTERNAL_STORAGE", "allow") + appops_set("ai.comma.plus.offroad", "WRITE_EXTERNAL_STORAGE", "allow") + +def appops_set(package, op, mode): + system(f"LD_LIBRARY_PATH= appops set {package} {op} {mode}") + +def pm_grant(package, permission): + system(f"pm grant {package} {permission}") + +def system(cmd): + try: + cloudlog.info("running %s" % cmd) + subprocess.check_output(cmd, stderr=subprocess.STDOUT, shell=True) + except subprocess.CalledProcessError as e: + cloudlog.event("running failed", + cmd=e.cmd, + output=e.output[-1024:], + returncode=e.returncode) + +# *** external functions *** + +def update_apks(): + # install apks + installed = get_installed_apks() + + install_apks = glob.glob(os.path.join(BASEDIR, "apk/*.apk")) + for apk in install_apks: + app = os.path.basename(apk)[:-4] + if app not in installed: + installed[app] = None + + cloudlog.info("installed apks %s" % (str(installed), )) + + for app in installed.keys(): + apk_path = os.path.join(BASEDIR, "apk/"+app+".apk") + if not os.path.exists(apk_path): + continue + + h1 = hashlib.sha1(open(apk_path, 'rb').read()).hexdigest() + h2 = None + if installed[app] is not None: + h2 = hashlib.sha1(open(installed[app], 'rb').read()).hexdigest() + cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2)) + + if h2 is None or h1 != h2: + cloudlog.info("installing %s" % app) + + success = install_apk(apk_path) + if not success: + cloudlog.info("needing to uninstall %s" % app) + system("pm uninstall %s" % app) + success = install_apk(apk_path) + + assert success + +def pm_apply_packages(cmd): + for p in android_packages: + system("pm %s %s" % (cmd, p)) + +if __name__ == "__main__": + update_apks() + diff --git a/common/common_pyx_setup.py b/common/common_pyx_setup.py new file mode 100644 index 0000000000..9d0f0fec8b --- /dev/null +++ b/common/common_pyx_setup.py @@ -0,0 +1,20 @@ +from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module +from Cython.Build import cythonize + +from common.cython_hacks import BuildExtWithoutPlatformSuffix + +sourcefiles = ['clock.pyx'] +extra_compile_args = ["-std=c++11"] + +setup(name='Common', + cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, + ext_modules=cythonize( + Extension( + "common_pyx", + language="c++", + sources=sourcefiles, + extra_compile_args=extra_compile_args, + ) + ), + nthreads=4, +) diff --git a/common/dbc.py b/common/dbc.py deleted file mode 100755 index aa52e40671..0000000000 --- a/common/dbc.py +++ /dev/null @@ -1,276 +0,0 @@ -import re -import os -import struct -import sys -import numbers -from collections import namedtuple, defaultdict - -def int_or_float(s): - # return number, trying to maintain int format - if s.isdigit(): - return int(s, 10) - else: - return float(s) - -DBCSignal = namedtuple( - "DBCSignal", ["name", "start_bit", "size", "is_little_endian", "is_signed", - "factor", "offset", "tmin", "tmax", "units"]) - - -class dbc(): - def __init__(self, fn): - self.name, _ = os.path.splitext(os.path.basename(fn)) - with open(fn, encoding="ascii") as f: - self.txt = f.readlines() - self._warned_addresses = set() - - # regexps from https://github.com/ebroecker/canmatrix/blob/master/canmatrix/importdbc.py - bo_regexp = re.compile(r"^BO\_ (\w+) (\w+) *: (\w+) (\w+)") - sg_regexp = re.compile(r"^SG\_ (\w+) : (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)") - sgm_regexp = re.compile(r"^SG\_ (\w+) (\w+) *: (\d+)\|(\d+)@(\d+)([\+|\-]) \(([0-9.+\-eE]+),([0-9.+\-eE]+)\) \[([0-9.+\-eE]+)\|([0-9.+\-eE]+)\] \"(.*)\" (.*)") - val_regexp = re.compile(r"VAL\_ (\w+) (\w+) (\s*[-+]?[0-9]+\s+\".+?\"[^;]*)") - - # A dictionary which maps message ids to tuples ((name, size), signals). - # name is the ASCII name of the message. - # size is the size of the message in bytes. - # signals is a list signals contained in the message. - # signals is a list of DBCSignal in order of increasing start_bit. - self.msgs = {} - - # A dictionary which maps message ids to a list of tuples (signal name, definition value pairs) - self.def_vals = defaultdict(list) - - # lookup to bit reverse each byte - self.bits_index = [(i & ~0b111) + ((-i-1) & 0b111) for i in range(64)] - - for l in self.txt: - l = l.strip() - - if l.startswith("BO_ "): - # new group - dat = bo_regexp.match(l) - - if dat is None: - print("bad BO {0}".format(l)) - - name = dat.group(2) - size = int(dat.group(3)) - ids = int(dat.group(1), 0) # could be hex - if ids in self.msgs: - sys.exit("Duplicate address detected %d %s" % (ids, self.name)) - - self.msgs[ids] = ((name, size), []) - - if l.startswith("SG_ "): - # new signal - dat = sg_regexp.match(l) - go = 0 - if dat is None: - dat = sgm_regexp.match(l) - go = 1 - - if dat is None: - print("bad SG {0}".format(l)) - - sgname = dat.group(1) - start_bit = int(dat.group(go+2)) - signal_size = int(dat.group(go+3)) - is_little_endian = int(dat.group(go+4))==1 - is_signed = dat.group(go+5)=='-' - factor = int_or_float(dat.group(go+6)) - offset = int_or_float(dat.group(go+7)) - tmin = int_or_float(dat.group(go+8)) - tmax = int_or_float(dat.group(go+9)) - units = dat.group(go+10) - - self.msgs[ids][1].append( - DBCSignal(sgname, start_bit, signal_size, is_little_endian, - is_signed, factor, offset, tmin, tmax, units)) - - if l.startswith("VAL_ "): - # new signal value/definition - dat = val_regexp.match(l) - - if dat is None: - print("bad VAL {0}".format(l)) - - ids = int(dat.group(1), 0) # could be hex - sgname = dat.group(2) - defvals = dat.group(3) - - defvals = defvals.replace("?",r"\?") #escape sequence in C++ - defvals = defvals.split('"')[:-1] - - # convert strings to UPPER_CASE_WITH_UNDERSCORES - defvals[1::2] = [d.strip().upper().replace(" ","_") for d in defvals[1::2]] - defvals = '"'+"".join(str(i) for i in defvals)+'"' - - self.def_vals[ids].append((sgname, defvals)) - - for msg in self.msgs.values(): - msg[1].sort(key=lambda x: x.start_bit) - - self.msg_name_to_address = {} - for address, m in self.msgs.items(): - name = m[0][0] - self.msg_name_to_address[name] = address - - def lookup_msg_id(self, msg_id): - if not isinstance(msg_id, numbers.Number): - msg_id = self.msg_name_to_address[msg_id] - return msg_id - - def reverse_bytes(self, x): - return ((x & 0xff00000000000000) >> 56) | \ - ((x & 0x00ff000000000000) >> 40) | \ - ((x & 0x0000ff0000000000) >> 24) | \ - ((x & 0x000000ff00000000) >> 8) | \ - ((x & 0x00000000ff000000) << 8) | \ - ((x & 0x0000000000ff0000) << 24) | \ - ((x & 0x000000000000ff00) << 40) | \ - ((x & 0x00000000000000ff) << 56) - - def encode(self, msg_id, dd): - """Encode a CAN message using the dbc. - - Inputs: - msg_id: The message ID. - dd: A dictionary mapping signal name to signal data. - """ - msg_id = self.lookup_msg_id(msg_id) - - msg_def = self.msgs[msg_id] - size = msg_def[0][1] - - result = 0 - for s in msg_def[1]: - ival = dd.get(s.name) - if ival is not None: - - ival = (ival / s.factor) - s.offset - ival = int(round(ival)) - - if s.is_signed and ival < 0: - ival = (1 << s.size) + ival - - if s.is_little_endian: - shift = s.start_bit - else: - b1 = (s.start_bit // 8) * 8 + (-s.start_bit - 1) % 8 - shift = 64 - (b1 + s.size) - - mask = ((1 << s.size) - 1) << shift - dat = (ival & ((1 << s.size) - 1)) << shift - - if s.is_little_endian: - mask = self.reverse_bytes(mask) - dat = self.reverse_bytes(dat) - - result &= ~mask - result |= dat - - result = struct.pack('>Q', result) - return result[:size] - - def decode(self, x, arr=None, debug=False): - """Decode a CAN message using the dbc. - - Inputs: - x: A collection with elements (address, time, data), where address is - the CAN address, time is the bus time, and data is the CAN data as a - hex string. - arr: Optional list of signals which should be decoded and returned. - debug: True to print debugging statements. - - Returns: - A tuple (name, data), where name is the name of the CAN message and data - is the decoded result. If arr is None, data is a dict of properties. - Otherwise data is a list of the same length as arr. - - Returns (None, None) if the message could not be decoded. - """ - - if arr is None: - out = {} - else: - out = [None]*len(arr) - - msg = self.msgs.get(x[0]) - if msg is None: - if x[0] not in self._warned_addresses: - #print("WARNING: Unknown message address {}".format(x[0])) - self._warned_addresses.add(x[0]) - return None, None - - name = msg[0][0] - if debug: - print(name) - - st = x[2].ljust(8, b'\x00') - le, be = None, None - - for s in msg[1]: - if arr is not None and s[0] not in arr: - continue - - start_bit = s[1] - signal_size = s[2] - little_endian = s[3] - signed = s[4] - factor = s[5] - offset = s[6] - - if little_endian: - if le is None: - le = struct.unpack("Q", st)[0] - tmp = be - b1 = (start_bit // 8) * 8 + (-start_bit - 1) % 8 - shift_amount = 64 - (b1 + signal_size) - - if shift_amount < 0: - continue - - tmp = (tmp >> shift_amount) & ((1 << signal_size) - 1) - if signed and (tmp >> (signal_size - 1)): - tmp -= (1 << signal_size) - - tmp = tmp * factor + offset - - # if debug: - # print("%40s %2d %2d %7.2f %s" % (s[0], s[1], s[2], tmp, s[-1])) - - if arr is None: - out[s[0]] = tmp - else: - out[arr.index(s[0])] = tmp - return name, out - - def get_signals(self, msg): - msg = self.lookup_msg_id(msg) - return [sgs.name for sgs in self.msgs[msg][1]] - - -if __name__ == "__main__": - from opendbc import DBC_PATH - import numpy as np - - dbc_test = dbc(os.path.join(DBC_PATH, 'toyota_prius_2017_pt_generated.dbc')) - msg = ('STEER_ANGLE_SENSOR', {'STEER_ANGLE': -6.0, 'STEER_RATE': 4, 'STEER_FRACTION': -0.2}) - encoded = dbc_test.encode(*msg) - decoded = dbc_test.decode((0x25, 0, encoded)) - assert decoded == msg - - dbc_test = dbc(os.path.join(DBC_PATH, 'hyundai_santa_fe_2019_ccan.dbc')) - decoded = dbc_test.decode((0x2b0, 0, "\xfa\xfe\x00\x07\x12")) - assert np.isclose(decoded[1]['SAS_Angle'], -26.2) - - msg = ('SAS11', {'SAS_Stat': 7.0, 'MsgCount': 0.0, 'SAS_Angle': -26.200000000000003, 'SAS_Speed': 0.0, 'CheckSum': 0.0}) - encoded = dbc_test.encode(*msg) - decoded = dbc_test.decode((0x2b0, 0, encoded)) - - assert decoded == msg diff --git a/common/kalman/Makefile b/common/kalman/Makefile deleted file mode 100644 index df748197f2..0000000000 --- a/common/kalman/Makefile +++ /dev/null @@ -1,10 +0,0 @@ -all: simple_kalman_impl.so - -simple_kalman_impl.so: simple_kalman_impl.pyx simple_kalman_impl.pxd simple_kalman_setup.py - python3 simple_kalman_setup.py build_ext --inplace - rm -rf build - rm simple_kalman_impl.c - -.PHONY: clean -clean: - rm -f simple_kalman_impl.so diff --git a/common/kalman/SConscript b/common/kalman/SConscript new file mode 100644 index 0000000000..abd7e04375 --- /dev/null +++ b/common/kalman/SConscript @@ -0,0 +1,6 @@ +Import('env') + +env.Command(['simple_kalman_impl.so'], + ['simple_kalman_impl.pyx', 'simple_kalman_impl.pxd', 'simple_kalman_setup.py'], + "cd common/kalman && python3 simple_kalman_setup.py build_ext --inplace") + diff --git a/common/kalman/simple_kalman.py b/common/kalman/simple_kalman.py index b6fdb7da2c..33289e4f50 100644 --- a/common/kalman/simple_kalman.py +++ b/common/kalman/simple_kalman.py @@ -1,9 +1,3 @@ # pylint: skip-file -import os -import subprocess - -kalman_dir = os.path.dirname(os.path.abspath(__file__)) -subprocess.check_call(["make", "simple_kalman_impl.so"], cwd=kalman_dir) - -from .simple_kalman_impl import KF1D as KF1D +from common.kalman.simple_kalman_impl import KF1D as KF1D assert KF1D diff --git a/common/kalman/simple_kalman_setup.py b/common/kalman/simple_kalman_setup.py index b7cad0ee2a..fb4886a0f2 100644 --- a/common/kalman/simple_kalman_setup.py +++ b/common/kalman/simple_kalman_setup.py @@ -1,4 +1,4 @@ -from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module +from distutils.core import Extension, setup from Cython.Build import cythonize diff --git a/common/params.py b/common/params.py index bbc66eaf97..5067aa8d42 100755 --- a/common/params.py +++ b/common/params.py @@ -55,6 +55,7 @@ keys = { "CalibrationParams": [TxType.PERSISTENT], "CarParams": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "CarVin": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], + "CommunityFeaturesToggle": [TxType.PERSISTENT], "CompletedTrainingVersion": [TxType.PERSISTENT], "ControlsParams": [TxType.PERSISTENT], "DoUninstall": [TxType.CLEAR_ON_MANAGER_START], @@ -65,12 +66,13 @@ keys = { "GithubSshKeys": [TxType.PERSISTENT], "HasAcceptedTerms": [TxType.PERSISTENT], "HasCompletedSetup": [TxType.PERSISTENT], + "IsLdwEnabled": [TxType.PERSISTENT], "IsGeofenceEnabled": [TxType.PERSISTENT], "IsMetric": [TxType.PERSISTENT], "IsRHD": [TxType.PERSISTENT], + "IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START], "IsUpdateAvailable": [TxType.PERSISTENT], "IsUploadRawEnabled": [TxType.PERSISTENT], - "IsUploadVideoOverCellularEnabled": [TxType.PERSISTENT], "LastUpdateTime": [TxType.PERSISTENT], "LimitSetSpeed": [TxType.PERSISTENT], "LimitSetSpeedNeural": [TxType.PERSISTENT], @@ -95,6 +97,7 @@ keys = { "Offroad_TemperatureTooHigh": [TxType.CLEAR_ON_MANAGER_START], "Offroad_PandaFirmwareMismatch": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT], "Offroad_InvalidTime": [TxType.CLEAR_ON_MANAGER_START], + "Offroad_IsTakingSnapshot": [TxType.CLEAR_ON_MANAGER_START], } diff --git a/common/realtime.py b/common/realtime.py index f8e489612a..c21222e88c 100644 --- a/common/realtime.py +++ b/common/realtime.py @@ -6,20 +6,12 @@ import subprocess import multiprocessing from cffi import FFI -# Build and load cython module -import pyximport -installer = pyximport.install(inplace=True, build_dir='/tmp') -from common.clock import monotonic_time, sec_since_boot # pylint: disable=no-name-in-module, import-error -pyximport.uninstall(*installer) -assert monotonic_time -assert sec_since_boot +from common.common_pyx import sec_since_boot # pylint: disable=no-name-in-module, import-error # time step for each process DT_CTRL = 0.01 # controlsd -DT_PLAN = 0.05 # mpc DT_MDL = 0.05 # model -DT_RDR = 0.05 # radar DT_DMON = 0.1 # driver monitoring DT_TRML = 0.5 # thermald and manager diff --git a/common/spinner.py b/common/spinner.py index 7da31a9dda..734015bcf0 100644 --- a/common/spinner.py +++ b/common/spinner.py @@ -2,22 +2,46 @@ import os import subprocess from common.basedir import BASEDIR + class Spinner(): - def __enter__(self): + def __init__(self): self.spinner_proc = subprocess.Popen(["./spinner"], stdin=subprocess.PIPE, cwd=os.path.join(BASEDIR, "selfdrive", "ui", "spinner"), close_fds=True) + + def __enter__(self): return self def update(self, spinner_text): self.spinner_proc.stdin.write(spinner_text.encode('utf8') + b"\n") self.spinner_proc.stdin.flush() + def close(self): + if self.spinner_proc is not None: + self.spinner_proc.stdin.close() + self.spinner_proc.terminate() + self.spinner_proc = None + + def __del__(self): + self.close() + def __exit__(self, type, value, traceback): - self.spinner_proc.stdin.close() - self.spinner_proc.terminate() + self.close() + +class FakeSpinner(): + def __init__(self): + pass + + def __enter__(self): + return self + + def update(self, _): + pass + + def __exit__(self, type, value, traceback): + pass if __name__ == "__main__": @@ -25,4 +49,5 @@ if __name__ == "__main__": with Spinner() as s: s.update("Spinner text") time.sleep(5.0) - + print("gone") + time.sleep(5.0) diff --git a/common/transformations/camera.py b/common/transformations/camera.py index 97c7c37d1b..960c063f94 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -125,7 +125,7 @@ def img_from_device(pt_device): #TODO please use generic img transform below def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics): - import cv2 # pylint: disable=no-name-in-module, import-error + import cv2 # pylint: disable=import-error size = img.shape[:2] rot = orient.rot_from_euler(eulers) @@ -183,7 +183,7 @@ def transform_img(base_img, alpha=1.0, beta=0, blur=0): - import cv2 # pylint: disable=no-name-in-module, import-error + import cv2 # pylint: disable=import-error cv2.setNumThreads(1) if yuv: @@ -241,7 +241,7 @@ def transform_img(base_img, def yuv_crop(frame, output_size, center=None): # output_size in camera coordinates so u,v # center in array coordinates so row, column - import cv2 # pylint: disable=no-name-in-module, import-error + import cv2 # pylint: disable=import-error rgb = cv2.cvtColor(frame, cv2.COLOR_YUV2RGB_I420) if not center: center = (rgb.shape[0]/2, rgb.shape[1]/2) diff --git a/installer/updater/update.json b/installer/updater/update.json index 2ff5690fd5..fb009b4c1c 100644 --- a/installer/updater/update.json +++ b/installer/updater/update.json @@ -1,7 +1,7 @@ { - "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-7a0117425bc4a6673958d265312994e124654a566228f3cec2f0f9bc8120a9ab.zip", - "ota_hash": "7a0117425bc4a6673958d265312994e124654a566228f3cec2f0f9bc8120a9ab", - "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-3dc234d868c29a3739f6ca3bd47b1c2d3c570d9f478b6849a4fada129ee4af76.img", + "ota_url": "https://commadist.azureedge.net/neosupdate/ota-signed-07df505453684371b6c22583ffbb74ee414fcd389a46ff369ffd1b6bac75414e.zip", + "ota_hash": "07df505453684371b6c22583ffbb74ee414fcd389a46ff369ffd1b6bac75414e", + "recovery_url": "https://commadist.azureedge.net/neosupdate/recovery-3a6f973295ded6e4ff5cfff3b12e19c80d3bf45e2e8dd8699da3fc25b23ed7c6.img", "recovery_len": 15848748, - "recovery_hash": "3dc234d868c29a3739f6ca3bd47b1c2d3c570d9f478b6849a4fada129ee4af76" + "recovery_hash": "3a6f973295ded6e4ff5cfff3b12e19c80d3bf45e2e8dd8699da3fc25b23ed7c6" } diff --git a/launch_chffrplus.sh b/launch_chffrplus.sh index 56c25e6ffe..5e2389fe37 100755 --- a/launch_chffrplus.sh +++ b/launch_chffrplus.sh @@ -11,17 +11,14 @@ if [ -z "$PASSIVE" ]; then fi function launch { + # Wifi scan + wpa_cli IFNAME=wlan0 SCAN + # apply update if [ "$(git rev-parse HEAD)" != "$(git rev-parse @{u})" ]; then git reset --hard @{u} && git clean -xdf && - # Touch all files on release2 after checkout to prevent rebuild - BRANCH=$(git rev-parse --abbrev-ref HEAD) - if [[ "$BRANCH" == "release2" ]]; then - touch ** - fi - exec "${BASH_SOURCE[0]}" fi @@ -40,7 +37,7 @@ function launch { fi # Check for NEOS update - if [ $(< /VERSION) != "12" ]; then + if [ $(< /VERSION) != "13" ]; then if [ -f "$DIR/scripts/continue.sh" ]; then cp "$DIR/scripts/continue.sh" "/data/data/com.termux/files/continue.sh" fi @@ -51,7 +48,7 @@ function launch { # handle pythonpath - ln -s /data/openpilot /data/pythonpath + ln -sfn $(pwd) /data/pythonpath export PYTHONPATH="$PWD" # start manager diff --git a/models/driving_model.dlc b/models/driving_model.dlc index 671483b028..869f41a90d 100644 --- a/models/driving_model.dlc +++ b/models/driving_model.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:28d88850097f33d58fac5ed95d41c8336fdc054832f97ce3937e56e29e4796bb -size 16950543 +oid sha256:53948734c96a5aee45fcaed9fd6191056328ed3467dcd3d40d25f310d38c2297 +size 15207664 diff --git a/models/monitoring_model.dlc b/models/monitoring_model.dlc deleted file mode 100644 index aae5c0be63..0000000000 --- a/models/monitoring_model.dlc +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:e6ea68609a1477fb55e27b066af5c65f343b930eaf8ade38d91df68ec594c5a7 -size 158713 diff --git a/models/monitoring_model_q.dlc b/models/monitoring_model_q.dlc new file mode 100644 index 0000000000..43041b029f --- /dev/null +++ b/models/monitoring_model_q.dlc @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:7f6305c44138a387e1757ed30a9bb6487d35c8919d0ae258998acb0e74b584e1 +size 186615 diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 7fa3f83e559081786a2442ed3cd00d34db61a51c..d7e011d8fb7ecc8846c74abd0543f3f2ba9814c4 100644 GIT binary patch literal 31596 zcmbrn34D`P)<1rqCu{d~0lEQM3X~QwrJ!X|(iB446k8UR%`v5*DX0W=W-2pd14XT% zqX;@sbVPAxQ1O)(s#I-*6davl-Zug207VVT3=EaNDQ(HqCTafP^CSgyoZo+IKX-lZ zdhWUBo_p@OH{Wky?EXJ8<=a1D*pL2((opn}e@PWnE`*r}GZ&^1W(LeunA>2+!{}f} z!i<1Pgo%NX!Gvr~*$>kLa~0+i%mtVZn0A=|f%yWa4W|F!Nz7FmqsL!Ayh6hnWa74n_l$0W%CH0VWDY0uy|Q zDSa^AFmJ=`h3SO(2h2AxXJJmk{0-*MFrUJF4ATVjKFn({zk}HZvlV6tv`@ib1M>vT zV=#}vJOo4EE8s4JxhE2)xUyUP#qbxw+zE3BOdd=w%ov!`55+RnJ0(ms_WhjN*;Dp{ ztVC{c+qhm&MNH8tE5$Hp@Dg*ftIQ*112Uy0qbj3WVPV?z3j189lbO>@>8UCC2m3;6 zw^{q$-IP|v4E8jpjTN_p-ju0F{tO-MIMTrx{q9haBpkm?pz@CQ(alg;D| z)DHVx`j?`HWG|nP;$&(Larn-71AQ4vG34XXihBDwJEfug^qowZH>9`El~_1@W!jtH zsWdW@NWS!SOVZTh-R>E*`q!o@efX)GPp$qnIBS$2Vo&lcPTli zl+3d%?qY6?>tNqsYLqU+wL2#t?Brw5cQbxLXDrV=oc3T}QH=~=Ue8;cUT7>VM4V<* zb7DhmNmBKS=7W8~8qUSkN)Mo0&2swzquo!i+&(v_X4yyG_PIRuEZj%N)I;s(EsL$U z#;2g2oz1QWK0E#2+IO&TT1^64kYwC$jDfo?X}dAnEf;IdXeOvp9pOG2t`$w*ZtQh? zFWs#3$NhDo)v_8jO5#w`X!llIQLm$DtF_Ly)wadf(fCs17Av2>#U|-g=_KBegzuE# zYmA4_W-gg0oQwKAR43JL`1r@Q9PE3$MhdvT1NiW2spl`Ylk$Pn)Jns(`KVXs;?Ytb zDdyC~yQ6JdZ;g@5*Y;X@ z%`STups*^r-k5+?$zC-ZW|6se*`gZKeihZg@O`H>*`urjhCOD&}_K&m%J@T&dmYuy)EJK<83teR80~S4}1;x zeVCc6%4}IP+*GyH=E&T;Z5i>&UVTD1CR|^PEgEnhg0k|K*`o(;_PZPV%4@E>Qcbtw^^ZqB zs^nyKQe@7Ri@6NfUeate_PJ8>wi>UwRFT{??-PH3_e$F`)O`daY=}G1x2;CyI?&fx z69-Jy=yn>T;A-=C8e?x@#ZA1YvSM#(?T_13fOd4dF*~%|jaNn7y*V#`oHiQm=NsZZ zD&&cAa~?{0uy4Ca1%8JfaAFU_^Ns42i1Y+wbc3Q8Y|NX$4sEEIW1>e{gSUWTZ-T57<-RxYdY`Q#0ddAhwu>y=2^DuQxwZtp% zM{;7_$zFv-;j&cc=gQq%tGmS!wa=w75orNvXZkNzd|;nTZ6i9H{)_bpg~xZ(0jA?T z+-@$P+s^H-HTHLibR2P?F5>dH8~vbIswSr6JtvkQ6JrM4sun$Bq@Lr;Pb~j~QR=32 zf}5%JEd&F7i>fKASZY#hL%wfAYLlXveE$sTn`Omn#I{^k8A85xr>e#CL}+x~`%L-y zKBnX?=c=?<7(0WNh}v0{IYPUzsP>A=;E*}u>~kmnODL0?Mwr%rFxv8s@asL>1+{g3 zX04zGwAWryL@BOt%BU+6NzoOhL}Cs3z6lwwg{GEyV`C(qknc>0yYs!q7(eS#l`It` zdR@a_P#QL|+a2dw)ex&&T$G%eQohs{b%h7DQSwWz^|ld3uUn-S)L z-^6;V!W^c&nU;~o_NH1v!!pfGH?-oWmCm;cZj~n09P+tBx9c_w1yhDwhMSKGGpBH4 z96C1km|&iAOfalGCd^)BxYnUjm6lx(tq=Lug~prW&2z3l0RI|5e7tEEApT%zY(=!C ztk;3w*juRG#Sbzit>uOS-jHZy%51Fdw7$oZdx3c-{>xif2N-YEj4Y9wMxlqZLzT8O zia`6^Yx?;OenF6SXUde_p}cHUVecMIN=e9<9~#$?Z7TB`3PZl1hbA>31uOG1gdPpW zl&DQdG$EfR#B|XmphNi;wl5UY_U*=gU`xwo??#z9H9ghIiGJn`IXD)Q)U+9l39Qzp zYAW(NBvMnYRjQHdL%wpfrP8WuDQH^%K|<*cL9OTVH^Y|MEKH)|1|;2Mu5cVbAn8df z0v=8d`Tj8|rkc^wNa>QBb>b)%h_EPVl9^(GO)3j8NyX)1oK>8Lb*C_HK-!(u5c0)` zIAG4O5U0&HX#k71kc`r*x#=ex3Gb33SP?FdH}PgE`g+O<9tXIiTabgxK|DUD()BlUK+ahUIfRxxj12=`sL7NS%Tgoy` zQa3-qb%%Vf4N5(nP9pl>_yMdknt4kY=Lu&CcZ4RnDGu-&CPMymC6}+%6I40So_htg zeK$)G$6(xzF_3pUfC)4Xnn68qk_S#|LO(SkLZpuH$d(JS58NEx|1`F~;7^ql9t%## zs;E~=!+}nYcsEww8O!W*l}*u@uLiEHw+PVxGRSEZW(9EA9aJFx*A;8M)y9^~N>HcI zTckS9pj2}PNkUC17Ot}CQ%O`wH0I_=cjap83P zhhjma{s{jB(1O@XRnyX3Yt<&Q|G0Du2oj>nl%*??RVDP6>C<6 ze7ZrkDMk|wEI12atmr#ESmw1qw%4jQMQIfHaspqXMc10U z`M5Q_hoCt|b7ztzeT@;bDZP?hKH_3*#IQ2#VvG=e*x1V^r>`W}p^dmnqU&Yr!>(gR zBs}CU2!~NV<)iXN7v+ntD&jhjZYjR2_@yhCq8*^Ql?eAb)+0>rf{1I3xRaNz#M}oR zeymA~c{SX`(EFEyM7t-W&l?7BU6rG9`@b4nMKe{_Gs@7Nez)=NLP|+_R$*@ax`k%B zR?p?~E@Zwv6AXsyHoC$yCD;k@kkU1e)X7 z76vRm>kRq+?ks4Eo0p3msZWM{8wbfJH7$n!SA&M@ZON}1rDt~48jQsaN;zLm6RFyOjQ%8yjE)BG!o39Li96?L0826pv011m!K&rs^1LW#1UTq zl})tvDYdo29`&%2HsLKb)3yow)MdT5t#1?dtJMe{P*W}U50>@XSH#iGUKjFp`lXr} z)Nxxts-fAM8;BO^^0+`0t%d8;t;!Z!1F2niVhvOlV4h;ns)UJ!p>;4?i`m)3`;(dB zibQp!5xi{__%}(kn)!L`Mu`{60>sDaWw}z*M7Os1FE)+kMY+Fyl6^q(Qt zH((t-;P4;skpik;haMPk49L8*7;z);_kY$SL%NgX7e4EeBh87>pBv@g8l?Dh=y;ED z>JjW%m|9_Sm}I7nrcI_=lV{yq0z0r-;P-46BrlFxw^@)m{k`YBSR`6b zjX1RRADHh{-_Tg!M`fXGRnr)AyoLj2-W|}IWumV*purdr{jVF$ZWs-Iv{o3a9#t~h zlYtZz|LZ}Pea$@rDsN1*;{69!2P)TV+lxjT6HUq6}V_P_h~DPp~9-63ke6 zkZ$QBIA^kU=U!mxBB4CDWlM~%NU zimoK-lD(mXdo0*jqt03(N1ft1)OV^zf>K}6y=sheZ?es^9PKeC@i`KgyhEBJJP4>iY%~gyG273OC8K$#%4dCGno4a19IYvP^H-F63H=!_KkBH?Xn4 zx7P}}YRD&0{unoh87$A$xTjb)+Mcmd8z!hX*`Br~MMug1+@l!B_tqE{or#5f=kOI= z=K_9#va#>28s%4MYZ~B6&yOpPst)hw(<^m=`~>xeW@kEIU@Du3S#s*^3i+*CCQexig+fZg>hiF6J2q4vpjjL%b7X*mq{j&R~v$?{Wz# zT$rxY-bxQXFbyqMtLM{>NyXI_)jiv|yK$Fsl&iKX%@!q(Z=YRPXnU(M>i#9x*>xM? zRJb-j%T>MA78|6?zSsD!QF4;p=Z#TkcNya&e#zPJH~C3R z=0SgNpVU7re|KZs#;uLD8>N=BmA7*AL;X;qp5bCW++_NmVp(e2CX=0BYC9?8VVBDl z?Pa~?>(5yez!8dEh&9#{XI^3p`Q8iBdSh5|&MFU=@S|M=@rg9=*-G9}2LmCn`VQ~{ z>p#f6rN-K-k+yf_TzklOPbgO*ckQiO26^1`f?WR^j2d$}QRJv=8!lrEhrTNCRiTfu zV3l@9Thu0M&#KT*x8trzJE(qY2ZuHDW6<^q2$R1F{_=?befS@b_}_+~^2A$q8q2P1 z0~m-tjB@u%E!NS*C~k6Iga6t!-kX5^wIsRLO7x1Uu-;~T$k=mF z&viV%j4@N24m*rn`oHPfHekr^#(HC-OVe=lSxFT_%VX?EpDluq@TjL-;!o7kNOGc7 z?qC?my;)C`W>Uyj3O{3iP)U(p6_Zlni*%T@D)_q_4jlIzj zp3uG}Y^9%bkr-xLjG?Y#Wu!JGd;!aI$__t|YSO3i-x@4-V5-=FvNO%-1j! z1W_6zHSr!A`{Y4wU1j9Ubx0R#tIpZniooe}A}*c-jMsl47iS>TNgPFv+Kp`+6PuI8 zUGUI)5d~;x>s7$K{04=Y_PIMlEW6*OYLOxo=F%!L)`Y7x_LNfTiO%KSw6pPqQUoqG zKht+)d_>-5s`o@^7aP!;moLxhdhxtx2@cgSA^8C5dysw1Sc@#@`GJLl5Z zO2j50)`eK^M(k|F5)XGAu@W&h+Z5NW;6lFd!4v$qn{b!b@V6mDItKGCjH-n1R!VA( zY3>BbQ8ivEx1=Jjc$als)mf{%YKb+bIL(^8dTEu>Yv7kw5TsQ6Ijh2=($)iBa@6)) z@TB3nAO(JomSI-|o-l~A3z`auhg{Vr^ez}XrdALZOr6&z1QtLBggE8Gabu*WG1@$H ztq{9VKd)A}sMUjW_3~|kRMIA>7PbkgaP?A@aS!s8w+WZQ#bz#K+NopOgelV5>Neqf z#4uf(kfN&+$iSS9>s69z#S!$&{w61~pN7`FeI15DH_|C;l z=`H429^7J>y!&v@1XrA|vzdXJVaj0u7cQGHB0-dNjB}@ZAdA*8?VztLlmgz4d?g{K zjs};eF=mXxBvoPj`tT!86KztM2YpjQ@<^`RLY(@4FG%?9sJK8PrF!i9%5Q_G2aA$@>zWoCvX$j}s*UCeJXE3N*<{}^Aa5%3*Y%g%c z6Ax~c0ameasZ~9L(PH~3|Df;tR=LQ{4*D()O2w94yCjK}y`lBr+arf`7&IMif$Z#- zcC7rjb`aD^o;(b@wV}G3B6TY~%1EvI2W4Wd;nw`2b*S!{H|pj^_{X{=*~u<0JJX$n z-8e!0g~3E~9D1Ze4>cwZF;5T1B8Jl{JW-J}zZp~_hEi8U5+slGE54Pao&+-y=8QKy z5P{ueP5ir(e@WPFpc%CHS?r!Ut0^#vzk_vKhO=zBc)7hUCwg)lj0hi1_Ca8PSH zWgAm^KlVni2{IP)&F`0iF3904?MKm9WOFe85t`GVU8J_Q2#oE)zRA%T^3Cj*BPEY- zcZhi;@J;T2O_1tSN(@(u^oA>Plj(|VOp;<=!1p4q~Fp+wjH|PrnRFQrQf!G^y8dH=Rxh4<#{t!@#-@~Kl{R9vVuld;bKa2Y9 zaH;-+rUcZfRZC4HO^pr8hFCKLPK12L!I#BdeY7P5`)`HwjBVF%QmfAXW;6D4G-C&S zueQ*hoA7!!%KHq4=%K2KgG7n-G5etJ-9RNyBRGlQ4axCjNRD9+z#N)B@!Mj?e z+89rqm_w91uc${37Pc4EV;81~VAxmKD@qv0*vc>sVK%rAGea-hKN_<0=Y@EEv?fX? zedl!Q0e|;3&Y#5edTxePh>h)NRGC;#w_Ey-D>bGd<85)H-^k^^% z`_}|dn7Zu`Bp^f-G$|efI($A>VxgTFHMOl0{%s2H1#vA?V$B zwCQ9c&izNJnxaiooM;Y%R8HdM^mNkP4Kx3aF+99m@FA( zFH9|35H^{x_Js zCYFD3a0n(ZqkN*}KLrzUAC`41WR8ZU6MSb2TEZ*IXmKTZ#68C|6QfMD^nL%d2yVRN zzXNhGiu;3q3M3%p+vA@c!Iygjw*f=aJ(ENVKj_=;&qbKh{MJ7{lJ8mn*hs$3{;UWV zO@}mvAU5cm8c>^+ni%->fsv4c#6&P_{3q#%r;@7tBM?u%mHw1S`yTMq%0O{T{Ryat z@~bc&kek65gYgLCvpk?cd!n?bFVUDtM3|LeJqqKI|D&+HhhR2@r?eIkp31Qr#k#oc zICm_qJsuk2n>cFxSRvvlm3n4?Mtby(Qe7Vnjl->Y`R`-!mykpp17*{o@3Vje+{2yV z9zhIEtR+u@fZ?l$;d@GIb(2;UZe zELO=}_&)TLBx^i;mj_73G#0+^2FB1S4j}dQKrE!vqY<|bwJEh~_#TFj(~g91mzXyV zzP}8RM0*%~_xoeuONMVTe4JJV-&20#6`sQ>Q(R2@xO!kYx zLEl&Y1APMzaAzTN_0WkKol~$+hTtWx)v-LzDImoQVJ87Tx&z!b?K5djpm>`Y&wdbJ z&=ghrU6srvDN>rhs|fiL2C^|C_XcGcou~m0Qm#V%&>Hir2t86ZN%3`{pO3WSE7~96 zo6wXpFQs%?*(B2u3OeaM7Pr;^`blW09I&-N(XG=FqTv$!eGiT@(N`o`H zr{N|y=Zd?N*eRbF&%1|Y8&hy*nq33=a#OM;B}?jOQ)H&SHZFg!wbHh8F0Gv!{l^V& z+P<^Z81p7D6Ij}c4XT$!bvk3O%P9yAf&O+FaIt{SbIA_7OIcLIN<(vU~g>yRW2b|Mj zFLxHeUgEq1_Px&AVJ~z}gsXGQ=Dqp ziO$im|9f!ft7m!2YO}hy7tI&D}$-40c1Sy(#hk$c4z?(bmCmCBK9V2DfYseWmUX-=UlZp=*z(_m{%{WVw#q<9g+#T{w5eVN~LutVANrKMyw4g0tW@FmLwzS_2`W{3Kg?8GOAoPInipW(` zoWegOoShpk2BGS`$h2D-6gX)s*#aULonz zSEN^@SG_0?oK`_6Grj60e5V9yHk~9QRW@&lset4Lb*3_t8@}}_% zqzhsRYq~p_&XC&H9WT`mNPDH=*-2|Df%GrC6L4;$Z!)B^EZ#Hhe%U?Tlvj-d9V~ah z^Yc_?re6HEe3aaSoP9DcCu0J)jGM_OTt9a+4e7|_TRu3TfcD60lcZZI?l*>+B*$sL zk%Rq4sx9M*Y-{eiN*e>0fgOiZx{uQlj)`X~GMuTH%C0O}Al+Byjgcg~NV5dzY9?MM z-3R)DRGjH(kDHh7OG2)u5Y9_X(tT1-$oGCI$3%V!;@=KQ|BEvjO?s0Qp*KTp3};e! zD%13)^bJbH>cV#Hg#G4A>n-yA(GU9aPe2%N_z;4=* z4Ko3T#yJ*lL6dTx(sFqvm!&kf39lMR9?>T3F$8@h2SHmy6IMlJoWK#{Ur0BNQ`g&} zHzw|ZI7{lPAvvYYV!oCFoPJBq!QKN&*{f<%hK#(*hM;eHOIQ~x=$qL>x>yBGm8GYV zhUHk@70^OHt1xueJ5CGxR3(|r`?BMHOM2D)mc@@mW#bHKvg~|x^U_s09!|e_)i}>- zVZWZ`rhL`dd~eCg)FgqMz~vacFF9hSZ8b9QZyobN(|#31Di-p6DwLV!9+tzro5b%= zx=Zo(;o!p_3tU_9DdB*@fO)rHeM;D;uLs{_u(Pr2#U1UZg+1V@@?EEexAa?^Clr72@VbVa zNGWtG2VIW!$B+hleDyKmRsATBx|Ex745?2GhxC?ae1B{dw8{?YNg5RNZ3xDBGAwE4 z^hdBOm`vyNL7yGHI|QzbQYDebAVxS1RGwrGgP#O_wSjm`%)I(XxXCOlI%oN+&1u%CYusO2y~-eqd)~0YvVxq zte|?!wZ+#Ba93&#z#ALRiMI@L;1~}u?E29rY6rE4+C_Bh>Z2qF;HF7MJr~xU&4Rv9 z{nGAOHiYpXVJ8%x8BKn|eHHe|siO2J=R#*ai-JKsEXu^!+_#Zb%hn*PtT);n?kKLA_r@ zeHZa1342uv*ZEbEG`0TO%`xH9)=+6`(=xc6<*QXSBFwg7{`F%$0D#ngf?xj5O+5*M z*`Y2^ba$6(Kym(1*QcUyQACe{13g~byYJW5+3G7Ed!2EgQwa@ZWfM_?!}?{wDB6oM zqSukQ-9Hi6@e^^>@1eZ*CYBZDSs+X)|0mmrlZ={}Y;H zIadwoX$~~7vzD$|(VV_U)B`vV-2!!VGwD4;zX0n)zl+mNFL?=??T`$jh8ImA;G|y( z{|Gm`ZKnHO+dDX`r89oMvsQ9j$@a>_sFio!MtTIz84WBel>0gA*#3mZb8(W%^#M-! zW8Fp%V0&{lb6ViDpBY*T%wfc+=nlTx<$2N z`CAHbfyQqF+}SX^DbZX2d2LaJs#Mx{?&oLNR3X3oWz5m6)L8Rr;c(s&jJ(pEVcR2E z)%nc|50@XCd)bXu3Ni&H@Rzj5!ex`50+nq@hqA{gPOH1}-D9Msw$mu>&|JB&4jQZ1 z9QY=w+E`Ik(PsO!LQ+v{YqJ*FYONBR<8m}pHSs3s?@AX4(pej;wi-XJI_#{i*lPUT z*;etUu?JMNplObcG*y-;NHb+fTjlxY*C!;nQKAUNPkW?=MOQdYn=RFx&YPXZMv{e& zgwDyfnrQb?TQYQ1`1O+Md=B(?pvh8iPr%&|4K(6npa&9*uZqbz4ND$PYmOE*L3Y`f zJ}8Gi2bUu|I|lk9+iSx2IjG;C-yYsOP`~X>Cb4ZY(_+PEJ&NH&tutIpL)p_^lD^@O zy=qjs3p^?v^NiHJZ5wUYKdu{AO+FP@k$M8~P$|)+D&cd7_Xy zQFq0r9gtijPCv<#I3FBP=x zi@+}oJDF{%0vd+xS1}hKi8EZ=nEx8iC*H6nn?LROD{3a_cm{FmiLp?gB^Xa?Zo=&q zKGI)|V$o~HSofPY1=>`IQA}GKhEw`O1PwoW&76h^zhJ~@(XBDbjXNF(0lNhEMq4c~ zXQkX8JzT1XTI^*PqQ-VX4)|?!fCvAd5t?;x=kfA-hp>Rm6j{YuWe{uFruB$?I+vE-&`3Mfms*pFUL3W} zoJVy_dTWZ3i>8+x>|~=c@;fU^fUWXm`cj3K=;nW$H$is+T$ z?nWYXp1w+5Q)iT(PrIR4Ho}r#nS#eQqs zM*3lga33(*O&EPwGr`3UNyTOr8~>i`zeJq9JRSTkXbkbWL-_JT8b~xl;XaN#WJ%^P z8f&J7e3oD$daVGiKGIBg+LT-z?zEi}o>Qlz4&24`yoTOCr&5S*P@Lr_%5a-%7+QXl zzS%V4I;od08u#DACwQA=ugYKhvg)O(gfy0O+IhT_=T{p)0?#nr#Kxa?wsmrf2M|7t z@QcQz4mN3@^S!j#j6Wd71f87X2d$2&RHx&yVOnD!Wlcbq_Y?rgbVk*~e- z?fQDerz72dXI1L>%q_?jjkbjAqFpn|;Jq8EwMRQS$wK2!qq2ii&reOv9BNZoi*w?? zLb$Iejc_Aj9my)&T_sYT+JqZ`i6NXr%&>f2%_hY<7I~9yUElp>`)ySUNlfleW+d7smj3FdNg|?-EbvJ zvd!4hIP&c0!Vbgd!cKdtC#i&Kj-r&K$aNGkTcHaYZ%P8j<|ggH%-&sH37#Y)HrX3R zcL>MIJi3O9mvoufQ|dh6Eus_;+ z;#7diTk7OuK!be*m?$M%0F#un$05ZyF1&@cKU##ztANKIn71M@LC?ir|L-N1d)2Ks z$*AYG)MuI#;XeNqPRzIkC$@+!{Hpk;lz)yV={1KCU7Gr-TT2YOT7vytv-CGh!4iM`W!3o zTU}p05}f>8K%%xr0Z7!={1_4vUAdNR=U!?T4yn{7xLN3+zP}%|i?ib}`hQ5A9Wm(n zC#tj`^?Ve1ei%Lf#Gd9+i9J7rULS_}B+_%@4rf6BI0o~%qmd(io92ax9sdA!d5I6?&xd??gkw$bIUy0BNl58j>Qi8>pAy|A1 zSa{soH)zC^2t0`2iqMF%Fb#c%G(94k=0BvO-2(QXPf9!EC6Zi+Y0VkJi#zTntdM$V zNsXWs*j4rt8fC4l5G$9t_5VfzZ5*cnbe{u(%Txh z8H=t&OVZAMA)GgSA$aU59xh7-ZSALIOnY270g2{$7{ueIG-P2O(#)*JKGWxxdtkdR zU1C!}NwY3qGME%ZPhOZEff4b75g3WP-uppEZ^fGBC9yZK<^f?Tum+r$dI)o-H=kRJ zeJykY!LvbLihZqicXdnyPV6OE(I>xRl)yIveDNDL(%+>!>s%+VU0_U-CB-E-%*9?5 zuMR*0y3xVLH1@q;gI(>I4cM<9saXL2`%N3T{~ydvP^q=|trqFjN|sxJ+4)Kh(d$=g zeoU_w>cksxpK$j?;O9kES??<%eToh^v>O5Q%fQZyB6dy-d?|dd9`hq=ecRd3gv*A{ zgzxP+o(yr0UIb=dhWS2%opgKuui96PYO#+;d*sFi9N+0)k=&p{-xfCS6*Z`czxnn2 zEyg$(Xo`n=YFJA>rB&`^WR?5xOW6@xzcMm5#CH*I8m4c{{*At^wcT4n5{YEYxM|Q9 zdLgv%7OKx~GkAA7fTQ5%cUS8g=811g7`!hy!h3%Mq|V{pzXp9M0ynP#oq5NWAt$Ui zUP&&R1qlUlxnUkRsbMBwsfcc#f!h&=Yv&hi2WK{obVG4!0EtP)@td^V(HO^jHt4r5k#lmm25|huZz`7p+jx&8o%Y%6c2`8zB;R3-!(p3+O3x~bsecEh*6HFE<^ok z^vCdyx6Aep#ePnqE$S&l`F8-;(D4jW?kBo(-Z|8DWDBK>I^T`Fhr0eHh9F7Q(Vmji z9qPIg`Tj(gZOfsq%R}EGalvMhonE8J_G5c>P zPTcwMe&{kc5k#e?C$OWeJ&?X?5PC;CarcfB_Q;zrb8_R{v@Xq(@X%D}w0K7&*Whh; zFzv&Zdb`VWusiw+L<^9~;N76H2)jpF8CW!im@t`DNGG z)zUM2s~0w$7F2mNngPAlhXJ|GX!Yi+#?^*H}(9X#InD z8H8@9J(RTFXm`)V{f-%BboQ8{p<81+tHXRng2-2ZDnbGWins_T`>x6J2+qH|YO22p z*q_mz7$6jWHAm~LcVu{tSfuVhy9rvf+wWX zU8?D)ooS{*Q{wD1&LY!uM*jS%blmhgmm*>%hm!$%MP+yOubPuA^;O*T9mc4R4#+WH zHOkI^)*~6tO#&Ao>X0ccbf0mrkXd-7Q9|d-0fsPirN5VMrM)ffR}KMo@4k~WOI^dM zX>T2#Nt+G~`bwdXLR9MJ96AUoOAO}78#b$%Rt<0pg=s}mjf1|d;A@^Sxz$)-_u7`u zTWVfb22SF}{_Q682ro6JQ$m(w&=(zyK=GZMPz=*5O|Yj+TCWBzdHrfhJ?IsnX#+H) zL}-54rQGsRSKS7pOt(U{WC*IOp*I(#+#d>~5|L|5Ok;Le-4>Qh5C(j6`16xZa996b zzyYS^NT-tRG)}Te&+M$$LmHFCr8e&sMisUR>W!yx&o0|yXozaulD@;pclLUcY{I6zIgI4QZAzb;uB@L)%r;hA6D#D}Z@2 zz-qXK_^meK6LkX4P*OB$rqvBm7}L*l6z4mtKF^nS#3TL_=&EyWrrT<#k&}xf$DDPO z!(&I|M&n3hNVp&Ucil{y6HJsj)7|s+A0&a(9C}TKw-rH)F3Dwt>=2YBIpMEQ0T)+b z+_=g0knPZ_R|(y_N-N?y&ccY}AA4@j?%!i`sP#Su@v z{l1!D9dwxjzUQI)fHOxS3jSxHgMS`pz<(3M8c_k?lPx3%CioFtTX9B4x=eJAEwiMj zE=%2nIyZq&twlcE0b*F$kv9C^NL0fpGxLl>xyyn9pA2VwlU!A!<{9S|$~1p4rkRsn zOzX!>5OV0}4mbOK&jv|zJQ==OgGo5yCf)G@gWp%s8ijOI2T4CS7GL#)q_>PNldiairaTDPyMqI9$LyKnzm74Lm zVeR+*J;?e|p#d96Y-1}RSjc}*7F#|9bV-GhFgAGe46xIG{A{UQ+X=|ddO zo!=HnTXqD}Wug?O@#EeJv|{1fgACs@gcM-3R?X#Pnk0B>hS!9Ac_F6RA!xycOb_{e zcMKL@zYi}*ac7VpSRC-}7D%rdV-I~d@oZQbJ{Klf0b&LkYl7Dcc$XqZ?CCFp1h3oB zigiJP*ChBFL|x=u`1S{Ke*mwshBD^x;YY@|=!?!;` zP#OU!1$>zT5BrN?(AN!};h^t;2n7{V?LsQ*{qaB;3ZDiD3R)rN-=OecxYe#@OcU1O zZW?+^BhuG!AJ=~nbIa>Grv-f;mwya4Mmg|PZ?34sIZ1a#FF+5N?s1U6gZ#Fzzn%QJ z>)~w*Wj}SG%C-K3sq=1X2Itd_Wj;0aBxDRP3g@kF+U9em?QhynDkinJ3sW%jkHdb` z_M$=!-4}!j5*YH3c<4Ew92JQUG352kRP$&dwBR%}foy`4rWH$u#Nj>YHYR-{5gap5CV$dQC0aQfOni!DpBs z!1-2p%9}>qQRifC<@gaeX~3QKq(r)^B*4L>OW^42j zzK7OmsVUx+-XNKzI7_Sepl>ep4SjC%SK5Y`jhIKbU@EOD_wR&D`cQ7HTMap|xgzK* z65lttcffJ|IKq@((fPE5)?)#(($b9~UsA9&ji9-Bz~MbElya|tz9C&9>F?6K3DZQ9 z=T5fd&kK+1vj=EYX>{+v8K7KQ?r9gw^LF+t6S@53@Nv51!lJy>!ht-t9a=|u9PGn+ zJnT>MBzTidhm})@m+EE|nKQZVa(5MU-Y^G3#!Su>?_#@YETu>%+s^NnZT7n)GdDRl&2=|@!JKJu> zyY47aYJ0O^q$hGYGQQ7?B{kI{C=XiAXp^LzBT5jtTN(-^ONR#?Wi2`BEsiBuxahLCS3+*jcq zfqM*YZ%UgWWI+#gSep=w8uW0dz;zA7O0{YfNDu5$a31H{O**^H*|8I8q++83Ng+fX|6LM`7(@c`L1zq*ry%%Xp{t zCA`zRohi%654Q^8D!9c6-w!|Gj~%>&-}f-?_ZGXx-TjJiseHdN!?gx)YNVdY!n*0~ zw`B%=@}L&}=6)^4l&LdT@0|NsTDA$RZ8`c<9@0HcnC+VpViU4l`~?1NXoA{pZyA9+ zulHM#|DU**6!7ipPa+zP+g?{4bZZbZvw!fCWJ0A$egXH2HFDGsT`hl{g#kMj7y5n8Cy? zjuF!;aN6j>X_v|n@@>aF--G`B9cHM#4C98Ete~IBa)05vtMsmUr^Oj<<&Fu<^YC^M zdV5;1<|TvfD4oaf&F_;1a&2!^V>Uv&9p4gjZHaiRa#@waHL1k?NJBRl|8+Ox=6A+( zJW`AYU0$qNl*zk@4_vH42sCF2ba&{L44Hbfal|QgDM99a;k2-r3;6o{qY>IC91^U0 z=-L2&xaSc1?h$$;iB?ZVFyxySN-i}t><|okK3D2WM32Ka(gVKw!PA04gO^CoMq8K$ z^fgh57FZLxJZKdr;(gA^T;7rjP;LzeD3W$0RyvVKAeczN4~~VgH;tY*`tEl0Tw*1x z4)j~C(BA?y_P`|^cv+a8r~YN-FW;@+`^%Sw3HtAT3A&W`ofXtJPgen|oL5;vcR=Z- zwU^P(&A92(gdS~1T<0SPyT#YdBnvxZC7XpTq$tD}oxZbbjCyZHm=~w^Bx6q5Q2$@Y8+5zA2kW!sV_a?_|^Y``1O<`F$X%BHz_Cp$|=IprtFYlD%Z1Velmfhp-gAOOr zK6(d^RZFttOc`h5OL(l@Cxw)CZ#Vp@fy)Lrl@;&d3)W?>9u@Qr53=m*xWiLm%Ddn1 zdqjLyfJ(pKA%G^3zE4e)bnG_&_3NxVR?5aCdlm_*nUd{`gp`>)?9`bY?97=so!Mo> znTI@)-n)7sAf2k3(qz2cQPV>Ako>+6pleF6xvbQXj#wGsPIJiDR8+lZh72p=!d!Ok z$fj>DCwmvMrECRzl>LG|$)0AvWv%=V|6gIRvv*kIzr_!w`iOn`AHPwW^Xv-iXCcOO zt$Zv@GB=DHji-L@<5qFM;w@tM4ypU%(vQSGgK5&r`JHs8$aO^{_=_F}ea zoiuKL9#~WNu<1R4>E#{@JH_zx0e=rg+(O-Wv(a z^xul%-I1_de@+Zo&5%Xy#W@XO6GYEZ! z&=iDp15Y9JB|_y0We#jY=zkE>bDbBeK^w>FxvYG<_d-z}my0u0_pgdoz}>?^Rn|W* z3}COI&ieMk>^e4qdB54nO^~}ENB;K*A1`?v`lT73$IXAea1)Z#TGv_9Y`kYJY(t*j zv?SGlvR(=rvW_6tn&J$Ewg%I)no)Aaz*4anTy{qlbEkRu>@n`)o_k7lSZhe{)M_5^ zRR-6fRXS*6a_;bryq$BGBL2Rh7zdiQ{es_@;$KrIH5nSX+&LmW<=hfAUe(6S23&5m zdtV)w&D;u<&)iZ^DOz7X_{4<}R*YWF{Q3#B5KaeHp{+y2+7}5;mp|m4ti4%&XkE zr|Rvxl^&X>M4zfKXJ3VJ!JLKh!2A>D2;PUi3YT{($oKRN*vg-I5}|?^l7{9%Eg(CCrh7E5n{M=-=ikwK%G= z-dA1!r=c$BJJDZ<(zskRv}U5++Fr+sr4{LD1H-*~ydZ@8^-&(gE)ZksRYHpOI@ZrD zO}Cbhh301F2VkZ#&8HH=w7R5S~9yIV@vDKWq!ICQoQkWLo0iHn@}Eu z6+NR(Fz2=jcKEIE+j36{2Hg0P8u1N&tLT^X-M$LnbKmf+FI`rLveYMWBZ}r=Azn8+ zDloiEa3JuKr=yyi5uaNl-r83tYMy*D{mIDR!>rO$S$eik+V`|QX?(UbN%Mr~XO;)V zm>2AG5Hm+(_bj#i%zU;k8lh;-?>+Zg>Z|#jWS8WOv4P839 z!28qz-|;RT#6jtCCy2~>$zv^#+iSQ)W6rn!9T{F4S%*q0l&`tTljyAS9Pfj z+KC2y@t+JUj)k;|=HG?7I%sim&{KZ4k!jIazZ`tr4{!0&K9@Po`@n?*&kPQ;N)-4w zl&VUOsUL+|53?CYKa8mpHiSmM2>&ZEgsT_e_Q7oDnfhax419lGtb;UJmRJ4Ec1lRC zzQFkQ-N1UQe}#;1-v!??|94Vp`%d`o^#^btWRFzV{xU*kezHsb6^fYl?eNX@KPQ&9 z*yi_b9sDg~?(%;t&uxENjLle0uXeZf6czEg8E(Ad{n|6;<2}*C=+@bI?8NT&AMWN8 zASp19#~U!XgX@p?9PXAR#JGPGiMi8{*NJl_?hSY`AmF>*pW;1|pmeQ`K#6W1?pZ;1 z`*e~e?NyOjbH?8k%J#LGJLMTaoE^4|&lQ*(E9C7D6(?2<&6X(7DZ!TUtW?>);zlgp zptR;D(3csN4fms8CkN(7%D2V%eZvM9BJT5n(vp+ncqwC4UM^vnDbu--cEh=+1m%nj z=y!&EgF)!6;a;W^vzG2y((E zgNMu$vVs9oju?b`GltxKYZlZU+|S0IJOcyk`np_r$8E_D_y;TLt25l|3-|_G=tf7K+SqGH3+tepOm}L&5%ezbCXlj9JkgeA zi;K#od3c4yVoa?!UOyF8f?2LJ3ep)0OTDrG6kbym>~m>N|Eb?D8N9@x6DZ);|A4Qm z|53}MrK#ZWJ(@=_@)todAdArOu)oKxSqJumUd@1SyAbfr2oCss{sCXc;DGOA_!a&Y z7^Oe<<0VghMza0zdTR<~W?uQit-#dZnxFtxddB7LyFF+q|Lrrp_*`%?2^$43d-Mw8f9QYVB zo_;xlgGQ4ydMM^F$24t+U>*MXo} zXy%{FcuurmR@Wlua~8^B|XkyAI}An8HZjfBb*Qdk6APLA{ADGG%iGehX;@QyxdXUo3}g z2u6dr@eA;tS~-3r2zA|aH+~NY{`-+;2J)68Z{aqaS0i5@wGr`4&|VqLRK(#1s&YKi zKM!{s%yyWUVQz&R!Hm20uW5x`DrFEcu@Tj5C=kU`@q17g30U}hg=a{>*qstFD8?g7 zjHeiir|?kv9h4Y<0})sJh;VEq|4=!vN5Ygg6MduP-v0u32lp+w^w|r3UP|8)mccy_ z7dXJ)dJk^|z^#TG1J?{U4(=4V32>!wli^aiVTY z#I1<9cBG^47bE2+M)D;^+@eT%Cn*vAMbg1%V0a*&vAROM;bwro#Vp1gcl|gXJ&Amy z^p}w~)yUZ2iWoZ#R|P}y9+;cu)bC3B59OS=>lb;GZl5%5;+(R(C;t4=HIJ_S<)af9 z%qg8yxNr_*uVG&NU#(qlj2p!jpS5GSY*8CRD%yfz@`*c?T#m~n5Vr!~oqZQ8cRtJZ z5&jg8@p^pTX1(j~j_(hIaDhUvg%%_P{ZNvEDi8tkjUc2T1VR;m1V}+j1r1RJ%7^Al zB_zZLS`>b7XZL(gIH5{iYdkaW-@Nzcz1bQ6{WFR^ju%%^j-gF3V;(toFaBQ)?LNTC zB;*V;QOHbspL`GMuTh9TwI6H80ZsqKn3iz}GX8L%hCK@y@%{p33}q07WV{5y=PuSV zrWaVxGfSG@20e{O<5D5o7U~pyO5=S3+M@?G?GDf;fFqd@)kS{}eWIUSgnjpGda{vh zph7&9PsAVF^d#EE^FbEtQm(^8u#=GU*#jNeozQ1L3Am&q?^1M050bZpdP}4;`S}AB z@@W&YpK9Yxpy>Zh!kVp8lce9>@4|edU zFa~{o?^EbPGUm}I-F^ys@;kxLV%%FR_@6eV`3$sAo(D@o_HOVU2RzpI1ln<25bP)l z*+4LmhPgmz(XNs-e_&!EB77H|=pr1U*1FDp&&7gyTUia<()e_Fge#AaW{bS()ueeL z%5rS!sfd7?F=vh`_7OY6#zzL7+8N z(zj$w@m^m#Vv&1p-x{PX5d^B?d+piA3UR#->R8#);X=A&x=`3V@Vc-aSu@!Hb`C>3 zc3ywOE>X2=vd&g~6-d?P7Dx$=P4m@)x$=b@Shk#lFM3&DnoCzy9WpK+q9U8{tne2B zX*-I0meuxsSxfVQ1vc%r1Nhu_VXohXxeejg9LTbS0|hse6)_6ryl#UN4)VBHSJLIZ z0B#HVKv$DWS~P!_@F7Ke62a8q9;V+_+HR(`A{T3h)#Q*eJ~;t9bEQ(6SK9%v`Su(} z5sI(I)a;sjjpDwv=J@dFo-VFgdyb56w|F33IM|NjXtwDHcSbmBMhA1~CK&}wAc!e| z5q1FYT>vKEQ|$7w;rv7{yO}OTLlj)V4w6At$)`rL-wAg;zbPD(0nbU8>nSeg5Ch?8 zhzHfM177un50$f#651C)f`jE~S|#s->5hf`@myz-#;2gs41;hxF>a5&bHR{RM_BuK z3-c{Zmh*kjPxC<3rCJQdkei|y>gQvK26AhaSYT=yZYUs2Bet^4wa`&&yg7IE74F8BxnL8Rfrp+fC`~ zu(G^n%RvB|tQ_f(FGX`3+U+=4EGW#oCOld^VwO4_sTY;+=OL%71>_>0kx6)=ChQ=I z(Ajz;ltb2##{%SuV#`tXQiN!hgG3F^yE^ozylo;0uUKIHjiit zJv-{aku(?~OAGwhZP|t^y1@c$^I0As;lXWTJ(-0rx$<a4 zu2E?=HC@RSw{tiZGFiB4xoQTefiRK59flj6*p?k-KD#lp3L2 z55Wi+lEm|c3f8^pVx`QHDFh-x9vh)NW#hhFWP2O7lHt9-7Tw@m&l^14aG{@h9PJHg zY%nr-)_VlhnMaKknt$OtULifsw?M62ExZP^BImD$Ztw1<*@E{=Vw&d%qc(n%d-gV* z?(|iRw-=l4$Nn?A4B)=RQ+x)a&tt{EySt1-R9q&B&`982%&-nCYLZP`rWoIk^Sp!d zOOy!|62H;dkU*7r;rRQf@p;Nb(paC!F~eB*#vmPolEATxQwI|nY#mbxT_vdk)`+pm z3`=!m&5N{HXtXZ5e%;2T#XQ!;L$H1n8)wZHY8R#moTCQVb!=mDl9jOsub@3ZFJG|K zk9M)dPd2334X^YJ9a;bM_`}~kJ-7VhS1^ME$twZ>=Ut~i{$1|viKjl2hYr_%{}p44n;KbkFI&f7_ z_E#e(miE0|{72=~o3H)s*_jvjZC!fg!}g)i-&?rmj{5s|p1Hc%*uQY{QhU$4r;dMq z?DlQ1zqT>kI3|NGXT?m7MQBTIb`UGcTemw))>z*lEn@2SjEHGSpb K!lAR({Lr_DyN%ZX literal 28676 zcmb`w34ByVwm)9?-rllz7BHJ|yE|Y=0Gk9fAZn)rO(z{l*c=7a&ITPo(uAOq=a>#b zCxXtXpaZy};2U)GHzW|pjD{eKqYm?%ZXiB3$3}ev!jRV?#J*Yk|2?-mERMe4`~5!u z@Tso7>eQ)oPMxYcb=gqGSnI1yqkQdO9sAMWa2ZM-4*#70+OL^rC){6)nC2b$KUeO? z^!jI+W<;p$uMnoZBd`2)-Kmv7olbSWTF5lt!}Y-3*#7@C%-#1ttM>w6j0wT}TZEqj ztWiHx_f6!7h06X8;bGMN985pP@r~tg4F6pH3%{595MLFTxC3rq4JWpD7yU+Apt5-! z+*NN$MBZ^brLgwF^Q@g+W?lvBQ#xnSKZPx>z6>B(yV;6P>e=&v+Ajgy~<<=WI-$ z>~zke_9=y2kuP9$&f;yHoWt5Fk6IunIO2VZ7+;Iv;M`|4%)2;hhGV8Bwa^0I!jIm%P-xToNgZgVU@PH?Nv zD8?zya$crmUQQS5%qe)WHo0kU&)0PdcRcFthacmucYM;#VtMZ;@J(}XFBsSKWo>lR zeQ<2-NT^g;!>ckx&)9+uj$b*@-s*bC(~hw4G}WKG)wgk1>vGkX5@v9h zMwTcp&E@hnO+CBnG+%9T?1Qg3n_QSy8`;Ft5}YsCM;63B{&*|(AL(hpr~{nRecX}K z8f#^>tSR0ZUEs0%+V}}0J$riw>*7(S-m%`15*_aoncBvA5^H>I2fORDK5ynP)pLs~ zUodlLfKjB%xMob`W!xt}x;=z{g&aqjN7pA8n~QU|4$n*aU2Z;|cL(PsLY{$$Yxw^z zGs3;M=ZiWGuzPpb-b;Ms*)GPEVI|Gw52@3GJ8Cn$kIRcfC`8v04u>P$nc#mNiOAW_uE4>gS-*8 zCCP425Zr%hx?@7O&|TZm%Tmjfb#bCybBERqQd)y-WQDSCYv?tu5_tlxXZ2eU`Z@!o}Kf;btr6;sgdFts#J`3`Y@dh3;UAYC+dz**p z<1Ip_`m4hZ!6$H^v$Q_0>+@!ncY~wo;vp^=Q+Sr79&spqf`aot=(ZtF$ zE^=C%%DrfLT+?PLT?}s`+3-NAu`R;ZfVi=3Zx2@&g_7$DKH*@uT0ifizzH_qeR?_H zn(RDBU+h3;eAm`HxhMmhPc}BrUoC!2XSy+ikPl^F=>-I!gE|5 zf5PQpJ|ElD!7_;!P`ONRL7r@ojG?xN<2j-DkEz7e8fDuo z?diYL%@u4elhZZKw0;lK0rv`*&28phX)yPm;rAd9X`d|MvNxOiAqTZB z%)on(E;%fx^m(*xMx-dbN0uC2@-{F`|&bqH<(7-QlF-<&v1OF z9_W~L#kgQ!e^(bbHTUWc-6tiE-@{md6@;G~H>t?CYC^TV&A)eQwMl85twuzhEK)(ikletppL)E<`}Zc(}U<3xE%ugV&xQ+O3twT|-&R+Vl& zqzK^%Go|58|C6$R_`fK-EmYQsF+~?{k!BkoOyx2*NVAKTo|z8d;7UfG*{<@M9UNh% zOMj8$a_z`B8d`abD~K7Ryw*!-Bc9r@b9bPG{eNpTcW^$nvRE4He4iEhVxpt_ltZ^& zj8^k~%AULPOw!n59vHeOK>c!eooS|aQ(Cu#T9rchp&1IZefl#q^vO*#B+E$yquJ=v znD$)aZjh}?ThUb$zsSC`$FzpiZ?_j+eWLUZuh=L@C8%ZEzy zP>KURVI@ag+p;LHJdK;$+<*0oFfKgUmqMj6mucJ9!ySa<%e8GeEs2GRvy-gMP*%e0 zR+I?(Z|#|-D@p_Yr-npJgf1Mo9|Nw8ltW^m$md+M(=J-VbZX=zA}3r9#RoLHbW6A; z;ExWZBUb~y%>m!4+k?6`$sD51@wVg@W>9N#8Tow32R`MvCg=mNh_8gi%1SOL-=z=v zakB4Y)j{7?8j275^MY}-PxYvM*+=`b??V0}q-*&_<%`h2i(gcR_*GXm;^fZ@`R0(H zwWu7kn!}scv?wf5mXQ{QQA>lOWt=4oV?8`Xl5tpChGq7CHMWv72z|=#QKqisIp#UJ zRFdlaVaRBCqD>2Km}BHJc{iVVi$~*)Du{E;bG+b~_e%|^k?+=!9_U!?Sma3grM42f zOP|o4XgVOvC%^$oPPA*!W1y^4?E(LPwCA)$&CW!P#3!i_3Mnl2BmB;g>4!t{ubLI7 zwwkxJ^CA;-+BTTSBg7%Z+NljqON>>}1^gcjMHAImZ?_+pSehBseF*+>NfQcd5RMFm zBhwTp>#9z+YucECnOF{IV&|Cm=Jn)wa6LgjpQ>$vbk!>?oKCO?{A+-_phW+Nya!X- z_Db`_7+pc$PN`UA`kjaoAJI6&v1+Z~!vh}fMQ%IkEXfw7TSYar+$5CLDh%^W78V$lnF`AUk3RnkhtBrA zs=s5D<3R`ZZJ^nI1Xxd>eRQ9z|46q2nErd<;XYTN(swTl_)k)}_tS1A${nMy^l7&W zWsU~^+^q7|p~Rm9N4m{Z4nR|7qT1rJC@uAt^_B*Ucl9ob?b#slJ2ptd^J7+Tkd&_9 zttM70*blDpaZg zu}hwKtTYsR0>x5N=qc9$K?QN$~^WcIG51z zy62$R##=~Fe+gQ;Ue?m}*R=HSG8E8$?>4SCC$>je5-mAxf@8w>zFeh6<+vX@^AFIO zAB|dp(}Y9a>cr!Y`K_CA7O|^Nb@q5I?K}kUeC%rbuJPBvRl`sAeAvZe zay`Fyyo((p?bG>74Z`?>`6ahBW#)&~#g1`b2VF>n+h#U%QfaTKGXb4^EUG+cSB{8BT^OJF{3fp zv8y@k!3XR!8tW0_CPTCA;rd79KGU4*n$gHL?rMIf`C0QRM?!n9&&BEBPT3XM6W*w` z1$CY0B~i?2Dfp=RgAv&Kpub7$@w>h_AAYY>Nhnv)GKV;Dt>A#cVhjk{vcKEiOZrA z4VAY#-cfNK0sp2zhFay`S-BY0^PHqIz6K|PzGp#a6O9c`mw?ydTs3ml#t0i$CJ$tU z)|Ju)LzxT*|jl)H;u^f6W){l>%Pf(n~EeMYfh5vwXaVY!_!ZiqMfSZvwK%8J( z{U)+8$#|cF0_=2*9`>EJU$C1Oh=oClxDLx^apyG!Vg zHPB3Q^6v2GARm@dL_6`e4Wu)R!8skKN|`3~htc1z=C{l$_fyX(D&KD|S-3vb@{M~H z{lDn=IcRspP3`iHK?sBG&Oj{KuCj54gWBa?9OMg2ySZJG(ZRh#z^{WgI!yD-Yi#Gi zyFqM7@{Uwt;k`t2QA7I1vQW+s*qJs|o_4s^11C<)^Glk1^#^nkFbt1N@~2}nY@QIe{Uc` z;vxh7v*h2!GOqxNJ0bCJExg^EZUOac1O8P3;(~RVf+w*c$(C&AytSE}XUPBZKpyt* zBS7070fU1VU#Z+Wi%OOuH5RE~AeFn8Is>VZNNq=|Ag88VqPo;v!2b=_>CZg(g!H`s z+W-FF2LhZp1$*aWxK(i40=`QlG?er|h1}2ka5~N%r{V zI5qHLH&jB{)AFb7YMT~&BIhg>dfk9^Jjj;{(C%m@&gY>AK0|E-AFj%P|FNN+mF6pv zs25X!vk{M|kA}ueA(KAL8!3ledWAO}A;D90mBoeG7THN=7@CH)#NT7zFZ0F>=$clz zVmS1}rBg34@{ag6PqKFebR^QV^P~jmF%-H-f(Gf0!HnrKgLg{NNT*PiG~D(BA91>H ziyC7|msFuz28k2@>%77moaH!KXCghU4dWu26NY&|PGPx!wcI~&2+b!Ny+(T@;p;6! z{y-pH?vL6T@=NV1q;vWqe|JD3_jKjF5bDFb|62QUo7fwqt8lsmoyg09o`(8pryqpp zi~T%Snk6P}TR&hR`ujh@@zG6i?7IfXAN~c7SFgc=(=y;T(k-ONySemKPn?y<*oOQs z1!ApH7@ZcQ)mdmX&juoq!s*rCuuz$XfCed4`pJL-=M+J>SM8@2lHex&jP$?ww}bGn zY2kk_d@r!^;5pLC=X)}N<@<&*NFvMojGaBt=Y64HR5~Ay=+FgvI_)qN&V$bWnxurr zt?W~RuT%)JJ~Vw%ITJjCSZ{B7o@j5A7~27T-qjrN`+8L<$s_MlFVztc`bY0;lEP@Z zm;d(-r|CtyEbAl$JyK7r$ zwV_dr^&}MPtE^X|vg1uw`DFf9H}UVqK|>Yx@Y#lC(bvYOuR?oS`eoz`X1yNG`4fhq&Z?O-(JJg@1Wfc0Nz2q&PRNJjqaL>Hq{hQ{7{Tqdc_iq%AOdILtGvhtr zNW1phs<&O1j@yA(>i4ug!iF8UJ>1H?;eh%#gKC=zar7bY`-tvn-&aJiVBbajSc&ik zxSW<4f=g6bZnZQwL61WlB%>Y~d`X5jT;AWR+fO>S{W7ug)Gs%nKRPKG@-N*#EZ+~J zoy%~vFK|mNi_SF;z-NX7k}2zeGy`rA+`cTgl$r&6<^@ZnBF?f5q0%@H0jBeCHlJ5A z0xNU4Cjl!ulgHtTqtW1$!G4QZi##<}>>?jCFS65_k3t{ejgV_-Xs=sJr$8+D3miLV zDKQ=;?PJP=vWPk0uVki_e`lJ}I0=1DiZ+Jp!VHRcPbBW?@4CYE$1x+$t`t&aoGn`0(|eQ_?p>ivt4ee{;7G6thc(%I&3cR9lTvGNrl#Ru zqg(Z^fSv5|M8))YDFG|Y?+0|ZUh$~*I04JO=v!WgnuPQ$&uE!8?jMzB&dX)v6WUWP z4L(khY~9clDf8xU2X6^+>5GFSAdkYpuUSKJ*z?7BNlFg+pB{`sj6(H;k=T(^3EfaQ zYK6%svP1sl{UkZ-2WZuOJ)jJ6`s3JV%Jcy~F9u96{*Gz#Mrm8ZEehP`jKDdR;Nw-% zv79d2T3~{HgcjG!G(d8)ye$knydTboS;KYR=QU`5uH62rz_29A5AAzNhL9|&K_9<{ zqdgUuCR(`3<1F8S(~ks{`{dbTYn%{Qd#as?kND#+89vae5FDqOMCHCnKF&${n}`=@gnikPw3J&H-A9C@pJ% z&akNdG0cWS4*hk2)?S*q&j!?hCrp3hJk3a^j5F)dFs&L0XQB0Im{w`+BU)8q^^3&n z7X=LqYaQg^jdZI0SS`~jwaVmHZ@t#4^rzu@xREyhbq>A=2=W|gS}?Lf0h&m@b;$ok zKj|b^gkB!tak4rSp^5P@>#D2;DcJLfTyj%^gr9G!>!3fk>>% zcOtZAfb@r?&y!K4w8r>Z?|7o2FH{fT6$v&B#dU0b)f zk{7qmqLEkk8K99bu4KBcvxfY6gGviapWsfhrp|$m)|(DqZx1Ac9;9A_<7sJ5@nQ$l z-|tBJ<&fWtniPBBms=e}{!@J~%PS$JsoS?Y9{44dB3eG$M|gT@AP7e^Xe=1|TWj#V z*7fpM_7-3xIYM&va~aNPoG?D$8WAen+M3p6Q1c$5%)Pzo4+i`-kU#U$p0!i`=yzblW7D=Am$sUONzZ5$J+1|GPNa93B!kFhZ^CZ2f%kh{iO}0Ed-LLF%EzI>Pa?cfipiy4pv0@9}V#%Q8 zvSaOgao^^X+mTvKjTTEm3HAmreTxU@Od+xw{<|teAv8V#{VcefIK4s+6x1@5@ zacOcoX(fl?OKp^gQ|e6Ufu%U5&Xi~!25f|rWEvY&F`hH~yH z(mf_^D@$_F2~v`sd=q@YG!gJ6=K9ne!iiNrSMCEP>ilgboNgO(73oTwM&5q~$82KE z^kq(DG3j5-;A`1nNyWJ!#(zh=_peK4rM8$OB~~2f;U+7`t36+KaZ219kBOAj#lK4B z(zxs#%S6j&^Y>>kD+X;#vL(WMc*#7r@6xe*Q`^^H-DFwad+nbkYEq4I-jY!&FKYHE zeVmd>++uDz8~?-U>t*O9Gu!sT9yLxzms^A`jjRWaun0#;56XZZl;}u#Jl&qTy3D~K z`Jf4D6uUVC(T#jks>DgDrRd_kd5Yadz6c@S9q=Ct2zt)K8x*?({x+22EC;(iykd78 zYPAGl8)8xHR(Nr563DPnSU~za0mc8~EM|+*qCo7e02{+u)ZVfrqa}Hr1}WPD+!W3- zb7s#OBWSiPFv-GOauB*VupDhf87TaC;10+VU;XqQOry5luHRmIDjRAQZ7wnJtVzqqJ7pOX$N$oPb4}^_~+7`&>1&$Z>(87 zWGN>mJF(Z7^)1{#E*(DCQQE9m5ZtMJlV~KGRH=fHd`JQz}Ox#OJ>cT^#N*O#<94&)zT39ADm-(JXAq}^3~O5y)|DLT4tXU> zwr4|YCvM*@-tFZw?)4Y}`5E<{9Vet6MlL(seL~u0+|)Wg|Cv>*n=<6O*%FAXp0n|Wp7ykzjeBD?9zZ{oJw z3x7CjPR7m(I56T4%vSKjyWp4MxVA-E$P5OYvZpw1^)r2j2fX6BS;vS%oyv$T~dmHGD6|rPpD1^Tf$}Y_C*pHP=Eb)qAFcUk(ipfuGz> z(`?rz=8Zc?l9;01u$rZ1TW^&VT4>9IEM0PQlOxCXcvzYadi#)n2U=N*R>uDyThWDD ziMgQ_E^YLjY>YVtW6pM*sf=vmpsR@(_mF>fpS3AbJ`WqjS>Q-$@p{l;_0W)i5pv?7 zgHn9?kTz7NVrWKdM6k6L)S75n44<>TwyaK;eRDzk`H-nVkhl!t`w$++5#elKr#HN- zQ`@ILyRY*TIkX^jw@3GWRabXEVV@yh^g0{OyW2Iea?`XB2YzT=464Fyw#fiyDav}u4Zx}j(7bWg=FKH^DJvU8Sg6sI>_{B{jl0&F!a&gd?lTTK=>F60T7 z^(A>Fha8`j2qg`UL$Fn9unP{?g>a@NNV*af^CZQ+^_82-cYj1{8{^rKdPz6RkgeQ%^p@lYi>)A7Iv^LJ#W>G_9S^@Q98`U6M@u+N zcK+9S=Pd8UVE;KSGi3ZMRv+$GCtSx)efDdz?f9+MPrCntehCNup|rg@Z7s0ojPc+$ zbv!JXy=2+iAU!LlI(aYU@SZr^lCpT%E9&xIGe-i(Xuz18wH?qse+|0lL5oxxmdUn7 zE4G+bp7Bl8juGj}&d&3&B?+w?!}z&QLwjMb83U{K2F%njZitF*oUOO^@9kMqr}cnF z;xDzXcW7;Kk#RWL9f3L8at(?tz!6yV(lxP!8I(W zCC|Zwmga(%&~yASwpw})0-|${Zyi&uDbb&F^DOCjXTx z^G=-Pb9%^iJysZJPFvRmt>-OAB5{X4EJO8`-n&`arcG*Mph_x5x>^qGkl#dW(VlM{ zQB=}Qe6$066mgA@cA!+-1P!3wu2OdV?fh}+RV^EL#{D0dn=K)H5P#*g%nWfly-B!n zmJ;hy!RZGb_Y_hO!qvLG_`K-_dwW>XJjd?#ojtDX`FZ8K&A7LubF9w?cUE$D?JW7W zlFRBs(!qnX`6@bAU^FfuQq@v(-B`I3OEVuXnf{ap7Sq zP;nZ6wa%=#6r0Om(w1_U=JE41rYmP+ar^z%R!5^5FArdI5GG?}yFDFkeS)@PK?4M5 zK~n-`_HOOC)}+UiTD2j3#Mnp&jPvpn1WzPpCAl;Q_SsVb;lfr8;-{gT9hY_iL+`oY zKhID2r<-K02p%7nwZn9bK5meQdm&TQ9+q(%=sCgqd2`*=fPV&BR>S7uVrv7e7&Kgr zO#s!tCx*9@41_MHh1E?)bjaRV@ZkM}{xs~NOjkx=9K1&$_{gfPeMeG|$7#&Zo8P-( zt@5=fUsb*KW#x;NF-a`rMEj9TJipxhC#+l3ENt9~_CuFA^}~pNi1_p7xHD|x?)LYS zB2(T*iSa10yM1xutLA;}I!ltJ(R@N~+1JL2zBbxtY2G*{rMh0M{7Ona5caz2wPPvO zdEBK)#j#|?#qVmipp68%mm`-r^tuJTBv^KsN1i!~_^cVKes#9~;y2aXk)Dil@3mJZ zj!WH$TH$~t*ca`7=)4A28G!cSB~F-c-fGsIq0(~`V^fD=3R=Ygro7;4^wgf~6l#X` z%H}$nk9n|m-{81oHh4PkaJRBN@1i2D!ND@Im&*IFz1;lJ@=RE^6ubMztIsBM^~Jmg z>u{zi@^KonXy-HvG2Bq(D~>Gk3DM782rKT8_Gk?S`l3DqQTC$&o4(%CA??#1!R~`n z-VSs~ABw3q)5S2>oNT(NVcX3In$u4GS=wj%v-F{Tv^O65{vl}(c2@h~KD3iP(k)&- zBLTCXi4{-_+lG|d6`+um$ar5^$Vy-36;sL5`0Mj5{VTjGQPM84A#`4jHSu}3zWJ}{Qejp;kqP! z3^2llJ$E6R7cusawP^*0BHVJ61^k^uMORl;6GjdIBOl8cNd-m@{|iQRz{mk$!WUpKz7>`A+d&B=xhe$Mx{;cJ#V{WzKZVcsWgY zIr<|y$r3HV%5>04c!*9u)20+87n!aE)(~BkT*J&!VCFL!Grt+?klbSYk7y+Zm^li} zxb5NIaG6#<16Gd0xn<0#FzQ|IuJb{9xO~HyN$;RBPbrx2efKQ9n6UG*9oW-8PQ_od z1tYy-qj`ac@S7RJuUa8o8pf}y{9&0V?#5^v>eLu*L!CU@Fq(ClE5Zarv7T-2kWOg1 z7ez~Q!Pio`E8ssk=+v*r+@Hhl=1pW4H9 zd=9-YKWZX&O3&JxpnK4Wi=iL8@-{dgULIe2xI28tt{zu8%e>bekU2f@FPuJlHl37b zVHWuiZ3he;l2@Bt5b&?Z$cU=H!^phy$eIQ_qzhs~KJodVh|h(M^2m-z-N~g#KQpo$`Iz*g(AbKCdM%ADsMBHxvgUH4Fo8S8i7RE^xN}aBH}1X08)Gj1 z=h}ytXV)qw>~2?#0jzV;+=YClp^~=Fg-!PLr#+wDIRYVv0lo8Ke(A=-O^i=rE zhSSnNwWBewwJxGwe$b|GuzV>EXupvDDUW~B;Fl7MO~lMbU}g_BzhKV8>?UGn2Qag2 zb-b5Li-sNDQ%a^kBK;H8I{?R4<5i#-2}|Y8IBmrVtmn6wVZR%v%9AnQY5y6d=8#P* z>1M-oU4psYZaRi@m#6mN-dB;=dJJbT!>^*Y{p71C9=iMWH34fVUPj%5^`IVS3o*hc z-MGiNBp4Q(Q>P_75-Dsn3qU?~kQs)|~7DwHT`->QX>HM3}ym6tP8DUtU9J z|MLf2psp_^P3)(VVlJhuANW#=)ZO+Y>Y8%uQ%N)TQz>$_(R-^*U5eO)sC5u2#5a+a z3}`%eVw9s`?fDn%4)pS94|S{Z7Gsa$DT`~;VwCfsHPgOEi520Axtq)}Zmiij-{0F~ zs-iKg5e5e~9X+2ZbKz&OlDv^lN$(*&k>)12_L980j_DZ=+HbJ&Cf}pZ`XPBdP- ztPGDn_0?tWT1xnduP*b~Vi_Gu?`xNZ@UgVlz>5Uf6dR&0HE?|P3zyk)goLO|>-poC zx#cgS)MK5Wt}}Kn4ZpL~vb+bqe0^EHNbFR}J#B6GbylqVbLVaA?(F>caK#?dQ*ZgB zCx>2WPPnYvL@%BjZqU-O3RjtY&$;MjT)a z*}N|Psw?J}&f7Nfo~JI)+F0#PuVv{@_uYjZm#yozToBlYo#)@()uk1lLwHYTL3qqx zU_%yAbR`O#k>c-U+ZC``b9Ro~cw>pBftN1N+HUL|7p~|s3N4)nwiCvvMoXt@`w5K4 z^scYd@h+u)MzM|JpZZL%7vZnLfw6x-^($aA{ju1pu~Nh z7eo1vcRDui>%1_WkMj@Y>yz_THXg5O6S0@$#0HF#d|^-9KUJH8Ho@6{IXdI7M~~h; zfU_%K3t?1Yc^n!-!=BSihy3~h12{+#W4LxAGcz*^dpyB+ufW4XkkboZ4xHBEV)~WX zPrAVq&(=*qi4+Cju?+1bJKu8b#7;>l1^?yIJSZ^_Z(J^2Cv{dV@9qp+cVFkPFe4AI zTPFJtVXl9*?g85%DD9Tc=QCKwAu$4wGGq#G>-=ovpD(G@L>bGLgEE$%M98u34fBV{d8~8n za>YqQ?fj+_l5x^qt-#*$4}rN2mu(w2TrOC?uk#-v3U~}-0Du070#r77DJ;{1Gr9JV zWYh&`--rDs>9l`6T6p&d-G9Az>C&Ypq5;HZdI+tNxwH?Ra9JVq+4?;-Hdz7XK^C! zX@i`!eh(~{U=4dS5AE$q#7V$T$D0njmDUYt4Y^5qVa-GShX!Bsj>)XWiOf#NqS=eA zi;G~DtsrWpy~=g$5nbvm$E7sakblNt2#fDt$6`=c=?1$y728F~k~c0FY=^u8HXXob zn2gOoc4{^*>ug*{lIcdQ77Sx`IcyIGsrDbGdty;*V?=X$XX8eeNf-uxGx)Pv7Wm8m zLo#t$wgZr`@)`;_=8C+uPPU)W9Lz1Z;cjjEW&D0duyfJx;nLqREqLqKY35Y&%(3j)?i3db%ytu9{wY1ltvPy!RDbk4sh^-Qihv!#oZPbAnvU^t|0{ zPr42Ro%vwQ&Xy!wQUNR^ljx1gOW0iQY(bqsFoamj1+NJq7*KE)5C@FqxJ zedJrm0e?epBTfMa{YPNMJm~+XKMdh^32m@J|C#=v#mJ!lgAZxEgiGr0N!*o?tq0vB zDs9P$ixbzQ&-K{TG@xFC^c}-0kFvpcNnuT+V6!<2?QWI^{Ey+TXQF%gsDOQi+ibO8oN48~Q z2;JWsv=I)Ln~U243Jv-ff5_Um0LJ&==#Kl(@N-%QaMwl_VrHv(_JIHEUa}Fp7p@*vg0l{sP!qar)a3PwM6MYTX!wcJ#u=0qPc+9`;3-MQ6OF1+#?p9%X31E} zLm8HJEq#!@OmTjg>|2yMog24FWvzfJu- z#=A=z^g9M=EO@0MV>1^*~)@dy1=`@=&cPsPZYNF)Cv-uKO6Oc%5h z-@iXNw&5{Ve-J6ZH|MhH)%OXhcoLU=7{8b|>8Ia=Yp?NnApNO5%iYx<+%fwldO4o` z6n-but1vz>@fgmno|n$r-*U|33OnAy3z3N(9nu2u@e%yK<#<6Y!iK=5>yYkW&0ahq zZP4j)W^IABj^5|Ps~s+mLu#Xr$M3T`q3Z&xv<}f)@~;OQo3xOTOJvEhM905!SZbUk;P=@{9DX-UV)%7UqL++6^;H1n zo?%(Tz_)?)97wfjx0*&L9G4Ch2mF5>Ob4ymL2*Pk(4*UN;vJH$n4f-@p6|YG&dbvI z;`huc?iF||nRqe{Gq-KXkqY_ItEagh(&Mh2i7Css&RUa{Zh@pIhQufi7@n5S8_B+g zjZbs)!HEQXl0fH8 zK`Xac+wr>7tC8*42K)>62j$m4Ap=Nm(LFD>8{gA{n+~UGAzQii0kS--3-I`TBEVB` zV(>7~ey{bV0b@U_bEt^3JN zoFHwI=5L!mjoFmT;XJ=}EVokjhKQ znuBQUzCPCvhY=@zNPX#PVH~s*iJg!f^#T8L1N)Px%>8{X-(ktYy$lJDa=EYt+)w3+ z4oGIuS@R@Y*6bipJhO*5f;i(&oHM92Mcxi+<)p2>npiIT2tu6Uh~%7fLfSKlZHDE` zBo4nHPU7+V@gxD?3Nb*FF!+4JU3uJQTdK-a2^%U{1WTNSGe)`DE5rv1lv8fzUr}uq zUQz7+yji_DfOjGel$$C!u-6_Nh`}6^l_du&%P1@O?}bXFrwx4TpbX9It>T6I znVloelc5hy8?{0BiHBW2m$tZ4(Z{YDO!PezLm%@URL6EiSo{6U2gxprpgPbz=$`<~ znvQ_jgl|Rx{#RfXz}c3Rq_)ZL(x2uSwMlSv$S@si7ZCniyAymn=>Imr85N#eLzI8= ze-7X@=(Mm_qfK+5uTvNF)Ca98HVYC^WIOZMBrOEgv)KH!ZcNx%Qip}5(iJ=1Xp zJ=x(Co?>j`THr}nx;DA?t+mOIf3x16fB1oQMJm{sIPU^UJ6+hk zKuVa-<2P|Shu_rcY%{mbVfmz61z6IC6jQX5TVOj`*9MCb$hyIPy4P!vYIT~nx)4tN zEqS#&?jr7bub0`!-2YAP zPX0^w9qSI2=!MTXo{Qwha5K0fu7X?3J<09lnz^04l276dKkA!$N#|?$U-Pf?Mho_> z7kr?Rv~~w5qAakYai!&bi5XR1iZ7Pq&q=(IPHqPLDTC&3D7`3@E*P(zr}(T;Tw%N@ z$L|frmBw%6_?@A+%6M9i-yVvqjmOTvVSBYIwe=06-4$x@>(=mZUO~$E*6m1Hykd9l z)>&cSY_kT=y@W496pL9-{2Eij*EspK!E?_SZo#Z%^`DTIa0C81LoXnfiP!?fW)1xY zu{6Y1B4!$T7O`6qE9SmDw;r-!fssqgcAhzx*T`iWElST*`C8EGhJJ0D=Ul&kcfXkS z*K;!(*?8vrqMjSC@;r(9gMCjHJP8}JXz!EO56)f3-Y@nWE$i}AP`|uCIj!kjpMO_i zxy{swl`8#AC4;33pFYMj(pynzz)E`ta>EJPGQEEV`ZmCxit_|*-?z?Mj&x(coCb^Z zO@QPb;7VaJHF25u$^6ZE1hMNp!}~=pGu*SgkxOSDHGS#Lqwp>UjKA+&eJ%jGGuZ#g zIa6bDs}^sCOun-EmR^MxtJGl0Idh|sp*`8qC}gTmZpZiOit+_tov<+xyYNhf$Ckel zI|t3A+2;zZGk_fdRBf%d6u27ky#Vfb;wREkwi#;;Yz+JFI>$1YZ%Vzn@gD3JIX>B$ z6{>N2{axVK2Iwh@)vyHejj7MEy@A)@Ikp_j1C9az%lP&PWbx!nji$`>c9Y>%v&%QK z9^9KmyAGD#<%tfp6kb1WxFx)DVm_gMN98+>W!@lU{Vv^2xmR5cz-gFUIL_mI>YuKu z-ko?GtiQTro;nm_cnOXda7a0AKjRJV)kt=213w>yV`MK6r-d5{cL3j`x)naz@5%3L z5iZBM1bs(p4#Kp~u7*!C1`+uUJN0SCJG#%$2cg{Uy6+|^-vpF9Dsii@`hmOw|CiEl zz2?R_wxH&52)jh+JpIpkZ8ldeRZ7-NNQKSj^ulC&@mScn7WZR+MwC`osq)~pTv=I3S)~EK(u>&qP;5STPg%)% z50#q;i%=Ey6zYM+DZToT6{uP6VJxWhtKR9DgQ6b-)%3kJ+Fh)IJ{j&6G9ymX4r5vG z@Uz)sX~WB;H1Dm}6nWl8pNzYPj}uL_<0ys>3vTU=!&`cmc1WtpjrJxWeMYaN zcEDdUK$_+~y@EFZFTkhEcOW8U7wN}@_iJmLhBtYq#nvsTT~mCk!p!^t4}I3rnprZW;apuN3K4v2>+zZh~H0K5g8`m71uuRQ-x0#oWNb! zQ`V>Rab`M>UK^z2wLx(N&SFw_ZA?#U;UpA7x2WM#Ip!c&ck zo~NC0yXbnCni*&dcte&d{pl$pBK-bJ>Dtfv~o5evu3?tQlHwR}drTR3TM;xZO^ zBD{)>Yry5_n8}K^=OIM4l6_aPq{4dW8>oglzd!})lIUVPOcC?mjRiQPN zZV|%wT7yt8@J_N#lA%7~G$Gfd8|j^bx5d2E45PS3jdwtMc|wQ10PODX^=@w*TgYdq zJv8pC&IFWCz!?K<-{kxO|Gy8>yckhVnW1&7GkCWmBM#qBJJmSG`Z`L#u8Tt0U`DbTWZjJcg zl&K>+HY4==z_W5|_d5psNBVw^l(zz2Rc6Q2a%#$Q(r^!T=jHL4DIR>&>a|~4k93ER zpqCcoz=0PBKkVXTu;;Ok!`pAXukVlce%K|%M0kD~N?F{GFVo?h2J7&0Z@_?oyIEIQ$Vj>=G~HRTJnrlSgcQ=D=$XE;*+ss>+_sydeC$CeCpN|+b= zW=f+%(@}aYm0kkbGh-;{uCk^FF|IoTb3^SrA_n|Lee;nvK2TV2Or9xCgx1H!jIgA( z&o96Wp-g<~>aHYwK?moruyvrh)PQ5@tq^gp6}|($BePUk)iDe0J~4NiZA23{LGOu1 zpHx15>KWCj4inB&OxXGTwvW@-H{W9=h=RE%RWZn&YR$l2rm>G3rTOX(&`$=ix^R&9 zP6l*e4Sg+bah;SoUo-WTYGTKP*)3AJasJ+8QsTH5oA1pSbIO#tuYEh-_n5**_igOW zx1_Hk3YOe8ud`mR7+gKfGj zZ$Z~<5YK5*6>7%dWTTkA%iP{w^&8-3=xsaM-Z8vc8uX`qNEX*8t+p!pXk&f>f7lWHl-V|Y?udYt9;EPv=!zkJB z|FSpJp{re7Id%>QJt@<%q*B#$c+GLtpu5rG8VYw)m336E{1srPu*OJjqwrDuhgvHR}`}2ww zlkf`KW=`C0{^5970c4uNEGh0%+qRo~kK>DGl5-a696$A4?4k3F(Ra?Ag?;|~A?W6T zetZ|B-#@>vA7_+({^tTK0Qaszd|3-$QmjPy^}(U@4y$8!zkl1H1M!y!GaZSJe*cSu z6^Q+2u)~rscj3s!F4_D-%yL}<^%o&eEDY~zT&YM-z7MQ zIr(9*-#@u8vFU!qN6zKF_l43gB7H&M)~5MZ^A9_?e*ZUvb8YjicsX~~Wlk;$rkxy| zgSSV)6uO6@_f)_&-iL6OMK}+cA#p~*V*#9ZVH7iMwEB9*A=t4QgStF4*DUDALGAh; zyixh%yzQm1$+RciX;$vS8zOqgGT;~S>iM>!e*Z0leR%WTY7uR(Rj~=vJlN0f`3RDZ zzF^FS!}=q9IOd8d@`(|*dGA8&^j381tQj*;w2PL*JuZ8n|AE0%*e?-X4Cfr~VNTeg zIk`Lf`~`y#&HUz!W-_ePssuM13oEn4)91M<3uhiY&C-52Z*1R>y)N-nCcb^X5^e(= z(am%4=}hc<(2fL`int1HJb0D#O#1c_eJ7=ud@D1(iag?DoJPstYWO4KYgaj|s~)Xf zQ(3jDq8ynu6>DlLS1p^AmCe}Bf8X{`B(bkiPTI*ddrO%n7w#pvQw2;jb1~C&En=G6 zOPD6s%rv*aJypmw@$g5$o&JBU=SIB)=+E;j^z|^)^uj4^O!FZ6`u+i?8GMLoxVZ?= zVVXP9&$xW1d7+4DPN3dd)az|zn)mI132;q9y*AXVgo{Pm1eCvwdbh*RhMNL+Bfbb% z+>L)Nr7DGjalqUywQM*N#uC>NW*B=9{c-SJ@Ok)?;R_UhlW7!`{}y0}zY^&x_*)TI z!!L(VlQ9{77<>)#umbFYX8}WsR z6Rv=BMq?*@(H)=-ejgmQF|~$iK6x})zJC_eP+Ak(=y)uc)`~Q$I~~pfmjy>{yoZJZ zK=gyipXDplVM-Zp}s9$na)(KyUNt}&556Zs(c(7~~;ym)GO`5YC%LfqWkiU90 z(>${-Sf0j6V<1O$Xx^#*@R(jkScZQl=-P6<&fAC&qMdF3c0KJO!1Dw(5P7{lns})0t@Ndk4ngVwwl)f@$YwFb&nIgx}Kuepnw&uScBjT^T!Y?`LR_ z@?+P4Cw_T7{|x#d+oZFAh1z%wae^%gSLGxSuKNELpRr z#!+3hcA4W^;iXGadKeFRxpSsnpOq3Bw527LE2(oM7Qo3S4?UAsmsudaNwVwG4@Bi@)(URl2Cwi>az zqU=#IH_vuct!0&u5>%L7P!UF68KS*qkE|?N^>E3`numi7Aj4k~BAhiPRB25(E3aIG zVf{-oA%*B&wB+U&&YYp6Kbcfy0!D5Y4XCt2EHcmd$@F3BLcU0it*WXKOCH7K$i0Dv zY8;ho#L}vgYJfZ=1Vj)Q$V3e`lki<3-jo)Ml!51}R;_%T`ls0pR>%U{sbcmpcK)Ty z(v>A;D@13>s`8R>HnY0As(PBZretYF&Es-2GD{9mF7aN)snnwn{>XyJmH#n~c|iZ)r<+Ujbc^e4fcdUP!^6Wfe&$b{otr;LoWFoD`ZGl`fU6r8 zFqtLAn#Y|qzg4T2RxX=HK$(eC(Nw{?ZCOkaG4D&xL_FjC(0ZC+wcqSS{V^(C$jzpL9KKLU4RXBfQIN(|jyK{i><682cDQ0VYCewB z^6*UWesuIJoILOX7sk)TafjkSRv2_r0eMWVtmjQoBb9v66Uaj@H=HY&nDXY_a8Z~ zCBQ)iO8pY;r4tk!!7%*yV+e!kzb21UqTB`w!2RFA%>I9dnt+l20Ej*KF7|pH=TpH5 zev!Xn7I#zp0N&|^3#&jKtAGXU6K2BG4>XVr2&sI7PkI}v8nUeMDn=R8CBmZ__@9enK KGhTc;>Hh 3: - sim_id_lines = sim_id_aidl_lines[1:4] - sim_id_fragments = [re.search(r"'([0-9\.]+)'", line).group(1) for line in sim_id_lines] - sim_id = reduce(lambda frag1, frag2: frag1.replace('.', '') + frag2.replace('.', ''), sim_id_fragments) - else: - sim_id = None + sim_state = android.getprop("gsm.sim.state").split(",") + network_type = android.getprop("gsm.network.type").split(',') + mcc_mnc = android.getprop("gsm.sim.operator.numeric") or None + + sim_id = android.parse_service_call_string(['iphonesubinfo', '11']) + cell_data_state = android.parse_service_call_unpack(['phone', '46'], ">q") + cell_data_connected = (cell_data_state == 2) return { 'sim_id': sim_id, 'mcc_mnc': mcc_mnc, 'network_type': network_type, - 'sim_state': sim_state + 'sim_state': sim_state, + 'data_connected': cell_data_connected } @dispatcher.add_method def takeSnapshot(): - from selfdrive.visiond.snapshot.snapshot import snapshot, jpeg_write + from selfdrive.camerad.snapshot.snapshot import snapshot, jpeg_write ret = snapshot() if ret is not None: def b64jpeg(x): @@ -173,7 +231,7 @@ def takeSnapshot(): return {'jpegBack': b64jpeg(ret[0]), 'jpegFront': b64jpeg(ret[1])} else: - raise Exception("not available while visiond is started") + raise Exception("not available while camerad is started") def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event): while not (end_event.is_set() or global_end_event.is_set()): @@ -184,10 +242,10 @@ def ws_proxy_recv(ws, local_sock, ssock, end_event, global_end_event): pass except Exception: cloudlog.exception("athenad.ws_proxy_recv.exception") - traceback.print_exc() break ssock.close() + local_sock.close() end_event.set() def ws_proxy_send(ws, local_sock, signal_sock, end_event): @@ -208,7 +266,6 @@ def ws_proxy_send(ws, local_sock, signal_sock, end_event): ws.send(data, ABNF.OPCODE_BINARY) except Exception: cloudlog.exception("athenad.ws_proxy_send.exception") - traceback.print_exc() end_event.set() def ws_recv(ws, end_event): @@ -220,7 +277,6 @@ def ws_recv(ws, end_event): pass except Exception: cloudlog.exception("athenad.ws_recv.exception") - traceback.print_exc() end_event.set() def ws_send(ws, end_event): @@ -232,7 +288,6 @@ def ws_send(ws, end_event): pass except Exception: cloudlog.exception("athenad.ws_send.exception") - traceback.print_exc() end_event.set() def backoff(retries): @@ -260,7 +315,6 @@ def main(gctx=None): except Exception: cloudlog.exception("athenad.main.exception") conn_retries += 1 - traceback.print_exc() time.sleep(backoff(conn_retries)) diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py old mode 100644 new mode 100755 index 034bfac5cb..2954c70b8b --- a/selfdrive/athena/manage_athenad.py +++ b/selfdrive/athena/manage_athenad.py @@ -5,7 +5,7 @@ from multiprocessing import Process import selfdrive.crash as crash from common.params import Params -from selfdrive.manager import launcher +from selfdrive.launcher import launcher from selfdrive.swaglog import cloudlog from selfdrive.version import version, dirty diff --git a/selfdrive/athena/test.py b/selfdrive/athena/test.py new file mode 100755 index 0000000000..99226176bc --- /dev/null +++ b/selfdrive/athena/test.py @@ -0,0 +1,194 @@ +#!/usr/bin/env python3 +import json +import os +import requests +import tempfile +import time +import threading +import queue +import unittest + +from multiprocessing import Process +from pathlib import Path +from unittest import mock +from websocket import ABNF +from websocket._exceptions import WebSocketConnectionClosedException + +from selfdrive.athena import athenad +from selfdrive.athena.athenad import dispatcher +from selfdrive.athena.test_helpers import MockWebsocket, MockParams, MockApi, EchoSocket, with_http_server +from cereal import messaging + +class TestAthenadMethods(unittest.TestCase): + @classmethod + def setUpClass(cls): + cls.SOCKET_PORT = 45454 + athenad.ROOT = tempfile.mkdtemp() + athenad.Params = MockParams + athenad.Api = MockApi + athenad.LOCAL_PORT_WHITELIST = set([cls.SOCKET_PORT]) + + def test_echo(self): + assert dispatcher["echo"]("bob") == "bob" + + def test_getMessage(self): + try: + dispatcher["getMessage"]("controlsState") + except TimeoutError: + pass + + def send_thermal(): + messaging.context = messaging.Context() + pub_sock = messaging.pub_sock("thermal") + start = time.time() + + while time.time() - start < 1: + msg = messaging.new_message() + msg.init('thermal') + pub_sock.send(msg.to_bytes()) + time.sleep(0.01) + + p = Process(target=send_thermal) + p.start() + try: + thermal = dispatcher["getMessage"]("thermal") + assert thermal['thermal'] + finally: + p.terminate() + + def test_listDataDirectory(self): + print(dispatcher["listDataDirectory"]()) + + @with_http_server + def test_do_upload(self): + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + + try: + item = athenad.UploadItem(path=fn, url="http://localhost:1238", headers={}, created_at=int(time.time()*1000), id='') + try: + athenad._do_upload(item) + except requests.exceptions.ConnectionError: + pass + + item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='') + resp = athenad._do_upload(item) + self.assertEqual(resp.status_code, 201) + finally: + os.unlink(fn) + + @with_http_server + def test_uploadFileToUrl(self): + not_exists_resp = dispatcher["uploadFileToUrl"]("does_not_exist.bz2", "http://localhost:1238", {}) + self.assertEqual(not_exists_resp, 404) + + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + + try: + resp = dispatcher["uploadFileToUrl"]("qlog.bz2", "http://localhost:44444/qlog.bz2", {}) + self.assertEqual(resp['enqueued'], 1) + self.assertDictContainsSubset({"path": fn, "url": "http://localhost:44444/qlog.bz2", "headers": {}}, resp['item']) + self.assertIsNotNone(resp['item'].get('id')) + self.assertEqual(athenad.upload_queue.qsize(), 1) + finally: + athenad.upload_queue = queue.Queue() + os.unlink(fn) + + @with_http_server + def test_upload_handler(self): + fn = os.path.join(athenad.ROOT, 'qlog.bz2') + Path(fn).touch() + item = athenad.UploadItem(path=fn, url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='') + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + + athenad.upload_queue.put_nowait(item) + try: + now = time.time() + while time.time() - now < 5: + if athenad.upload_queue.qsize() == 0: + break + self.assertEqual(athenad.upload_queue.qsize(), 0) + finally: + end_event.set() + athenad.upload_queue = queue.Queue() + os.unlink(fn) + + def test_cancelUpload(self): + item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id') + athenad.upload_queue.put_nowait(item) + dispatcher["cancelUpload"](item.id) + + self.assertIn(item.id, athenad.cancelled_uploads) + + end_event = threading.Event() + thread = threading.Thread(target=athenad.upload_handler, args=(end_event,)) + thread.start() + try: + now = time.time() + while time.time() - now < 5: + if athenad.upload_queue.qsize() == 0 and len(athenad.cancelled_uploads) == 0: + break + self.assertEqual(athenad.upload_queue.qsize(), 0) + self.assertEqual(len(athenad.cancelled_uploads), 0) + finally: + end_event.set() + athenad.upload_queue = queue.Queue() + + def test_listUploadQueue(self): + item = athenad.UploadItem(path="qlog.bz2", url="http://localhost:44444/qlog.bz2", headers={}, created_at=int(time.time()*1000), id='id') + athenad.upload_queue.put_nowait(item) + + try: + items = dispatcher["listUploadQueue"]() + self.assertEqual(len(items), 1) + self.assertDictEqual(items[0], item._asdict()) + finally: + athenad.upload_queue = queue.Queue() + + @mock.patch('selfdrive.athena.athenad.create_connection') + def test_startLocalProxy(self, mock_create_connection): + end_event = threading.Event() + + ws_recv = queue.Queue() + ws_send = queue.Queue() + mock_ws = MockWebsocket(ws_recv, ws_send) + mock_create_connection.return_value = mock_ws + + echo_socket = EchoSocket(self.SOCKET_PORT) + socket_thread = threading.Thread(target=echo_socket.run) + socket_thread.start() + + athenad.startLocalProxy(end_event, 'ws://localhost:1234', self.SOCKET_PORT) + + ws_recv.put_nowait(b'ping') + try: + recv = ws_send.get(timeout=5) + assert recv == (b'ping', ABNF.OPCODE_BINARY), recv + finally: + # signal websocket close to athenad.ws_proxy_recv + ws_recv.put_nowait(WebSocketConnectionClosedException()) + socket_thread.join() + + def test_getSshAuthorizedKeys(self): + keys = dispatcher["getSshAuthorizedKeys"]() + self.assertEqual(keys, MockParams().params["GithubSshKeys"].decode('utf-8')) + + def test_jsonrpc_handler(self): + end_event = threading.Event() + thread = threading.Thread(target=athenad.jsonrpc_handler, args=(end_event,)) + thread.daemon = True + thread.start() + athenad.payload_queue.put_nowait(json.dumps({"method": "echo", "params": ["hello"], "jsonrpc": "2.0", "id": 0})) + try: + resp = athenad.response_queue.get(timeout=3) + self.assertDictEqual(resp.data, {'result': 'hello', 'id': 0, 'jsonrpc': '2.0'}) + finally: + end_event.set() + thread.join() + +if __name__ == '__main__': + unittest.main() diff --git a/selfdrive/athena/test_helpers.py b/selfdrive/athena/test_helpers.py new file mode 100644 index 0000000000..a6b2d48978 --- /dev/null +++ b/selfdrive/athena/test_helpers.py @@ -0,0 +1,97 @@ +import http.server +import requests +import socket +import time +from functools import wraps +from multiprocessing import Process + +class EchoSocket(): + def __init__(self, port): + self.socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.socket.bind(('127.0.0.1', port)) + self.socket.listen(1) + + def run(self): + conn, client_address = self.socket.accept() + conn.settimeout(5.0) + + try: + while True: + data = conn.recv(4096) + if data: + print(f'EchoSocket got {data}') + conn.sendall(data) + else: + break + finally: + conn.shutdown(0) + conn.close() + self.socket.shutdown(0) + self.socket.close() + +class MockApi(): + def __init__(self, dongle_id): + pass + + def get_token(self): + return "fake-token" + +class MockParams(): + def __init__(self): + self.params = { + "DongleId": b"0000000000000000", + "GithubSshKeys": b"ssh-rsa AAAAB3NzaC1yc2EAAAADAQABAAABAQC307aE+nuHzTAgaJhzSf5v7ZZQW9gaperjhCmyPyl4PzY7T1mDGenTlVTN7yoVFZ9UfO9oMQqo0n1OwDIiqbIFxqnhrHU0cYfj88rI85m5BEKlNu5RdaVTj1tcbaPpQc5kZEolaI1nDDjzV0lwS7jo5VYDHseiJHlik3HH1SgtdtsuamGR2T80q1SyW+5rHoMOJG73IH2553NnWuikKiuikGHUYBd00K1ilVAK2xSiMWJp55tQfZ0ecr9QjEsJ+J/efL4HqGNXhffxvypCXvbUYAFSddOwXUPo5BTKevpxMtH+2YrkpSjocWA04VnTYFiPG6U4ItKmbLOTFZtPzoez private" + } + + def get(self, k, encoding=None): + ret = self.params.get(k) + if ret is not None and encoding is not None: + ret = ret.decode(encoding) + return ret + +class MockWebsocket(): + def __init__(self, recv_queue, send_queue): + self.recv_queue = recv_queue + self.send_queue = send_queue + + def recv(self): + data = self.recv_queue.get() + if isinstance(data, Exception): + raise data + return data + + def send(self, data, opcode): + self.send_queue.put_nowait((data, opcode)) + +class HTTPRequestHandler(http.server.SimpleHTTPRequestHandler): + def do_PUT(self): + length = int(self.headers['Content-Length']) + self.rfile.read(length) + self.send_response(201, "Created") + self.end_headers() + +def with_http_server(func): + @wraps(func) + def inner(*args, **kwargs): + p = Process(target=http.server.test, + kwargs={ + 'HandlerClass': HTTPRequestHandler, + 'port': 44444, + 'bind': '127.0.0.1'}) + p.start() + now = time.time() + while 1: + if time.time() - now > 5: + raise Exception('HTTP Server did not start') + try: + requests.put('http://localhost:44444/qlog.bz2', data='') + break + except requests.exceptions.ConnectionError: + time.sleep(0.1) + + try: + return func(*args, **kwargs) + finally: + p.terminate() + + return inner diff --git a/selfdrive/boardd/.gitignore b/selfdrive/boardd/.gitignore index b030dc97cd..1f653bde8f 100644 --- a/selfdrive/boardd/.gitignore +++ b/selfdrive/boardd/.gitignore @@ -1 +1,2 @@ boardd +boardd_api_impl.cpp diff --git a/selfdrive/boardd/Makefile b/selfdrive/boardd/Makefile deleted file mode 100644 index f9d0155c6f..0000000000 --- a/selfdrive/boardd/Makefile +++ /dev/null @@ -1,110 +0,0 @@ -CC = clang -CXX = clang++ - -ARCH := $(shell uname -m) -OS := $(shell uname -o) - -BASEDIR = ../.. -PHONELIBS = ../../phonelibs - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args - -CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS) -CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS) - -MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging -MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a - -JSON_FLAGS = -I$(PHONELIBS)/json/src - -EXTRA_LIBS = -lusb-1.0 - -# ifeq ($(OS),GNU/Linux) -# # for Drive PX2 -# ZMQ_LIBS = -lczmq -lzmq -# CEREAL_LIBS = -lcapnp -lkj -lcapnp_c -# EXTRA_LIBS = -lusb-1.0 -lpthread -# endif -ifeq ($(ARCH),aarch64) -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 -EXTRA_LIBS += -lgnustl_shared -endif - - -ifeq ($(ARCH),x86_64) -ZMQ_FLAGS = -I$(PHONELIBS)/zmq/aarch64/include -EXTRA_LIBS = -lusb-1.0 -lpthread -CXXFLAGS += -I/usr/include/libusb-1.0 -CFLAGS += -I/usr/include/libusb-1.0 -endif - -.PHONY: all -all: boardd - -include ../common/cereal.mk - -OBJS = boardd.o \ - can_list_to_can_capnp.o \ - ../common/swaglog.o \ - ../common/params.o \ - ../common/util.o \ - $(PHONELIBS)/json/src/json.o \ - $(CEREAL_OBJS) - -DEPS := $(OBJS:.o=.d) - -boardd: $(OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(CEREAL_LIBS) \ - $(EXTRA_LIBS) - -boardd.o: boardd.cc - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) -MMD \ - -I$(PHONELIBS)/android_system_core/include \ - $(CEREAL_CFLAGS) \ - $(CEREAL_CXXFLAGS) \ - $(ZMQ_FLAGS) \ - $(MESSAGING_FLAGS) \ - -I../ \ - -I../../ \ - -c -o '$@' '$<' - - -boardd_api_impl.so: libcan_list_to_can_capnp.a boardd_api_impl.pyx boardd_setup.py - python3 boardd_setup.py build_ext --inplace - rm -rf build - rm -f boardd_api_impl.cpp - -libcan_list_to_can_capnp.a: can_list_to_can_capnp.o $(CEREAL_OBJS) - ar rcsD '$@' $^ - -%.o: %.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -MMD \ - -Iinclude -I.. -I../.. \ - $(CEREAL_CFLAGS) \ - $(ZMQ_FLAGS) \ - $(JSON_FLAGS) \ - -c -o '$@' '$<' - -%.o: %.cc - @echo "[ CC ] $@" - $(CXX) $(CXXFLAGS) -MMD \ - -Iinclude -I.. -I../.. \ - $(CEREAL_CXXFLAGS) \ - $(ZMQ_FLAGS) \ - $(MESSAGING_FLAGS) \ - -c -o '$@' '$<' - -.PHONY: clean -clean: - rm -f boardd libcan_list_to_can_capnp.a boardd_api_impl.so $(OBJS) $(DEPS) - --include $(DEPS) diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript new file mode 100644 index 0000000000..14c1ff780d --- /dev/null +++ b/selfdrive/boardd/SConscript @@ -0,0 +1,9 @@ +Import('env', 'common', 'messaging') + +env.Program('boardd.cc', LIBS=['usb-1.0', common, messaging, 'pthread', 'zmq', 'capnp', 'kj']) +env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc']) + +env.Command(['boardd_api_impl.so'], + ['libcan_list_to_can_capnp.a', 'boardd_api_impl.pyx', 'boardd_setup.py'], + "cd selfdrive/boardd && python3 boardd_setup.py build_ext --inplace") + diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 573b1aee0f..e8a313d904 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -33,6 +33,11 @@ #define RECV_SIZE (0x1000) #define TIMEOUT 0 +#define MAX_IR_POWER 0.5f +#define MIN_IR_POWER 0.0f +#define CUTOFF_GAIN 0.015625f // iso400 +#define SATURATE_GAIN 0.0625f // iso1600 + namespace { volatile sig_atomic_t do_exit = 0; @@ -56,7 +61,7 @@ bool fake_send = false; bool loopback_can = false; cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN; bool is_pigeon = false; -const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 24 * 3; // turn off charge after 3 days +const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 30; // turn off charge after 30 hrs const uint32_t VBATT_START_CHARGING = 11500; const uint32_t VBATT_PAUSE_CHARGING = 10500; uint32_t no_ignition_cnt = 0; @@ -71,10 +76,15 @@ void pigeon_init(); void *pigeon_thread(void *crap); void *safety_setter_thread(void *s) { + // diagnostic only is the default, needed for VIN query + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + char *value_vin; size_t value_vin_sz = 0; - // switch to no_output when CarVin param is read + // switch to SILENT when CarVin param is read while (1) { if (do_exit) return NULL; const int result = read_db_value(NULL, "CarVin", &value_vin, &value_vin_sz); @@ -179,9 +189,6 @@ bool usb_connect() { } else { goto fail; } - // power off ESP - libusb_control_transfer(dev_handle, 0xc0, 0xd9, 0, 0, NULL, 0, TIMEOUT); - // power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton #ifndef __x86_64__ if (!connected_once) { @@ -320,11 +327,13 @@ void can_health(PubSocket *publisher) { // copied from panda/board/main.c struct __attribute__((packed)) health { + uint32_t uptime; uint32_t voltage; uint32_t current; uint32_t can_send_errs; uint32_t can_fwd_errs; uint32_t gmlan_send_errs; + uint32_t faults; uint8_t ignition_line; uint8_t ignition_can; uint8_t controls_allowed; @@ -332,26 +341,33 @@ void can_health(PubSocket *publisher) { uint8_t car_harness_status; uint8_t usb_power_mode; uint8_t safety_model; + uint8_t fault_status; + uint8_t power_save_enabled; } health; // recv from board pthread_mutex_lock(&usb_lock); - do { cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT); if (cnt != sizeof(health)) { handle_usb_issue(cnt, __func__); } } while(cnt != sizeof(health)); - pthread_mutex_unlock(&usb_lock); + // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node + if (health.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) { + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + } + bool ignition = ((health.ignition_line != 0) || (health.ignition_can != 0)); - if (!ignition) { - no_ignition_cnt += 1; - } else { + if (ignition) { no_ignition_cnt = 0; + } else { + no_ignition_cnt += 1; } #ifndef __x86_64__ @@ -369,6 +385,23 @@ void can_health(PubSocket *publisher) { libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT); pthread_mutex_unlock(&usb_lock); } + // set power save state enabled when car is off and viceversa when it's on + if (ignition && (health.power_save_enabled == 1)) { + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0xc0, 0xe7, 0, 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + } + if (!ignition && (health.power_save_enabled == 0)) { + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0xc0, 0xe7, 1, 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + } + // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect + if (!ignition && (health.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) { + pthread_mutex_lock(&usb_lock); + libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT); + pthread_mutex_unlock(&usb_lock); + } #endif // clear VIN, CarParams, and set new safety on car start @@ -379,11 +412,6 @@ void can_health(PubSocket *publisher) { result = delete_db_value(NULL, "CarParams"); assert((result == 0) || (result == ERR_NO_VALUE)); - // diagnostic only is the default, needed for VIN query - pthread_mutex_lock(&usb_lock); - libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, NULL, 0, TIMEOUT); - pthread_mutex_unlock(&usb_lock); - if (safety_setter_thread_handle == -1) { err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL); assert(err == 0); @@ -428,10 +456,11 @@ void can_health(PubSocket *publisher) { auto healthData = event.initHealth(); // set fields + healthData.setUptime(health.uptime); healthData.setVoltage(health.voltage); healthData.setCurrent(health.current); if (spoofing_started) { - healthData.setIgnitionLine(1); + healthData.setIgnitionLine(true); } else { healthData.setIgnitionLine(health.ignition_line); } @@ -446,6 +475,8 @@ void can_health(PubSocket *publisher) { healthData.setUsbPowerMode(cereal::HealthData::UsbPowerMode(health.usb_power_mode)); healthData.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_model)); healthData.setFanSpeedRpm(fan_speed_rpm); + healthData.setFaultStatus(cereal::HealthData::FaultStatus(health.fault_status)); + healthData.setPowerSaveEnabled((bool)(health.power_save_enabled)); // send to health auto words = capnp::messageToFlatArray(msg); @@ -477,7 +508,7 @@ void can_send(SubSocket *subscriber) { delete msg; return; } - int msg_count = event.getCan().size(); + int msg_count = event.getSendcan().size(); uint32_t *send = (uint32_t*)malloc(msg_count*0x10); memset(send, 0, msg_count*0x10); @@ -524,6 +555,7 @@ void *can_send_thread(void *crap) { // sendcan = 8017 Context * context = Context::create(); SubSocket * subscriber = SubSocket::create(context, "sendcan"); + assert(subscriber != NULL); // drain sendcan to delete any stale messages from previous runs @@ -548,6 +580,7 @@ void *can_recv_thread(void *crap) { // can = 8006 Context * c = Context::create(); PubSocket * publisher = PubSocket::create(c, "can"); + assert(publisher != NULL); // run at 100hz const uint64_t dt = 10000000ULL; @@ -576,6 +609,7 @@ void *can_health_thread(void *crap) { // health = 8011 Context * c = Context::create(); PubSocket * publisher = PubSocket::create(c, "health"); + assert(publisher != NULL); // run at 2hz while (!do_exit) { @@ -589,9 +623,11 @@ void *hardware_control_thread(void *crap) { LOGD("start hardware control thread"); Context * c = Context::create(); SubSocket * thermal_sock = SubSocket::create(c, "thermal"); - SubSocket * driver_monitoring_sock = SubSocket::create(c, "driverMonitoring"); + SubSocket * front_frame_sock = SubSocket::create(c, "frontFrame"); + assert(thermal_sock != NULL); + assert(front_frame_sock != NULL); - Poller * poller = Poller::create({thermal_sock, driver_monitoring_sock}); + Poller * poller = Poller::create({thermal_sock, front_frame_sock}); // Wait for hardware type to be set. while (hw_type == cereal::HealthData::HwType::UNKNOWN){ @@ -603,8 +639,10 @@ void *hardware_control_thread(void *crap) { uint16_t prev_fan_speed = 999; uint16_t prev_ir_pwr = 999; + unsigned int cnt = 0; while (!do_exit) { + cnt++; for (auto sock : poller->poll(1000)){ Message * msg = sock->receive(); if (msg == NULL) continue; @@ -620,17 +658,25 @@ void *hardware_control_thread(void *crap) { auto type = event.which(); if(type == cereal::Event::THERMAL){ uint16_t fan_speed = event.getThermal().getFanSpeed(); - if (fan_speed != prev_fan_speed){ + if (fan_speed != prev_fan_speed || cnt % 100 == 0){ pthread_mutex_lock(&usb_lock); libusb_control_transfer(dev_handle, 0x40, 0xb1, fan_speed, 0, NULL, 0, TIMEOUT); pthread_mutex_unlock(&usb_lock); prev_fan_speed = fan_speed; } - } else if (type == cereal::Event::DRIVER_MONITORING){ - uint16_t ir_pwr = 100.0 * event.getDriverMonitoring().getIrPwr(); - - if (ir_pwr != prev_ir_pwr){ + } else if (type == cereal::Event::FRONT_FRAME){ + float cur_front_gain = event.getFrontFrame().getGainFrac(); + uint16_t ir_pwr; + if (cur_front_gain <= CUTOFF_GAIN) { + ir_pwr = 100.0 * MIN_IR_POWER; + } else if (cur_front_gain > SATURATE_GAIN) { + ir_pwr = 100.0 * MAX_IR_POWER; + } else { + ir_pwr = 100.0 * (MIN_IR_POWER + ((cur_front_gain - CUTOFF_GAIN) * (MAX_IR_POWER - MIN_IR_POWER) / (SATURATE_GAIN - CUTOFF_GAIN))); + } + + if (ir_pwr != prev_ir_pwr || cnt % 100 == 0 || ir_pwr >= 50.0){ pthread_mutex_lock(&usb_lock); libusb_control_transfer(dev_handle, 0x40, 0xb0, ir_pwr, 0, NULL, 0, TIMEOUT); pthread_mutex_unlock(&usb_lock); @@ -758,6 +804,7 @@ void *pigeon_thread(void *crap) { // ubloxRaw = 8042 Context * context = Context::create(); PubSocket * publisher = PubSocket::create(context, "ubloxRaw"); + assert(publisher != NULL); // run at ~100hz unsigned char dat[0x1000]; diff --git a/selfdrive/boardd/boardd.py b/selfdrive/boardd/boardd.py index d3bcb14710..527f1f4f52 100644 --- a/selfdrive/boardd/boardd.py +++ b/selfdrive/boardd/boardd.py @@ -1,14 +1,9 @@ # pylint: skip-file -import os -import subprocess -# Cython -boardd_api_dir = os.path.dirname(os.path.abspath(__file__)) -subprocess.check_call(["make", "boardd_api_impl.so"], cwd=boardd_api_dir) +# Cython, now uses scons to build from selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp assert can_list_to_can_capnp - def can_capnp_to_can_list(can, src_filter=None): ret = [] for msg in can: diff --git a/selfdrive/boardd/boardd_setup.py b/selfdrive/boardd/boardd_setup.py index 887a1b7105..f987c7aa29 100644 --- a/selfdrive/boardd/boardd_setup.py +++ b/selfdrive/boardd/boardd_setup.py @@ -1,13 +1,15 @@ import subprocess -from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module +from distutils.core import Extension, setup from Cython.Build import cythonize from common.cython_hacks import BuildExtWithoutPlatformSuffix +from common.basedir import BASEDIR +import os -PHONELIBS = '../../phonelibs' +PHONELIBS = os.path.join(BASEDIR, 'phonelibs') -ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg +ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() ARCH_DIR = 'x64' if ARCH == "x86_64" else 'aarch64' setup(name='Boardd API Implementation', @@ -15,7 +17,7 @@ setup(name='Boardd API Implementation', ext_modules=cythonize( Extension( "boardd_api_impl", - libraries=[':libcan_list_to_can_capnp.a', ':libcapnp.a', ':libcapnp.a', ':libkj.a'], + libraries=[':libcan_list_to_can_capnp.a', ':libcapnp.a', ':libkj.a'] if ARCH == "x86_64" else [':libcan_list_to_can_capnp.a', 'capnp', 'kj'], library_dirs=[ './', PHONELIBS + '/capnp-cpp/' + ARCH_DIR + '/lib/', diff --git a/selfdrive/boardd/tests/boardd_old.py b/selfdrive/boardd/tests/boardd_old.py index f963004423..48c7a94595 100755 --- a/selfdrive/boardd/tests/boardd_old.py +++ b/selfdrive/boardd/tests/boardd_old.py @@ -10,7 +10,7 @@ import os import struct import time -import selfdrive.messaging as messaging +import cereal.messaging as messaging from common.realtime import Ratekeeper from selfdrive.swaglog import cloudlog from selfdrive.boardd.boardd import can_capnp_to_can_list diff --git a/selfdrive/boardd/tests/replay_many.py b/selfdrive/boardd/tests/replay_many.py index cd94806a87..024867769d 100755 --- a/selfdrive/boardd/tests/replay_many.py +++ b/selfdrive/boardd/tests/replay_many.py @@ -1,4 +1,5 @@ #!/usr/bin/env python3 +import os import sys import time import signal @@ -6,7 +7,11 @@ import traceback from panda import Panda from multiprocessing import Pool -import selfdrive.messaging as messaging +jungle = "JUNGLE" in os.environ +if jungle: + from panda_jungle import PandaJungle # pylint: disable=import-error + +import cereal.messaging as messaging from selfdrive.boardd.boardd import can_capnp_to_can_list def initializer(): @@ -14,11 +19,15 @@ def initializer(): source: https://stackoverflow.com/a/44869451 """ signal.signal(signal.SIGINT, signal.SIG_IGN) -def send_thread(serial): +def send_thread(sender_serial): + global jungle try: - panda = Panda(serial) - panda.set_safety_mode(Panda.SAFETY_ALLOUTPUT) - panda.set_can_loopback(False) + if jungle: + sender = PandaJungle(sender_serial) + else: + sender = Panda(sender_serial) + sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT) + sender.set_can_loopback(False) can_sock = messaging.sub_sock('can') @@ -27,25 +36,27 @@ def send_thread(serial): tsc = messaging.recv_one(can_sock) snd = can_capnp_to_can_list(tsc.can) snd = list(filter(lambda x: x[-1] <= 2, snd)) - panda.can_send_many(snd) + sender.can_send_many(snd) # Drain panda message buffer - panda.can_recv() + sender.can_recv() except Exception: traceback.print_exc() - if __name__ == "__main__": - serials = Panda.list() - num_pandas = len(serials) + if jungle: + serials = PandaJungle.list() + else: + serials = Panda.list() + num_senders = len(serials) - if num_pandas == 0: - print("No pandas found. Exiting") + if num_senders == 0: + print("No senders found. Exiting") sys.exit(1) else: - print("%d pandas found. Starting broadcast" % num_pandas) + print("%d senders found. Starting broadcast" % num_senders) - pool = Pool(num_pandas, initializer=initializer) + pool = Pool(num_senders, initializer=initializer) pool.map_async(send_thread, serials) while True: diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index fefb3bf254..cb88776a0a 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -6,7 +6,7 @@ import random import time from selfdrive.boardd.boardd import can_list_to_can_capnp -from selfdrive.messaging import drain_sock, pub_sock, sub_sock +from cereal.messaging import drain_sock, pub_sock, sub_sock def get_test_string(): return b"test"+os.urandom(10) diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript new file mode 100644 index 0000000000..12e1afe5ac --- /dev/null +++ b/selfdrive/camerad/SConscript @@ -0,0 +1,19 @@ +Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc', 'cereal') + +libs = ['m', 'pthread', common, 'jpeg', 'json', cereal, 'OpenCL', messaging, 'czmq', 'zmq', 'capnp', 'kj', 'capnp_c', visionipc, gpucommon] + +if arch == "aarch64": + libs += ['gsl', 'CB', 'adreno_utils', 'EGL', 'GLESv3', 'cutils', 'ui'] + cameras = ['cameras/camera_qcom.c'] +else: + libs += [] + cameras = ['cameras/camera_frame_stream.cc'] + +env.SharedLibrary('snapshot/visionipc', + ["#selfdrive/common/visionipc.c", "#selfdrive/common/ipc.c"]) + +env.Program('camerad', [ + 'main.cc', + 'transforms/rgb_to_yuv.c', + cameras, + ], LIBS=libs) diff --git a/selfdrive/visiond/bufs.h b/selfdrive/camerad/bufs.h similarity index 100% rename from selfdrive/visiond/bufs.h rename to selfdrive/camerad/bufs.h diff --git a/selfdrive/visiond/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h similarity index 97% rename from selfdrive/visiond/cameras/camera_common.h rename to selfdrive/camerad/cameras/camera_common.h index cea6a9d125..187952e22d 100644 --- a/selfdrive/visiond/cameras/camera_common.h +++ b/selfdrive/camerad/cameras/camera_common.h @@ -35,6 +35,7 @@ typedef struct FrameMetadata { float lens_sag; float lens_err; float lens_true_pos; + float gain_frac; } FrameMetadata; extern CameraInfo cameras_supported[CAMERA_ID_MAX]; diff --git a/selfdrive/visiond/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc similarity index 99% rename from selfdrive/visiond/cameras/camera_frame_stream.cc rename to selfdrive/camerad/cameras/camera_frame_stream.cc index fe1bba3bb8..1a9f8a931f 100644 --- a/selfdrive/visiond/cameras/camera_frame_stream.cc +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -55,6 +55,7 @@ void run_frame_stream(DualCameraState *s) { int err; Context * context = Context::create(); SubSocket * recorder_sock = SubSocket::create(context, "frame"); + assert(recorder_sock != NULL); CameraState *const rear_camera = &s->rear; auto *tb = &rear_camera->camera_tb; diff --git a/selfdrive/visiond/cameras/camera_frame_stream.h b/selfdrive/camerad/cameras/camera_frame_stream.h similarity index 100% rename from selfdrive/visiond/cameras/camera_frame_stream.h rename to selfdrive/camerad/cameras/camera_frame_stream.h diff --git a/selfdrive/visiond/cameras/camera_qcom.c b/selfdrive/camerad/cameras/camera_qcom.c similarity index 99% rename from selfdrive/visiond/cameras/camera_qcom.c rename to selfdrive/camerad/cameras/camera_qcom.c index 39fd189b5a..c0f10f13b1 100644 --- a/selfdrive/visiond/cameras/camera_qcom.c +++ b/selfdrive/camerad/cameras/camera_qcom.c @@ -1068,17 +1068,17 @@ static void camera_open(CameraState *s, bool rear) { struct msm_ois_cfg_data ois_cfg_data = {0}; // open devices + char *sensor_dev; if (rear) { s->csid_fd = open("/dev/v4l-subdev3", O_RDWR | O_NONBLOCK); assert(s->csid_fd >= 0); s->csiphy_fd = open("/dev/v4l-subdev0", O_RDWR | O_NONBLOCK); assert(s->csiphy_fd >= 0); if (s->device == DEVICE_LP3) { - s->sensor_fd = open("/dev/v4l-subdev17", O_RDWR | O_NONBLOCK); + sensor_dev = "/dev/v4l-subdev17"; } else { - s->sensor_fd = open("/dev/v4l-subdev18", O_RDWR | O_NONBLOCK); + sensor_dev = "/dev/v4l-subdev18"; } - assert(s->sensor_fd >= 0); if (s->device == DEVICE_LP3) { s->isp_fd = open("/dev/v4l-subdev13", O_RDWR | O_NONBLOCK); } else { @@ -1101,11 +1101,10 @@ static void camera_open(CameraState *s, bool rear) { s->csiphy_fd = open("/dev/v4l-subdev2", O_RDWR | O_NONBLOCK); assert(s->csiphy_fd >= 0); if (s->device == DEVICE_LP3) { - s->sensor_fd = open("/dev/v4l-subdev18", O_RDWR | O_NONBLOCK); + sensor_dev = "/dev/v4l-subdev18"; } else { - s->sensor_fd = open("/dev/v4l-subdev19", O_RDWR | O_NONBLOCK); + sensor_dev = "/dev/v4l-subdev19"; } - assert(s->sensor_fd >= 0); if (s->device == DEVICE_LP3) { s->isp_fd = open("/dev/v4l-subdev14", O_RDWR | O_NONBLOCK); } else { @@ -1116,6 +1115,16 @@ static void camera_open(CameraState *s, bool rear) { assert(s->eeprom_fd >= 0); } + // wait for sensor device + // on first startup, these devices aren't present yet + for (int i = 0; i < 10; i++) { + s->sensor_fd = open(sensor_dev, O_RDWR | O_NONBLOCK); + if (s->sensor_fd >= 0) break; + LOGW("waiting for sensors..."); + sleep(1); + } + assert(s->sensor_fd >= 0); + // *** SHUTDOWN ALL *** // CSIPHY: release csiphy @@ -2177,6 +2186,7 @@ void cameras_run(DualCameraState *s) { .lens_sag = c->last_sag_acc_z, .lens_err = c->focus_err, .lens_true_pos = c->lens_true_pos, + .gain_frac = c->cur_gain_frac, }; c->frame_metadata_idx = (c->frame_metadata_idx+1)%METADATA_BUF_COUNT; pthread_mutex_unlock(&c->frame_info_lock); diff --git a/selfdrive/visiond/cameras/camera_qcom.h b/selfdrive/camerad/cameras/camera_qcom.h similarity index 100% rename from selfdrive/visiond/cameras/camera_qcom.h rename to selfdrive/camerad/cameras/camera_qcom.h diff --git a/selfdrive/visiond/cameras/debayer.cl b/selfdrive/camerad/cameras/debayer.cl similarity index 100% rename from selfdrive/visiond/cameras/debayer.cl rename to selfdrive/camerad/cameras/debayer.cl diff --git a/selfdrive/visiond/cameras/sensor_i2c.h b/selfdrive/camerad/cameras/sensor_i2c.h similarity index 100% rename from selfdrive/visiond/cameras/sensor_i2c.h rename to selfdrive/camerad/cameras/sensor_i2c.h diff --git a/selfdrive/visiond/include/msm_cam_sensor.h b/selfdrive/camerad/include/msm_cam_sensor.h similarity index 100% rename from selfdrive/visiond/include/msm_cam_sensor.h rename to selfdrive/camerad/include/msm_cam_sensor.h diff --git a/selfdrive/visiond/include/msm_camsensor_sdk.h b/selfdrive/camerad/include/msm_camsensor_sdk.h similarity index 100% rename from selfdrive/visiond/include/msm_camsensor_sdk.h rename to selfdrive/camerad/include/msm_camsensor_sdk.h diff --git a/selfdrive/visiond/include/msmb_camera.h b/selfdrive/camerad/include/msmb_camera.h similarity index 100% rename from selfdrive/visiond/include/msmb_camera.h rename to selfdrive/camerad/include/msmb_camera.h diff --git a/selfdrive/visiond/include/msmb_isp.h b/selfdrive/camerad/include/msmb_isp.h similarity index 100% rename from selfdrive/visiond/include/msmb_isp.h rename to selfdrive/camerad/include/msmb_isp.h diff --git a/selfdrive/visiond/include/msmb_ispif.h b/selfdrive/camerad/include/msmb_ispif.h similarity index 100% rename from selfdrive/visiond/include/msmb_ispif.h rename to selfdrive/camerad/include/msmb_ispif.h diff --git a/selfdrive/visiond/visiond.cc b/selfdrive/camerad/main.cc similarity index 75% rename from selfdrive/visiond/visiond.cc rename to selfdrive/camerad/main.cc index 31546f956d..7679a5248d 100644 --- a/selfdrive/visiond/visiond.cc +++ b/selfdrive/camerad/main.cc @@ -1,91 +1,44 @@ -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include +#include #include -#include - -#include - -#ifdef __APPLE__ -#include -#else -#include -#endif - -#include -#include -#include -#include +#include #ifdef QCOM -#include +#include "cameras/camera_qcom.h" #else -#include +#include "cameras/camera_frame_stream.h" #endif -#include "common/version.h" #include "common/util.h" -#include "common/timing.h" -#include "common/mat.h" #include "common/swaglog.h" + #include "common/visionipc.h" #include "common/visionbuf.h" #include "common/visionimg.h" -#include "common/buffering.h" - -#include "clutil.h" -#include "bufs.h" - -#ifdef QCOM -#include "cameras/camera_qcom.h" -#else -#include "cameras/camera_frame_stream.h" -#endif #include "messaging.hpp" - -// 3 models -#include "models/driving.h" -#include "models/monitoring.h" -#include "models/posenet.h" - #include "transforms/rgb_to_yuv.h" -#include "cereal/gen/cpp/log.capnp.h" +#include "clutil.h" +#include "bufs.h" -#define M_PI 3.14159265358979323846 +#include +#include +#include +#include +#include "cereal/gen/cpp/log.capnp.h" #define UI_BUF_COUNT 4 - -// #define DUMP_RGB - -//#define DEBUG_DRIVER_MONITOR - -// send net input on port 9000 -//#define SEND_NET_INPUT - #define YUV_COUNT 40 #define MAX_CLIENTS 5 -#ifdef __APPLE__ -typedef void (*sighandler_t) (int); -#endif - extern "C" { volatile sig_atomic_t do_exit = 0; } -namespace { +void set_do_exit(int sig) { + do_exit = 1; +} struct VisionState; @@ -105,7 +58,6 @@ struct VisionClientStreamState { }; struct VisionState { - int frame_width, frame_height; int frame_stride; int frame_size; @@ -127,6 +79,7 @@ struct VisionState { mat3 yuv_transform; TBuffer *yuv_tb; + TBuffer *yuv_front_tb; // TODO: refactor for both cameras? Pool yuv_pool; @@ -160,18 +113,6 @@ struct VisionState { int front_meteringbox_xmin, front_meteringbox_xmax; int front_meteringbox_ymin, front_meteringbox_ymax; - ModelState model; - ModelData model_bufs[UI_BUF_COUNT]; - - MonitoringState monitoring; - PubSocket *monitoring_sock; - - PosenetState posenet; - - // Protected by transform_lock. - bool run_model; - mat3 cur_transform; - pthread_mutex_t transform_lock; cl_mem camera_bufs_cl[FRAME_BUF_COUNT]; VisionBuf camera_bufs[FRAME_BUF_COUNT]; @@ -186,220 +127,436 @@ struct VisionState { zsock_t *terminate_pub; Context * msg_context; - PubSocket *recorder_sock; - PubSocket *posenet_sock; + PubSocket *frame_sock; + PubSocket *front_frame_sock; PubSocket *thumbnail_sock; pthread_mutex_t clients_lock; VisionClientState clients[MAX_CLIENTS]; - }; -void hexdump(uint8_t *d, int l) { - for (int i = 0; i < l; i++) { - if (i%0x10 == 0 && i != 0) printf("\n"); - printf("%02X ", d[i]); - } - printf("\n"); -} - -int mkpath(char* file_path, mode_t mode) { - assert(file_path && *file_path); - char* p; - for (p=strchr(file_path+1, '/'); p; p=strchr(p+1, '/')) { - *p='\0'; - if (mkdir(file_path, mode)==-1) { - if (errno!=EEXIST) { *p='/'; return -1; } - } - *p='/'; - } - return 0; -} - -////////// cl stuff +// frontview thread +void* frontview_thread(void *arg) { + int err; + VisionState *s = (VisionState*)arg; -cl_program build_debayer_program(VisionState *s, - int frame_width, int frame_height, int frame_stride, - int rgb_width, int rgb_height, int rgb_stride, - int bayer_flip, int hdr) { - assert(rgb_width == frame_width/2); - assert(rgb_height == frame_height/2); + set_thread_name("frontview"); - char args[4096]; - snprintf(args, sizeof(args), - "-cl-fast-relaxed-math -cl-denorms-are-zero " - "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d " - "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " - "-DBAYER_FLIP=%d -DHDR=%d", - frame_width, frame_height, frame_stride, - rgb_width, rgb_height, rgb_stride, - bayer_flip, hdr); - return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/debayer.cl", args); -} + s->msg_context = Context::create(); -void cl_init(VisionState *s) { - int err; - cl_platform_id platform_id = NULL; - cl_uint num_devices; - cl_uint num_platforms; + // we subscribe to this for placement of the AE metering box + // TODO: the loop is bad, ideally models shouldn't affect sensors + Context *msg_context = Context::create(); + SubSocket *monitoring_sock = SubSocket::create(msg_context, "driverMonitoring", "127.0.0.1", true); + assert(monitoring_sock != NULL); - err = clGetPlatformIDs(1, &platform_id, &num_platforms); - assert(err == 0); - err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1, - &s->device_id, &num_devices); + cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err); assert(err == 0); - cl_print_info(platform_id, s->device_id); - printf("\n"); + for (int cnt = 0; !do_exit; cnt++) { + int buf_idx = tbuffer_acquire(&s->cameras.front.camera_tb); + if (buf_idx < 0) { + break; + } - s->context = clCreateContext(NULL, 1, &s->device_id, NULL, NULL, &err); - assert(err == 0); -} + int ui_idx = tbuffer_select(&s->ui_front_tb); + FrameMetadata frame_data = s->cameras.front.camera_bufs_metadata[buf_idx]; -void cl_free(VisionState *s) { - int err; + double t1 = millis_since_boot(); - err = clReleaseContext(s->context); - assert(err == 0); -} + err = clSetKernelArg(s->krnl_debayer_front, 0, sizeof(cl_mem), &s->front_camera_bufs_cl[buf_idx]); + assert(err == 0); + err = clSetKernelArg(s->krnl_debayer_front, 1, sizeof(cl_mem), &s->rgb_front_bufs_cl[ui_idx]); + assert(err == 0); + float digital_gain = 1.0; + err = clSetKernelArg(s->krnl_debayer_front, 2, sizeof(float), &digital_gain); + assert(err == 0); -void init_buffers(VisionState *s) { - int err; + cl_event debayer_event; + const size_t debayer_work_size = s->rgb_front_height; + const size_t debayer_local_work_size = 128; + err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 1, NULL, + &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); + assert(err == 0); + clWaitForEvents(1, &debayer_event); + clReleaseEvent(debayer_event); - // allocate camera buffers + tbuffer_release(&s->cameras.front.camera_tb, buf_idx); - for (int i=0; icamera_bufs[i] = visionbuf_allocate_cl(s->frame_size, s->device_id, s->context, - &s->camera_bufs_cl[i]); - // TODO: make lengths correct - s->focus_bufs[i] = visionbuf_allocate(0xb80); - s->stats_bufs[i] = visionbuf_allocate(0xb80); - } + visionbuf_sync(&s->rgb_front_bufs[ui_idx], VISIONBUF_SYNC_FROM_DEVICE); - for (int i=0; ifront_camera_bufs[i] = visionbuf_allocate_cl(s->cameras.front.frame_size, - s->device_id, s->context, - &s->front_camera_bufs_cl[i]); - } + // set front camera metering target + Message *msg = monitoring_sock->receive(true); + if (msg != NULL) { + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); - // processing buffers - if (s->cameras.rear.ci.bayer) { - s->rgb_width = s->frame_width/2; - s->rgb_height = s->frame_height/2; - } else { - s->rgb_width = s->frame_width; - s->rgb_height = s->frame_height; - } + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); - for (int i=0; irgb_width, s->rgb_height, &s->rgb_bufs[i]); - s->rgb_bufs_cl[i] = visionbuf_to_cl(&s->rgb_bufs[i], s->device_id, s->context); - if (i == 0){ - s->rgb_stride = img.stride; - s->rgb_buf_size = img.size; - } - } - tbuffer_init(&s->ui_tb, UI_BUF_COUNT, "rgb"); + float face_prob = event.getDriverMonitoring().getFaceProb(); + float face_position[2]; + face_position[0] = event.getDriverMonitoring().getFacePosition()[0]; + face_position[1] = event.getDriverMonitoring().getFacePosition()[1]; - //assert(s->cameras.front.ci.bayer); - s->rgb_front_width = s->cameras.front.ci.frame_width/2; - s->rgb_front_height = s->cameras.front.ci.frame_height/2; + // set front camera metering target + if (face_prob > 0.4) + { + int x_offset = s->rgb_front_width - 0.5 * s->rgb_front_height; + s->front_meteringbox_xmin = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) - 72; + s->front_meteringbox_xmax = x_offset + (face_position[0] + 0.5) * (0.5 * s->rgb_front_height) + 72; + s->front_meteringbox_ymin = (face_position[1] + 0.5) * (s->rgb_front_height) - 72; + s->front_meteringbox_ymax = (face_position[1] + 0.5) * (s->rgb_front_height) + 72; + } + else // use default setting if no face + { + s->front_meteringbox_ymin = s->rgb_front_height * 1 / 3; + s->front_meteringbox_ymax = s->rgb_front_height * 1; + s->front_meteringbox_xmin = s->rgb_front_width * 3 / 5; + s->front_meteringbox_xmax = s->rgb_front_width; + } - for (int i=0; irgb_front_width, s->rgb_front_height, &s->rgb_front_bufs[i]); - s->rgb_front_bufs_cl[i] = visionbuf_to_cl(&s->rgb_front_bufs[i], s->device_id, s->context); - if (i == 0){ - s->rgb_front_stride = img.stride; - s->rgb_front_buf_size = img.size; + delete msg; } - } - tbuffer_init(&s->ui_front_tb, UI_BUF_COUNT, "frontrgb"); - // yuv back for recording and orbd - pool_init(&s->yuv_pool, YUV_COUNT); + // auto exposure + const uint8_t *bgr_front_ptr = (const uint8_t*)s->rgb_front_bufs[ui_idx].addr; +#ifndef DEBUG_DRIVER_MONITOR + if (cnt % 3 == 0) +#endif + { + // use driver face crop for AE + int x_start; + int x_end; + int y_start; + int y_end; - s->yuv_tb = pool_get_tbuffer(&s->yuv_pool); //only for visionserver... + if (s->front_meteringbox_xmax > 0) + { + x_start = s->front_meteringbox_xmin<0 ? 0:s->front_meteringbox_xmin; + x_end = s->front_meteringbox_xmax>=s->rgb_front_width ? s->rgb_front_width-1:s->front_meteringbox_xmax; + y_start = s->front_meteringbox_ymin<0 ? 0:s->front_meteringbox_ymin; + y_end = s->front_meteringbox_ymax>=s->rgb_front_height ? s->rgb_front_height-1:s->front_meteringbox_ymax; + } + else + { + y_start = s->rgb_front_height * 1 / 3; + y_end = s->rgb_front_height * 1; + x_start = s->rgb_front_width * 3 / 5; + x_end = s->rgb_front_width; + } - s->yuv_width = s->rgb_width; - s->yuv_height = s->rgb_height; - s->yuv_buf_size = s->rgb_width * s->rgb_height * 3 / 2; + uint32_t lum_binning[256] = {0,}; + for (int y = y_start; y < y_end; ++y) { + for (int x = x_start; x < x_end; x += 2) { // every 2nd col + const uint8_t *pix = &bgr_front_ptr[y * s->rgb_front_stride + x * 3]; + unsigned int lum = (unsigned int)pix[0] + pix[1] + pix[2]; +#ifdef DEBUG_DRIVER_MONITOR + uint8_t *pix_rw = (uint8_t *)pix; - for (int i=0; iyuv_ion[i] = visionbuf_allocate_cl(s->yuv_buf_size, s->device_id, s->context, &s->yuv_cl[i]); - s->yuv_bufs[i].y = (uint8_t*)s->yuv_ion[i].addr; - s->yuv_bufs[i].u = s->yuv_bufs[i].y + (s->yuv_width * s->yuv_height); - s->yuv_bufs[i].v = s->yuv_bufs[i].u + (s->yuv_width/2 * s->yuv_height/2); - } + // set all the autoexposure pixels to pure green (pixel format is bgr) + pix_rw[0] = pix_rw[2] = 0; + pix_rw[1] = 0xff; +#endif + lum_binning[std::min(lum / 3, 255u)]++; + } + } + const unsigned int lum_total = (y_end - y_start) * (x_end - x_start)/2; + unsigned int lum_cur = 0; + int lum_med = 0; + for (lum_med=0; lum_med<256; lum_med++) { + lum_cur += lum_binning[lum_med]; + if (lum_cur >= lum_total / 2) { + break; + } + } + camera_autoexposure(&s->cameras.front, lum_med / 256.0); + } - // yuv front for recording - pool_init(&s->yuv_front_pool, YUV_COUNT); + // push YUV buffer + int yuv_idx = pool_select(&s->yuv_front_pool); + s->yuv_front_metas[yuv_idx] = frame_data; - s->yuv_front_width = s->rgb_front_width; - s->yuv_front_height = s->rgb_front_height; - s->yuv_front_buf_size = s->rgb_front_width * s->rgb_front_height * 3 / 2; + rgb_to_yuv_queue(&s->front_rgb_to_yuv_state, q, s->rgb_front_bufs_cl[ui_idx], s->yuv_front_cl[yuv_idx]); + visionbuf_sync(&s->yuv_front_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); + s->yuv_front_metas[yuv_idx] = frame_data; - for (int i=0; iyuv_front_ion[i] = visionbuf_allocate_cl(s->yuv_front_buf_size, s->device_id, s->context, &s->yuv_front_cl[i]); - s->yuv_front_bufs[i].y = (uint8_t*)s->yuv_front_ion[i].addr; - s->yuv_front_bufs[i].u = s->yuv_front_bufs[i].y + (s->yuv_front_width * s->yuv_front_height); - s->yuv_front_bufs[i].v = s->yuv_front_bufs[i].u + (s->yuv_front_width/2 * s->yuv_front_height/2); - } + // no reference required cause we don't use this in visiond + //pool_acquire(&s->yuv_front_pool, yuv_idx); + pool_push(&s->yuv_front_pool, yuv_idx); + //pool_release(&s->yuv_front_pool, yuv_idx); - if (s->cameras.rear.ci.bayer) { - // debayering does a 2x downscale - s->yuv_transform = transform_scale_buffer(s->cameras.rear.transform, 0.5); - } else { - s->yuv_transform = s->cameras.rear.transform; - } + // send frame event + { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); - if (s->cameras.rear.ci.bayer) { - s->prg_debayer_rear = build_debayer_program(s, s->cameras.rear.ci.frame_width, s->cameras.rear.ci.frame_height, - s->cameras.rear.ci.frame_stride, - s->rgb_width, s->rgb_height, s->rgb_stride, - s->cameras.rear.ci.bayer_flip, s->cameras.rear.ci.hdr); - s->krnl_debayer_rear = clCreateKernel(s->prg_debayer_rear, "debayer10", &err); - assert(err == 0); - } + auto framed = event.initFrontFrame(); + framed.setFrameId(frame_data.frame_id); + framed.setEncodeId(cnt); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setFrameLength(frame_data.frame_length); + framed.setIntegLines(frame_data.integ_lines); + framed.setGlobalGain(frame_data.global_gain); + framed.setLensPos(frame_data.lens_pos); + framed.setLensSag(frame_data.lens_sag); + framed.setLensErr(frame_data.lens_err); + framed.setLensTruePos(frame_data.lens_true_pos); + framed.setGainFrac(frame_data.gain_frac); + framed.setFrameType(cereal::FrameData::FrameType::FRONT); - if (s->cameras.front.ci.bayer) { - s->prg_debayer_front = build_debayer_program(s, s->cameras.front.ci.frame_width, s->cameras.front.ci.frame_height, - s->cameras.front.ci.frame_stride, - s->rgb_front_width, s->rgb_front_height, s->rgb_front_stride, - s->cameras.front.ci.bayer_flip, s->cameras.front.ci.hdr); + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + s->front_frame_sock->send((char*)bytes.begin(), bytes.size()); + } - s->krnl_debayer_front = clCreateKernel(s->prg_debayer_front, "debayer10", &err); - assert(err == 0); + /*FILE *f = fopen("/tmp/test2", "wb"); + printf("%d %d\n", s->rgb_front_height, s->rgb_front_stride); + fwrite(bgr_front_ptr, 1, s->rgb_front_stride * s->rgb_front_height, f); + fclose(f);*/ + + tbuffer_dispatch(&s->ui_front_tb, ui_idx); + + double t2 = millis_since_boot(); + + //LOGD("front process: %.2fms", t2-t1); } - rgb_to_yuv_init(&s->rgb_to_yuv_state, s->context, s->device_id, s->yuv_width, s->yuv_height, s->rgb_stride); - rgb_to_yuv_init(&s->front_rgb_to_yuv_state, s->context, s->device_id, s->yuv_front_width, s->yuv_front_height, s->rgb_front_stride); + delete monitoring_sock; + + return NULL; } +// processing +void* processing_thread(void *arg) { + int err; + VisionState *s = (VisionState*)arg; -void free_buffers(VisionState *s) { - // free bufs - for (int i=0; icamera_bufs[i]); - visionbuf_free(&s->focus_bufs[i]); - visionbuf_free(&s->stats_bufs[i]); - } + set_thread_name("processing"); - for (int i=0; ifront_camera_bufs[i]); - } + err = set_realtime_priority(1); + LOG("setpriority returns %d", err); - for (int i=0; irgb_bufs[i]); - } + // init cl stuff + const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; + cl_command_queue q = clCreateCommandQueueWithProperties(s->context, s->device_id, props, &err); + assert(err == 0); - for (int i=0; irgb_front_bufs[i]); - } + // init the net + LOG("processing start!"); - for (int i=0; iyuv_ion[i]); + for (int cnt = 0; !do_exit; cnt++) { + int buf_idx = tbuffer_acquire(&s->cameras.rear.camera_tb); + // int buf_idx = camera_acquire_buffer(s); + if (buf_idx < 0) { + break; + } + + double t1 = millis_since_boot(); + + FrameMetadata frame_data = s->cameras.rear.camera_bufs_metadata[buf_idx]; + uint32_t frame_id = frame_data.frame_id; + + if (frame_id == -1) { + LOGE("no frame data? wtf"); + tbuffer_release(&s->cameras.rear.camera_tb, buf_idx); + continue; + } + + int ui_idx = tbuffer_select(&s->ui_tb); + int rgb_idx = ui_idx; + + cl_event debayer_event; + if (s->cameras.rear.ci.bayer) { + err = clSetKernelArg(s->krnl_debayer_rear, 0, sizeof(cl_mem), &s->camera_bufs_cl[buf_idx]); + cl_check_error(err); + err = clSetKernelArg(s->krnl_debayer_rear, 1, sizeof(cl_mem), &s->rgb_bufs_cl[rgb_idx]); + cl_check_error(err); + err = clSetKernelArg(s->krnl_debayer_rear, 2, sizeof(float), &s->cameras.rear.digital_gain); + assert(err == 0); + + const size_t debayer_work_size = s->rgb_height; // doesn't divide evenly, is this okay? + const size_t debayer_local_work_size = 128; + err = clEnqueueNDRangeKernel(q, s->krnl_debayer_rear, 1, NULL, + &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); + assert(err == 0); + } else { + assert(s->rgb_buf_size >= s->frame_size); + assert(s->rgb_stride == s->frame_stride); + err = clEnqueueCopyBuffer(q, s->camera_bufs_cl[buf_idx], s->rgb_bufs_cl[rgb_idx], + 0, 0, s->rgb_buf_size, 0, 0, &debayer_event); + assert(err == 0); + } + + clWaitForEvents(1, &debayer_event); + clReleaseEvent(debayer_event); + + tbuffer_release(&s->cameras.rear.camera_tb, buf_idx); + + visionbuf_sync(&s->rgb_bufs[rgb_idx], VISIONBUF_SYNC_FROM_DEVICE); + + + double t2 = millis_since_boot(); + + uint8_t *bgr_ptr = (uint8_t*)s->rgb_bufs[rgb_idx].addr; + + double yt1 = millis_since_boot(); + + int yuv_idx = pool_select(&s->yuv_pool); + + s->yuv_metas[yuv_idx] = frame_data; + + uint8_t* yuv_ptr_y = s->yuv_bufs[yuv_idx].y; + uint8_t* yuv_ptr_u = s->yuv_bufs[yuv_idx].u; + uint8_t* yuv_ptr_v = s->yuv_bufs[yuv_idx].v; + cl_mem yuv_cl = s->yuv_cl[yuv_idx]; + rgb_to_yuv_queue(&s->rgb_to_yuv_state, q, s->rgb_bufs_cl[rgb_idx], yuv_cl); + visionbuf_sync(&s->yuv_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); + + double yt2 = millis_since_boot(); + + // keep another reference around till were done processing + pool_acquire(&s->yuv_pool, yuv_idx); + pool_push(&s->yuv_pool, yuv_idx); + + // send frame event + { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initFrame(); + framed.setFrameId(frame_data.frame_id); + framed.setEncodeId(cnt); + framed.setTimestampEof(frame_data.timestamp_eof); + framed.setFrameLength(frame_data.frame_length); + framed.setIntegLines(frame_data.integ_lines); + framed.setGlobalGain(frame_data.global_gain); + framed.setLensPos(frame_data.lens_pos); + framed.setLensSag(frame_data.lens_sag); + framed.setLensErr(frame_data.lens_err); + framed.setLensTruePos(frame_data.lens_true_pos); + framed.setGainFrac(frame_data.gain_frac); + +#ifndef QCOM + framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); +#endif + + kj::ArrayPtr transform_vs(&s->yuv_transform.v[0], 9); + framed.setTransform(transform_vs); + + if (s->frame_sock != NULL) { + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + s->frame_sock->send((char*)bytes.begin(), bytes.size()); + } + } + + // one thumbnail per 5 seconds (instead of %5 == 0 posenet) + if (cnt % 100 == 3) { + uint8_t* thumbnail_buffer = NULL; + uint64_t thumbnail_len = 0; + + unsigned char *row = (unsigned char *)malloc(s->rgb_width/4*3); + + struct jpeg_compress_struct cinfo; + struct jpeg_error_mgr jerr; + + cinfo.err = jpeg_std_error(&jerr); + jpeg_create_compress(&cinfo); + jpeg_mem_dest(&cinfo, &thumbnail_buffer, &thumbnail_len); + + cinfo.image_width = s->rgb_width / 4; + cinfo.image_height = s->rgb_height / 4; + cinfo.input_components = 3; + cinfo.in_color_space = JCS_RGB; + + jpeg_set_defaults(&cinfo); + jpeg_set_quality(&cinfo, 50, true); + jpeg_start_compress(&cinfo, true); + + JSAMPROW row_pointer[1]; + for (int i = 0; i < s->rgb_height - 4; i+=4) { + for (int j = 0; j < s->rgb_width*3; j+=12) { + for (int k = 0; k < 3; k++) { + uint16_t dat = 0; + dat += bgr_ptr[s->rgb_stride*i + j + k]; + dat += bgr_ptr[s->rgb_stride*i + j+3 + k]; + dat += bgr_ptr[s->rgb_stride*(i+1) + j + k]; + dat += bgr_ptr[s->rgb_stride*(i+1) + j+3 + k]; + dat += bgr_ptr[s->rgb_stride*(i+2) + j + k]; + dat += bgr_ptr[s->rgb_stride*(i+2) + j+3 + k]; + dat += bgr_ptr[s->rgb_stride*(i+3) + j + k]; + dat += bgr_ptr[s->rgb_stride*(i+3) + j+3 + k]; + + row[(j/4) + (2-k)] = dat/8; + } + } + row_pointer[0] = row; + jpeg_write_scanlines(&cinfo, row_pointer, 1); + } + free(row); + jpeg_finish_compress(&cinfo); + + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto thumbnaild = event.initThumbnail(); + thumbnaild.setFrameId(frame_data.frame_id); + thumbnaild.setTimestampEof(frame_data.timestamp_eof); + thumbnaild.setThumbnail(kj::arrayPtr((const uint8_t*)thumbnail_buffer, thumbnail_len)); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + if (s->thumbnail_sock != NULL) { + s->thumbnail_sock->send((char*)bytes.begin(), bytes.size()); + } + + free(thumbnail_buffer); + } + + tbuffer_dispatch(&s->ui_tb, ui_idx); + + // auto exposure over big box + const int exposure_x = 290; + const int exposure_y = 282 + 40; + const int exposure_height = 314; + const int exposure_width = 560; + if (cnt % 3 == 0) { + // find median box luminance for AE + uint32_t lum_binning[256] = {0,}; + for (int y=0; yyuv_width) + exposure_x + x]; + lum_binning[lum]++; + } + } + const unsigned int lum_total = exposure_height * exposure_width; + unsigned int lum_cur = 0; + int lum_med = 0; + for (lum_med=0; lum_med<256; lum_med++) { + // shouldn't be any values less than 16 - yuv footroom + lum_cur += lum_binning[lum_med]; + if (lum_cur >= lum_total / 2) { + break; + } + } + // double avg = (double)acc / (big_box_width * big_box_height) - 16; + // printf("avg %d\n", lum_med); + + camera_autoexposure(&s->cameras.rear, lum_med / 256.0); + } + + pool_release(&s->yuv_pool, yuv_idx); + double t5 = millis_since_boot(); + LOGD("queued: %.2fms, yuv: %.2f, | processing: %.3fms", (t2-t1), (yt2-yt1), (t5-t1)); } + + return NULL; } +// visionserver void* visionserver_client_thread(void* arg) { int err; VisionClientState *client = (VisionClientState*)arg; @@ -414,7 +571,7 @@ void* visionserver_client_thread(void* arg) { VisionClientStreamState streams[VISION_STREAM_MAX] = {{0}}; - LOG("client start fd %d\n", fd); + LOGW("client start fd %d", fd); while (true) { zmq_pollitem_t polls[2+VISION_STREAM_MAX] = {{0}}; @@ -441,6 +598,7 @@ void* visionserver_client_thread(void* arg) { } int ret = zmq_poll(polls, num_polls, -1); if (ret < 0) { + if (errno == EINTR) continue; LOGE("poll failed (%d)", ret); break; } @@ -515,7 +673,7 @@ void* visionserver_client_thread(void* arg) { rep.fds[i] = s->yuv_front_ion[i].fd; } if (stream->tb) { - assert(false); + stream->tbuffer = s->yuv_front_tb; } else { stream->queue = pool_get_queue(&s->yuv_front_pool); } @@ -525,10 +683,10 @@ void* visionserver_client_thread(void* arg) { if (stream_type == VISION_STREAM_RGB_BACK || stream_type == VISION_STREAM_RGB_FRONT) { - stream_bufs->buf_info.ui_info = (VisionUIInfo){ + /*stream_bufs->buf_info.ui_info = (VisionUIInfo){ .transformed_width = s->model.in.transformed_width, .transformed_height = s->model.in.transformed_height, - }; + };*/ } vipc_send(fd, &rep); streams[stream_type].subscribed = true; @@ -558,726 +716,326 @@ void* visionserver_client_thread(void* arg) { if (stream_i < VISION_STREAM_MAX) { streams[stream_i].bufs_outstanding++; int idx; - if (streams[stream_i].tb) { - idx = tbuffer_acquire(streams[stream_i].tbuffer); - } else { - idx = poolq_pop(streams[stream_i].queue); - } - if (idx < 0) { - break; - } - VisionPacket rep = { - .type = VIPC_STREAM_ACQUIRE, - .d = {.stream_acq = { - .type = (VisionStreamType)stream_i, - .idx = idx, - }}, - }; - if (stream_i == VISION_STREAM_YUV) { - rep.d.stream_acq.extra.frame_id = s->yuv_metas[idx].frame_id; - rep.d.stream_acq.extra.timestamp_eof = s->yuv_metas[idx].timestamp_eof; - } else if (stream_i == VISION_STREAM_YUV_FRONT) { - rep.d.stream_acq.extra.frame_id = s->yuv_front_metas[idx].frame_id; - rep.d.stream_acq.extra.timestamp_eof = s->yuv_front_metas[idx].timestamp_eof; - } - vipc_send(fd, &rep); - } - } - } - - LOG("client end fd %d\n", fd); - - for (int i=0; iclients_lock); - client->running = false; - pthread_mutex_unlock(&s->clients_lock); - - return NULL; -} - -void* visionserver_thread(void* arg) { - int err; - VisionState *s = (VisionState*)arg; - - set_thread_name("visionserver"); - - zsock_t *terminate = zsock_new_sub(">inproc://terminate", ""); - assert(terminate); - void* terminate_raw = zsock_resolve(terminate); - - unlink(VIPC_SOCKET_PATH); - - int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0); - struct sockaddr_un addr = { - .sun_family = AF_UNIX, - .sun_path = VIPC_SOCKET_PATH, - }; - err = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); - assert(err == 0); - - err = listen(sock, 3); - assert(err == 0); - - // printf("waiting\n"); - - while (!do_exit) { - zmq_pollitem_t polls[2] = {{0}}; - polls[0].socket = terminate_raw; - polls[0].events = ZMQ_POLLIN; - polls[1].fd = sock; - polls[1].events = ZMQ_POLLIN; - - int ret = zmq_poll(polls, ARRAYSIZE(polls), -1); - if (ret < 0) { - LOGE("poll failed (%d)", ret); - break; - } - if (polls[0].revents) { - break; - } else if (!polls[1].revents) { - continue; - } - - int fd = accept(sock, NULL, NULL); - assert(fd >= 0); - - pthread_mutex_lock(&s->clients_lock); - - int client_idx = 0; - for (; client_idx < MAX_CLIENTS; client_idx++) { - if (!s->clients[client_idx].running) break; - } - - if (client_idx >= MAX_CLIENTS) { - LOG("ignoring visionserver connection, max clients connected"); - close(fd); - - pthread_mutex_unlock(&s->clients_lock); - continue; - } - - VisionClientState *client = &s->clients[client_idx]; - client->s = s; - client->fd = fd; - client->running = true; - - err = pthread_create(&client->thread_handle, NULL, - visionserver_client_thread, client); - assert(err == 0); - - pthread_mutex_unlock(&s->clients_lock); - } - - for (int i=0; iclients_lock); - bool running = s->clients[i].running; - pthread_mutex_unlock(&s->clients_lock); - if (running) { - err = pthread_join(s->clients[i].thread_handle, NULL); - assert(err == 0); - } - } - - close(sock); - zsock_destroy(&terminate); - - return NULL; -} - -void* monitoring_thread(void *arg) { - int err; - VisionState *s = (VisionState*)arg; - - set_thread_name("monitoring"); - - TBuffer *tb = pool_get_tbuffer(&s->yuv_front_pool); - - cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err); - assert(err == 0); - - double last = 0; - while (!do_exit) { - int buf_idx = tbuffer_acquire(tb); - if (buf_idx < 0) { - break; - } - - FrameMetadata frame_data = s->yuv_front_metas[buf_idx]; - - // only process every frame - if ((frame_data.frame_id % 1) == 0) { - - double t1 = millis_since_boot(); - - MonitoringResult res = monitoring_eval_frame(&s->monitoring, q, - s->yuv_front_cl[buf_idx], s->yuv_front_width, s->yuv_front_height); - - double t2 = millis_since_boot(); - - // set front camera metering target - if (res.face_prob > 0.4) - { - int x_offset = s->rgb_front_width - 0.5 * s->rgb_front_height; - s->front_meteringbox_xmin = x_offset + (res.face_position[0] + 0.5) * (0.5 * s->rgb_front_height) - 72; - s->front_meteringbox_xmax = x_offset + (res.face_position[0] + 0.5) * (0.5 * s->rgb_front_height) + 72; - s->front_meteringbox_ymin = (res.face_position[1] + 0.5) * (s->rgb_front_height) - 72; - s->front_meteringbox_ymax = (res.face_position[1] + 0.5) * (s->rgb_front_height) + 72; - } - else // use default setting if no face - { - s->front_meteringbox_ymin = s->rgb_front_height * 1 / 3; - s->front_meteringbox_ymax = s->rgb_front_height * 1; - s->front_meteringbox_xmin = s->rgb_front_width * 3 / 5; - s->front_meteringbox_xmax = s->rgb_front_width; - } - - // send dm packet - monitoring_publish(s->monitoring_sock, frame_data.frame_id, res, ir_target_set(&s->cameras.front.cur_gain_frac, res)); - - //t2 = millis_since_boot(); - //LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); - last = t1; - } - - tbuffer_release(tb, buf_idx); - } - - return NULL; -} - -void* frontview_thread(void *arg) { - int err; - VisionState *s = (VisionState*)arg; - - set_thread_name("frontview"); - - cl_command_queue q = clCreateCommandQueue(s->context, s->device_id, 0, &err); - assert(err == 0); - - for (int cnt = 0; !do_exit; cnt++) { - int buf_idx = tbuffer_acquire(&s->cameras.front.camera_tb); - if (buf_idx < 0) { - break; - } - - int ui_idx = tbuffer_select(&s->ui_front_tb); - FrameMetadata frame_data = s->cameras.front.camera_bufs_metadata[buf_idx]; - - double t1 = millis_since_boot(); - - err = clSetKernelArg(s->krnl_debayer_front, 0, sizeof(cl_mem), &s->front_camera_bufs_cl[buf_idx]); - assert(err == 0); - err = clSetKernelArg(s->krnl_debayer_front, 1, sizeof(cl_mem), &s->rgb_front_bufs_cl[ui_idx]); - assert(err == 0); - float digital_gain = 1.0; - err = clSetKernelArg(s->krnl_debayer_front, 2, sizeof(float), &digital_gain); - assert(err == 0); - - cl_event debayer_event; - const size_t debayer_work_size = s->rgb_front_height; - const size_t debayer_local_work_size = 128; - err = clEnqueueNDRangeKernel(q, s->krnl_debayer_front, 1, NULL, - &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); - assert(err == 0); - clWaitForEvents(1, &debayer_event); - clReleaseEvent(debayer_event); - - tbuffer_release(&s->cameras.front.camera_tb, buf_idx); - - visionbuf_sync(&s->rgb_front_bufs[ui_idx], VISIONBUF_SYNC_FROM_DEVICE); - - // auto exposure - const uint8_t *bgr_front_ptr = (const uint8_t*)s->rgb_front_bufs[ui_idx].addr; -#ifndef DEBUG_DRIVER_MONITOR - if (cnt % 3 == 0) -#endif - { - // use driver face crop for AE - int x_start; - int x_end; - int y_start; - int y_end; - - if (s->front_meteringbox_xmax > 0) - { - x_start = s->front_meteringbox_xmin<0 ? 0:s->front_meteringbox_xmin; - x_end = s->front_meteringbox_xmax>=s->rgb_front_width ? s->rgb_front_width-1:s->front_meteringbox_xmax; - y_start = s->front_meteringbox_ymin<0 ? 0:s->front_meteringbox_ymin; - y_end = s->front_meteringbox_ymax>=s->rgb_front_height ? s->rgb_front_height-1:s->front_meteringbox_ymax; - } - else - { - y_start = s->rgb_front_height * 1 / 3; - y_end = s->rgb_front_height * 1; - x_start = s->rgb_front_width * 3 / 5; - x_end = s->rgb_front_width; - } - - uint32_t lum_binning[256] = {0,}; - for (int y = y_start; y < y_end; ++y) { - for (int x = x_start; x < x_end; x += 2) { // every 2nd col - const uint8_t *pix = &bgr_front_ptr[y * s->rgb_front_stride + x * 3]; - unsigned int lum = (unsigned int)pix[0] + pix[1] + pix[2]; -#ifdef DEBUG_DRIVER_MONITOR - uint8_t *pix_rw = (uint8_t *)pix; - - // set all the autoexposure pixels to pure green (pixel format is bgr) - pix_rw[0] = pix_rw[2] = 0; - pix_rw[1] = 0xff; -#endif - lum_binning[std::min(lum / 3, 255u)]++; + if (streams[stream_i].tb) { + idx = tbuffer_acquire(streams[stream_i].tbuffer); + } else { + idx = poolq_pop(streams[stream_i].queue); } - } - const unsigned int lum_total = (y_end - y_start) * (x_end - x_start)/2; - unsigned int lum_cur = 0; - int lum_med = 0; - for (lum_med=0; lum_med<256; lum_med++) { - lum_cur += lum_binning[lum_med]; - if (lum_cur >= lum_total / 2) { + if (idx < 0) { break; } + VisionPacket rep = { + .type = VIPC_STREAM_ACQUIRE, + .d = {.stream_acq = { + .type = (VisionStreamType)stream_i, + .idx = idx, + }}, + }; + if (stream_i == VISION_STREAM_YUV) { + rep.d.stream_acq.extra.frame_id = s->yuv_metas[idx].frame_id; + rep.d.stream_acq.extra.timestamp_eof = s->yuv_metas[idx].timestamp_eof; + } else if (stream_i == VISION_STREAM_YUV_FRONT) { + rep.d.stream_acq.extra.frame_id = s->yuv_front_metas[idx].frame_id; + rep.d.stream_acq.extra.timestamp_eof = s->yuv_front_metas[idx].timestamp_eof; + } + vipc_send(fd, &rep); } - camera_autoexposure(&s->cameras.front, lum_med / 256.0); } + } - // push YUV buffer - int yuv_idx = pool_select(&s->yuv_front_pool); - s->yuv_front_metas[yuv_idx] = frame_data; - - rgb_to_yuv_queue(&s->front_rgb_to_yuv_state, q, s->rgb_front_bufs_cl[ui_idx], s->yuv_front_cl[yuv_idx]); - visionbuf_sync(&s->yuv_front_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); - s->yuv_front_metas[yuv_idx] = frame_data; - - // no reference required cause we don't use this in visiond - //pool_acquire(&s->yuv_front_pool, yuv_idx); - pool_push(&s->yuv_front_pool, yuv_idx); - //pool_release(&s->yuv_front_pool, yuv_idx); - - /*FILE *f = fopen("/tmp/test2", "wb"); - printf("%d %d\n", s->rgb_front_height, s->rgb_front_stride); - fwrite(bgr_front_ptr, 1, s->rgb_front_stride * s->rgb_front_height, f); - fclose(f);*/ + LOGW("client end fd %d", fd); - tbuffer_dispatch(&s->ui_front_tb, ui_idx); + for (int i=0; iclients_lock); + client->running = false; + pthread_mutex_unlock(&s->clients_lock); return NULL; } -void* processing_thread(void *arg) { +void* visionserver_thread(void* arg) { int err; VisionState *s = (VisionState*)arg; - set_thread_name("processing"); + set_thread_name("visionserver"); - err = set_realtime_priority(1); - LOG("setpriority returns %d", err); + zsock_t *terminate = zsock_new_sub(">inproc://terminate", ""); + assert(terminate); + void* terminate_raw = zsock_resolve(terminate); - // init cl stuff - const cl_queue_properties props[] = {0}; //CL_QUEUE_PRIORITY_KHR, CL_QUEUE_PRIORITY_HIGH_KHR, 0}; - cl_command_queue q = clCreateCommandQueueWithProperties(s->context, s->device_id, props, &err); - assert(err == 0); + unlink(VIPC_SOCKET_PATH); - Context * context = Context::create(); - PubSocket * model_sock = PubSocket::create(context, "model"); + int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0); + struct sockaddr_un addr = { + .sun_family = AF_UNIX, + .sun_path = VIPC_SOCKET_PATH, + }; + err = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); + assert(err == 0); -#ifdef SEND_NET_INPUT - zsock_t *img_sock = zsock_new_pub("@tcp://*:9000"); - assert(img_sock); - void *img_sock_raw = zsock_resolve(img_sock); -#else - void *img_sock_raw = NULL; -#endif + err = listen(sock, 3); + assert(err == 0); -#ifdef DUMP_RGB - s->rgb_width = s->frame_width; - s->rgb_height = s->frame_height; - FILE *dump_rgb_file = fopen("/sdcard/dump.rgb", "wb"); -#endif + // printf("waiting\n"); - // init the net - LOG("processing start!"); + while (!do_exit) { + zmq_pollitem_t polls[2] = {{0}}; + polls[0].socket = terminate_raw; + polls[0].events = ZMQ_POLLIN; + polls[1].fd = sock; + polls[1].events = ZMQ_POLLIN; - for (int cnt = 0; !do_exit; cnt++) { - int buf_idx = tbuffer_acquire(&s->cameras.rear.camera_tb); - // int buf_idx = camera_acquire_buffer(s); - if (buf_idx < 0) { + int ret = zmq_poll(polls, ARRAYSIZE(polls), -1); + if (ret < 0) { + LOGE("poll failed (%d)", ret); break; } - - double t1 = millis_since_boot(); - - FrameMetadata frame_data = s->cameras.rear.camera_bufs_metadata[buf_idx]; - uint32_t frame_id = frame_data.frame_id; - - if (frame_id == -1) { - LOGE("no frame data? wtf"); - tbuffer_release(&s->cameras.rear.camera_tb, buf_idx); + if (polls[0].revents) { + break; + } else if (!polls[1].revents) { continue; } - int ui_idx = tbuffer_select(&s->ui_tb); - int rgb_idx = ui_idx; - - cl_event debayer_event; - if (s->cameras.rear.ci.bayer) { - err = clSetKernelArg(s->krnl_debayer_rear, 0, sizeof(cl_mem), &s->camera_bufs_cl[buf_idx]); - cl_check_error(err); - err = clSetKernelArg(s->krnl_debayer_rear, 1, sizeof(cl_mem), &s->rgb_bufs_cl[rgb_idx]); - cl_check_error(err); - err = clSetKernelArg(s->krnl_debayer_rear, 2, sizeof(float), &s->cameras.rear.digital_gain); - assert(err == 0); - - const size_t debayer_work_size = s->rgb_height; // doesn't divide evenly, is this okay? - const size_t debayer_local_work_size = 128; - err = clEnqueueNDRangeKernel(q, s->krnl_debayer_rear, 1, NULL, - &debayer_work_size, &debayer_local_work_size, 0, 0, &debayer_event); - assert(err == 0); - } else { - assert(s->rgb_buf_size >= s->frame_size); - assert(s->rgb_stride == s->frame_stride); - err = clEnqueueCopyBuffer(q, s->camera_bufs_cl[buf_idx], s->rgb_bufs_cl[rgb_idx], - 0, 0, s->rgb_buf_size, 0, 0, &debayer_event); - assert(err == 0); - } - - clWaitForEvents(1, &debayer_event); - clReleaseEvent(debayer_event); - - tbuffer_release(&s->cameras.rear.camera_tb, buf_idx); - - visionbuf_sync(&s->rgb_bufs[rgb_idx], VISIONBUF_SYNC_FROM_DEVICE); - - - double t2 = millis_since_boot(); - - uint8_t *bgr_ptr = (uint8_t*)s->rgb_bufs[rgb_idx].addr; - -#ifdef DUMP_RGB - if (cnt % 20 == 0) { - fwrite(bgr_ptr, s->rgb_buf_size, 1, dump_rgb_file); - LOG("%d x %d", s->rgb_width, s->rgb_height); - assert(1==2); - } -#endif - - double yt1 = millis_since_boot(); - - int yuv_idx = pool_select(&s->yuv_pool); - - s->yuv_metas[yuv_idx] = frame_data; - - uint8_t* yuv_ptr_y = s->yuv_bufs[yuv_idx].y; - uint8_t* yuv_ptr_u = s->yuv_bufs[yuv_idx].u; - uint8_t* yuv_ptr_v = s->yuv_bufs[yuv_idx].v; - cl_mem yuv_cl = s->yuv_cl[yuv_idx]; - rgb_to_yuv_queue(&s->rgb_to_yuv_state, q, s->rgb_bufs_cl[rgb_idx], yuv_cl); - visionbuf_sync(&s->yuv_ion[yuv_idx], VISIONBUF_SYNC_FROM_DEVICE); - - double yt2 = millis_since_boot(); - // keep another reference around till were done processing - pool_acquire(&s->yuv_pool, yuv_idx); - - pool_push(&s->yuv_pool, yuv_idx); - - pthread_mutex_lock(&s->transform_lock); - mat3 transform = s->cur_transform; - const bool run_model_this_iter = s->run_model; - pthread_mutex_unlock(&s->transform_lock); - - double mt1 = 0, mt2 = 0; - if (run_model_this_iter) { - - mat3 model_transform = matmul3(s->yuv_transform, transform); + int fd = accept(sock, NULL, NULL); + assert(fd >= 0); - mt1 = millis_since_boot(); - s->model_bufs[ui_idx] = - model_eval_frame(&s->model, q, yuv_cl, s->yuv_width, s->yuv_height, - model_transform, img_sock_raw, NULL); - mt2 = millis_since_boot(); + pthread_mutex_lock(&s->clients_lock); - model_publish(model_sock, frame_id, s->model_bufs[ui_idx], frame_data.timestamp_eof); + int client_idx = 0; + for (; client_idx < MAX_CLIENTS; client_idx++) { + if (!s->clients[client_idx].running) break; } + if (client_idx >= MAX_CLIENTS) { + LOG("ignoring visionserver connection, max clients connected"); + close(fd); - // send frame event - { - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - - auto framed = event.initFrame(); - framed.setFrameId(frame_data.frame_id); - framed.setEncodeId(cnt); - framed.setTimestampEof(frame_data.timestamp_eof); - framed.setFrameLength(frame_data.frame_length); - framed.setIntegLines(frame_data.integ_lines); - framed.setGlobalGain(frame_data.global_gain); - framed.setLensPos(frame_data.lens_pos); - framed.setLensSag(frame_data.lens_sag); - framed.setLensErr(frame_data.lens_err); - framed.setLensTruePos(frame_data.lens_true_pos); - - -#ifndef QCOM - framed.setImage(kj::arrayPtr((const uint8_t*)s->yuv_ion[yuv_idx].addr, s->yuv_buf_size)); -#endif - - kj::ArrayPtr transform_vs(&s->yuv_transform.v[0], 9); - framed.setTransform(transform_vs); - - if (s->recorder_sock != NULL) { - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - s->recorder_sock->send((char*)bytes.begin(), bytes.size()); - } + pthread_mutex_unlock(&s->clients_lock); + continue; } - // push the frame to the posenet - // TODO: This doesn't always have to run - double pt1 = 0, pt2 = 0, pt3 = 0; - pt1 = millis_since_boot(); - posenet_push(&s->posenet, yuv_ptr_y, s->yuv_width); - pt2 = millis_since_boot(); + VisionClientState *client = &s->clients[client_idx]; + client->s = s; + client->fd = fd; + client->running = true; - // posenet runs every 5 - if (cnt % 5 == 0) { - posenet_eval(&s->posenet); + err = pthread_create(&client->thread_handle, NULL, + visionserver_client_thread, client); + assert(err == 0); - // send posenet event - { - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - - auto posenetd = event.initCameraOdometry(); - kj::ArrayPtr trans_vs(&s->posenet.output[0], 3); - posenetd.setTrans(trans_vs); - kj::ArrayPtr rot_vs(&s->posenet.output[3], 3); - posenetd.setRot(rot_vs); - kj::ArrayPtr trans_std_vs(&s->posenet.output[6], 3); - posenetd.setTransStd(trans_std_vs); - kj::ArrayPtr rot_std_vs(&s->posenet.output[9], 3); - posenetd.setRotStd(rot_std_vs); - posenetd.setTimestampEof(frame_data.timestamp_eof); - posenetd.setFrameId(frame_id); + pthread_mutex_unlock(&s->clients_lock); + } - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - s->posenet_sock->send((char*)bytes.begin(), bytes.size()); - } - pt3 = millis_since_boot(); - LOGD("pre: %.2fms | posenet: %.2fms", (pt2-pt1), (pt3-pt1)); + for (int i=0; iclients_lock); + bool running = s->clients[i].running; + pthread_mutex_unlock(&s->clients_lock); + if (running) { + err = pthread_join(s->clients[i].thread_handle, NULL); + assert(err == 0); } + } - // one thumbnail per 5 seconds (instead of %5 == 0 posenet) - if (cnt % 100 == 3) { - uint8_t* thumbnail_buffer = NULL; - uint64_t thumbnail_len = 0; - - unsigned char *row = (unsigned char *)malloc(s->rgb_width/2*3); - mt1 = millis_since_boot(); - - struct jpeg_compress_struct cinfo; - struct jpeg_error_mgr jerr; + close(sock); + zsock_destroy(&terminate); - cinfo.err = jpeg_std_error(&jerr); - jpeg_create_compress(&cinfo); - jpeg_mem_dest(&cinfo, &thumbnail_buffer, &thumbnail_len); + return NULL; +} - cinfo.image_width = s->rgb_width / 2; - cinfo.image_height = s->rgb_height / 2; - cinfo.input_components = 3; - cinfo.in_color_space = JCS_RGB; - jpeg_set_defaults(&cinfo); - jpeg_set_quality(&cinfo, 50, true); - jpeg_start_compress(&cinfo, true); +////////// cl stuff - JSAMPROW row_pointer[1]; - for (int i = 0; i < s->rgb_height; i+=2) { - for (int j = 0; j < s->rgb_width*3; j+=6) { - for (int k = 0; k < 3; k++) { - uint16_t dat = 0; - dat += bgr_ptr[s->rgb_stride*i + j + k]; - dat += bgr_ptr[s->rgb_stride*i + j+3 + k]; - dat += bgr_ptr[s->rgb_stride*(i+1) + j + k]; - dat += bgr_ptr[s->rgb_stride*(i+1) + j+3 + k]; - row[(j/2) + (2-k)] = dat/4; - } - } - row_pointer[0] = row; - jpeg_write_scanlines(&cinfo, row_pointer, 1); - } - free(row); - jpeg_finish_compress(&cinfo); +cl_program build_debayer_program(VisionState *s, + int frame_width, int frame_height, int frame_stride, + int rgb_width, int rgb_height, int rgb_stride, + int bayer_flip, int hdr) { + assert(rgb_width == frame_width/2); + assert(rgb_height == frame_height/2); - mt2 = millis_since_boot(); - //printf("jpeg produced %lu bytes in %f\n", thumbnail_len, mt2-mt1); + char args[4096]; + snprintf(args, sizeof(args), + "-cl-fast-relaxed-math -cl-denorms-are-zero " + "-DFRAME_WIDTH=%d -DFRAME_HEIGHT=%d -DFRAME_STRIDE=%d " + "-DRGB_WIDTH=%d -DRGB_HEIGHT=%d -DRGB_STRIDE=%d " + "-DBAYER_FLIP=%d -DHDR=%d", + frame_width, frame_height, frame_stride, + rgb_width, rgb_height, rgb_stride, + bayer_flip, hdr); + return CLU_LOAD_FROM_FILE(s->context, s->device_id, "cameras/debayer.cl", args); +} - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); +void cl_init(VisionState *s) { + int err; + cl_platform_id platform_id = NULL; + cl_uint num_devices; + cl_uint num_platforms; - auto thumbnaild = event.initThumbnail(); - thumbnaild.setFrameId(frame_data.frame_id); - thumbnaild.setTimestampEof(frame_data.timestamp_eof); - thumbnaild.setThumbnail(kj::arrayPtr((const uint8_t*)thumbnail_buffer, thumbnail_len)); + err = clGetPlatformIDs(1, &platform_id, &num_platforms); + assert(err == 0); + err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1, + &s->device_id, &num_devices); + assert(err == 0); - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - s->thumbnail_sock->send((char*)bytes.begin(), bytes.size()); + cl_print_info(platform_id, s->device_id); + printf("\n"); - free(thumbnail_buffer); - } + s->context = clCreateContext(NULL, 1, &s->device_id, NULL, NULL, &err); + assert(err == 0); +} - tbuffer_dispatch(&s->ui_tb, ui_idx); +void cl_free(VisionState *s) { + int err; - // auto exposure over big box - const int exposure_x = 290; - const int exposure_y = 282 + 40; - const int exposure_height = 314; - const int exposure_width = 560; - if (cnt % 3 == 0) { - // find median box luminance for AE - uint32_t lum_binning[256] = {0,}; - for (int y=0; yyuv_width) + exposure_x + x]; - lum_binning[lum]++; - } - } - const unsigned int lum_total = exposure_height * exposure_width; - unsigned int lum_cur = 0; - int lum_med = 0; - for (lum_med=0; lum_med<256; lum_med++) { - // shouldn't be any values less than 16 - yuv footroom - lum_cur += lum_binning[lum_med]; - if (lum_cur >= lum_total / 2) { - break; - } - } - // double avg = (double)acc / (big_box_width * big_box_height) - 16; - // printf("avg %d\n", lum_med); + err = clReleaseContext(s->context); + assert(err == 0); +} - camera_autoexposure(&s->cameras.rear, lum_med / 256.0); - } +void init_buffers(VisionState *s) { + int err; - pool_release(&s->yuv_pool, yuv_idx); + // allocate camera buffers - // if (cnt%40 == 0) { - // FILE* of = fopen("/sdcard/tmp.yuv", "wb"); - // fwrite(transformed_ptr_y, 1, s->transformed_width*s->transformed_height, of); - // fwrite(transformed_ptr_u, 1, (s->transformed_width/2)*(s->transformed_height/2), of); - // fwrite(transformed_ptr_v, 1, (s->transformed_width/2)*(s->transformed_height/2), of); - // fclose(of); - // } + for (int i=0; icamera_bufs[i] = visionbuf_allocate_cl(s->frame_size, s->device_id, s->context, + &s->camera_bufs_cl[i]); + // TODO: make lengths correct + s->focus_bufs[i] = visionbuf_allocate(0xb80); + s->stats_bufs[i] = visionbuf_allocate(0xb80); + } - double t5 = millis_since_boot(); + for (int i=0; ifront_camera_bufs[i] = visionbuf_allocate_cl(s->cameras.front.frame_size, + s->device_id, s->context, + &s->front_camera_bufs_cl[i]); + } - LOGD("queued: %.2fms, yuv: %.2f, model: %.2fms | processing: %.3fms", - (t2-t1), (yt2-yt1), (mt2-mt1), (t5-t1)); + // processing buffers + if (s->cameras.rear.ci.bayer) { + s->rgb_width = s->frame_width/2; + s->rgb_height = s->frame_height/2; + } else { + s->rgb_width = s->frame_width; + s->rgb_height = s->frame_height; } -#ifdef DUMP_RGB - fclose(dump_rgb_file); -#endif + for (int i=0; irgb_width, s->rgb_height, &s->rgb_bufs[i]); + s->rgb_bufs_cl[i] = visionbuf_to_cl(&s->rgb_bufs[i], s->device_id, s->context); + if (i == 0){ + s->rgb_stride = img.stride; + s->rgb_buf_size = img.size; + } + } + tbuffer_init(&s->ui_tb, UI_BUF_COUNT, "rgb"); - delete model_sock; - delete context; + //assert(s->cameras.front.ci.bayer); + s->rgb_front_width = s->cameras.front.ci.frame_width/2; + s->rgb_front_height = s->cameras.front.ci.frame_height/2; - return NULL; -} + for (int i=0; irgb_front_width, s->rgb_front_height, &s->rgb_front_bufs[i]); + s->rgb_front_bufs_cl[i] = visionbuf_to_cl(&s->rgb_front_bufs[i], s->device_id, s->context); + if (i == 0){ + s->rgb_front_stride = img.stride; + s->rgb_front_buf_size = img.size; + } + } + tbuffer_init(&s->ui_front_tb, UI_BUF_COUNT, "frontrgb"); -void* live_thread(void *arg) { - int err; - VisionState *s = (VisionState*)arg; + // yuv back for recording and orbd + pool_init(&s->yuv_pool, YUV_COUNT); + s->yuv_tb = pool_get_tbuffer(&s->yuv_pool); //only for visionserver... - set_thread_name("live"); - - Context * c = Context::create(); - SubSocket * live_calibration_sock = SubSocket::create(c, "liveCalibration"); - Poller * poller = Poller::create({live_calibration_sock}); - - /* - import numpy as np - from common.transformations.model import medmodel_frame_from_road_frame - medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)] - ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground) - */ - Eigen::Matrix ground_from_medmodel_frame; - ground_from_medmodel_frame << - 0.00000000e+00, 0.00000000e+00, 1.00000000e+00, - -1.09890110e-03, 0.00000000e+00, 2.81318681e-01, - -1.84808520e-20, 9.00738606e-04,-4.28751576e-02; - - Eigen::Matrix eon_intrinsics; - eon_intrinsics << - 910.0, 0.0, 582.0, - 0.0, 910.0, 437.0, - 0.0, 0.0, 1.0; + s->yuv_width = s->rgb_width; + s->yuv_height = s->rgb_height; + s->yuv_buf_size = s->rgb_width * s->rgb_height * 3 / 2; - while (!do_exit) { - for (auto sock : poller->poll(10)){ - Message * msg = sock->receive(); + for (int i=0; iyuv_ion[i] = visionbuf_allocate_cl(s->yuv_buf_size, s->device_id, s->context, &s->yuv_cl[i]); + s->yuv_bufs[i].y = (uint8_t*)s->yuv_ion[i].addr; + s->yuv_bufs[i].u = s->yuv_bufs[i].y + (s->yuv_width * s->yuv_height); + s->yuv_bufs[i].v = s->yuv_bufs[i].u + (s->yuv_width/2 * s->yuv_height/2); + } - auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); - memcpy(amsg.begin(), msg->getData(), msg->getSize()); + // yuv front for recording + pool_init(&s->yuv_front_pool, YUV_COUNT); + s->yuv_front_tb = pool_get_tbuffer(&s->yuv_front_pool); - capnp::FlatArrayMessageReader cmsg(amsg); - cereal::Event::Reader event = cmsg.getRoot(); + s->yuv_front_width = s->rgb_front_width; + s->yuv_front_height = s->rgb_front_height; + s->yuv_front_buf_size = s->rgb_front_width * s->rgb_front_height * 3 / 2; - if (event.isLiveCalibration()) { - pthread_mutex_lock(&s->transform_lock); + for (int i=0; iyuv_front_ion[i] = visionbuf_allocate_cl(s->yuv_front_buf_size, s->device_id, s->context, &s->yuv_front_cl[i]); + s->yuv_front_bufs[i].y = (uint8_t*)s->yuv_front_ion[i].addr; + s->yuv_front_bufs[i].u = s->yuv_front_bufs[i].y + (s->yuv_front_width * s->yuv_front_height); + s->yuv_front_bufs[i].v = s->yuv_front_bufs[i].u + (s->yuv_front_width/2 * s->yuv_front_height/2); + } - auto extrinsic_matrix = event.getLiveCalibration().getExtrinsicMatrix(); - Eigen::Matrix extrinsic_matrix_eigen; - for (int i = 0; i < 4*3; i++){ - extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i]; - } + if (s->cameras.rear.ci.bayer) { + // debayering does a 2x downscale + s->yuv_transform = transform_scale_buffer(s->cameras.rear.transform, 0.5); + } else { + s->yuv_transform = s->cameras.rear.transform; + } - auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen; - Eigen::Matrix camera_frame_from_ground; - camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0); - camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1); - camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3); + if (s->cameras.rear.ci.bayer) { + s->prg_debayer_rear = build_debayer_program(s, s->cameras.rear.ci.frame_width, s->cameras.rear.ci.frame_height, + s->cameras.rear.ci.frame_stride, + s->rgb_width, s->rgb_height, s->rgb_stride, + s->cameras.rear.ci.bayer_flip, s->cameras.rear.ci.hdr); + s->krnl_debayer_rear = clCreateKernel(s->prg_debayer_rear, "debayer10", &err); + assert(err == 0); + } - auto warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame; + if (s->cameras.front.ci.bayer) { + s->prg_debayer_front = build_debayer_program(s, s->cameras.front.ci.frame_width, s->cameras.front.ci.frame_height, + s->cameras.front.ci.frame_stride, + s->rgb_front_width, s->rgb_front_height, s->rgb_front_stride, + s->cameras.front.ci.bayer_flip, s->cameras.front.ci.hdr); - for (int i=0; i<3*3; i++) { - s->cur_transform.v[i] = warp_matrix(i / 3, i % 3); - } + s->krnl_debayer_front = clCreateKernel(s->prg_debayer_front, "debayer10", &err); + assert(err == 0); + } - s->run_model = true; - pthread_mutex_unlock(&s->transform_lock); - } + rgb_to_yuv_init(&s->rgb_to_yuv_state, s->context, s->device_id, s->yuv_width, s->yuv_height, s->rgb_stride); + rgb_to_yuv_init(&s->front_rgb_to_yuv_state, s->context, s->device_id, s->yuv_front_width, s->yuv_front_height, s->rgb_front_stride); +} - delete msg; - } +void free_buffers(VisionState *s) { + // free bufs + for (int i=0; icamera_bufs[i]); + visionbuf_free(&s->focus_bufs[i]); + visionbuf_free(&s->stats_bufs[i]); + } + for (int i=0; ifront_camera_bufs[i]); } + for (int i=0; irgb_bufs[i]); + } - return NULL; -} + for (int i=0; irgb_front_bufs[i]); + } -void set_do_exit(int sig) { - do_exit = 1; + for (int i=0; iyuv_ion[i]); + } } void party(VisionState *s) { @@ -1286,33 +1044,20 @@ void party(VisionState *s) { s->terminate_pub = zsock_new_pub("@inproc://terminate"); assert(s->terminate_pub); -#ifndef __APPLE__ pthread_t visionserver_thread_handle; err = pthread_create(&visionserver_thread_handle, NULL, visionserver_thread, s); assert(err == 0); -#endif pthread_t proc_thread_handle; err = pthread_create(&proc_thread_handle, NULL, processing_thread, s); assert(err == 0); -#ifdef QCOM pthread_t frontview_thread_handle; err = pthread_create(&frontview_thread_handle, NULL, frontview_thread, s); assert(err == 0); -#endif - - pthread_t monitoring_thread_handle; - err = pthread_create(&monitoring_thread_handle, NULL, monitoring_thread, s); - assert(err == 0); - - pthread_t live_thread_handle; - err = pthread_create(&live_thread_handle, NULL, - live_thread, s); - assert(err == 0); // priority for cameras err = set_realtime_priority(1); @@ -1327,59 +1072,35 @@ void party(VisionState *s) { zsock_signal(s->terminate_pub, 0); -#ifdef QCOM LOG("joining frontview_thread"); err = pthread_join(frontview_thread_handle, NULL); assert(err == 0); -#endif -#ifndef __APPLE__ LOG("joining visionserver_thread"); err = pthread_join(visionserver_thread_handle, NULL); assert(err == 0); -#endif LOG("joining proc_thread"); err = pthread_join(proc_thread_handle, NULL); assert(err == 0); - LOG("joining live_thread"); - err = pthread_join(live_thread_handle, NULL); - assert(err == 0); - zsock_destroy (&s->terminate_pub); } -} - -int main(int argc, char **argv) { +int main(int argc, char *argv[]) { int err; + set_realtime_priority(1); zsys_handler_set(NULL); signal(SIGINT, (sighandler_t)set_do_exit); signal(SIGTERM, (sighandler_t)set_do_exit); - // boringssl via curl via the calibration api can sometimes - // try to write to a closed socket. just ignore SIGPIPE - signal(SIGPIPE, SIG_IGN); - - bool test_run = false; - if (argc > 1 && strcmp(argv[1], "-t") == 0) { - // immediately tear everything down. useful for caching opencl - test_run = true; - } - VisionState state = {0}; VisionState *s = &state; clu_init(); cl_init(s); - model_init(&s->model, s->device_id, s->context, true); - monitoring_init(&s->monitoring, s->device_id, s->context); - posenet_init(&s->posenet); - - cameras_init(&s->cameras); s->frame_width = s->cameras.rear.ci.frame_width; @@ -1387,42 +1108,29 @@ int main(int argc, char **argv) { s->frame_stride = s->cameras.rear.ci.frame_stride; s->frame_size = s->cameras.rear.frame_size; - // Do not run the model until we receive valid calibration. - s->run_model = false; - pthread_mutex_init(&s->transform_lock, NULL); - init_buffers(s); +#ifdef QCOM s->msg_context = Context::create(); - - #ifdef QCOM - s->recorder_sock = PubSocket::create(s->msg_context, "frame"); - #endif - - s->monitoring_sock = PubSocket::create(s->msg_context, "driverMonitoring"); - s->posenet_sock = PubSocket::create(s->msg_context, "cameraOdometry"); + s->frame_sock = PubSocket::create(s->msg_context, "frame"); + s->front_frame_sock = PubSocket::create(s->msg_context, "frontFrame"); s->thumbnail_sock = PubSocket::create(s->msg_context, "thumbnail"); - + assert(s->frame_sock != NULL); + assert(s->front_frame_sock != NULL); + assert(s->thumbnail_sock != NULL); +#endif cameras_open(&s->cameras, &s->camera_bufs[0], &s->focus_bufs[0], &s->stats_bufs[0], &s->front_camera_bufs[0]); - if (test_run) { - do_exit = true; - } party(s); - - delete s->recorder_sock; - delete s->monitoring_sock; - delete s->posenet_sock; +#ifdef QCOM + delete s->frame_sock; + delete s->front_frame_sock; delete s->thumbnail_sock; delete s->msg_context; +#endif - model_free(&s->model); - monitoring_free(&s->monitoring); free_buffers(s); - cl_free(s); - - return 0; } diff --git a/selfdrive/can/__init__.py b/selfdrive/camerad/snapshot/__init__.py similarity index 100% rename from selfdrive/can/__init__.py rename to selfdrive/camerad/snapshot/__init__.py diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py new file mode 100755 index 0000000000..f6a9ffe9fb --- /dev/null +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -0,0 +1,72 @@ +#!/usr/bin/env python3 +import os +import json +import signal +import subprocess +import time +from PIL import Image +from common.basedir import BASEDIR +from common.params import Params +from selfdrive.camerad.snapshot.visionipc import VisionIPC + +with open(BASEDIR + "/selfdrive/controls/lib/alerts_offroad.json") as json_file: + OFFROAD_ALERTS = json.load(json_file) + + +def jpeg_write(fn, dat): + img = Image.fromarray(dat) + img.save(fn, "JPEG") + + +def snapshot(): + params = Params() + front_camera_allowed = int(params.get("RecordFront")) + + params.put("IsTakingSnapshot", "1") + params.put("Offroad_IsTakingSnapshot", json.dumps(OFFROAD_ALERTS["Offroad_IsTakingSnapshot"])) + time.sleep(2.0) # Give thermald time to read the param, or if just started give camerad time to start + + # Check if camerad is already started + ps = subprocess.Popen("ps | grep camerad", shell=True, stdout=subprocess.PIPE) + ret = list(filter(lambda x: 'grep ' not in x, ps.communicate()[0].decode('utf-8').strip().split("\n"))) + if len(ret) > 0: + params.put("IsTakingSnapshot", "0") + params.delete("Offroad_IsTakingSnapshot") + return None + + proc = subprocess.Popen(os.path.join(BASEDIR, "selfdrive/camerad/camerad"), cwd=os.path.join(BASEDIR, "selfdrive/camerad")) + time.sleep(1.0) + + ret = None + start_time = time.time() + while time.time() - start_time < 5.0: + try: + ipc = VisionIPC() + pic = ipc.get() + del ipc + + if front_camera_allowed: + ipc_front = VisionIPC(front=True) + fpic = ipc_front.get() + del ipc_front + else: + fpic = None + + ret = pic, fpic + break + except Exception: + time.sleep(1) + + proc.send_signal(signal.SIGINT) + proc.communicate() + + params.put("IsTakingSnapshot", "0") + params.delete("Offroad_IsTakingSnapshot") + return ret + + +if __name__ == "__main__": + pic, fpic = snapshot() + print(pic.shape) + jpeg_write("/tmp/back.jpg", pic) + jpeg_write("/tmp/front.jpg", fpic) diff --git a/selfdrive/visiond/snapshot/visionipc.py b/selfdrive/camerad/snapshot/visionipc.py similarity index 90% rename from selfdrive/visiond/snapshot/visionipc.py rename to selfdrive/camerad/snapshot/visionipc.py index 1129025081..816db41208 100644 --- a/selfdrive/visiond/snapshot/visionipc.py +++ b/selfdrive/camerad/snapshot/visionipc.py @@ -1,14 +1,11 @@ #!/usr/bin/env python3 import os -import subprocess from cffi import FFI import numpy as np gf_dir = os.path.dirname(os.path.abspath(__file__)) -subprocess.check_call(["make"], cwd=gf_dir) - ffi = FFI() ffi.cdef(""" @@ -68,6 +65,11 @@ void visionstream_destroy(VisionStream *s); """ ) + +class VisionIPCError(Exception): + pass + + class VisionIPC(): def __init__(self, front=False): self.clib = ffi.dlopen(os.path.join(gf_dir, "libvisionipc.so")) @@ -76,11 +78,16 @@ class VisionIPC(): self.buf_info = ffi.new("VisionStreamBufs*") err = self.clib.visionstream_init(self.s, self.clib.VISION_STREAM_RGB_FRONT if front else self.clib.VISION_STREAM_RGB_BACK, True, self.buf_info) - assert err == 0 + + if err != 0: + self.clib.visionstream_destroy(self.s) + raise VisionIPCError + + def __del__(self): + self.clib.visionstream_destroy(self.s) def get(self): buf = self.clib.visionstream_get(self.s, ffi.NULL) pbuf = ffi.buffer(buf.addr, buf.len) ret = np.frombuffer(pbuf, dtype=np.uint8).reshape((-1, self.buf_info.stride//3, 3)) return ret[:self.buf_info.height, :self.buf_info.width, [2,1,0]] - diff --git a/selfdrive/visiond/transforms/rgb_to_yuv.c b/selfdrive/camerad/transforms/rgb_to_yuv.c similarity index 100% rename from selfdrive/visiond/transforms/rgb_to_yuv.c rename to selfdrive/camerad/transforms/rgb_to_yuv.c diff --git a/selfdrive/visiond/transforms/rgb_to_yuv.cl b/selfdrive/camerad/transforms/rgb_to_yuv.cl similarity index 100% rename from selfdrive/visiond/transforms/rgb_to_yuv.cl rename to selfdrive/camerad/transforms/rgb_to_yuv.cl diff --git a/selfdrive/visiond/transforms/rgb_to_yuv.h b/selfdrive/camerad/transforms/rgb_to_yuv.h similarity index 100% rename from selfdrive/visiond/transforms/rgb_to_yuv.h rename to selfdrive/camerad/transforms/rgb_to_yuv.h diff --git a/selfdrive/visiond/transforms/rgb_to_yuv_test.cc b/selfdrive/camerad/transforms/rgb_to_yuv_test.cc similarity index 100% rename from selfdrive/visiond/transforms/rgb_to_yuv_test.cc rename to selfdrive/camerad/transforms/rgb_to_yuv_test.cc diff --git a/selfdrive/can/Makefile b/selfdrive/can/Makefile deleted file mode 100644 index cc1175b8c1..0000000000 --- a/selfdrive/can/Makefile +++ /dev/null @@ -1,97 +0,0 @@ -CC = clang -CXX = clang++ - -BASEDIR = ../.. -PHONELIBS := ../../phonelibs - -UNAME_S := $(shell uname -s) -UNAME_M := $(shell uname -m) - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args \ - -Wno-deprecated-declarations - -CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) -LDFLAGS = - -ifeq ($(ARCH),aarch64) -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 -endif - -OBJDIR = obj - -OPENDBC_PATH := $(shell python3 -c 'import opendbc; print(opendbc.DBC_PATH)') - -DBC_SOURCES := $(sort $(wildcard $(OPENDBC_PATH)/*.dbc)) -DBC_OBJS := $(patsubst $(OPENDBC_PATH)/%.dbc,$(OBJDIR)/%.o,$(DBC_SOURCES)) -DBC_CCS := $(patsubst $(OPENDBC_PATH)/%.dbc,dbc_out/%.cc,$(DBC_SOURCES)) -.SECONDARY: $(DBC_CCS) - -LIBDBC_OBJS := $(OBJDIR)/dbc.o $(OBJDIR)/parser.o $(OBJDIR)/packer.o $(OBJDIR)/common.o - -CWD := $(shell pwd) - -.PHONY: all -all: $(OBJDIR) libdbc.so parser_pyx.so - -include ../common/cereal.mk - -# make sure cereal is built -libdbc.so:: ../../cereal/gen/cpp/log.capnp.h - -../../cereal/gen/cpp/log.capnp.h: - cd ../../cereal && make - -libdbc.so:: $(LIBDBC_OBJS) $(DBC_OBJS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -shared -o '$@' $^ \ - -I. -I../.. \ - $(CXXFLAGS) \ - $(LDFLAGS) \ - $(CEREAL_CXXFLAGS) \ - $(CEREAL_LIBS) - -packer_impl.so: packer_impl.pyx packer_setup.py - python3 packer_setup.py build_ext --inplace - rm -rf build - rm -f packer_impl.cpp - -parser_pyx.so: libdbc.so parser_pyx_setup.py parser_pyx.pyx common.pxd - python3 parser_pyx_setup.py build_ext --inplace - rm -rf build - rm -f parser_pyx.cpp - -$(OBJDIR)/%.o: %.cc - @echo "[ CXX ] $@" - $(CXX) -fPIC -c -o '$@' $^ \ - -I. -I../.. \ - $(CXXFLAGS) \ - $(CEREAL_CXXFLAGS) \ - -$(OBJDIR)/%.o: dbc_out/%.cc - @echo "[ CXX ] $@" - $(CXX) -fPIC -c -o '$@' $^ \ - -I. -I../.. \ - $(CXXFLAGS) \ - $(CEREAL_CXXFLAGS) \ - -dbc_out/%.cc: process_dbc.py dbc_template.cc $(OPENDBC_PATH)/%.dbc - @echo "[ DBC GEN ] $@" - ./process_dbc.py $(OPENDBC_PATH) '$@' - -$(OBJDIR): - mkdir -p $@ - -.PHONY: clean $(OBJDIR) -clean: - rm -rf libdbc.so* - rm -f dbc_out/*.cc - rm -f dbcs.txt - rm -f dbcs.csv - rm -f *.so - rm -rf $(OBJDIR)/* diff --git a/selfdrive/can/can_define.py b/selfdrive/can/can_define.py deleted file mode 100644 index eb708869f0..0000000000 --- a/selfdrive/can/can_define.py +++ /dev/null @@ -1,39 +0,0 @@ -from collections import defaultdict -from selfdrive.can.libdbc_py import libdbc, ffi - -class CANDefine(): - def __init__(self, dbc_name): - self.dv = defaultdict(dict) - self.dbc_name = dbc_name - self.dbc = libdbc.dbc_lookup(dbc_name.encode('utf8')) - - num_vals = self.dbc[0].num_vals - - self.address_to_msg_name = {} - num_msgs = self.dbc[0].num_msgs - for i in range(num_msgs): - msg = self.dbc[0].msgs[i] - name = ffi.string(msg.name).decode('utf8') - address = msg.address - self.address_to_msg_name[address] = name - - for i in range(num_vals): - val = self.dbc[0].vals[i] - - sgname = ffi.string(val.name).decode('utf8') - address = val.address - def_val = ffi.string(val.def_val).decode('utf8') - - #separate definition/value pairs - def_val = def_val.split() - values = [int(v) for v in def_val[::2]] - defs = def_val[1::2] - - if address not in self.dv: - self.dv[address] = {} - msgname = self.address_to_msg_name[address] - self.dv[msgname] = {} - - # two ways to lookup: address or msg name - self.dv[address][sgname] = dict(zip(values, defs)) - self.dv[msgname][sgname] = self.dv[address][sgname] diff --git a/selfdrive/can/common.cc b/selfdrive/can/common.cc deleted file mode 100644 index 5a131eb6c7..0000000000 --- a/selfdrive/can/common.cc +++ /dev/null @@ -1,165 +0,0 @@ -#include "common.h" - -unsigned int honda_checksum(unsigned int address, uint64_t d, int l) { - d >>= ((8-l)*8); // remove padding - d >>= 4; // remove checksum - - int s = 0; - while (address) { s += (address & 0xF); address >>= 4; } - while (d) { s += (d & 0xF); d >>= 4; } - s = 8-s; - s &= 0xF; - - return s; -} - -unsigned int toyota_checksum(unsigned int address, uint64_t d, int l) { - d >>= ((8-l)*8); // remove padding - d >>= 8; // remove checksum - - unsigned int s = l; - while (address) { s += address & 0xff; address >>= 8; } - while (d) { s += d & 0xff; d >>= 8; } - - return s & 0xFF; -} - -// Static lookup table for fast computation of CRC8 poly 0x2F, aka 8H2F/AUTOSAR -uint8_t crc8_lut_8h2f[256]; - -void gen_crc_lookup_table(uint8_t poly, uint8_t crc_lut[]) { - uint8_t crc; - int i, j; - - for (i = 0; i < 256; i++) { - crc = i; - for (j = 0; j < 8; j++) { - if ((crc & 0x80) != 0) - crc = (uint8_t)((crc << 1) ^ poly); - else - crc <<= 1; - } - crc_lut[i] = crc; - } -} - -void init_crc_lookup_tables() { - // At init time, set up static lookup tables for fast CRC computation. - - gen_crc_lookup_table(0x2F, crc8_lut_8h2f); // CRC-8 8H2F/AUTOSAR for Volkswagen -} - -unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l) { - // Volkswagen uses standard CRC8 8H2F/AUTOSAR, but they compute it with - // a magic variable padding byte tacked onto the end of the payload. - // https://www.autosar.org/fileadmin/user_upload/standards/classic/4-3/AUTOSAR_SWS_CRCLibrary.pdf - - uint8_t *dat = (uint8_t *)&d; - uint8_t crc = 0xFF; // Standard init value for CRC8 8H2F/AUTOSAR - - // CRC the payload first, skipping over the first byte where the CRC lives. - for (int i = 1; i < l; i++) { - crc ^= dat[i]; - crc = crc8_lut_8h2f[crc]; - } - - // Look up and apply the magic final CRC padding byte, which permutes by CAN - // address, and additionally (for SOME addresses) by the message counter. - uint8_t counter = dat[1] & 0x0F; - switch(address) { - case 0x86: // LWI_01 Steering Angle - crc ^= (uint8_t[]){0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86,0x86}[counter]; - break; - case 0x9F: // EPS_01 Electric Power Steering - crc ^= (uint8_t[]){0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5,0xF5}[counter]; - break; - case 0xAD: // Getriebe_11 Automatic Gearbox - crc ^= (uint8_t[]){0x3F,0x69,0x39,0xDC,0x94,0xF9,0x14,0x64,0xD8,0x6A,0x34,0xCE,0xA2,0x55,0xB5,0x2C}[counter]; - break; - case 0xFD: // ESP_21 Electronic Stability Program - crc ^= (uint8_t[]){0xB4,0xEF,0xF8,0x49,0x1E,0xE5,0xC2,0xC0,0x97,0x19,0x3C,0xC9,0xF1,0x98,0xD6,0x61}[counter]; - break; - case 0x106: // ESP_05 Electronic Stability Program - crc ^= (uint8_t[]){0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07,0x07}[counter]; - break; - case 0x117: // ACC_10 Automatic Cruise Control - crc ^= (uint8_t[]){0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC,0xAC}[counter]; - break; - case 0x122: // ACC_06 Automatic Cruise Control - crc ^= (uint8_t[]){0x37,0x7D,0xF3,0xA9,0x18,0x46,0x6D,0x4D,0x3D,0x71,0x92,0x9C,0xE5,0x32,0x10,0xB9}[counter]; - break; - case 0x126: // HCA_01 Heading Control Assist - crc ^= (uint8_t[]){0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA,0xDA}[counter]; - break; - case 0x12B: // GRA_ACC_01 Steering wheel controls for ACC - crc ^= (uint8_t[]){0x6A,0x38,0xB4,0x27,0x22,0xEF,0xE1,0xBB,0xF8,0x80,0x84,0x49,0xC7,0x9E,0x1E,0x2B}[counter]; - break; - case 0x187: // EV_Gearshift "Gear" selection data for EVs with no gearbox - crc ^= (uint8_t[]){0x7F,0xED,0x17,0xC2,0x7C,0xEB,0x44,0x21,0x01,0xFA,0xDB,0x15,0x4A,0x6B,0x23,0x05}[counter]; - break; - case 0x30C: // ACC_02 Automatic Cruise Control - crc ^= (uint8_t[]){0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F,0x0F}[counter]; - break; - case 0x3C0: // Klemmen_Status_01 ignition and starting status - crc ^= (uint8_t[]){0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3,0xC3}[counter]; - break; - case 0x65D: // ESP_20 Electronic Stability Program - crc ^= (uint8_t[]){0xAC,0xB3,0xAB,0xEB,0x7A,0xE1,0x3B,0xF7,0x73,0xBA,0x7C,0x9E,0x06,0x5F,0x02,0xD9}[counter]; - break; - default: // As-yet undefined CAN message, CRC check expected to fail - printf("Attempt to CRC check undefined Volkswagen message 0x%02X\n", address); - crc ^= (uint8_t[]){0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00}[counter]; - break; - } - crc = crc8_lut_8h2f[crc]; - - return crc ^ 0xFF; // Return after standard final XOR for CRC8 8H2F/AUTOSAR -} - - -unsigned int pedal_checksum(uint64_t d, int l) { - uint8_t crc = 0xFF; - uint8_t poly = 0xD5; // standard crc8 - - d >>= ((8-l)*8); // remove padding - d >>= 8; // remove checksum - - uint8_t *dat = (uint8_t *)&d; - - int i, j; - for (i = 0; i < l - 1; i++) { - crc ^= dat[i]; - for (j = 0; j < 8; j++) { - if ((crc & 0x80) != 0) { - crc = (uint8_t)((crc << 1) ^ poly); - } - else { - crc <<= 1; - } - } - } - return crc; -} - - -uint64_t read_u64_be(const uint8_t* v) { - return (((uint64_t)v[0] << 56) - | ((uint64_t)v[1] << 48) - | ((uint64_t)v[2] << 40) - | ((uint64_t)v[3] << 32) - | ((uint64_t)v[4] << 24) - | ((uint64_t)v[5] << 16) - | ((uint64_t)v[6] << 8) - | (uint64_t)v[7]); -} - -uint64_t read_u64_le(const uint8_t* v) { - return ((uint64_t)v[0] - | ((uint64_t)v[1] << 8) - | ((uint64_t)v[2] << 16) - | ((uint64_t)v[3] << 24) - | ((uint64_t)v[4] << 32) - | ((uint64_t)v[5] << 40) - | ((uint64_t)v[6] << 48) - | ((uint64_t)v[7] << 56)); -} diff --git a/selfdrive/can/common.h b/selfdrive/can/common.h deleted file mode 100644 index a496c3918b..0000000000 --- a/selfdrive/can/common.h +++ /dev/null @@ -1,59 +0,0 @@ -#pragma once - -#include -#include - -#include "common_dbc.h" -#include -#include "cereal/gen/cpp/log.capnp.h" - -#define MAX_BAD_COUNTER 5 - -// Helper functions -unsigned int honda_checksum(unsigned int address, uint64_t d, int l); -unsigned int toyota_checksum(unsigned int address, uint64_t d, int l); -void init_crc_lookup_tables(); -unsigned int volkswagen_crc(unsigned int address, uint64_t d, int l); -unsigned int pedal_checksum(uint64_t d, int l); -uint64_t read_u64_be(const uint8_t* v); -uint64_t read_u64_le(const uint8_t* v); - -class MessageState { -public: - uint32_t address; - unsigned int size; - - std::vector parse_sigs; - std::vector vals; - - uint16_t ts; - uint64_t seen; - uint64_t check_threshold; - - uint8_t counter; - uint8_t counter_fail; - - bool parse(uint64_t sec, uint16_t ts_, uint8_t * dat); - bool update_counter_generic(int64_t v, int cnt_size); -}; - -class CANParser { -private: - const int bus; - - const DBC *dbc = NULL; - std::unordered_map message_states; - -public: - bool can_valid = false; - uint64_t last_sec = 0; - - CANParser(int abus, const std::string& dbc_name, - const std::vector &options, - const std::vector &sigoptions); - void UpdateCans(uint64_t sec, const capnp::List::Reader& cans); - void UpdateValid(uint64_t sec); - void update_string(std::string data); - std::vector query_latest(); - -}; diff --git a/selfdrive/can/common.pxd b/selfdrive/can/common.pxd deleted file mode 100644 index d26f8da46f..0000000000 --- a/selfdrive/can/common.pxd +++ /dev/null @@ -1,73 +0,0 @@ -# distutils: language = c++ -#cython: language_level=3 - -from libc.stdint cimport uint32_t, uint64_t, uint16_t -from libcpp.vector cimport vector -from libcpp.map cimport map -from libcpp.string cimport string -from libcpp.unordered_set cimport unordered_set -from libcpp cimport bool - - -cdef extern from "common.h": - - ctypedef enum SignalType: - DEFAULT, - HONDA_CHECKSUM, - HONDA_COUNTER, - TOYOTA_CHECKSUM, - PEDAL_CHECKSUM, - PEDAL_COUNTER, - VOLKSWAGEN_CHECKSUM, - VOLKSWAGEN_COUNTER - - cdef struct Signal: - const char* name - int b1, b2, bo - bool is_signed - double factor, offset - SignalType type - - cdef struct Msg: - const char* name - uint32_t address - unsigned int size - size_t num_sigs - const Signal *sigs - - cdef struct Val: - const char* name - uint32_t address - const char* def_val - const Signal *sigs - - cdef struct DBC: - const char* name - size_t num_msgs - const Msg *msgs - const Val *vals - size_t num_vals - - cdef struct SignalParseOptions: - uint32_t address - const char* name - double default_value - - - cdef struct MessageParseOptions: - uint32_t address - int check_frequency - - cdef struct SignalValue: - uint32_t address - uint16_t ts - const char* name - double value - - cdef const DBC* dbc_lookup(const string); - - cdef cppclass CANParser: - bool can_valid - CANParser(int, string, vector[MessageParseOptions], vector[SignalParseOptions]) - void update_string(string) - vector[SignalValue] query_latest() diff --git a/selfdrive/can/common_dbc.h b/selfdrive/can/common_dbc.h deleted file mode 100644 index ae6f443ec9..0000000000 --- a/selfdrive/can/common_dbc.h +++ /dev/null @@ -1,82 +0,0 @@ -#pragma once - -#include -#include -#include - -#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0])) - -struct SignalPackValue { - const char* name; - double value; -}; - -struct SignalParseOptions { - uint32_t address; - const char* name; - double default_value; -}; - -struct MessageParseOptions { - uint32_t address; - int check_frequency; -}; - -struct SignalValue { - uint32_t address; - uint16_t ts; - const char* name; - double value; -}; - -enum SignalType { - DEFAULT, - HONDA_CHECKSUM, - HONDA_COUNTER, - TOYOTA_CHECKSUM, - PEDAL_CHECKSUM, - PEDAL_COUNTER, - VOLKSWAGEN_CHECKSUM, - VOLKSWAGEN_COUNTER, -}; - -struct Signal { - const char* name; - int b1, b2, bo; - bool is_signed; - double factor, offset; - bool is_little_endian; - SignalType type; -}; - -struct Msg { - const char* name; - uint32_t address; - unsigned int size; - size_t num_sigs; - const Signal *sigs; -}; - -struct Val { - const char* name; - uint32_t address; - const char* def_val; - const Signal *sigs; -}; - -struct DBC { - const char* name; - size_t num_msgs; - const Msg *msgs; - const Val *vals; - size_t num_vals; -}; - -const DBC* dbc_lookup(const std::string& dbc_name); - -void dbc_register(const DBC* dbc); - -#define dbc_init(dbc) \ -static void __attribute__((constructor)) do_dbc_init_ ## dbc(void) { \ - dbc_register(&dbc); \ -} diff --git a/selfdrive/can/dbc.cc b/selfdrive/can/dbc.cc deleted file mode 100644 index 6587de7fe0..0000000000 --- a/selfdrive/can/dbc.cc +++ /dev/null @@ -1,31 +0,0 @@ -#include - -#include "common_dbc.h" - -namespace { - -std::vector& get_dbcs() { - static std::vector vec; - return vec; -} - -} - -const DBC* dbc_lookup(const std::string& dbc_name) { - for (const auto& dbci : get_dbcs()) { - if (dbc_name == dbci->name) { - return dbci; - } - } - return NULL; -} - -void dbc_register(const DBC* dbc) { - get_dbcs().push_back(dbc); -} - -extern "C" { - const DBC* dbc_lookup(const char* dbc_name) { - return dbc_lookup(std::string(dbc_name)); - } -} diff --git a/selfdrive/can/dbc_out/.gitignore b/selfdrive/can/dbc_out/.gitignore deleted file mode 100644 index 4625581a85..0000000000 --- a/selfdrive/can/dbc_out/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -*.cc - diff --git a/selfdrive/can/dbc_out/.gitkeep b/selfdrive/can/dbc_out/.gitkeep deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/selfdrive/can/dbc_template.cc b/selfdrive/can/dbc_template.cc deleted file mode 100644 index f9540fcee9..0000000000 --- a/selfdrive/can/dbc_template.cc +++ /dev/null @@ -1,81 +0,0 @@ -#include "common_dbc.h" - -namespace { - -{% for address, msg_name, msg_size, sigs in msgs %} -const Signal sigs_{{address}}[] = { - {% for sig in sigs %} - { - {% if sig.is_little_endian %} - {% set b1 = sig.start_bit %} - {% else %} - {% set b1 = (sig.start_bit//8)*8 + (-sig.start_bit-1) % 8 %} - {% endif %} - .name = "{{sig.name}}", - .b1 = {{b1}}, - .b2 = {{sig.size}}, - .bo = {{64 - (b1 + sig.size)}}, - .is_signed = {{"true" if sig.is_signed else "false"}}, - .factor = {{sig.factor}}, - .offset = {{sig.offset}}, - .is_little_endian = {{"true" if sig.is_little_endian else "false"}}, - {% if checksum_type == "honda" and sig.name == "CHECKSUM" %} - .type = SignalType::HONDA_CHECKSUM, - {% elif checksum_type == "honda" and sig.name == "COUNTER" %} - .type = SignalType::HONDA_COUNTER, - {% elif checksum_type == "toyota" and sig.name == "CHECKSUM" %} - .type = SignalType::TOYOTA_CHECKSUM, - {% elif checksum_type == "volkswagen" and sig.name == "CHECKSUM" %} - .type = SignalType::VOLKSWAGEN_CHECKSUM, - {% elif checksum_type == "volkswagen" and sig.name == "COUNTER" %} - .type = SignalType::VOLKSWAGEN_COUNTER, - {% elif address in [512, 513] and sig.name == "CHECKSUM_PEDAL" %} - .type = SignalType::PEDAL_CHECKSUM, - {% elif address in [512, 513] and sig.name == "COUNTER_PEDAL" %} - .type = SignalType::PEDAL_COUNTER, - {% else %} - .type = SignalType::DEFAULT, - {% endif %} - }, - {% endfor %} -}; -{% endfor %} - -const Msg msgs[] = { -{% for address, msg_name, msg_size, sigs in msgs %} - {% set address_hex = "0x%X" % address %} - { - .name = "{{msg_name}}", - .address = {{address_hex}}, - .size = {{msg_size}}, - .num_sigs = ARRAYSIZE(sigs_{{address}}), - .sigs = sigs_{{address}}, - }, -{% endfor %} -}; - -const Val vals[] = { -{% for address, sig in def_vals %} - {% for sg_name, def_val in sig %} - {% set address_hex = "0x%X" % address %} - { - .name = "{{sg_name}}", - .address = {{address_hex}}, - .def_val = {{def_val}}, - .sigs = sigs_{{address}}, - }, - {% endfor %} -{% endfor %} -}; - -} - -const DBC {{dbc.name}} = { - .name = "{{dbc.name}}", - .num_msgs = ARRAYSIZE(msgs), - .msgs = msgs, - .vals = vals, - .num_vals = ARRAYSIZE(vals), -}; - -dbc_init({{dbc.name}}) diff --git a/selfdrive/can/libdbc_py.py b/selfdrive/can/libdbc_py.py deleted file mode 100644 index 92093f9316..0000000000 --- a/selfdrive/can/libdbc_py.py +++ /dev/null @@ -1,95 +0,0 @@ -import os -import subprocess - -from cffi import FFI - -can_dir = os.path.dirname(os.path.abspath(__file__)) -libdbc_fn = os.path.join(can_dir, "libdbc.so") -subprocess.check_call(["make", "-j3"], cwd=can_dir) # don't use all the cores to avoid overheating - -ffi = FFI() -ffi.cdef(""" - -typedef struct { - const char* name; - double value; -} SignalPackValue; - -typedef struct { - uint32_t address; - const char* name; - double default_value; -} SignalParseOptions; - -typedef struct { - uint32_t address; - int check_frequency; -} MessageParseOptions; - -typedef struct { - uint32_t address; - uint16_t ts; - const char* name; - double value; -} SignalValue; - - -typedef enum { - DEFAULT, - HONDA_CHECKSUM, - HONDA_COUNTER, - TOYOTA_CHECKSUM, - PEDAL_CHECKSUM, - PEDAL_COUNTER, - VOLKSWAGEN_CHECKSUM, - VOLKSWAGEN_COUNTER, -} SignalType; - -typedef struct { - const char* name; - int b1, b2, bo; - bool is_signed; - double factor, offset; - SignalType type; -} Signal; - -typedef struct { - const char* name; - uint32_t address; - unsigned int size; - size_t num_sigs; - const Signal *sigs; -} Msg; - -typedef struct { - const char* name; - uint32_t address; - const char* def_val; - const Signal *sigs; -} Val; - -typedef struct { - const char* name; - size_t num_msgs; - const Msg *msgs; - const Val *vals; - size_t num_vals; -} DBC; - - -void* can_init(int bus, const char* dbc_name, - size_t num_message_options, const MessageParseOptions* message_options, - size_t num_signal_options, const SignalParseOptions* signal_options); - -void can_update_string(void *can, const char* dat, int len); - -size_t can_query_latest(void* can, bool *out_can_valid, size_t out_values_size, SignalValue* out_values); - -const DBC* dbc_lookup(const char* dbc_name); - -void* canpack_init(const char* dbc_name); - -uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals, int counter); -""") - -libdbc = ffi.dlopen(libdbc_fn) diff --git a/selfdrive/can/packer.cc b/selfdrive/can/packer.cc deleted file mode 100644 index c658e56731..0000000000 --- a/selfdrive/can/packer.cc +++ /dev/null @@ -1,139 +0,0 @@ -#include -#include -#include -#include -#include - -#include "common.h" - -#define WARN printf - -namespace { - - // this is the same as read_u64_le, but uses uint64_t as in/out - uint64_t ReverseBytes(uint64_t x) { - return ((x & 0xff00000000000000ull) >> 56) | - ((x & 0x00ff000000000000ull) >> 40) | - ((x & 0x0000ff0000000000ull) >> 24) | - ((x & 0x000000ff00000000ull) >> 8) | - ((x & 0x00000000ff000000ull) << 8) | - ((x & 0x0000000000ff0000ull) << 24) | - ((x & 0x000000000000ff00ull) << 40) | - ((x & 0x00000000000000ffull) << 56); - } - - uint64_t set_value(uint64_t ret, Signal sig, int64_t ival){ - int shift = sig.is_little_endian? sig.b1 : sig.bo; - uint64_t mask = ((1ULL << sig.b2)-1) << shift; - uint64_t dat = (ival & ((1ULL << sig.b2)-1)) << shift; - if (sig.is_little_endian) { - dat = ReverseBytes(dat); - mask = ReverseBytes(mask); - } - ret &= ~mask; - ret |= dat; - return ret; - } - - class CANPacker { - public: - CANPacker(const std::string& dbc_name) { - dbc = dbc_lookup(dbc_name); - assert(dbc); - - for (int i=0; inum_msgs; i++) { - const Msg* msg = &dbc->msgs[i]; - message_lookup[msg->address] = *msg; - for (int j=0; jnum_sigs; j++) { - const Signal* sig = &msg->sigs[j]; - signal_lookup[std::make_pair(msg->address, std::string(sig->name))] = *sig; - } - } - init_crc_lookup_tables(); - } - - uint64_t pack(uint32_t address, const std::vector &signals, int counter) { - uint64_t ret = 0; - for (const auto& sigval : signals) { - std::string name = std::string(sigval.name); - double value = sigval.value; - - auto sig_it = signal_lookup.find(std::make_pair(address, name)); - if (sig_it == signal_lookup.end()) { - WARN("undefined signal %s - %d\n", name.c_str(), address); - continue; - } - auto sig = sig_it->second; - - int64_t ival = (int64_t)(round((value - sig.offset) / sig.factor)); - if (ival < 0) { - ival = (1ULL << sig.b2) + ival; - } - - ret = set_value(ret, sig, ival); - } - - if (counter >= 0){ - auto sig_it = signal_lookup.find(std::make_pair(address, "COUNTER")); - if (sig_it == signal_lookup.end()) { - WARN("COUNTER not defined\n"); - return ret; - } - auto sig = sig_it->second; - - if ((sig.type != SignalType::HONDA_COUNTER) && (sig.type != SignalType::VOLKSWAGEN_COUNTER)) { - WARN("COUNTER signal type not valid\n"); - } - - ret = set_value(ret, sig, counter); - } - - auto sig_it_checksum = signal_lookup.find(std::make_pair(address, "CHECKSUM")); - if (sig_it_checksum != signal_lookup.end()) { - auto sig = sig_it_checksum->second; - if (sig.type == SignalType::HONDA_CHECKSUM) { - unsigned int chksm = honda_checksum(address, ret, message_lookup[address].size); - ret = set_value(ret, sig, chksm); - } else if (sig.type == SignalType::TOYOTA_CHECKSUM) { - unsigned int chksm = toyota_checksum(address, ret, message_lookup[address].size); - ret = set_value(ret, sig, chksm); - } else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) { - // FIXME: Hackish fix for an endianness issue. The message is in reverse byte order - // until later in the pack process. Checksums can be run backwards, CRCs not so much. - // The correct fix is unclear but this works for the moment. - unsigned int chksm = volkswagen_crc(address, ReverseBytes(ret), message_lookup[address].size); - ret = set_value(ret, sig, chksm); - } else { - //WARN("CHECKSUM signal type not valid\n"); - } - } - - return ret; - } - - - private: - const DBC *dbc = NULL; - std::map, Signal> signal_lookup; - std::map message_lookup; - }; - -} - -extern "C" { - void* canpack_init(const char* dbc_name) { - CANPacker *ret = new CANPacker(std::string(dbc_name)); - return (void*)ret; - } - - uint64_t canpack_pack(void* inst, uint32_t address, size_t num_vals, const SignalPackValue *vals, int counter, bool checksum) { - CANPacker *cp = (CANPacker*)inst; - - return cp->pack(address, std::vector(vals, vals+num_vals), counter); - } - - uint64_t canpack_pack_vector(void* inst, uint32_t address, const std::vector &signals, int counter) { - CANPacker *cp = (CANPacker*)inst; - return cp->pack(address, signals, counter); - } -} diff --git a/selfdrive/can/packer.py b/selfdrive/can/packer.py deleted file mode 100644 index ccb537c956..0000000000 --- a/selfdrive/can/packer.py +++ /dev/null @@ -1,9 +0,0 @@ -# pylint: skip-file -import os -import subprocess - -can_dir = os.path.dirname(os.path.abspath(__file__)) -subprocess.check_call(["make", "-j3", "packer_impl.so"], cwd=can_dir) # don't use all the cores to avoid overheating - -from selfdrive.can.packer_impl import CANPacker -assert CANPacker diff --git a/selfdrive/can/packer_impl.pyx b/selfdrive/can/packer_impl.pyx deleted file mode 100644 index d7c6119b75..0000000000 --- a/selfdrive/can/packer_impl.pyx +++ /dev/null @@ -1,124 +0,0 @@ -# distutils: language = c++ -# cython: c_string_encoding=ascii, language_level=3 - -from libc.stdint cimport uint32_t, uint64_t -from libcpp.vector cimport vector -from libcpp.map cimport map -from libcpp.string cimport string -from libcpp cimport bool -from posix.dlfcn cimport dlopen, dlsym, RTLD_LAZY -import os -import subprocess - -cdef struct SignalPackValue: - const char* name - double value - -ctypedef enum SignalType: - DEFAULT, - HONDA_CHECKSUM, - HONDA_COUNTER, - TOYOTA_CHECKSUM, - PEDAL_CHECKSUM, - PEDAL_COUNTER, - VOLKSWAGEN_CHECKSUM, - VOLKSWAGEN_COUNTER - -cdef struct Signal: - const char* name - int b1, b2, bo - bool is_signed - double factor, offset - SignalType type - - - -cdef struct Msg: - const char* name - uint32_t address - unsigned int size - size_t num_sigs - const Signal *sigs - -cdef struct Val: - const char* name - uint32_t address - const char* def_val - const Signal *sigs - -cdef struct DBC: - const char* name - size_t num_msgs - const Msg *msgs - const Val *vals - size_t num_vals - -ctypedef void * (*canpack_init_func)(const char* dbc_name) -ctypedef uint64_t (*canpack_pack_vector_func)(void* inst, uint32_t address, const vector[SignalPackValue] &signals, int counter) -ctypedef const DBC * (*dbc_lookup_func)(const char* dbc_name) - - -cdef class CANPacker(): - cdef void *packer - cdef const DBC *dbc - cdef map[string, (int, int)] name_to_address_and_size - cdef map[int, int] address_to_size - cdef canpack_init_func canpack_init - cdef canpack_pack_vector_func canpack_pack_vector - cdef dbc_lookup_func dbc_lookup - - def __init__(self, dbc_name): - can_dir = os.path.dirname(os.path.abspath(__file__)) - libdbc_fn = os.path.join(can_dir, "libdbc.so") - libdbc_fn = str(libdbc_fn).encode('utf8') - subprocess.check_call(["make"], cwd=can_dir) - - cdef void *libdbc = dlopen(libdbc_fn, RTLD_LAZY) - self.canpack_init = dlsym(libdbc, 'canpack_init') - self.canpack_pack_vector = dlsym(libdbc, 'canpack_pack_vector') - self.dbc_lookup = dlsym(libdbc, 'dbc_lookup') - - self.packer = self.canpack_init(dbc_name) - self.dbc = self.dbc_lookup(dbc_name) - num_msgs = self.dbc[0].num_msgs - for i in range(num_msgs): - msg = self.dbc[0].msgs[i] - self.name_to_address_and_size[string(msg.name)] = (msg.address, msg.size) - self.address_to_size[msg.address] = msg.size - - cdef uint64_t pack(self, addr, values, counter): - cdef vector[SignalPackValue] values_thing - cdef SignalPackValue spv - - names = [] - - for name, value in values.iteritems(): - n = name.encode('utf8') - names.append(n) # TODO: find better way to keep reference to temp string arround - - spv.name = n - spv.value = value - values_thing.push_back(spv) - - return self.canpack_pack_vector(self.packer, addr, values_thing, counter) - - cdef inline uint64_t ReverseBytes(self, uint64_t x): - return (((x & 0xff00000000000000ull) >> 56) | - ((x & 0x00ff000000000000ull) >> 40) | - ((x & 0x0000ff0000000000ull) >> 24) | - ((x & 0x000000ff00000000ull) >> 8) | - ((x & 0x00000000ff000000ull) << 8) | - ((x & 0x0000000000ff0000ull) << 24) | - ((x & 0x000000000000ff00ull) << 40) | - ((x & 0x00000000000000ffull) << 56)) - - cpdef make_can_msg(self, name_or_addr, bus, values, counter=-1): - cdef int addr, size - if type(name_or_addr) == int: - addr = name_or_addr - size = self.address_to_size[name_or_addr] - else: - addr, size = self.name_to_address_and_size[name_or_addr.encode('utf8')] - cdef uint64_t val = self.pack(addr, values, counter) - val = self.ReverseBytes(val) - return [addr, 0, (&val)[:size], bus] diff --git a/selfdrive/can/packer_setup.py b/selfdrive/can/packer_setup.py deleted file mode 100644 index 8b25e60580..0000000000 --- a/selfdrive/can/packer_setup.py +++ /dev/null @@ -1,9 +0,0 @@ -from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module - -from Cython.Build import cythonize - -from common.cython_hacks import BuildExtWithoutPlatformSuffix - -setup(name='CAN Packer API Implementation', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize(Extension("packer_impl", ["packer_impl.pyx"], language="c++", extra_compile_args=["-std=c++11"]))) diff --git a/selfdrive/can/parser.cc b/selfdrive/can/parser.cc deleted file mode 100644 index e204112e3c..0000000000 --- a/selfdrive/can/parser.cc +++ /dev/null @@ -1,239 +0,0 @@ -#include -#include - -#include -#include -#include -#include -#include - -#include "common.h" - -#define DEBUG(...) -// #define DEBUG printf -#define INFO printf - - -bool MessageState::parse(uint64_t sec, uint16_t ts_, uint8_t * dat) { - uint64_t dat_le = read_u64_le(dat); - uint64_t dat_be = read_u64_be(dat); - - for (int i=0; i < parse_sigs.size(); i++) { - auto& sig = parse_sigs[i]; - int64_t tmp; - - if (sig.is_little_endian){ - tmp = (dat_le >> sig.b1) & ((1ULL << sig.b2)-1); - } else { - tmp = (dat_be >> sig.bo) & ((1ULL << sig.b2)-1); - } - - if (sig.is_signed) { - tmp -= (tmp >> (sig.b2-1)) ? (1ULL << sig.b2) : 0; //signed - } - - DEBUG("parse 0x%X %s -> %lld\n", address, sig.name, tmp); - - if (sig.type == SignalType::HONDA_CHECKSUM) { - if (honda_checksum(address, dat_be, size) != tmp) { - INFO("0x%X CHECKSUM FAIL\n", address); - return false; - } - } else if (sig.type == SignalType::HONDA_COUNTER) { - if (!update_counter_generic(tmp, sig.b2)) { - return false; - } - } else if (sig.type == SignalType::TOYOTA_CHECKSUM) { - if (toyota_checksum(address, dat_be, size) != tmp) { - INFO("0x%X CHECKSUM FAIL\n", address); - return false; - } - } else if (sig.type == SignalType::VOLKSWAGEN_CHECKSUM) { - if (volkswagen_crc(address, dat_le, size) != tmp) { - INFO("0x%X CRC FAIL\n", address); - return false; - } - } else if (sig.type == SignalType::VOLKSWAGEN_COUNTER) { - if (!update_counter_generic(tmp, sig.b2)) { - return false; - } - } else if (sig.type == SignalType::PEDAL_CHECKSUM) { - if (pedal_checksum(dat_be, size) != tmp) { - INFO("0x%X PEDAL CHECKSUM FAIL\n", address); - return false; - } - } else if (sig.type == SignalType::PEDAL_COUNTER) { - if (!update_counter_generic(tmp, sig.b2)) { - return false; - } - } - - vals[i] = tmp * sig.factor + sig.offset; - } - ts = ts_; - seen = sec; - - return true; -} - - -bool MessageState::update_counter_generic(int64_t v, int cnt_size) { - uint8_t old_counter = counter; - counter = v; - if (((old_counter+1) & ((1 << cnt_size) -1)) != v) { - counter_fail += 1; - if (counter_fail > 1) { - INFO("0x%X COUNTER FAIL %d -- %d vs %d\n", address, counter_fail, old_counter, (int)v); - } - if (counter_fail >= MAX_BAD_COUNTER) { - return false; - } - } else if (counter_fail > 0) { - counter_fail--; - } - return true; -} - - -CANParser::CANParser(int abus, const std::string& dbc_name, - const std::vector &options, - const std::vector &sigoptions) - : bus(abus) { - - dbc = dbc_lookup(dbc_name); - assert(dbc); - init_crc_lookup_tables(); - - for (const auto& op : options) { - MessageState state = { - .address = op.address, - // .check_frequency = op.check_frequency, - }; - - // msg is not valid if a message isn't received for 10 consecutive steps - if (op.check_frequency > 0) { - state.check_threshold = (1000000000ULL / op.check_frequency) * 10; - } - - - const Msg* msg = NULL; - for (int i=0; inum_msgs; i++) { - if (dbc->msgs[i].address == op.address) { - msg = &dbc->msgs[i]; - break; - } - } - if (!msg) { - fprintf(stderr, "CANParser: could not find message 0x%X in DBC %s\n", op.address, dbc_name.c_str()); - assert(false); - } - - state.size = msg->size; - - // track checksums and counters for this message - for (int i=0; inum_sigs; i++) { - const Signal *sig = &msg->sigs[i]; - if (sig->type != SignalType::DEFAULT) { - state.parse_sigs.push_back(*sig); - state.vals.push_back(0); - } - } - - // track requested signals for this message - for (const auto& sigop : sigoptions) { - if (sigop.address != op.address) continue; - - for (int i=0; inum_sigs; i++) { - const Signal *sig = &msg->sigs[i]; - if (strcmp(sig->name, sigop.name) == 0 - && sig->type == SignalType::DEFAULT) { - state.parse_sigs.push_back(*sig); - state.vals.push_back(sigop.default_value); - break; - } - } - - } - - message_states[state.address] = state; - } -} - -void CANParser::UpdateCans(uint64_t sec, const capnp::List::Reader& cans) { - int msg_count = cans.size(); - uint64_t p; - - DEBUG("got %d messages\n", msg_count); - - // parse the messages - for (int i = 0; i < msg_count; i++) { - auto cmsg = cans[i]; - if (cmsg.getSrc() != bus) { - // DEBUG("skip %d: wrong bus\n", cmsg.getAddress()); - continue; - } - auto state_it = message_states.find(cmsg.getAddress()); - if (state_it == message_states.end()) { - // DEBUG("skip %d: not specified\n", cmsg.getAddress()); - continue; - } - - if (cmsg.getDat().size() > 8) continue; //shouldnt ever happen - uint8_t dat[8] = {0}; - memcpy(dat, cmsg.getDat().begin(), cmsg.getDat().size()); - - state_it->second.parse(sec, cmsg.getBusTime(), dat); - } -} - -void CANParser::UpdateValid(uint64_t sec) { - can_valid = true; - for (const auto& kv : message_states) { - const auto& state = kv.second; - if (state.check_threshold > 0 && (sec - state.seen) > state.check_threshold) { - if (state.seen > 0) { - DEBUG("0x%X TIMEOUT\n", state.address); - } - can_valid = false; - } - } -} - -void CANParser::update_string(std::string data) { - // format for board, make copy due to alignment issues, will be freed on out of scope - auto amsg = kj::heapArray((data.length() / sizeof(capnp::word)) + 1); - memcpy(amsg.begin(), data.data(), data.length()); - - // extract the messages - capnp::FlatArrayMessageReader cmsg(amsg); - cereal::Event::Reader event = cmsg.getRoot(); - - last_sec = event.getLogMonoTime(); - - auto cans = event.getCan(); - UpdateCans(last_sec, cans); - - UpdateValid(last_sec); -} - - -std::vector CANParser::query_latest() { - std::vector ret; - - for (const auto& kv : message_states) { - const auto& state = kv.second; - if (last_sec != 0 && state.seen != last_sec) continue; - - for (int i=0; iself.address_to_msg_name[cv.address].c_str() - cv_name = cv.name - - self.vl[cv.address][cv_name] = cv.value - self.ts[cv.address][cv_name] = cv.ts - - self.vl[name][cv_name] = cv.value - self.ts[name][cv_name] = cv.ts - - updated_val.insert(cv.address) - - return updated_val - - def update_string(self, dat): - self.can.update_string(dat) - return self.update_vl() - - def update_strings(self, strings): - updated_vals = set() - - for s in strings: - updated_val = self.update_string(s) - updated_vals.update(updated_val) - - return updated_vals diff --git a/selfdrive/can/parser_pyx_setup.py b/selfdrive/can/parser_pyx_setup.py deleted file mode 100644 index a024c7a640..0000000000 --- a/selfdrive/can/parser_pyx_setup.py +++ /dev/null @@ -1,35 +0,0 @@ -import os -import subprocess -from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module - -from Cython.Build import cythonize - -from common.basedir import BASEDIR -from common.cython_hacks import BuildExtWithoutPlatformSuffix - -sourcefiles = ['parser_pyx.pyx'] -extra_compile_args = ["-std=c++11"] -ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg - -if ARCH == "aarch64": - extra_compile_args += ["-Wno-deprecated-register"] - -setup(name='CAN parser', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize( - Extension( - "parser_pyx", - language="c++", - sources=sourcefiles, - extra_compile_args=extra_compile_args, - include_dirs=[ - BASEDIR, - os.path.join(BASEDIR, 'phonelibs', 'capnp-cpp/include'), - ], - extra_link_args=[ - os.path.join(BASEDIR, 'selfdrive', 'can', 'libdbc.so'), - ], - ) - ), - nthreads=4, -) diff --git a/selfdrive/can/plant_can_parser.py b/selfdrive/can/plant_can_parser.py deleted file mode 100644 index 37ea335374..0000000000 --- a/selfdrive/can/plant_can_parser.py +++ /dev/null @@ -1,131 +0,0 @@ -### old can parser just used by plant.py for regression tests -import os -import opendbc -from collections import defaultdict - -from selfdrive.car.honda.hondacan import fix -from common.realtime import sec_since_boot -from common.dbc import dbc - -class CANParser(): - def __init__(self, dbc_f, signals, checks=None): - ### input: - # dbc_f : dbc file - # signals : List of tuples (name, address, ival) where - # - name is the signal name. - # - address is the corresponding message address. - # - ival is the initial value. - # checks : List of pairs (address, frequency) where - # - address is the message address of a message for which health should be - # monitored. - # - frequency is the frequency at which health should be monitored. - - checks = [] if checks is None else checks - self.msgs_ck = {check[0] for check in checks} - self.frqs = dict(checks) - self.can_valid = False # start with False CAN assumption - # list of received msg we want to monitor counter and checksum for - # read dbc file - self.can_dbc = dbc(os.path.join(opendbc.DBC_PATH, dbc_f)) - # initialize variables to initial values - self.vl = {} # signal values - self.ts = {} # time stamp recorded in log - self.ct = {} # current time stamp - self.ok = {} # valid message? - self.cn = {} # message counter - self.cn_vl = {} # message counter mismatch value - self.ck = {} # message checksum status - - for _, addr, _ in signals: - self.vl[addr] = {} - self.ts[addr] = {} - self.ct[addr] = sec_since_boot() - self.ok[addr] = True - self.cn[addr] = 0 - self.cn_vl[addr] = 0 - self.ck[addr] = False - - for name, addr, ival in signals: - self.vl[addr][name] = ival - self.ts[addr][name] = 0 - - self._msgs = [s[1] for s in signals] - self._sgs = [s[0] for s in signals] - - self._message_indices = defaultdict(list) - for i, x in enumerate(self._msgs): - self._message_indices[x].append(i) - - def update_can(self, can_recv): - msgs_upd = [] - cn_vl_max = 5 # no more than 5 wrong counter checks - - self.sec_since_boot_cached = sec_since_boot() - - # we are subscribing to PID_XXX, else data from USB - for msg, ts, cdat, _ in can_recv: - idxs = self._message_indices[msg] - if idxs: - msgs_upd.append(msg) - # read the entire message - out = self.can_dbc.decode((msg, 0, cdat))[1] - # checksum check - self.ck[msg] = True - if "CHECKSUM" in out.keys() and msg in self.msgs_ck: - # remove checksum (half byte) - ck_portion = cdat[:-1] + (cdat[-1] & 0xF0).to_bytes(1, 'little') - # recalculate checksum - msg_vl = fix(ck_portion, msg) - # compare recalculated vs received checksum - if msg_vl != cdat: - print("CHECKSUM FAIL: {0}".format(hex(msg))) - self.ck[msg] = False - self.ok[msg] = False - # counter check - cn = 0 - if "COUNTER" in out.keys(): - cn = out["COUNTER"] - # check counter validity if it's a relevant message - if cn != ((self.cn[msg] + 1) % 4) and msg in self.msgs_ck and "COUNTER" in out.keys(): - #print("FAILED COUNTER: {0}".format(hex(msg)() - self.cn_vl[msg] += 1 # counter check failed - else: - self.cn_vl[msg] -= 1 # counter check passed - # message status is invalid if we received too many wrong counter values - if self.cn_vl[msg] >= cn_vl_max: - print("COUNTER WRONG: {0}".format(hex(msg))) - self.ok[msg] = False - - # update msg time stamps and counter value - self.ct[msg] = self.sec_since_boot_cached - self.cn[msg] = cn - self.cn_vl[msg] = min(max(self.cn_vl[msg], 0), cn_vl_max) - - # set msg valid status if checksum is good and wrong counter counter is zero - if self.ck[msg] and self.cn_vl[msg] == 0: - self.ok[msg] = True - - # update value of signals in the - for ii in idxs: - sg = self._sgs[ii] - self.vl[msg][sg] = out[sg] - self.ts[msg][sg] = ts - - # for each message, check if it's too long since last time we received it - self._check_dead_msgs() - - # assess overall can validity: if there is one relevant message invalid, then set can validity flag to False - self.can_valid = True - - if False in self.ok.values(): - #print("CAN INVALID!") - self.can_valid = False - - return msgs_upd - - def _check_dead_msgs(self): - ### input: - ## simple stuff for now: msg is not valid if a message isn't received for 10 consecutive steps - for msg in set(self._msgs): - if msg in self.msgs_ck and self.sec_since_boot_cached - self.ct[msg] > 10./self.frqs[msg]: - self.ok[msg] = False diff --git a/selfdrive/can/process_dbc.py b/selfdrive/can/process_dbc.py deleted file mode 100755 index dc544f741f..0000000000 --- a/selfdrive/can/process_dbc.py +++ /dev/null @@ -1,105 +0,0 @@ -#!/usr/bin/env python3 -from __future__ import print_function -import os -import sys - -import jinja2 - -from collections import Counter -from common.dbc import dbc - -def main(): - if len(sys.argv) != 3: - print("usage: %s dbc_directory output_filename" % (sys.argv[0],)) - sys.exit(0) - - dbc_dir = sys.argv[1] - out_fn = sys.argv[2] - - dbc_name = os.path.split(out_fn)[-1].replace('.cc', '') - - template_fn = os.path.join(os.path.dirname(__file__), "dbc_template.cc") - - with open(template_fn, "r") as template_f: - template = jinja2.Template(template_f.read(), trim_blocks=True, lstrip_blocks=True) - - can_dbc = dbc(os.path.join(dbc_dir, dbc_name + '.dbc')) - - msgs = [(address, msg_name, msg_size, sorted(msg_sigs, key=lambda s: s.name not in ("COUNTER", "CHECKSUM"))) # process counter and checksums first - for address, ((msg_name, msg_size), msg_sigs) in sorted(can_dbc.msgs.items()) if msg_sigs] - - def_vals = {a: sorted(set(b)) for a, b in can_dbc.def_vals.items()} # remove duplicates - def_vals = sorted(def_vals.items()) - - if can_dbc.name.startswith(("honda_", "acura_")): - checksum_type = "honda" - checksum_size = 4 - counter_size = 2 - checksum_start_bit = 3 - counter_start_bit = 5 - little_endian = False - elif can_dbc.name.startswith(("toyota_", "lexus_")): - checksum_type = "toyota" - checksum_size = 8 - counter_size = None - checksum_start_bit = 7 - counter_start_bit = None - little_endian = False - elif can_dbc.name.startswith(("vw_", "volkswagen_", "audi_", "seat_", "skoda_")): - checksum_type = "volkswagen" - checksum_size = 8 - counter_size = 4 - checksum_start_bit = 0 - counter_start_bit = 0 - little_endian = True - else: - checksum_type = None - checksum_size = None - counter_size = None - checksum_start_bit = None - counter_start_bit = None - little_endian = None - - # sanity checks on expected COUNTER and CHECKSUM rules, as packer and parser auto-compute those signals - for address, msg_name, msg_size, sigs in msgs: - dbc_msg_name = dbc_name + " " + msg_name - for sig in sigs: - if checksum_type is not None: - # checksum rules - if sig.name == "CHECKSUM": - if sig.size != checksum_size: - sys.exit("%s: CHECKSUM is not %d bits long" % (dbc_msg_name, checksum_size)) - if sig.start_bit % 8 != checksum_start_bit: - sys.exit("%s: CHECKSUM starts at wrong bit" % dbc_msg_name) - if little_endian != sig.is_little_endian: - sys.exit("%s: CHECKSUM has wrong endianess" % dbc_msg_name) - # counter rules - if sig.name == "COUNTER": - if counter_size is not None and sig.size != counter_size: - sys.exit("%s: COUNTER is not %d bits long" % (dbc_msg_name, counter_size)) - if counter_start_bit is not None and sig.start_bit % 8 != counter_start_bit: - print(counter_start_bit, sig.start_bit) - sys.exit("%s: COUNTER starts at wrong bit" % dbc_msg_name) - if little_endian != sig.is_little_endian: - sys.exit("%s: COUNTER has wrong endianess" % dbc_msg_name) - # pedal rules - if address in [0x200, 0x201]: - if sig.name == "COUNTER_PEDAL" and sig.size != 4: - sys.exit("%s: PEDAL COUNTER is not 4 bits long" % dbc_msg_name) - if sig.name == "CHECKSUM_PEDAL" and sig.size != 8: - sys.exit("%s: PEDAL CHECKSUM is not 8 bits long" % dbc_msg_name) - - # Fail on duplicate message names - c = Counter([msg_name for address, msg_name, msg_size, sigs in msgs]) - for name, count in c.items(): - if count > 1: - sys.exit("%s: Duplicate message name in DBC file %s" % (dbc_name, name)) - - parser_code = template.render(dbc=can_dbc, checksum_type=checksum_type, msgs=msgs, def_vals=def_vals, len=len) - - - with open(out_fn, "w") as out_f: - out_f.write(parser_code) - -if __name__ == '__main__': - main() diff --git a/selfdrive/can/tests/test_packer_parser.py b/selfdrive/can/tests/test_packer_parser.py deleted file mode 100644 index 9b8a886c46..0000000000 --- a/selfdrive/can/tests/test_packer_parser.py +++ /dev/null @@ -1,92 +0,0 @@ -#!/usr/bin/env python3 - -import unittest - -from selfdrive.can.parser import CANParser -from selfdrive.can.packer import CANPacker - -from selfdrive.boardd.boardd import can_list_to_can_capnp - -from selfdrive.car.honda.interface import CarInterface as HondaInterface -from selfdrive.car.honda.values import CAR as HONDA_CAR -from selfdrive.car.honda.values import DBC as HONDA_DBC - -from selfdrive.car.subaru.interface import CarInterface as SubaruInterface -from selfdrive.car.subaru.values import CAR as SUBARU_CAR -from selfdrive.car.subaru.values import DBC as SUBARU_DBC - -class TestCanParserPacker(unittest.TestCase): - def test_civic(self): - CP = HondaInterface.get_params(HONDA_CAR.CIVIC) - - signals = [ - ("STEER_TORQUE", "STEERING_CONTROL", 0), - ("STEER_TORQUE_REQUEST", "STEERING_CONTROL", 0), - ] - checks = [] - - parser = CANParser(HONDA_DBC[CP.carFingerprint]['pt'], signals, checks, 0) - packer = CANPacker(HONDA_DBC[CP.carFingerprint]['pt']) - - idx = 0 - - for steer in range(0, 255): - for active in [1, 0]: - values = { - "STEER_TORQUE": steer, - "STEER_TORQUE_REQUEST": active, - } - - msgs = packer.make_can_msg("STEERING_CONTROL", 0, values, idx) - bts = can_list_to_can_capnp([msgs]) - - parser.update_string(bts) - - self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE"], steer) - self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["STEER_TORQUE_REQUEST"], active) - self.assertAlmostEqual(parser.vl["STEERING_CONTROL"]["COUNTER"], idx % 4) - - idx += 1 - - def test_subaru(self): - # Subuaru is little endian - CP = SubaruInterface.get_params(SUBARU_CAR.IMPREZA) - - signals = [ - ("Counter", "ES_LKAS", 0), - ("LKAS_Output", "ES_LKAS", 0), - ("LKAS_Request", "ES_LKAS", 0), - ("SET_1", "ES_LKAS", 0), - - ] - - checks = [] - - parser = CANParser(SUBARU_DBC[CP.carFingerprint]['pt'], signals, checks, 0) - packer = CANPacker(SUBARU_DBC[CP.carFingerprint]['pt']) - - idx = 0 - - for steer in range(0, 255): - for active in [1, 0]: - values = { - "Counter": idx, - "LKAS_Output": steer, - "LKAS_Request": active, - "SET_1": 1 - } - - msgs = packer.make_can_msg("ES_LKAS", 0, values) - bts = can_list_to_can_capnp([msgs]) - parser.update_string(bts) - - self.assertAlmostEqual(parser.vl["ES_LKAS"]["LKAS_Output"], steer) - self.assertAlmostEqual(parser.vl["ES_LKAS"]["LKAS_Request"], active) - self.assertAlmostEqual(parser.vl["ES_LKAS"]["SET_1"], 1) - self.assertAlmostEqual(parser.vl["ES_LKAS"]["Counter"], idx % 16) - - idx += 1 - - -if __name__ == "__main__": - unittest.main() diff --git a/selfdrive/car/__init__.py b/selfdrive/car/__init__.py index b0232daf44..6ad29d4b3a 100644 --- a/selfdrive/car/__init__.py +++ b/selfdrive/car/__init__.py @@ -124,3 +124,8 @@ def is_ecu_disconnected(fingerprint, fingerprint_list, ecu_fingerprint, car, ecu ecu_in_car = True return ecu_in_car and not any(msg in fingerprint for msg in ecu_fingerprint[ecu]) + + +def make_can_msg(addr, dat, bus): + return [addr, 0, dat, bus] + diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index bc7d9676f5..e3a96617be 100644 --- a/selfdrive/car/car_helpers.py +++ b/selfdrive/car/car_helpers.py @@ -4,7 +4,7 @@ from common.basedir import BASEDIR from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars from selfdrive.car.vin import get_vin, VIN_UNKNOWN from selfdrive.swaglog import cloudlog -import selfdrive.messaging as messaging +import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint def get_startup_alert(car_recognized, controller_available): diff --git a/selfdrive/car/chrysler/carcontroller.py b/selfdrive/car/chrysler/carcontroller.py index ed92b1427d..4b8919d2df 100644 --- a/selfdrive/car/chrysler/carcontroller.py +++ b/selfdrive/car/chrysler/carcontroller.py @@ -2,7 +2,7 @@ from selfdrive.car import apply_toyota_steer_torque_limits from selfdrive.car.chrysler.chryslercan import create_lkas_hud, create_lkas_command, \ create_wheel_buttons from selfdrive.car.chrysler.values import ECU, CAR, SteerLimitParams -from selfdrive.can.packer import CANPacker +from opendbc.can.packer import CANPacker class CarController(): def __init__(self, dbc_name, car_fingerprint, enable_camera): @@ -16,6 +16,7 @@ class CarController(): self.car_fingerprint = car_fingerprint self.alert_active = False self.gone_fast_yet = False + self.steer_rate_limited = False self.fake_ecus = set() if enable_camera: @@ -32,9 +33,10 @@ class CarController(): # *** compute control surfaces *** # steer torque - apply_steer = actuators.steer * SteerLimitParams.STEER_MAX - apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.apply_steer_last, + new_steer = actuators.steer * SteerLimitParams.STEER_MAX + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_motor, SteerLimitParams) + self.steer_rate_limited = new_steer != apply_steer moving_fast = CS.v_ego > CS.CP.minSteerSpeed # for status message if CS.v_ego > (CS.CP.minSteerSpeed - 0.5): # for command high bit diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 93b354e0f8..54b4a5f4c6 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -1,5 +1,5 @@ from cereal import car -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from selfdrive.car.chrysler.values import DBC, STEER_THRESHOLD from common.kalman.simple_kalman import KF1D diff --git a/selfdrive/car/chrysler/chryslercan.py b/selfdrive/car/chrysler/chryslercan.py index 047ae82580..3c5a913fca 100644 --- a/selfdrive/car/chrysler/chryslercan.py +++ b/selfdrive/car/chrysler/chryslercan.py @@ -1,4 +1,5 @@ from cereal import car +from selfdrive.car import make_can_msg GearShifter = car.CarState.GearShifter @@ -41,16 +42,12 @@ def calc_checksum(data): return ~checksum & 0xFF -def make_can_msg(addr, dat): - return [addr, 0, dat, 0] - - def create_lkas_hud(packer, gear, lkas_active, hud_alert, hud_count, lkas_car_model): # LKAS_HUD 0x2a6 (678) Controls what lane-keeping icon is displayed. if hud_alert == VisualAlert.steerRequired: msg = b'\x00\x00\x00\x03\x00\x00\x00\x00' - return make_can_msg(0x2a6, msg) + return make_can_msg(0x2a6, msg, 0) color = 1 # default values are for park or neutral in 2017 are 0 0, but trying 1 1 for 2019 lines = 1 @@ -100,4 +97,4 @@ def create_wheel_buttons(frame): counter = (frame % 10) << 4 dat = start + counter.to_bytes(1, 'little') dat = dat + calc_checksum(dat).to_bytes(1, 'little') - return make_can_msg(0x23b, dat) + return make_can_msg(0x23b, dat, 0) diff --git a/selfdrive/car/chrysler/interface.py b/selfdrive/car/chrysler/interface.py index 8a3cd750a4..2a6422ecbf 100755 --- a/selfdrive/car/chrysler/interface.py +++ b/selfdrive/car/chrysler/interface.py @@ -58,6 +58,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kf = 0.00006 # full torque for 10 deg at 80mph means 0.00007818594 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.7 + ret.steerLimitTimer = 0.4 if candidate in (CAR.JEEP_CHEROKEE, CAR.JEEP_CHEROKEE_2019): ret.wheelbase = 2.91 # in meters @@ -66,7 +67,6 @@ class CarInterface(CarInterfaceBase): ret.centerToFront = ret.wheelbase * 0.44 - ret.minSteerSpeed = 3.8 # m/s ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this if candidate in (CAR.PACIFICA_2019_HYBRID, CAR.JEEP_CHEROKEE_2019): @@ -96,7 +96,6 @@ class CarInterface(CarInterfaceBase): print("ECU Camera Simulated: {0}".format(ret.enableCamera)) ret.openpilotLongitudinalControl = False - ret.steerLimitAlert = True ret.stoppingControl = False ret.startAccel = 0.0 @@ -151,6 +150,7 @@ class CarInterface(CarInterfaceBase): ret.steeringTorque = self.CS.steer_torque_driver ret.steeringPressed = self.CS.steer_override + ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_status # same as main_on diff --git a/selfdrive/car/chrysler/radar_interface.py b/selfdrive/car/chrysler/radar_interface.py index 3130c2ca71..0713bf1d16 100755 --- a/selfdrive/car/chrysler/radar_interface.py +++ b/selfdrive/car/chrysler/radar_interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 import os -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from cereal import car from selfdrive.car.interfaces import RadarInterfaceBase diff --git a/selfdrive/car/chrysler/test_chryslercan.py b/selfdrive/car/chrysler/test_chryslercan.py index 292fec5222..637cdc9ffa 100644 --- a/selfdrive/car/chrysler/test_chryslercan.py +++ b/selfdrive/car/chrysler/test_chryslercan.py @@ -1,7 +1,7 @@ import unittest from cereal import car -from selfdrive.can.packer import CANPacker +from opendbc.can.packer import CANPacker from selfdrive.car.chrysler import chryslercan VisualAlert = car.CarControl.HUDControl.VisualAlert diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index b466f03ea0..db574f38a9 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -1,7 +1,7 @@ from cereal import car -from selfdrive.car.ford.fordcan import make_can_msg, create_steer_command, create_lkas_ui, \ - spam_cancel_button -from selfdrive.can.packer import CANPacker +from selfdrive.car import make_can_msg +from selfdrive.car.ford.fordcan import create_steer_command, create_lkas_ui, spam_cancel_button +from opendbc.can.packer import CANPacker MAX_STEER_DELTA = 1 @@ -48,37 +48,37 @@ class CarController(): if (frame % 100) == 0: - can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0, False)) - #can_sends.append(make_can_msg(984, '\x00\x00\x00\x00\x80\x45\x60\x30', 0, False)) + can_sends.append(make_can_msg(973, b'\x00\x00\x00\x00\x00\x00\x00\x00', 0)) + #can_sends.append(make_can_msg(984, b'\x00\x00\x00\x00\x80\x45\x60\x30', 0)) if (frame % 100) == 0 or (self.enabled_last != enabled) or (self.main_on_last != CS.main_on) or \ (self.steer_alert_last != steer_alert): can_sends.append(create_lkas_ui(self.packer, CS.main_on, enabled, steer_alert)) if (frame % 200) == 0: - can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1, False)) + can_sends.append(make_can_msg(1875, b'\x80\xb0\x55\x55\x78\x90\x00\x00', 1)) if (frame % 10) == 0: - can_sends.append(make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1, False)) - can_sends.append(make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1, False)) + can_sends.append(make_can_msg(1648, b'\x00\x00\x00\x40\x00\x00\x50\x00', 1)) + can_sends.append(make_can_msg(1649, b'\x10\x10\xf1\x70\x04\x00\x00\x00', 1)) - can_sends.append(make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1, False)) - can_sends.append(make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1, False)) - can_sends.append(make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1, False)) - can_sends.append(make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1, False)) + can_sends.append(make_can_msg(1664, b'\x00\x00\x03\xe8\x00\x01\xa9\xb2', 1)) + can_sends.append(make_can_msg(1674, b'\x08\x00\x00\xff\x0c\xfb\x6a\x08', 1)) + can_sends.append(make_can_msg(1675, b'\x00\x00\x3b\x60\x37\x00\x00\x00', 1)) + can_sends.append(make_can_msg(1690, b'\x70\x00\x00\x55\x86\x1c\xe0\x00', 1)) - can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1, False)) - can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1, False)) - can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1, False)) - can_sends.append(make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1, False)) - can_sends.append(make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1, False)) - can_sends.append(make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1, False)) + can_sends.append(make_can_msg(1910, b'\x06\x4b\x06\x4b\x42\xd3\x11\x30', 1)) + can_sends.append(make_can_msg(1911, b'\x48\x53\x37\x54\x48\x53\x37\x54', 1)) + can_sends.append(make_can_msg(1912, b'\x31\x34\x47\x30\x38\x31\x43\x42', 1)) + can_sends.append(make_can_msg(1913, b'\x31\x34\x47\x30\x38\x32\x43\x42', 1)) + can_sends.append(make_can_msg(1969, b'\xf4\x40\x00\x00\x00\x00\x00\x00', 1)) + can_sends.append(make_can_msg(1971, b'\x0b\xc0\x00\x00\x00\x00\x00\x00', 1)) static_msgs = range(1653, 1658) for addr in static_msgs: cnt = (frame % 10) + 1 - can_sends.append(make_can_msg(addr, (cnt<<4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1, False)) + can_sends.append(make_can_msg(addr, (cnt<<4).to_bytes(1, 'little') + b'\x00\x00\x00\x00\x00\x00\x00', 1)) self.enabled_last = enabled self.main_on_last = CS.main_on diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index b6ddf45259..7484e064ca 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -1,4 +1,4 @@ -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from common.numpy_fast import mean from selfdrive.config import Conversions as CV from selfdrive.car.ford.values import DBC @@ -79,6 +79,7 @@ class CarState(): self.pcm_acc_status = cp.vl["Cruise_Status"]['Cruise_State'] self.main_on = cp.vl["Cruise_Status"]['Cruise_State'] != 0 self.lkas_state = cp.vl["Lane_Keep_Assist_Status"]['LaActAvail_D_Actl'] + # TODO: we also need raw driver torque, needed for Assisted Lane Change self.steer_override = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl'] self.steer_error = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] self.user_gas = cp.vl["EngineData_14"]['ApedPosScal_Pc_Actl'] diff --git a/selfdrive/car/ford/fordcan.py b/selfdrive/car/ford/fordcan.py index a55f298964..dd0c15415e 100644 --- a/selfdrive/car/ford/fordcan.py +++ b/selfdrive/car/ford/fordcan.py @@ -2,10 +2,6 @@ from common.numpy_fast import clip from selfdrive.car.ford.values import MAX_ANGLE -def make_can_msg(addr, dat, alt, cks=False): - return [addr, 0, dat, alt] - - def create_steer_command(packer, angle_cmd, enabled, lkas_state, angle_steers, curvature, lkas_action): """Creates a CAN message for the Ford Steer Command.""" diff --git a/selfdrive/car/ford/interface.py b/selfdrive/car/ford/interface.py index a05144c133..5b2deed550 100755 --- a/selfdrive/car/ford/interface.py +++ b/selfdrive/car/ford/interface.py @@ -56,6 +56,7 @@ class CarInterface(CarInterfaceBase): ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.01], [0.005]] # TODO: tune this ret.lateralTuning.pid.kf = 1. / MAX_ANGLE # MAX Steer angle to normalize FF ret.steerActuatorDelay = 0.1 # Default delay, not measured yet + ret.steerLimitTimer = 0.8 ret.steerRateCost = 1.0 ret.centerToFront = ret.wheelbase * 0.44 tire_stiffness_factor = 0.5328 @@ -89,7 +90,6 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = False cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) - ret.steerLimitAlert = False ret.stoppingControl = False ret.startAccel = 0.0 diff --git a/selfdrive/car/ford/radar_interface.py b/selfdrive/car/ford/radar_interface.py index 2538a185de..048f556fd3 100755 --- a/selfdrive/car/ford/radar_interface.py +++ b/selfdrive/car/ford/radar_interface.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from cereal import car -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from selfdrive.car.ford.values import DBC from selfdrive.config import Conversions as CV from selfdrive.car.interfaces import RadarInterfaceBase diff --git a/selfdrive/car/gm/carcontroller.py b/selfdrive/car/gm/carcontroller.py index c4647c8228..53eccf1f38 100644 --- a/selfdrive/car/gm/carcontroller.py +++ b/selfdrive/car/gm/carcontroller.py @@ -5,7 +5,7 @@ from selfdrive.config import Conversions as CV from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.gm import gmcan from selfdrive.car.gm.values import DBC, SUPERCRUISE_CARS -from selfdrive.can.packer import CANPacker +from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -76,6 +76,7 @@ class CarController(): self.apply_steer_last = 0 self.car_fingerprint = car_fingerprint self.lka_icon_status_last = (False, False) + self.steer_rate_limited = False # Setup detection helper. Routes commands to # an appropriate CAN bus number. @@ -102,8 +103,9 @@ class CarController(): if (frame % P.STEER_STEP) == 0: lkas_enabled = enabled and not CS.steer_not_allowed and CS.v_ego > P.MIN_STEER_SPEED if lkas_enabled: - apply_steer = actuators.steer * P.STEER_MAX - apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) + new_steer = actuators.steer * P.STEER_MAX + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P) + self.steer_rate_limited = new_steer != apply_steer else: apply_steer = 0 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 7da8d65272..8a083cb535 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -2,7 +2,7 @@ from cereal import car from common.numpy_fast import mean from common.kalman.simple_kalman import KF1D from selfdrive.config import Conversions as CV -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from selfdrive.car.gm.values import DBC, CAR, parse_gear_shifter, \ CruiseButtons, is_eps_status_ok, \ STEER_THRESHOLD, SUPERCRUISE_CARS diff --git a/selfdrive/car/gm/gmcan.py b/selfdrive/car/gm/gmcan.py index ada05658ba..68b129e743 100644 --- a/selfdrive/car/gm/gmcan.py +++ b/selfdrive/car/gm/gmcan.py @@ -1,3 +1,5 @@ +from selfdrive.car import make_can_msg + def create_steering_control(packer, bus, apply_steer, idx, lkas_active): values = { @@ -37,7 +39,7 @@ def create_steering_control_ct6(packer, canbus, apply_steer, v_ego, idx, enabled def create_adas_keepalive(bus): dat = b"\x00\x00\x00\x00\x00\x00\x00" - return [[0x409, 0, dat, bus], [0x40a, 0, dat, bus]] + return [make_can_msg(0x409, dat, bus), make_can_msg(0x40a, dat, bus)] def create_gas_regen_command(packer, bus, throttle, idx, acc_engaged, at_full_stop): values = { @@ -106,13 +108,13 @@ def create_adas_time_status(bus, tt, idx): chksum = 0x1000 - dat[0] - dat[1] - dat[2] - dat[3] chksum = chksum & 0xfff dat += [0x40 + (chksum >> 8), chksum & 0xff, 0x12] - return [0xa1, 0, bytes(dat), bus] + return make_can_msg(0xa1, bytes(dat), bus) def create_adas_steering_status(bus, idx): dat = [idx << 6, 0xf0, 0x20, 0, 0, 0] chksum = 0x60 + sum(dat) dat += [chksum >> 8, chksum & 0xff] - return [0x306, 0, bytes(dat), bus] + return make_can_msg(0x306, bytes(dat), bus) def create_adas_accelerometer_speed_status(bus, speed_ms, idx): spd = int(speed_ms * 16) & 0xfff @@ -125,10 +127,10 @@ def create_adas_accelerometer_speed_status(bus, speed_ms, idx): dat = [0x08, spd >> 4, ((spd & 0xf) << 4) | (accel >> 8), accel & 0xff, 0] chksum = 0x62 + far_range_mode + (idx << 2) + dat[0] + dat[1] + dat[2] + dat[3] + dat[4] dat += [(idx << 5) + (far_range_mode << 4) + (near_range_mode << 3) + (chksum >> 8), chksum & 0xff] - return [0x308, 0, bytes(dat), bus] + return make_can_msg(0x308, bytes(dat), bus) def create_adas_headlights_status(bus): - return [0x310, 0, b"\x42\x04", bus] + return make_can_msg(0x310, b"\x42\x04", bus) def create_lka_icon_command(bus, active, critical, steer): if active and steer == 1: @@ -143,7 +145,7 @@ def create_lka_icon_command(bus, active, critical, steer): dat = b"\x40\x40\x18" else: dat = b"\x00\x00\x00" - return [0x104c006c, 0, dat, bus] + return make_can_msg(0x104c006c, dat, bus) # TODO: WIP ''' @@ -173,7 +175,7 @@ def create_friction_brake_command_ct6(packer, bus, apply_brake, idx, near_stop, dat = packer.make_can_msg("EBCMFrictionBrakeCmd", 0, values)[2] # msg is 0x315 but we are doing the panda forwarding - return [0x314, 0, dat, 2] + return make_can_msg(0x314, dat, 2) def create_gas_regen_command_ct6(bus, throttle, idx, acc_engaged, at_full_stop): cntrs = [0, 7, 10, 13] @@ -186,6 +188,6 @@ def create_gas_regen_command_ct6(bus, throttle, idx, acc_engaged, at_full_stop): chk2 = (0x100 - gas_low - idx) & 0xff dat = [(idx << 6) | eng_bit, 0xc2 | full_stop, gas_high, gas_low, (1 - eng_bit) | (cntrs[idx] << 1), 0x5d - full_stop, chk1, chk2] - return [0x2cb, 0, "".join(map(chr, dat)), bus] + return make_can_msg(0x2cb, "".join(map(chr, dat)), bus) ''' diff --git a/selfdrive/car/gm/interface.py b/selfdrive/car/gm/interface.py index 213f767652..eee03b11d3 100755 --- a/selfdrive/car/gm/interface.py +++ b/selfdrive/car/gm/interface.py @@ -52,6 +52,9 @@ class CarInterface(CarInterfaceBase): ret.isPandaBlack = has_relay ret.enableCruise = False + # GM port is considered a community feature, since it disables AEB; + # TODO: make a port that uses a car harness and it only intercepts the camera + ret.communityFeature = True # Presence of a camera on the object bus is ok. # Have to go to read_only if ASCM is online (ACC-enabled cars), @@ -158,13 +161,13 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.deadzoneBP = [0.] ret.longitudinalTuning.deadzoneV = [0.] - ret.steerLimitAlert = True - ret.stoppingControl = True ret.startAccel = 0.8 ret.steerActuatorDelay = 0.1 # Default delay, not measured yet ret.steerRateCost = 1.0 + ret.steerLimitTimer = 0.4 + ret.radarTimeStep = 0.0667 # GM radar runs at 15Hz instead of standard 20Hz ret.steerControlType = car.CarParams.SteerControlType.torque return ret @@ -206,6 +209,7 @@ class CarInterface(CarInterfaceBase): # timer resets when the user uses the steering wheel. ret.steeringPressed = self.CS.steer_override ret.steeringTorque = self.CS.steer_torque_driver + ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False # cruise state ret.cruiseState.available = bool(self.CS.main_on) diff --git a/selfdrive/car/gm/radar_interface.py b/selfdrive/car/gm/radar_interface.py index 1fb39c5c43..d36db9422d 100755 --- a/selfdrive/car/gm/radar_interface.py +++ b/selfdrive/car/gm/radar_interface.py @@ -3,7 +3,7 @@ from __future__ import print_function import math import time from cereal import car -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from selfdrive.car.gm.interface import CanBus from selfdrive.car.gm.values import DBC, CAR from selfdrive.config import Conversions as CV @@ -55,10 +55,11 @@ class RadarInterface(RadarInterfaceBase): self.trigger_msg = LAST_RADAR_MSG self.updated_messages = set() + self.radar_ts = CP.radarTimeStep def update(self, can_strings): if self.rcp is None: - time.sleep(0.05) # nothing to do + time.sleep(self.radar_ts) # nothing to do return car.RadarData.new_message() vls = self.rcp.update_strings(can_strings) diff --git a/selfdrive/car/honda/carcontroller.py b/selfdrive/car/honda/carcontroller.py index 1058e2f21a..2ba51ce4e6 100644 --- a/selfdrive/car/honda/carcontroller.py +++ b/selfdrive/car/honda/carcontroller.py @@ -5,7 +5,7 @@ from common.numpy_fast import clip from selfdrive.car import create_gas_command from selfdrive.car.honda import hondacan from selfdrive.car.honda.values import AH, CruiseButtons, CAR -from selfdrive.can.packer import CANPacker +from opendbc.can.packer import CANPacker def actuator_hystereses(brake, braking, brake_steady, v_ego, car_fingerprint): @@ -148,7 +148,7 @@ class CarController(): # Send dashboard UI commands. if (frame % 10) == 0: idx = (frame//10) % 4 - can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack)) + can_sends.extend(hondacan.create_ui_commands(self.packer, pcm_speed, hud, CS.CP.carFingerprint, CS.is_metric, idx, CS.CP.isPandaBlack, CS.stock_hud)) if CS.CP.radarOffCan: # If using stock ACC, spam cancel command to kill gas when OP disengages. @@ -164,7 +164,7 @@ class CarController(): ts = frame * DT_CTRL pump_on, self.last_pump_ts = brake_pump_hysteresis(apply_brake, self.apply_brake_last, self.last_pump_ts, ts) can_sends.append(hondacan.create_brake_command(self.packer, apply_brake, pump_on, - pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack)) + pcm_override, pcm_cancel_cmd, hud.fcw, idx, CS.CP.carFingerprint, CS.CP.isPandaBlack, CS.stock_brake)) self.apply_brake_last = apply_brake if CS.CP.enableGasInterceptor: diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 792542167b..55f86d25ce 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -2,8 +2,8 @@ from cereal import car from collections import defaultdict from common.numpy_fast import interp from common.kalman.simple_kalman import KF1D -from selfdrive.can.can_define import CANDefine -from selfdrive.can.parser import CANParser +from opendbc.can.can_define import CANDefine +from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH @@ -171,6 +171,20 @@ def get_can_parser(CP): def get_cam_can_parser(CP): signals = [] + if CP.carFingerprint in HONDA_BOSCH: + signals += [("ACCEL_COMMAND", "ACC_CONTROL", 0), + ("AEB_STATUS", "ACC_CONTROL", 0)] + else: + signals += [("COMPUTER_BRAKE", "BRAKE_COMMAND", 0), + ("AEB_REQ_1", "BRAKE_COMMAND", 0), + ("FCW", "BRAKE_COMMAND", 0), + ("CHIME", "BRAKE_COMMAND", 0), + ("FCM_OFF", "ACC_HUD", 0), + ("FCM_OFF_2", "ACC_HUD", 0), + ("FCM_PROBLEM", "ACC_HUD", 0), + ("ICONS", "ACC_HUD", 0)] + + # all hondas except CRV, RDX and 2019 Odyssey@China use 0xe4 for steering checks = [(0xe4, 100)] if CP.carFingerprint in [CAR.CRV, CAR.ACURA_RDX, CAR.ODYSSEY_CHN]: @@ -354,3 +368,16 @@ class CarState(): # TODO: discover the CAN msg that has the imperial unit bit for all other cars self.is_metric = not cp.vl["HUD_SETTING"]['IMPERIAL_UNIT'] if self.CP.carFingerprint in (CAR.CIVIC) else False + + if self.CP.carFingerprint in HONDA_BOSCH: + self.stock_aeb = bool(cp_cam.vl["ACC_CONTROL"]["AEB_STATUS"] and cp_cam.vl["ACC_CONTROL"]["ACCEL_COMMAND"] < -1e-5) + else: + self.stock_aeb = bool(cp_cam.vl["BRAKE_COMMAND"]["AEB_REQ_1"] and cp_cam.vl["BRAKE_COMMAND"]["COMPUTER_BRAKE"] > 1e-5) + + if self.CP.carFingerprint in HONDA_BOSCH: + self.stock_hud = False + self.stock_fcw = False + else: + self.stock_fcw = bool(cp_cam.vl["BRAKE_COMMAND"]["FCW"] != 0) + self.stock_hud = cp_cam.vl["ACC_HUD"] + self.stock_brake = cp_cam.vl["BRAKE_COMMAND"] diff --git a/selfdrive/car/honda/hondacan.py b/selfdrive/car/honda/hondacan.py index c6b9a12e83..855a0e7dce 100644 --- a/selfdrive/car/honda/hondacan.py +++ b/selfdrive/car/honda/hondacan.py @@ -1,22 +1,6 @@ -import struct from selfdrive.config import Conversions as CV from selfdrive.car.honda.values import CAR, HONDA_BOSCH -# *** Honda specific *** -def can_cksum(mm): - s = 0 - for c in mm: - s += (c>>4) - s += c & 0xF - s = 8-s - s %= 0x10 - return s - - -def fix(msg, addr): - msg2 = msg[0:-1] + (msg[-1] | can_cksum(struct.pack("I", addr)+msg)).to_bytes(1, 'little') - return msg2 - def get_pt_bus(car_fingerprint, has_relay): return 1 if car_fingerprint in HONDA_BOSCH and has_relay else 0 @@ -26,7 +10,7 @@ def get_lkas_cmd_bus(car_fingerprint, has_relay): return 2 if car_fingerprint in HONDA_BOSCH and not has_relay else 0 -def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay): +def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_cmd, fcw, idx, car_fingerprint, has_relay, stock_brake): # TODO: do we loose pressure if we keep pump off for long? brakelights = apply_brake > 0 brake_rq = apply_brake > 0 @@ -41,9 +25,8 @@ def create_brake_command(packer, apply_brake, pump_on, pcm_override, pcm_cancel_ "COMPUTER_BRAKE_REQUEST": brake_rq, "SET_ME_1": 1, "BRAKE_LIGHTS": brakelights, - "CHIME": 0, - # TODO: Why are there two bits for fcw? According to dbc file the first bit should also work - "FCW": fcw << 1, + "CHIME": stock_brake["CHIME"], # chime issued when disabling FCM + "FCW": fcw << 1, # TODO: Why are there two bits for fcw? "AEB_REQ_1": 0, "AEB_REQ_2": 0, "AEB_STATUS": 0, @@ -61,7 +44,7 @@ def create_steering_control(packer, apply_steer, lkas_active, car_fingerprint, i return packer.make_can_msg("STEERING_CONTROL", bus, values, idx) -def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay): +def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, has_relay, stock_hud): commands = [] bus_pt = get_pt_bus(car_fingerprint, has_relay) bus_lkas = get_lkas_cmd_bus(car_fingerprint, has_relay) @@ -77,6 +60,10 @@ def create_ui_commands(packer, pcm_speed, hud, car_fingerprint, is_metric, idx, 'IMPERIAL_UNIT': int(not is_metric), 'SET_ME_X01_2': 1, 'SET_ME_X01': 1, + "FCM_OFF": stock_hud["FCM_OFF"], + "FCM_OFF_2": stock_hud["FCM_OFF_2"], + "FCM_PROBLEM": stock_hud["FCM_PROBLEM"], + "ICONS": stock_hud["ICONS"], } commands.append(packer.make_can_msg("ACC_HUD", bus_pt, acc_hud_values, idx)) diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index 3c2ecfa8dd..9ffc92e75b 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -155,11 +155,10 @@ class CarInterface(CarInterfaceBase): cloudlog.warning("ECU Gas Interceptor: %r", ret.enableGasInterceptor) ret.enableCruise = not ret.enableGasInterceptor + ret.communityFeature = ret.enableGasInterceptor - # Optimized car params: tire_stiffness_factor and steerRatio are a result of a vehicle - # model optimization process. Certain Hondas have an extra steering sensor at the bottom - # of the steering rack, which improves controls quality as it removes the steering column - # torsion from feedback. + # Certain Hondas have an extra steering sensor at the bottom of the steering rack, + # which improves controls quality as it removes the steering column torsion from feedback. # Tire stiffness factor fictitiously lower if it includes the steering column torsion effect. # For modeling details, see p.198-200 in "The Science of Vehicle Dynamics (2014), M. Guiggiani" @@ -183,7 +182,7 @@ class CarInterface(CarInterfaceBase): elif candidate in (CAR.ACCORD, CAR.ACCORD_15, CAR.ACCORDH): stop_and_go = True if not candidate == CAR.ACCORDH: # Hybrid uses same brake msg as hatch - ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg + ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.83 ret.centerToFront = ret.wheelbase * 0.39 @@ -213,8 +212,8 @@ class CarInterface(CarInterfaceBase): ret.mass = 3572. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.62 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 16.89 # as spec - tire_stiffness_factor = 0.444 # not optimized yet + ret.steerRatio = 16.89 # as spec + tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] @@ -223,11 +222,11 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.CRV_5G: stop_and_go = True - ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg + ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 3410. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 16.0 # 12.3 is spec end-to-end + ret.steerRatio = 16.0 # 12.3 is spec end-to-end tire_stiffness_factor = 0.677 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -237,11 +236,11 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.CRV_HYBRID: stop_and_go = True - ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg + ret.safetyParam = 1 # Accord and CRV 5G use an alternate user brake msg ret.mass = 1667. + STD_CARGO_KG # mean of 4 models in kg ret.wheelbase = 2.66 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 16.0 # 12.3 is spec end-to-end + ret.steerRatio = 16.0 # 12.3 is spec end-to-end tire_stiffness_factor = 0.677 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -255,7 +254,7 @@ class CarInterface(CarInterfaceBase): ret.wheelbase = 2.53 ret.centerToFront = ret.wheelbase * 0.39 ret.steerRatio = 13.06 - tire_stiffness_factor = 0.75 # not optimized yet + tire_stiffness_factor = 0.75 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.25], [0.06]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] @@ -267,8 +266,8 @@ class CarInterface(CarInterfaceBase): ret.mass = 3935. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 2.68 ret.centerToFront = ret.wheelbase * 0.38 - ret.steerRatio = 15.0 # as spec - tire_stiffness_factor = 0.444 # not optimized yet + ret.steerRatio = 15.0 # as spec + tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] @@ -280,7 +279,7 @@ class CarInterface(CarInterfaceBase): ret.mass = 4471. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 3.00 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 14.35 # as spec + ret.steerRatio = 14.35 # as spec tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] ret.longitudinalTuning.kpBP = [0., 5., 35.] @@ -290,11 +289,11 @@ class CarInterface(CarInterfaceBase): elif candidate == CAR.ODYSSEY_CHN: stop_and_go = False - ret.mass = 1849.2 + STD_CARGO_KG # mean of 4 models in kg - ret.wheelbase = 2.90 # spec - ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY - ret.steerRatio = 14.35 # from CAR.ODYSSEY - tire_stiffness_factor = 0.82 # from CAR.ODYSSEY + ret.mass = 1849.2 + STD_CARGO_KG # mean of 4 models in kg + ret.wheelbase = 2.90 + ret.centerToFront = ret.wheelbase * 0.41 # from CAR.ODYSSEY + ret.steerRatio = 14.35 + tire_stiffness_factor = 0.82 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.45], [0.135]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] @@ -305,9 +304,9 @@ class CarInterface(CarInterfaceBase): stop_and_go = False ret.mass = 4204. * CV.LB_TO_KG + STD_CARGO_KG # average weight ret.wheelbase = 2.82 - ret.centerToFront = ret.wheelbase * 0.428 # average weight distribution - ret.steerRatio = 17.25 # as spec - tire_stiffness_factor = 0.444 # not optimized yet + ret.centerToFront = ret.wheelbase * 0.428 + ret.steerRatio = 17.25 # as spec + tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] @@ -319,8 +318,8 @@ class CarInterface(CarInterfaceBase): ret.mass = 4515. * CV.LB_TO_KG + STD_CARGO_KG ret.wheelbase = 3.18 ret.centerToFront = ret.wheelbase * 0.41 - ret.steerRatio = 15.59 # as spec - tire_stiffness_factor = 0.444 # not optimized yet + ret.steerRatio = 15.59 # as spec + tire_stiffness_factor = 0.444 ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.38], [0.11]] ret.longitudinalTuning.kpBP = [0., 5., 35.] ret.longitudinalTuning.kpV = [1.2, 0.8, 0.5] @@ -362,11 +361,11 @@ class CarInterface(CarInterfaceBase): ret.longitudinalTuning.deadzoneV = [0.] ret.stoppingControl = True - ret.steerLimitAlert = True ret.startAccel = 0.5 ret.steerActuatorDelay = 0.1 ret.steerRateCost = 0.5 + ret.steerLimitTimer = 0.8 return ret @@ -381,7 +380,7 @@ class CarInterface(CarInterfaceBase): # create message ret = car.CarState.new_message() - ret.canValid = self.cp.can_valid + ret.canValid = self.cp.can_valid and self.cp_cam.can_valid # speeds ret.vEgo = self.CS.v_ego @@ -435,6 +434,9 @@ class CarInterface(CarInterfaceBase): ret.doorOpen = not self.CS.door_all_closed ret.seatbeltUnlatched = not self.CS.seatbelt + ret.stockAeb = self.CS.stock_aeb + ret.stockFcw = self.CS.stock_fcw + if self.CS.left_blinker_on != self.CS.prev_left_blinker_on: be = car.CarState.ButtonEvent.new_message() be.type = ButtonType.leftBlinker @@ -483,9 +485,6 @@ class CarInterface(CarInterfaceBase): # events events = [] - # wait 1.0s before throwing the alert to avoid it popping when you turn off the car - if self.cp_cam.can_invalid_cnt >= 100 and self.CS.CP.carFingerprint not in HONDA_BOSCH and self.CP.enableCamera: - events.append(create_event('invalidGiraffeHonda', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) if self.CS.steer_error: events.append(create_event('steerUnavailable', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE, ET.PERMANENT])) elif self.CS.steer_warning: @@ -522,7 +521,7 @@ class CarInterface(CarInterfaceBase): # it can happen that car cruise disables while comma system is enabled: need to # keep braking if needed or if the speed is very low - if self.CP.enableCruise and not ret.cruiseState.enabled and c.actuators.brake <= 0.: + if self.CP.enableCruise and not ret.cruiseState.enabled and (c.actuators.brake <= 0. or not self.CP.openpilotLongitudinalControl): # non loud alert if cruise disbales below 25mph as expected (+ a little margin) if ret.vEgo < self.CP.minEnableSpeed + 2.: events.append(create_event('speedTooLow', [ET.IMMEDIATE_DISABLE])) @@ -549,7 +548,7 @@ class CarInterface(CarInterfaceBase): # KEEP THIS EVENT LAST! send enable event if button is pressed and there are # NO_ENTRY events, so controlsd will display alerts. Also not send enable events # too close in time, so a no_entry will not be followed by another one. - # TODO: button press should be the only thing that triggers enble + # TODO: button press should be the only thing that triggers enable if ((cur_time - self.last_enable_pressed) < 0.2 and (cur_time - self.last_enable_sent) > 0.2 and ret.cruiseState.enabled) or \ diff --git a/selfdrive/car/honda/radar_interface.py b/selfdrive/car/honda/radar_interface.py index 2415e95a35..3c07cd74cd 100755 --- a/selfdrive/car/honda/radar_interface.py +++ b/selfdrive/car/honda/radar_interface.py @@ -2,8 +2,7 @@ import os import time from cereal import car -from selfdrive.can.parser import CANParser -from common.realtime import DT_RDR +from opendbc.can.parser import CANParser from selfdrive.car.interfaces import RadarInterfaceBase def _create_nidec_can_parser(): @@ -27,8 +26,9 @@ class RadarInterface(RadarInterfaceBase): self.radar_fault = False self.radar_wrong_config = False self.radar_off_can = CP.radarOffCan + self.radar_ts = CP.radarTimeStep - self.delay = int(0.1 / DT_RDR) # 0.1s delay of radar + self.delay = int(round(0.1 / CP.radarTimeStep)) # 0.1s delay of radar # Nidec self.rcp = _create_nidec_can_parser() @@ -40,7 +40,7 @@ class RadarInterface(RadarInterfaceBase): # radard at 20Hz and return no points if self.radar_off_can: if 'NO_RADAR_SLEEP' not in os.environ: - time.sleep(0.05) + time.sleep(self.radar_ts) return car.RadarData.new_message() vls = self.rcp.update_strings(can_strings) diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index daf3ca3142..0a6144bab6 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -1,10 +1,7 @@ from selfdrive.car import apply_std_steer_torque_limits -from selfdrive.car.hyundai.hyundaican import create_lkas11, create_lkas12, \ - create_1191, create_1156, \ - create_clu11 +from selfdrive.car.hyundai.hyundaican import create_lkas11, create_clu11 from selfdrive.car.hyundai.values import Buttons, SteerLimitParams -from selfdrive.can.packer import CANPacker - +from opendbc.can.packer import CANPacker class CarController(): @@ -14,18 +11,15 @@ class CarController(): self.lkas11_cnt = 0 self.cnt = 0 self.last_resume_cnt = 0 - # True when giraffe switch 2 is low and we need to replace all the camera messages - # otherwise we forward the camera msgs and we just replace the lkas cmd signals - self.camera_disconnected = False - self.packer = CANPacker(dbc_name) + self.steer_rate_limited = False def update(self, enabled, CS, actuators, pcm_cancel_cmd, hud_alert): ### Steering Torque - apply_steer = actuators.steer * SteerLimitParams.STEER_MAX - - apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) + new_steer = actuators.steer * SteerLimitParams.STEER_MAX + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, SteerLimitParams) + self.steer_rate_limited = new_steer != apply_steer if not enabled: apply_steer = 0 @@ -39,16 +33,8 @@ class CarController(): self.lkas11_cnt = self.cnt % 0x10 self.clu11_cnt = self.cnt % 0x10 - if self.camera_disconnected: - if (self.cnt % 10) == 0: - can_sends.append(create_lkas12()) - if (self.cnt % 50) == 0: - can_sends.append(create_1191()) - if (self.cnt % 7) == 0: - can_sends.append(create_1156()) - can_sends.append(create_lkas11(self.packer, self.car_fingerprint, apply_steer, steer_req, self.lkas11_cnt, - enabled, CS.lkas11, hud_alert, keep_stock=(not self.camera_disconnected))) + enabled, CS.lkas11, hud_alert, keep_stock=True)) if pcm_cancel_cmd: can_sends.append(create_clu11(self.packer, CS.clu11, Buttons.CANCEL)) diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index a7e3448a0e..7ebb8d70d6 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -1,6 +1,6 @@ from cereal import car from selfdrive.car.hyundai.values import DBC, STEER_THRESHOLD -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV from common.kalman.simple_kalman import KF1D diff --git a/selfdrive/car/hyundai/hyundaican.py b/selfdrive/car/hyundai/hyundaican.py index 8783932055..910e05ad7c 100644 --- a/selfdrive/car/hyundai/hyundaican.py +++ b/selfdrive/car/hyundai/hyundaican.py @@ -3,9 +3,6 @@ from selfdrive.car.hyundai.values import CHECKSUM hyundai_checksum = crcmod.mkCrcFun(0x11D, initCrc=0xFD, rev=False, xorOut=0xdf) -def make_can_msg(addr, dat, alt): - return [addr, 0, dat, alt] - def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, lkas11, hud_alert, keep_stock=False): values = { "CF_Lkas_Bca_R": 3 if enabled else 0, @@ -47,17 +44,6 @@ def create_lkas11(packer, car_fingerprint, apply_steer, steer_req, cnt, enabled, return packer.make_can_msg("LKAS11", 0, values) -def create_lkas12(): - return make_can_msg(1342, b"\x00\x00\x00\x00\x60\x05", 0) - - -def create_1191(): - return make_can_msg(1191, b"\x01\x00", 0) - - -def create_1156(): - return make_can_msg(1156, b"\x08\x20\xfe\x3f\x00\xe0\xfd\x3f", 0) - def create_clu11(packer, clu11, button): values = { "CF_Clu_CruiseSwState": button, diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 7d6e372cf6..39d04d3bc8 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -52,6 +52,7 @@ class CarInterface(CarInterfaceBase): ret.steerActuatorDelay = 0.1 # Default delay ret.steerRateCost = 0.5 + ret.steerLimitTimer = 0.4 tire_stiffness_factor = 1. if candidate == CAR.SANTA_FE: @@ -143,7 +144,6 @@ class CarInterface(CarInterfaceBase): ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay ret.openpilotLongitudinalControl = False - ret.steerLimitAlert = False ret.stoppingControl = False ret.startAccel = 0.0 @@ -159,7 +159,7 @@ class CarInterface(CarInterfaceBase): # create message ret = car.CarState.new_message() - ret.canValid = self.cp.can_valid # TODO: check cp_cam validity + ret.canValid = self.cp.can_valid and self.cp_cam.can_valid # speeds ret.vEgo = self.CS.v_ego diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py index 0c7c2c81dc..7a32752781 100644 --- a/selfdrive/car/interfaces.py +++ b/selfdrive/car/interfaces.py @@ -33,11 +33,12 @@ class RadarInterfaceBase(): def __init__(self, CP): self.pts = {} self.delay = 0 + self.radar_ts = CP.radarTimeStep def update(self, can_strings): ret = car.RadarData.new_message() if 'NO_RADAR_SLEEP' not in os.environ: - time.sleep(0.05) # radard runs on RI updates + time.sleep(self.radar_ts) # radard runs on RI updates return ret diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index a46b93aa61..30df9f2fd3 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -2,7 +2,7 @@ from cereal import car from selfdrive.config import Conversions as CV from selfdrive.swaglog import cloudlog -import selfdrive.messaging as messaging +import cereal.messaging as messaging from selfdrive.car import gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase diff --git a/selfdrive/car/subaru/carcontroller.py b/selfdrive/car/subaru/carcontroller.py index e7b9b7d94d..69f573fef8 100644 --- a/selfdrive/car/subaru/carcontroller.py +++ b/selfdrive/car/subaru/carcontroller.py @@ -2,7 +2,7 @@ from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.subaru import subarucan from selfdrive.car.subaru.values import DBC -from selfdrive.can.packer import CANPacker +from opendbc.can.packer import CANPacker class CarControllerParams(): @@ -25,6 +25,7 @@ class CarController(): self.car_fingerprint = car_fingerprint self.es_distance_cnt = -1 self.es_lkas_cnt = -1 + self.steer_rate_limited = False # Setup detection helper. Routes commands to # an appropriate CAN bus number. @@ -48,8 +49,9 @@ class CarController(): # limits due to driver torque - apply_steer = int(round(apply_steer)) - apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steer_torque_driver, P) + new_steer = int(round(apply_steer)) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steer_torque_driver, P) + self.steer_rate_limited = new_steer != apply_steer lkas_enabled = enabled and not CS.steer_not_allowed diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index f0256de3d0..57bd1bee79 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -1,7 +1,7 @@ import copy from common.kalman.simple_kalman import KF1D from selfdrive.config import Conversions as CV -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from selfdrive.car.subaru.values import DBC, STEER_THRESHOLD def get_powertrain_can_parser(CP): diff --git a/selfdrive/car/subaru/interface.py b/selfdrive/car/subaru/interface.py index e2195b00a1..cd102766df 100644 --- a/selfdrive/car/subaru/interface.py +++ b/selfdrive/car/subaru/interface.py @@ -46,13 +46,13 @@ class CarInterface(CarInterfaceBase): ret.safetyModel = car.CarParams.SafetyModel.subaru ret.enableCruise = True - ret.steerLimitAlert = True # force openpilot to fake the stock camera, since car harness is not supported yet and old style giraffe (with switches) # was never released ret.enableCamera = True ret.steerRateCost = 0.7 + ret.steerLimitTimer = 0.4 if candidate in [CAR.IMPREZA]: ret.mass = 1568. + STD_CARGO_KG @@ -124,6 +124,7 @@ class CarInterface(CarInterfaceBase): # timer resets when the user uses the steering wheel. ret.steeringPressed = self.CS.steer_override ret.steeringTorque = self.CS.steer_torque_driver + ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False ret.gas = self.CS.pedal_gas / 255. ret.gasPressed = self.CS.user_gas_pressed diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index ae37ec463d..0a8f7979d4 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -1,13 +1,11 @@ from cereal import car from common.numpy_fast import clip, interp -from selfdrive.car import apply_toyota_steer_torque_limits -from selfdrive.car import create_gas_command -from selfdrive.car.toyota.toyotacan import make_can_msg, \ - create_steer_command, create_ui_command, \ +from selfdrive.car import apply_toyota_steer_torque_limits, create_gas_command, make_can_msg +from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_command, \ create_ipas_steer_command, create_accel_command, \ create_acc_cancel_command, create_fcw_command -from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, TSS2_CAR, SteerLimitParams -from selfdrive.can.packer import CANPacker +from selfdrive.car.toyota.values import CAR, ECU, STATIC_MSGS, SteerLimitParams +from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -100,6 +98,7 @@ class CarController(): self.steer_angle_enabled = False self.ipas_reset_counter = 0 self.last_fault_frame = -200 + self.steer_rate_limited = False self.fake_ecus = set() if enable_camera: self.fake_ecus.add(ECU.CAM) @@ -128,9 +127,9 @@ class CarController(): apply_accel = clip(apply_accel * ACCEL_SCALE, ACCEL_MIN, ACCEL_MAX) # steer torque - apply_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) - - apply_steer = apply_toyota_steer_torque_limits(apply_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams) + new_steer = int(round(actuators.steer * SteerLimitParams.STEER_MAX)) + apply_steer = apply_toyota_steer_torque_limits(new_steer, self.last_steer, CS.steer_torque_motor, SteerLimitParams) + self.steer_rate_limited = new_steer != apply_steer # only cut torque when steer state is a known fault if CS.steer_state in [9, 25]: @@ -199,14 +198,14 @@ class CarController(): elif ECU.APGS in self.fake_ecus: can_sends.append(create_ipas_steer_command(self.packer, 0, 0, True)) - # accel cmd comes from DSU, but we can spam can to cancel the system even if we are using lat only control - if (frame % 3 == 0 and ECU.DSU in self.fake_ecus) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): + # we can spam can to cancel the system even if we are using lat only control + if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and ECU.CAM in self.fake_ecus): lead = lead or CS.v_ego < 12. # at low speed we always assume the lead is present do ACC can be engaged # Lexus IS uses a different cancellation message if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS: can_sends.append(create_acc_cancel_command(self.packer)) - elif ECU.DSU in self.fake_ecus: + elif CS.CP.openpilotLongitudinalControl: can_sends.append(create_accel_command(self.packer, apply_accel, pcm_cancel_cmd, self.standstill_req, lead)) else: can_sends.append(create_accel_command(self.packer, 0, pcm_cancel_cmd, False, lead)) @@ -236,13 +235,13 @@ class CarController(): if (frame % 100 == 0 or send_ui) and ECU.CAM in self.fake_ecus: can_sends.append(create_ui_command(self.packer, steer, pcm_cancel_cmd, left_line, right_line, left_lane_depart, right_lane_depart)) - if frame % 100 == 0 and ECU.DSU in self.fake_ecus and self.car_fingerprint not in TSS2_CAR: + if frame % 100 == 0 and ECU.DSU in self.fake_ecus: can_sends.append(create_fcw_command(self.packer, fcw)) #*** static msgs *** for (addr, ecu, cars, bus, fr_step, vl) in STATIC_MSGS: if frame % fr_step == 0 and ecu in self.fake_ecus and self.car_fingerprint in cars: - can_sends.append(make_can_msg(addr, vl, bus, False)) + can_sends.append(make_can_msg(addr, vl, bus)) return can_sends diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index fc0260ef56..0fbf7a7970 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -1,8 +1,8 @@ from cereal import car from common.numpy_fast import mean from common.kalman.simple_kalman import KF1D -from selfdrive.can.can_define import CANDefine -from selfdrive.can.parser import CANParser +from opendbc.can.can_define import CANDefine +from opendbc.can.parser import CANParser from selfdrive.config import Conversions as CV from selfdrive.car.toyota.values import CAR, DBC, STEER_THRESHOLD, TSS2_CAR, NO_DSU_CAR @@ -86,7 +86,7 @@ def get_can_parser(CP): def get_cam_can_parser(CP): - signals = [] + signals = [("FORCE", "PRE_COLLISION", 0), ("PRECOLLISION_ACTIVE", "PRE_COLLISION", 0)] # use steering message to check if panda is connected to frc checks = [("STEERING_LKA", 42)] @@ -118,7 +118,7 @@ class CarState(): K=[[0.12287673], [0.29666309]]) self.v_ego = 0.0 - def update(self, cp): + def update(self, cp, cp_cam): # update prevs, update must run once per loop self.prev_left_blinker_on = self.left_blinker_on self.prev_right_blinker_on = self.right_blinker_on @@ -198,3 +198,5 @@ class CarState(): self.generic_toggle = cp.vl["AUTOPARK_STATUS"]['STATE'] != 0 else: self.generic_toggle = bool(cp.vl["LIGHT_STALK"]['AUTO_HIGH_BEAM']) + + self.stock_aeb = bool(cp_cam.vl["PRE_COLLISION"]["PRECOLLISION_ACTIVE"] and cp_cam.vl["PRE_COLLISION"]["FORCE"] < -1e-5) diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py index a351590ca1..3c6772228c 100755 --- a/selfdrive/car/toyota/interface.py +++ b/selfdrive/car/toyota/interface.py @@ -48,10 +48,10 @@ class CarInterface(CarInterfaceBase): ret.safetyModel = car.CarParams.SafetyModel.toyota - # pedal - ret.enableCruise = not ret.enableGasInterceptor + ret.enableCruise = True ret.steerActuatorDelay = 0.12 # Default delay, Prius has larger delay + ret.steerLimitTimer = 0.4 if candidate not in [CAR.PRIUS, CAR.RAV4, CAR.RAV4H]: # These cars use LQR/INDI ret.lateralTuning.init('pid') @@ -247,10 +247,11 @@ class CarInterface(CarInterfaceBase): ret.brakeMaxV = [1.] ret.enableCamera = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.CAM) or has_relay - ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.DSU) or (has_relay and candidate in TSS2_CAR) + # In TSS2 cars the camera does long control + ret.enableDsu = is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.DSU) and candidate not in TSS2_CAR ret.enableApgs = False # is_ecu_disconnected(fingerprint[0], FINGERPRINTS, ECU_FINGERPRINT, candidate, ECU.APGS) ret.enableGasInterceptor = 0x201 in fingerprint[0] - ret.openpilotLongitudinalControl = ret.enableCamera and ret.enableDsu + ret.openpilotLongitudinalControl = ret.enableCamera and (ret.enableDsu or candidate in TSS2_CAR) cloudlog.warning("ECU Camera Simulated: %r", ret.enableCamera) cloudlog.warning("ECU DSU Simulated: %r", ret.enableDsu) cloudlog.warning("ECU APGS Simulated: %r", ret.enableApgs) @@ -260,7 +261,8 @@ class CarInterface(CarInterfaceBase): # to a negative value, so it won't matter. ret.minEnableSpeed = -1. if (stop_and_go or ret.enableGasInterceptor) else 19. * CV.MPH_TO_MS - ret.steerLimitAlert = False + # removing the DSU disables AEB and it's considered a community maintained feature + ret.communityFeature = ret.enableGasInterceptor or ret.enableDsu ret.longitudinalTuning.deadzoneBP = [0., 9.] ret.longitudinalTuning.deadzoneV = [0., .15] @@ -288,12 +290,12 @@ class CarInterface(CarInterfaceBase): self.cp.update_strings(can_strings) self.cp_cam.update_strings(can_strings) - self.CS.update(self.cp) + self.CS.update(self.cp, self.cp_cam) # create message ret = car.CarState.new_message() - ret.canValid = self.cp.can_valid + ret.canValid = self.cp.can_valid and self.cp_cam.can_valid # speeds ret.vEgo = self.CS.v_ego @@ -329,6 +331,7 @@ class CarInterface(CarInterfaceBase): ret.steeringTorque = self.CS.steer_torque_driver ret.steeringTorqueEps = self.CS.steer_torque_motor ret.steeringPressed = self.CS.steer_override + ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False # cruise state ret.cruiseState.enabled = self.CS.pcm_acc_active @@ -365,29 +368,30 @@ class CarInterface(CarInterfaceBase): ret.seatbeltUnlatched = not self.CS.seatbelt ret.genericToggle = self.CS.generic_toggle + ret.stockAeb = self.CS.stock_aeb # events events = [] - if self.cp_cam.can_invalid_cnt >= 100 and self.CP.enableCamera: - events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT, ET.NO_ENTRY])) - if not ret.gearShifter == GearShifter.drive and self.CP.enableDsu: + if self.cp_cam.can_invalid_cnt >= 200 and self.CP.enableCamera: + events.append(create_event('invalidGiraffeToyota', [ET.PERMANENT])) + if not ret.gearShifter == GearShifter.drive and self.CP.openpilotLongitudinalControl: events.append(create_event('wrongGear', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.doorOpen: events.append(create_event('doorOpen', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if ret.seatbeltUnlatched: events.append(create_event('seatbeltNotLatched', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if self.CS.esp_disabled and self.CP.enableDsu: + if self.CS.esp_disabled and self.CP.openpilotLongitudinalControl: events.append(create_event('espDisabled', [ET.NO_ENTRY, ET.SOFT_DISABLE])) - if not self.CS.main_on and self.CP.enableDsu: + if not self.CS.main_on and self.CP.openpilotLongitudinalControl: events.append(create_event('wrongCarMode', [ET.NO_ENTRY, ET.USER_DISABLE])) - if ret.gearShifter == GearShifter.reverse and self.CP.enableDsu: + if ret.gearShifter == GearShifter.reverse and self.CP.openpilotLongitudinalControl: events.append(create_event('reverseGear', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) if self.CS.steer_error: events.append(create_event('steerTempUnavailable', [ET.NO_ENTRY, ET.WARNING])) - if self.CS.low_speed_lockout and self.CP.enableDsu: + if self.CS.low_speed_lockout and self.CP.openpilotLongitudinalControl: events.append(create_event('lowSpeedLockout', [ET.NO_ENTRY, ET.PERMANENT])) - if ret.vEgo < self.CP.minEnableSpeed and self.CP.enableDsu: + if ret.vEgo < self.CP.minEnableSpeed and self.CP.openpilotLongitudinalControl: events.append(create_event('speedTooLow', [ET.NO_ENTRY])) if c.actuators.gas > 0.1: # some margin on the actuator to not false trigger cancellation while stopping diff --git a/selfdrive/car/toyota/radar_interface.py b/selfdrive/car/toyota/radar_interface.py index a24a911a44..80977b706c 100755 --- a/selfdrive/car/toyota/radar_interface.py +++ b/selfdrive/car/toyota/radar_interface.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import os import time -from selfdrive.can.parser import CANParser +from opendbc.can.parser import CANParser from cereal import car from selfdrive.car.toyota.values import NO_DSU_CAR, DBC, TSS2_CAR from selfdrive.car.interfaces import RadarInterfaceBase @@ -35,6 +35,7 @@ class RadarInterface(RadarInterfaceBase): self.track_id = 0 self.delay = 0 # Delay of radar + self.radar_ts = CP.radarTimeStep if CP.carFingerprint in TSS2_CAR: self.RADAR_A_MSGS = list(range(0x180, 0x190)) @@ -55,7 +56,7 @@ class RadarInterface(RadarInterfaceBase): def update(self, can_strings): if self.no_radar: - time.sleep(0.05) + time.sleep(self.radar_ts) return car.RadarData.new_message() vls = self.rcp.update_strings(can_strings) diff --git a/selfdrive/car/toyota/toyotacan.py b/selfdrive/car/toyota/toyotacan.py index 92d1c2ea26..48d6229c95 100644 --- a/selfdrive/car/toyota/toyotacan.py +++ b/selfdrive/car/toyota/toyotacan.py @@ -1,27 +1,3 @@ -import struct - - -# *** Toyota specific *** - -def fix(msg, addr): - checksum = 0 - idh = (addr & 0xff00) >> 8 - idl = (addr & 0xff) - - checksum = idh + idl + len(msg) + 1 - for d_byte in msg: - checksum += d_byte - - #return msg + chr(checksum & 0xFF) - return msg + struct.pack("B", checksum & 0xFF) - - -def make_can_msg(addr, dat, alt, cks=False): - if cks: - dat = fix(dat, addr) - return [addr, 0, dat, alt] - - def create_ipas_steer_command(packer, steer, enabled, apgs_enabled): """Creates a CAN message for the Toyota Steer Command.""" if steer < 0: diff --git a/selfdrive/car/vin.py b/selfdrive/car/vin.py index eac0316db4..605e22d50d 100755 --- a/selfdrive/car/vin.py +++ b/selfdrive/car/vin.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -import selfdrive.messaging as messaging +import cereal.messaging as messaging from selfdrive.boardd.boardd import can_list_to_can_capnp VIN_UNKNOWN = "0" * 17 @@ -44,6 +44,7 @@ class VinQuery(): self.responded = False self.never_responded = True self.dat = b"" + self.got_vin = False self.vin = VIN_UNKNOWN def check_response(self, msg): @@ -57,20 +58,24 @@ class VinQuery(): if self.cnt == self.cnts[self.step]: self.responded = True self.step += 1 + if self.step == len(self.cnts): + self.got_vin = True def send_query(self, sendcan): - # keep sending VIN qury if ECU isn't responsing. + # keep sending VIN query if ECU isn't responsing. # sendcan is probably not ready due to the zmq slow joiner syndrome - if self.never_responded or (self.responded and self.step < len(self.cnts)): + if self.never_responded or (self.responded and not self.got_vin): sendcan.send(can_list_to_can_capnp([self.query_ext_msgs[self.step]], msgtype='sendcan')) sendcan.send(can_list_to_can_capnp([self.query_nor_msgs[self.step]], msgtype='sendcan')) self.responded = False self.cnt = 0 def get_vin(self): - # only report vin if procedure is finished - if self.step == len(self.cnts) and self.cnt == self.cnts[-1]: - self.vin = self.dat[3:].decode('utf8') + if self.got_vin: + try: + self.vin = self.dat[3:].decode('utf8') + except UnicodeDecodeError: + pass # have seen unexpected non-unicode characters return self.vin @@ -79,11 +84,13 @@ def get_vin(logcan, sendcan, bus, query_time=1.): frame = 0 # 1s max of VIN query time - while frame < query_time * 100: + while frame < query_time * 100 and not vin_query.got_vin: a = messaging.get_one_can(logcan) for can in a.can: vin_query.check_response(can) + if vin_query.got_vin: + break vin_query.send_query(sendcan) frame += 1 diff --git a/selfdrive/car/volkswagen/carcontroller.py b/selfdrive/car/volkswagen/carcontroller.py index c17b5f60a1..610e1d5a09 100644 --- a/selfdrive/car/volkswagen/carcontroller.py +++ b/selfdrive/car/volkswagen/carcontroller.py @@ -2,7 +2,7 @@ from cereal import car from selfdrive.car import apply_std_steer_torque_limits from selfdrive.car.volkswagen import volkswagencan from selfdrive.car.volkswagen.values import DBC, MQB_LDW_MESSAGES, BUTTON_STATES, CarControllerParams -from selfdrive.can.packer import CANPacker +from opendbc.can.packer import CANPacker VisualAlert = car.CarControl.HUDControl.VisualAlert @@ -14,7 +14,7 @@ class CarController(): # Setup detection helper. Routes commands to an appropriate CAN bus number. self.canbus = canbus - self.packer_gw = CANPacker(DBC[car_fingerprint]['pt']) + self.packer_pt = CANPacker(DBC[car_fingerprint]['pt']) self.hcaSameTorqueCount = 0 self.hcaEnabledFrameCount = 0 @@ -23,6 +23,8 @@ class CarController(): self.graMsgStartFramePrev = 0 self.graMsgBusCounterPrev = 0 + self.steer_rate_limited = False + def update(self, enabled, CS, frame, actuators, visual_alert, audible_alert, leftLaneVisible, rightLaneVisible): """ Controls thread """ @@ -53,8 +55,9 @@ class CarController(): # is inherently handled by scaling to STEER_MAX. The rack doesn't seem # to care about up/down rate, but we have some evidence it may do its # own rate limiting, and matching OP helps for accurate tuning. - apply_steer = int(round(actuators.steer * P.STEER_MAX)) - apply_steer = apply_std_steer_torque_limits(apply_steer, self.apply_steer_last, CS.steeringTorque, P) + new_steer = int(round(actuators.steer * P.STEER_MAX)) + apply_steer = apply_std_steer_torque_limits(new_steer, self.apply_steer_last, CS.steeringTorque, P) + self.steer_rate_limited = new_steer != apply_steer # FAULT AVOIDANCE: HCA must not be enabled for >360 seconds. Sending # a single frame with HCA disabled is an effective workaround. @@ -99,7 +102,7 @@ class CarController(): self.apply_steer_last = apply_steer idx = (frame / P.HCA_STEP) % 16 - can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_gw, canbus.gateway, apply_steer, + can_sends.append(volkswagencan.create_mqb_steering_control(self.packer_pt, canbus.pt, apply_steer, idx, hcaEnabled)) #-------------------------------------------------------------------------- @@ -120,7 +123,7 @@ class CarController(): else: hud_alert = MQB_LDW_MESSAGES["none"] - can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_gw, canbus.gateway, hcaEnabled, + can_sends.append(volkswagencan.create_mqb_hud_control(self.packer_pt, canbus.pt, hcaEnabled, CS.steeringPressed, hud_alert, leftLaneVisible, rightLaneVisible)) @@ -179,7 +182,7 @@ class CarController(): if self.graMsgSentCount == 0: self.graMsgStartFramePrev = frame idx = (CS.graMsgBusCounter + 1) % 16 - can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_gw, canbus.extended, self.graButtonStatesToSend, CS, idx)) + can_sends.append(volkswagencan.create_mqb_acc_buttons_control(self.packer_pt, canbus.pt, self.graButtonStatesToSend, CS, idx)) self.graMsgSentCount += 1 if self.graMsgSentCount >= P.GRA_VBP_COUNT: self.graButtonStatesToSend = None diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index a39cbae8c5..58a19274d3 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -2,14 +2,13 @@ import numpy as np from cereal import car from common.kalman.simple_kalman import KF1D from selfdrive.config import Conversions as CV -from selfdrive.can.parser import CANParser -from selfdrive.can.can_define import CANDefine -from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES -from selfdrive.car.volkswagen.carcontroller import CarControllerParams +from opendbc.can.parser import CANParser +from opendbc.can.can_define import CANDefine +from selfdrive.car.volkswagen.values import DBC, BUTTON_STATES, CarControllerParams GEAR = car.CarState.GearShifter -def get_mqb_gateway_can_parser(CP, canbus): +def get_mqb_pt_can_parser(CP, canbus): # this function generates lists for signal, messages and initial values signals = [ # sig_name, sig_address, default @@ -45,6 +44,9 @@ def get_mqb_gateway_can_parser(CP, canbus): ("KBI_MFA_v_Einheit_02", "Einheiten_01", 0), # MPH vs KMH speed display ("KBI_Handbremse", "Kombi_01", 0), # Manual handbrake applied ("TSK_Fahrzeugmasse_02", "Motor_16", 0), # Estimated vehicle mass from drivetrain coordinator + ("ACC_Status_ACC", "ACC_06", 0), # ACC engagement status + ("ACC_Typ", "ACC_06", 0), # ACC type (follow to stop, stop&go) + ("SetSpeed", "ACC_02", 0), # ACC set speed ("GRA_Hauptschalter", "GRA_ACC_01", 0), # ACC button, on/off ("GRA_Abbrechen", "GRA_ACC_01", 0), # ACC button, cancel ("GRA_Tip_Setzen", "GRA_ACC_01", 0), # ACC button, set @@ -65,8 +67,10 @@ def get_mqb_gateway_can_parser(CP, canbus): ("ESP_19", 100), # From J104 ABS/ESP controller ("ESP_05", 50), # From J104 ABS/ESP controller ("ESP_21", 50), # From J104 ABS/ESP controller + ("ACC_06", 50), # From J428 ACC radar control module ("Motor_20", 50), # From J623 Engine control module ("GRA_ACC_01", 33), # From J??? steering wheel control buttons + ("ACC_02", 17), # From J428 ACC radar control module ("Getriebe_11", 20), # From J743 Auto transmission control module ("Gateway_72", 10), # From J533 CAN gateway (aggregated data) ("Motor_14", 10), # From J623 Engine control module @@ -76,24 +80,24 @@ def get_mqb_gateway_can_parser(CP, canbus): ("Einheiten_01", 1), # From J??? not known if gateway, cluster, or BCM ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.gateway) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.pt) -def get_mqb_extended_can_parser(CP, canbus): +# A single signal is monitored from the camera CAN bus, and then ignored, +# so the presence of CAN traffic can be verified with cam_cp.valid. + +def get_mqb_cam_can_parser(CP, canbus): signals = [ # sig_name, sig_address, default - ("ACC_Status_ACC", "ACC_06", 0), # ACC engagement status - ("ACC_Typ", "ACC_06", 0), # ACC type (follow to stop, stop&go) - ("SetSpeed", "ACC_02", 0), # ACC set speed + ("Kombi_Lamp_Green", "LDW_02", 0), # Lane Assist status LED ] checks = [ # sig_address, frequency - ("ACC_06", 50), # From J428 ACC radar control module - ("ACC_02", 17), # From J428 ACC radar control module + ("LDW_02", 10) # From R242 Driver assistance camera ] - return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.extended) + return CANParser(DBC[CP.carFingerprint]['pt'], signals, checks, canbus.cam) def parse_gear_shifter(gear, vals): # Return mapping of gearshift position to selected gear. @@ -123,12 +127,12 @@ class CarState(): C=[1., 0.], K=[[0.12287673], [0.29666309]]) - def update(self, gw_cp, ex_cp): + def update(self, pt_cp): # Update vehicle speed and acceleration from ABS wheel speeds. - self.wheelSpeedFL = gw_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS - self.wheelSpeedFR = gw_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS - self.wheelSpeedRL = gw_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS - self.wheelSpeedRR = gw_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS + self.wheelSpeedFL = pt_cp.vl["ESP_19"]['ESP_VL_Radgeschw_02'] * CV.KPH_TO_MS + self.wheelSpeedFR = pt_cp.vl["ESP_19"]['ESP_VR_Radgeschw_02'] * CV.KPH_TO_MS + self.wheelSpeedRL = pt_cp.vl["ESP_19"]['ESP_HL_Radgeschw_02'] * CV.KPH_TO_MS + self.wheelSpeedRR = pt_cp.vl["ESP_19"]['ESP_HR_Radgeschw_02'] * CV.KPH_TO_MS self.vEgoRaw = float(np.mean([self.wheelSpeedFL, self.wheelSpeedFR, self.wheelSpeedRL, self.wheelSpeedRR])) v_ego_x = self.v_ego_kf.update(self.vEgoRaw) @@ -138,40 +142,40 @@ class CarState(): # Update steering angle, rate, yaw rate, and driver input torque. VW send # the sign/direction in a separate signal so they must be recombined. - self.steeringAngle = gw_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(gw_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] - self.steeringRate = gw_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(gw_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] - self.steeringTorque = gw_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(gw_cp.vl["EPS_01"]['Driver_Strain_VZ'])] + self.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] + self.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1,-1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])] + self.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1,-1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])] self.steeringPressed = abs(self.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE - self.yawRate = gw_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(gw_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD + self.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1,-1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD # Update gas, brakes, and gearshift. - self.gas = gw_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0 + self.gas = pt_cp.vl["Motor_20"]['MO_Fahrpedalrohwert_01'] / 100.0 self.gasPressed = self.gas > 0 - self.brake = gw_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects - self.brakePressed = bool(gw_cp.vl["ESP_05"]['ESP_Fahrer_bremst']) - self.brakeLights = bool(gw_cp.vl["ESP_05"]['ESP_Status_Bremsdruck']) + self.brake = pt_cp.vl["ESP_05"]['ESP_Bremsdruck'] / 250.0 # FIXME: this is pressure in Bar, not sure what OP expects + self.brakePressed = bool(pt_cp.vl["ESP_05"]['ESP_Fahrer_bremst']) + self.brakeLights = bool(pt_cp.vl["ESP_05"]['ESP_Status_Bremsdruck']) # Update gear and/or clutch position data. - can_gear_shifter = int(gw_cp.vl["Getriebe_11"]['GE_Fahrstufe']) + can_gear_shifter = int(pt_cp.vl["Getriebe_11"]['GE_Fahrstufe']) self.gearShifter = parse_gear_shifter(can_gear_shifter, self.shifter_values) # Update door and trunk/hatch lid open status. - self.doorOpen = any([gw_cp.vl["Gateway_72"]['ZV_FT_offen'], - gw_cp.vl["Gateway_72"]['ZV_BT_offen'], - gw_cp.vl["Gateway_72"]['ZV_HFS_offen'], - gw_cp.vl["Gateway_72"]['ZV_HBFS_offen'], - gw_cp.vl["Gateway_72"]['ZV_HD_offen']]) + self.doorOpen = any([pt_cp.vl["Gateway_72"]['ZV_FT_offen'], + pt_cp.vl["Gateway_72"]['ZV_BT_offen'], + pt_cp.vl["Gateway_72"]['ZV_HFS_offen'], + pt_cp.vl["Gateway_72"]['ZV_HBFS_offen'], + pt_cp.vl["Gateway_72"]['ZV_HD_offen']]) # Update seatbelt fastened status. - self.seatbeltUnlatched = False if gw_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True + self.seatbeltUnlatched = False if pt_cp.vl["Airbag_02"]["AB_Gurtschloss_FA"] == 3 else True # Update driver preference for metric. VW stores many different unit # preferences, including separate units for for distance vs. speed. # We use the speed preference for OP. - self.displayMetricUnits = not gw_cp.vl["Einheiten_01"]["KBI_MFA_v_Einheit_02"] + self.displayMetricUnits = not pt_cp.vl["Einheiten_01"]["KBI_MFA_v_Einheit_02"] # Update ACC radar status. - accStatus = ex_cp.vl["ACC_06"]['ACC_Status_ACC'] + accStatus = pt_cp.vl["ACC_06"]['ACC_Status_ACC'] if accStatus == 1: # ACC okay but disabled self.accFault = False @@ -195,35 +199,35 @@ class CarState(): # Update ACC setpoint. When the setpoint is zero or there's an error, the # radar sends a set-speed of ~90.69 m/s / 203mph. - self.accSetSpeed = ex_cp.vl["ACC_02"]['SetSpeed'] + self.accSetSpeed = pt_cp.vl["ACC_02"]['SetSpeed'] if self.accSetSpeed > 90: self.accSetSpeed = 0 # Update control button states for turn signals and ACC controls. - self.buttonStates["leftBlinker"] = bool(gw_cp.vl["Gateway_72"]['BH_Blinker_li']) - self.buttonStates["leftBlinker"] = bool(gw_cp.vl["Gateway_72"]['BH_Blinker_re']) - self.buttonStates["accelCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Hoch']) - self.buttonStates["decelCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Runter']) - self.buttonStates["cancel"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Abbrechen']) - self.buttonStates["setCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Setzen']) - self.buttonStates["resumeCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Wiederaufnahme']) - self.buttonStates["gapAdjustCruise"] = bool(gw_cp.vl["GRA_ACC_01"]['GRA_Verstellung_Zeitluecke']) + self.buttonStates["leftBlinker"] = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_li']) + self.buttonStates["rightBlinker"] = bool(pt_cp.vl["Gateway_72"]['BH_Blinker_re']) + self.buttonStates["accelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Hoch']) + self.buttonStates["decelCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Runter']) + self.buttonStates["cancel"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Abbrechen']) + self.buttonStates["setCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Setzen']) + self.buttonStates["resumeCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Wiederaufnahme']) + self.buttonStates["gapAdjustCruise"] = bool(pt_cp.vl["GRA_ACC_01"]['GRA_Verstellung_Zeitluecke']) # Read ACC hardware button type configuration info that has to pass thru # to the radar. Ends up being different for steering wheel buttons vs # third stalk type controls. - self.graHauptschalter = gw_cp.vl["GRA_ACC_01"]['GRA_Hauptschalter'] - self.graTypHauptschalter = gw_cp.vl["GRA_ACC_01"]['GRA_Typ_Hauptschalter'] - self.graButtonTypeInfo = gw_cp.vl["GRA_ACC_01"]['GRA_ButtonTypeInfo'] - self.graTipStufe2 = gw_cp.vl["GRA_ACC_01"]['GRA_Tip_Stufe_2'] + self.graHauptschalter = pt_cp.vl["GRA_ACC_01"]['GRA_Hauptschalter'] + self.graTypHauptschalter = pt_cp.vl["GRA_ACC_01"]['GRA_Typ_Hauptschalter'] + self.graButtonTypeInfo = pt_cp.vl["GRA_ACC_01"]['GRA_ButtonTypeInfo'] + self.graTipStufe2 = pt_cp.vl["GRA_ACC_01"]['GRA_Tip_Stufe_2'] # Pick up the GRA_ACC_01 CAN message counter so we can sync to it for # later cruise-control button spamming. - self.graMsgBusCounter = gw_cp.vl["GRA_ACC_01"]['COUNTER'] + self.graMsgBusCounter = pt_cp.vl["GRA_ACC_01"]['COUNTER'] # Check to make sure the electric power steering rack is configured to # accept and respond to HCA_01 messages and has not encountered a fault. - self.steeringFault = not gw_cp.vl["EPS_01"]["HCA_Ready"] + self.steeringFault = not pt_cp.vl["EPS_01"]["HCA_Ready"] # Additional safety checks performed in CarInterface. - self.parkingBrakeSet = bool(gw_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well - self.stabilityControlDisabled = gw_cp.vl["ESP_21"]['ESP_Tastung_passiv'] + self.parkingBrakeSet = bool(pt_cp.vl["Kombi_01"]['KBI_Handbremse']) # FIXME: need to include an EPB check as well + self.stabilityControlDisabled = pt_cp.vl["ESP_21"]['ESP_Tastung_passiv'] diff --git a/selfdrive/car/volkswagen/interface.py b/selfdrive/car/volkswagen/interface.py index 272c86bece..14c58b3679 100644 --- a/selfdrive/car/volkswagen/interface.py +++ b/selfdrive/car/volkswagen/interface.py @@ -3,7 +3,7 @@ from selfdrive.config import Conversions as CV from selfdrive.controls.lib.drive_helpers import create_event, EventTypes as ET from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.car.volkswagen.values import CAR, BUTTON_STATES -from selfdrive.car.volkswagen.carstate import CarState, get_mqb_gateway_can_parser, get_mqb_extended_can_parser +from selfdrive.car.volkswagen.carstate import CarState, get_mqb_pt_can_parser, get_mqb_cam_can_parser from common.params import Params from selfdrive.car import STD_CARGO_KG, scale_rot_inertia, scale_tire_stiffness, gen_empty_fingerprint from selfdrive.car.interfaces import CarInterfaceBase @@ -11,8 +11,8 @@ from selfdrive.car.interfaces import CarInterfaceBase GEAR = car.CarState.GearShifter class CANBUS: - gateway = 0 - extended = 2 + pt = 0 + cam = 2 class CarInterface(CarInterfaceBase): def __init__(self, CP, CarController): @@ -30,8 +30,8 @@ class CarInterface(CarInterfaceBase): # *** init the major players *** self.CS = CarState(CP, CANBUS) self.VM = VehicleModel(CP) - self.gw_cp = get_mqb_gateway_can_parser(CP, CANBUS) - self.ex_cp = get_mqb_extended_can_parser(CP, CANBUS) + self.pt_cp = get_mqb_pt_can_parser(CP, CANBUS) + self.cam_cp = get_mqb_cam_can_parser(CP, CANBUS) # sending if read only is False if CarController is not None: @@ -56,12 +56,11 @@ class CarInterface(CarInterfaceBase): ret.enableCruise = True # Stock ACC still controls acceleration and braking ret.openpilotLongitudinalControl = False ret.steerControlType = car.CarParams.SteerControlType.torque - # Steer torque is strongly rate limit and max value is decently high. Off to avoid false positives - ret.steerLimitAlert = False # Additional common MQB parameters that may be overridden per-vehicle ret.steerRateCost = 0.5 ret.steerActuatorDelay = 0.05 # Hopefully all MQB racks are similar here + ret.steerLimitTimer = 0.4 ret.steerMaxBP = [0.] # m/s ret.steerMaxV = [1.] @@ -125,10 +124,12 @@ class CarInterface(CarInterfaceBase): ret = car.CarState.new_message() # Process the most recent CAN message traffic, and check for validity - self.gw_cp.update_strings(can_strings) - self.ex_cp.update_strings(can_strings) - self.CS.update(self.gw_cp, self.ex_cp) - ret.canValid = self.gw_cp.can_valid and self.ex_cp.can_valid + # The camera CAN has no signals we use at this time, but we process it + # anyway so we can test connectivity with can_valid + self.pt_cp.update_strings(can_strings) + self.cam_cp.update_strings(can_strings) + self.CS.update(self.pt_cp) + ret.canValid = self.pt_cp.can_valid and self.cam_cp.can_valid # Wheel and vehicle speed, yaw rate ret.wheelSpeeds.fl = self.CS.wheelSpeedFL @@ -145,6 +146,7 @@ class CarInterface(CarInterfaceBase): ret.steeringRate = self.CS.steeringRate ret.steeringTorque = self.CS.steeringTorque ret.steeringPressed = self.CS.steeringPressed + ret.steeringRateLimited = self.CC.steer_rate_limited if self.CC is not None else False ret.yawRate = self.CS.yawRate # Gas, brakes and shifting diff --git a/selfdrive/common/SConscript b/selfdrive/common/SConscript new file mode 100644 index 0000000000..6f40e6a8f0 --- /dev/null +++ b/selfdrive/common/SConscript @@ -0,0 +1,36 @@ +Import('env', 'arch', 'SHARED') + +if SHARED: + fxn = env.SharedLibrary +else: + fxn = env.Library + +_common = fxn('common', ['params.cc', 'swaglog.c', 'util.c', 'cqueue.c'], LIBS="json") +_visionipc = fxn('visionipc', ['visionipc.c', 'ipc.c']) + +files = [ + 'buffering.c', + 'clutil.c', + 'efd.c', + 'glutil.c', + 'visionimg.cc', +] + +if arch == "aarch64": + defines = {} + files += [ + 'framebuffer.cc', + 'touch.c', + 'visionbuf_ion.c', + ] + _gpu_libs = ['gui', 'adreno_utils'] +else: + defines = {"CLU_NO_CACHE": None} + files += [ + 'visionbuf_cl.c', + ] + _gpu_libs = ["GL"] + +_gpucommon = fxn('gpucommon', files, CPPDEFINES=defines, LIBS=_gpu_libs) +Export('_common', '_visionipc', '_gpucommon', '_gpu_libs') + diff --git a/selfdrive/common/cereal.mk b/selfdrive/common/cereal.mk deleted file mode 100644 index ed05efc08c..0000000000 --- a/selfdrive/common/cereal.mk +++ /dev/null @@ -1,38 +0,0 @@ -UNAME_M ?= $(shell uname -m) -UNAME_S ?= $(shell uname -s) - -ifeq ($(UNAME_S),Darwin) -CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include -CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/mac/include -CEREAL_LIBS = $(PHONELIBS)/capnp-cpp/mac/lib/libcapnp.a \ - $(PHONELIBS)/capnp-cpp/mac/lib/libkj.a \ - $(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a - -else ifeq ($(UNAME_M),x86_64) -CEREAL_CFLAGS = -I$(PHONELIBS)/capnp-c/include -CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include -ifeq ($(CEREAL_LIBS),) - CEREAL_LIBS = -L$(PHONELIBS)/capnp-cpp/x64/lib/ \ - -L$(PHONELIBS)/capnp-c/x64/lib/ \ - -l:libcapnp.a -l:libkj.a -l:libcapnp_c.a -endif - -else - -#CEREAL_CXXFLAGS = -I$(PHONELIBS)/capnp-cpp/include -ifeq ($(CEREAL_LIBS),) - CEREAL_LIBS = -l:libcapn.a -l:libcapnp.a -l:libkj.a -endif -endif - -CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o ../../cereal/gen/c/car.capnp.o - -log.capnp.o: ../../cereal/gen/cpp/log.capnp.c++ - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) $(CEREAL_CXXFLAGS) \ - -c -o '$@' '$<' - -car.capnp.o: ../../cereal/gen/cpp/car.capnp.c++ - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) $(CEREAL_CXXFLAGS) \ - -c -o '$@' '$<' diff --git a/selfdrive/visiond/clutil.c b/selfdrive/common/clutil.c similarity index 100% rename from selfdrive/visiond/clutil.c rename to selfdrive/common/clutil.c diff --git a/selfdrive/visiond/clutil.h b/selfdrive/common/clutil.h similarity index 100% rename from selfdrive/visiond/clutil.h rename to selfdrive/common/clutil.h diff --git a/selfdrive/common/messaging.h b/selfdrive/common/messaging.h index 725129508f..dd1198e826 100644 --- a/selfdrive/common/messaging.h +++ b/selfdrive/common/messaging.h @@ -1,4 +1,4 @@ -// the c version of selfdrive/messaging.py +// the c version of cereal/messaging.py #include diff --git a/selfdrive/common/modeldata.h b/selfdrive/common/modeldata.h index 6c0cd006f6..555a75614c 100644 --- a/selfdrive/common/modeldata.h +++ b/selfdrive/common/modeldata.h @@ -4,6 +4,8 @@ #define MODEL_PATH_DISTANCE 192 #define POLYFIT_DEGREE 4 #define SPEED_PERCENTILES 10 +#define DESIRE_PRED_SIZE 32 +#define OTHER_META_SIZE 4 typedef struct PathData { float points[MODEL_PATH_DISTANCE]; @@ -31,6 +33,7 @@ typedef struct ModelData { PathData right_lane; LeadData lead; LeadData lead_future; + float meta[OTHER_META_SIZE + DESIRE_PRED_SIZE]; float speed[SPEED_PERCENTILES]; } ModelData; diff --git a/selfdrive/common/swaglog.h b/selfdrive/common/swaglog.h index 33e77f618a..3a828ed49b 100644 --- a/selfdrive/common/swaglog.h +++ b/selfdrive/common/swaglog.h @@ -1,7 +1,7 @@ #ifndef SWAGLOG_H #define SWAGLOG_H -#include "common/timing.h" +#include "selfdrive/common/timing.h" #define CLOUDLOG_DEBUG 10 #define CLOUDLOG_INFO 20 diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h index 9b2cb69535..6f1cf7ff00 100644 --- a/selfdrive/common/version.h +++ b/selfdrive/common/version.h @@ -1 +1 @@ -#define COMMA_VERSION "0.6.6-release" +#define COMMA_VERSION "0.7-release" diff --git a/selfdrive/common/visionbuf_cl.c b/selfdrive/common/visionbuf_cl.c index 58c66a854d..333bed55c6 100644 --- a/selfdrive/common/visionbuf_cl.c +++ b/selfdrive/common/visionbuf_cl.c @@ -1,8 +1,12 @@ #include "visionbuf.h" +#include #include #include #include +#include +#include +#include #ifdef __APPLE__ #include @@ -10,13 +14,27 @@ #include #endif +int offset = 0; +void *malloc_with_fd(size_t len, int *fd) { + char full_path[0x100]; + snprintf(full_path, sizeof(full_path)-1, "/dev/shm/visionbuf_%d_%d", getpid(), offset++); + *fd = open(full_path, O_RDWR | O_CREAT, 0777); + unlink(full_path); + ftruncate(*fd, len); + void *addr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0); + return addr; +} + VisionBuf visionbuf_allocate(size_t len) { // const size_t alignment = 4096; // void* addr = aligned_alloc(alignment, alignment * ((len - 1) / alignment + 1)); - void* addr = calloc(1, len); + //void* addr = calloc(1, len); + + int fd; + void *addr = malloc_with_fd(len, &fd); return (VisionBuf){ - .len = len, .addr = addr, .handle = 1, .fd = -1, + .len = len, .addr = addr, .handle = 1, .fd = fd, }; } @@ -37,7 +55,8 @@ VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context c clSVMAlloc(ctx, CL_MEM_READ_WRITE | CL_MEM_SVM_FINE_GRAIN_BUFFER, len, 0); assert(host_ptr); #else - void* host_ptr = calloc(1, len); + int fd; + void* host_ptr = malloc_with_fd(len, &fd); cl_command_queue q = clCreateCommandQueue(ctx, device_id, 0, &err); assert(err == 0); @@ -49,7 +68,7 @@ VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context c *out_mem = mem; return (VisionBuf){ - .len = len, .addr = host_ptr, .handle = 0, .fd = -1, + .len = len, .addr = host_ptr, .handle = 0, .fd = fd, .device_id = device_id, .ctx = ctx, .buf_cl = mem, #if __OPENCL_VERSION__ < 200 @@ -76,14 +95,16 @@ void visionbuf_sync(const VisionBuf* buf, int dir) { void visionbuf_free(const VisionBuf* buf) { if (buf->handle) { - free(buf->addr); + munmap(buf->addr, buf->len); + close(buf->fd); } else { int err = clReleaseMemObject(buf->buf_cl); assert(err == 0); #if __OPENCL_VERSION__ >= 200 clSVMFree(buf->ctx, buf->addr); #else - free(buf->addr); + munmap(buf->addr, buf->len); + close(buf->fd); #endif } } diff --git a/selfdrive/common/visionipc.c b/selfdrive/common/visionipc.c index 314f7d0a55..b05365890c 100644 --- a/selfdrive/common/visionipc.c +++ b/selfdrive/common/visionipc.c @@ -35,6 +35,7 @@ int vipc_recv(int fd, VisionPacket *out_p) { p2.d = p.d; *out_p = p2; } + //printf("%d = vipc_recv(%d, %d): %d %d %d %u\n", ret, fd, p2.num_fds, out_p->d.stream_bufs.type, out_p->d.stream_bufs.width, out_p->d.stream_bufs.height, out_p->d.stream_bufs.buf_len); return ret; } @@ -45,7 +46,9 @@ int vipc_send(int fd, const VisionPacket *p2) { .type = p2->type, .d = p2->d, }; - return ipc_sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL); + int ret = ipc_sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL); + //printf("%d = vipc_send(%d, %d): %d %d %d %u\n", ret, fd, p2->num_fds, p2->d.stream_bufs.type, p2->d.stream_bufs.width, p2->d.stream_bufs.height, p2->d.stream_bufs.buf_len); + return ret; } void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs, diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 1550116617..f6a27902bd 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -7,7 +7,7 @@ from common.numpy_fast import clip from common.realtime import sec_since_boot, set_realtime_priority, Ratekeeper, DT_CTRL from common.profiler import Profiler from common.params import Params, put_nonblocking -import selfdrive.messaging as messaging +import cereal.messaging as messaging from selfdrive.config import Conversions as CV from selfdrive.boardd.boardd import can_list_to_can_capnp from selfdrive.car.car_helpers import get_car, get_startup_alert @@ -23,15 +23,30 @@ from selfdrive.controls.lib.latcontrol_indi import LatControlINDI from selfdrive.controls.lib.latcontrol_lqr import LatControlLQR from selfdrive.controls.lib.alertmanager import AlertManager from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS +from selfdrive.controls.lib.driver_monitor import DriverStatus, MAX_TERMINAL_ALERTS, MAX_TERMINAL_DURATION from selfdrive.controls.lib.planner import LON_MPC_STEP from selfdrive.controls.lib.gps_helpers import is_rhd_region from selfdrive.locationd.calibration_helpers import Calibration, Filter +LANE_DEPARTURE_THRESHOLD = 0.1 + ThermalStatus = log.ThermalData.ThermalStatus State = log.ControlsState.OpenpilotState HwType = log.HealthData.HwType +LaneChangeState = log.PathPlan.LaneChangeState +LaneChangeDirection = log.PathPlan.LaneChangeDirection + + +def add_lane_change_event(events, path_plan): + if path_plan.laneChangeState == LaneChangeState.preLaneChange: + if path_plan.laneChangeDirection == LaneChangeDirection.left: + events.append(create_event('preLaneChangeLeft', [ET.WARNING])) + else: + events.append(create_event('preLaneChangeRight', [ET.WARNING])) + elif path_plan.laneChangeState in [LaneChangeState.laneChangeStarting, LaneChangeState.laneChangeFinishing]: + events.append(create_event('laneChange', [ET.WARNING])) + def isActive(state): """Check if the actuators are enabled""" @@ -52,8 +67,7 @@ def events_to_bytes(events): return ret -def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, - driver_status, state, mismatch_counter, params): +def data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params): """Receive data from sockets and create events for battery, temperature and disk space""" # Update carstate from CAN and create events @@ -63,16 +77,17 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space sm.update(0) events = list(CS.events) + add_lane_change_event(events, sm['pathPlan']) enabled = isEnabled(state) # Check for CAN timeout if not can_strs: events.append(create_event('canError', [ET.NO_ENTRY, ET.IMMEDIATE_DISABLE])) - if sm.updated['thermal']: - overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red - free_space = sm['thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed - low_battery = sm['thermal'].batteryPercent < 1 and sm['thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed + overtemp = sm['thermal'].thermalStatus >= ThermalStatus.red + free_space = sm['thermal'].freeSpace < 0.07 # under 7% of space free no enable allowed + low_battery = sm['thermal'].batteryPercent < 1 and sm['thermal'].chargingError # at zero percent battery, while discharging, OP should not allowed + mem_low = sm['thermal'].memUsedPercent > 90 # Create events for battery, temperature and disk space if low_battery: @@ -81,6 +96,11 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space events.append(create_event('overheat', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if free_space: events.append(create_event('outOfSpace', [ET.NO_ENTRY])) + if mem_low: + events.append(create_event('lowMemory', [ET.NO_ENTRY, ET.SOFT_DISABLE, ET.PERMANENT])) + + if CS.stockAeb: + events.append(create_event('stockAeb', [])) # GPS coords RHD parsing, once every restart if sm.updated['gpsLocation'] and not driver_status.is_rhd_region_checked: @@ -90,9 +110,8 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space put_nonblocking("IsRHD", "1" if is_rhd else "0") # Handle calibration - if sm.updated['liveCalibration']: - cal_status = sm['liveCalibration'].calStatus - cal_perc = sm['liveCalibration'].calPerc + cal_status = sm['liveCalibration'].calStatus + cal_perc = sm['liveCalibration'].calPerc cal_rpy = [0,0,0] if cal_status != Calibration.CALIBRATED: @@ -112,21 +131,23 @@ def data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space if not enabled: mismatch_counter = 0 - if sm.updated['health']: - controls_allowed = sm['health'].controlsAllowed - if not controls_allowed and enabled: - mismatch_counter += 1 - if mismatch_counter >= 2: - events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) + controls_allowed = sm['health'].controlsAllowed + if not controls_allowed and enabled: + mismatch_counter += 1 + if mismatch_counter >= 200: + events.append(create_event('controlsMismatch', [ET.IMMEDIATE_DISABLE])) # Driver monitoring + if sm.updated['model']: + driver_status.set_policy(sm['model']) + if sm.updated['driverMonitoring']: driver_status.get_pose(sm['driverMonitoring'], cal_rpy, CS.vEgo, enabled) - if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS: + if driver_status.terminal_alert_cnt >= MAX_TERMINAL_ALERTS or driver_status.terminal_time >= MAX_TERMINAL_DURATION: events.append(create_event("tooDistracted", [ET.NO_ENTRY])) - return CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter + return CS, events, cal_perc, mismatch_counter def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_kph, AM): @@ -217,7 +238,7 @@ def state_transition(frame, CS, CP, state, events, soft_disable_timer, v_cruise_ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, - AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc): + AM, rk, driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame): """Given the state, this function returns an actuators packet""" actuators = car.CarControl.Actuators.new_message() @@ -230,11 +251,14 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr v_cruise_kph != v_cruise_kph_last or \ CS.steeringPressed + if CS.leftBlinker or CS.rightBlinker: + last_blinker_frame = frame + # add eventual driver distracted events events = driver_status.update(events, driver_engaged, isActive(state), CS.standstill) # send FCW alert if triggered by planner - if plan.fcw: + if plan.fcw or CS.stockFcw: AM.add(frame, "fcw", enabled) # State specific actions @@ -264,11 +288,16 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr actuators.gas, actuators.brake = LoC.update(active, CS.vEgo, CS.brakePressed, CS.standstill, CS.cruiseState.standstill, v_cruise_kph, v_acc_sol, plan.vTargetFuture, a_acc_sol, CP) # Steering PID loop and lateral MPC - actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CP, path_plan) + actuators.steer, actuators.steerAngle, lac_log = LaC.update(active, CS.vEgo, CS.steeringAngle, CS.steeringRate, CS.steeringTorqueEps, CS.steeringPressed, CS.steeringRateLimited, CP, path_plan) # Send a "steering required alert" if saturation count has reached the limit - if LaC.sat_flag and CP.steerLimitAlert: - AM.add(frame, "steerSaturated", enabled) + if lac_log.saturated and not CS.steeringPressed: + # Check if we deviated from the path + left_deviation = actuators.steer > 0 and path_plan.dPoly[3] > 0.1 + right_deviation = actuators.steer < 0 and path_plan.dPoly[3] < -0.1 + + if left_deviation or right_deviation: + AM.add(frame, "steerSaturated", enabled) # Parse permanent warnings to display constantly for e in get_events(events, [ET.PERMANENT]): @@ -281,13 +310,12 @@ def state_control(frame, rcv_frame, plan, path_plan, CS, CP, state, events, v_cr extra_text_2 = str(int(round(Filter.MIN_SPEED * CV.MS_TO_MPH))) + " mph" AM.add(frame, str(e) + "Permanent", enabled, extra_text_1=extra_text_1, extra_text_2=extra_text_2) - AM.process_alerts(frame) - - return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log + return actuators, v_cruise_kph, driver_status, v_acc_sol, a_acc_sol, lac_log, last_blinker_frame def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, - driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev): + driver_status, LaC, LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, + last_blinker_frame, is_ldw_enabled): """Send actuators and hud commands to the car, send controlsstate and MPC logging""" CC = car.CarControl.new_message() @@ -309,18 +337,29 @@ def data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk right_lane_visible = sm['pathPlan'].rProb > 0.5 left_lane_visible = sm['pathPlan'].lProb > 0.5 - CC.hudControl.rightLaneVisible = bool(right_lane_visible) CC.hudControl.leftLaneVisible = bool(left_lane_visible) - blinker = CS.leftBlinker or CS.rightBlinker - ldw_allowed = CS.vEgo > 12.5 and not blinker + recent_blinker = (sm.frame - last_blinker_frame) * DT_CTRL < 5.0 # 5s blinker cooldown + ldw_allowed = CS.vEgo > 31 * CV.MPH_TO_MS and not recent_blinker and is_ldw_enabled and not isActive(state) + + md = sm['model'] + if len(md.meta.desirePrediction): + l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] + r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] + + l_lane_close = left_lane_visible and (sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET)) + r_lane_close = right_lane_visible and (sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET)) + + if ldw_allowed: + CC.hudControl.leftLaneDepart = bool(l_lane_change_prob > LANE_DEPARTURE_THRESHOLD and l_lane_close) + CC.hudControl.rightLaneDepart = bool(r_lane_change_prob > LANE_DEPARTURE_THRESHOLD and r_lane_close) - if len(list(sm['pathPlan'].rPoly)) == 4: - CC.hudControl.rightLaneDepart = bool(ldw_allowed and sm['pathPlan'].rPoly[3] > -(1.08 + CAMERA_OFFSET) and right_lane_visible) - if len(list(sm['pathPlan'].lPoly)) == 4: - CC.hudControl.leftLaneDepart = bool(ldw_allowed and sm['pathPlan'].lPoly[3] < (1.08 - CAMERA_OFFSET) and left_lane_visible) + if CC.hudControl.rightLaneDepart or CC.hudControl.leftLaneDepart: + AM.add(sm.frame, 'ldwPermanent', False) + events.append(create_event('ldw', [ET.PERMANENT])) + AM.process_alerts(sm.frame) CC.hudControl.visualAlert = AM.visual_alert if not read_only: @@ -425,8 +464,10 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): params = Params() is_metric = params.get("IsMetric", encoding='utf8') == "1" + is_ldw_enabled = params.get("IsLdwEnabled", encoding='utf8') == "1" passive = params.get("Passive", encoding='utf8') == "1" openpilot_enabled_toggle = params.get("OpenpilotEnabledToggle", encoding='utf8') == "1" + community_feature_toggle = params.get("CommunityFeaturesToggle", encoding='utf8') == "1" passive = passive or not openpilot_enabled_toggle @@ -436,7 +477,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if sm is None: sm = messaging.SubMaster(['thermal', 'health', 'liveCalibration', 'driverMonitoring', 'plan', 'pathPlan', \ - 'gpsLocation'], ignore_alive=['gpsLocation']) + 'model', 'gpsLocation'], ignore_alive=['gpsLocation']) if can_sock is None: @@ -454,7 +495,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): car_recognized = CP.carName != 'mock' # If stock camera is disconnected, we loaded car controls and it's not chffrplus controller_available = CP.enableCamera and CI.CC is not None and not passive - read_only = not car_recognized or not controller_available or CP.dashcamOnly + community_feature_disallowed = CP.communityFeature and not community_feature_toggle + read_only = not car_recognized or not controller_available or CP.dashcamOnly or community_feature_disallowed if read_only: CP.safetyModel = car.CarParams.SafetyModel.noOutput @@ -487,16 +529,14 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): soft_disable_timer = 0 v_cruise_kph = 255 v_cruise_kph_last = 0 - overtemp = False - free_space = False - cal_status = Calibration.INVALID - cal_perc = 0 mismatch_counter = 0 - low_battery = False + last_blinker_frame = 0 events_prev = [] + sm['liveCalibration'].calStatus = Calibration.INVALID sm['pathPlan'].sensorValid = True sm['pathPlan'].posenetValid = True + sm['thermal'].freeSpace = 1. # detect sound card presence sounds_available = not os.path.isfile('/EON') or (os.path.isdir('/proc/asound/card0') and open('/proc/asound/card0/state').read().strip() == 'ONLINE') @@ -513,9 +553,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("Ratekeeper", ignore=True) # Sample data and compute car events - CS, events, cal_status, cal_perc, overtemp, free_space, low_battery, mismatch_counter =\ - data_sample(CI, CC, sm, can_sock, cal_status, cal_perc, overtemp, free_space, low_battery, - driver_status, state, mismatch_counter, params) + CS, events, cal_perc, mismatch_counter = data_sample(CI, CC, sm, can_sock, driver_status, state, mismatch_counter, params) prof.checkpoint("Sample") # Create alerts @@ -528,7 +566,7 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): if not sm['pathPlan'].paramsValid: events.append(create_event('vehicleModelInvalid', [ET.WARNING])) if not sm['pathPlan'].posenetValid: - events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.SOFT_DISABLE])) + events.append(create_event('posenetInvalid', [ET.NO_ENTRY, ET.WARNING])) if not sm['plan'].radarValid: events.append(create_event('radarFault', [ET.NO_ENTRY, ET.SOFT_DISABLE])) if sm['plan'].radarCanError: @@ -539,6 +577,8 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): events.append(create_event('soundsUnavailable', [ET.NO_ENTRY, ET.PERMANENT])) if internet_needed: events.append(create_event('internetConnectivityNeeded', [ET.NO_ENTRY, ET.PERMANENT])) + if community_feature_disallowed: + events.append(create_event('communityFeatureDisallowed', [ET.PERMANENT])) # Only allow engagement with brake pressed when stopped behind another stopped car if CS.brakePressed and sm['plan'].vTargetFuture >= STARTING_TARGET_SPEED and not CP.radarOffCan and CS.vEgo < 0.3: @@ -551,15 +591,15 @@ def controlsd_thread(sm=None, pm=None, can_sock=None): prof.checkpoint("State transition") # Compute actuators (runs PID loops and lateral MPC) - actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log = \ + actuators, v_cruise_kph, driver_status, v_acc, a_acc, lac_log, last_blinker_frame = \ state_control(sm.frame, sm.rcv_frame, sm['plan'], sm['pathPlan'], CS, CP, state, events, v_cruise_kph, v_cruise_kph_last, AM, rk, - driver_status, LaC, LoC, read_only, is_metric, cal_perc) + driver_status, LaC, LoC, read_only, is_metric, cal_perc, last_blinker_frame) prof.checkpoint("State Control") # Publish data CC, events_prev = data_send(sm, pm, CS, CI, CP, VM, state, events, actuators, v_cruise_kph, rk, AM, driver_status, LaC, - LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev) + LoC, read_only, start_time, v_acc, a_acc, lac_log, events_prev, last_blinker_frame, is_ldw_enabled) prof.checkpoint("Sent") rk.monitor_time() diff --git a/selfdrive/controls/lib/alerts.py b/selfdrive/controls/lib/alerts.py index 57e3969eea..29dd4d588e 100644 --- a/selfdrive/controls/lib/alerts.py +++ b/selfdrive/controls/lib/alerts.py @@ -226,6 +226,33 @@ ALERTS = [ "", AlertStatus.userPrompt, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.none, .1, .1, .1), + Alert( + "preLaneChangeLeft", + "Steer Left to Start Lane Change", + "Monitor Other Vehicles", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), + + Alert( + "preLaneChangeRight", + "Steer Right to Start Lane Change", + "Monitor Other Vehicles", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1, alert_rate=0.75), + + Alert( + "laneChange", + "Changing Lane", + "Monitor Other Vehicles", + AlertStatus.normal, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.none, .0, .1, .1), + + Alert( + "posenetInvalid", + "TAKE CONTROL", + "Vision Model Output Uncertain", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning1, .4, 2., 3.), # Non-entry only alerts Alert( @@ -390,10 +417,11 @@ ALERTS = [ AlertStatus.critical, AlertSize.full, Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), + Alert( - "posenetInvalid", + "lowMemory", "TAKE CONTROL IMMEDIATELY", - "Vision Failure: Check Camera View", + "Low Memory: Reboot Your EON", AlertStatus.critical, AlertSize.full, Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeat, .1, 2., 2.), @@ -476,13 +504,6 @@ ALERTS = [ AlertStatus.normal, AlertSize.mid, Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), - Alert( - "invalidGiraffeHonda", - "Invalid Giraffe Configuration", - "Set 0111 for openpilot. 1011 for stock", - AlertStatus.normal, AlertSize.mid, - Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), - # Cancellation alerts causing non-entry Alert( "overheatNoEntry", @@ -557,7 +578,7 @@ ALERTS = [ Alert( "posenetInvalidNoEntry", "openpilot Unavailable", - "Vision Failure: Check Camera View", + "Vision Model Output Uncertain", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), @@ -625,30 +646,23 @@ ALERTS = [ Priority.LOW, VisualAlert.none, AudibleAlert.chimeError, .4, 2., 3.), Alert( - "invalidGiraffeHondaNoEntry", - "openpilot Unavailable", - "Set 0111 for openpilot. 1011 for stock", - AlertStatus.normal, AlertSize.mid, - Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), - - Alert( - "invalidGiraffeToyotaNoEntry", + "commIssueNoEntry", "openpilot Unavailable", - "Visit comma.ai/tg", + "Communication Issue between Processes", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), Alert( - "commIssueNoEntry", + "internetConnectivityNeededNoEntry", "openpilot Unavailable", - "Communication Issue between Processes", + "Please Connect to Internet", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), Alert( - "internetConnectivityNeededNoEntry", + "lowMemoryNoEntry", "openpilot Unavailable", - "Internet Connectivity Needed", + "Low Memory: Reboot Your EON", AlertStatus.normal, AlertSize.mid, Priority.LOW, VisualAlert.none, AudibleAlert.chimeDisengage, .4, 2., 3.), @@ -681,13 +695,6 @@ ALERTS = [ AlertStatus.normal, AlertSize.mid, Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), - Alert( - "invalidGiraffeHondaPermanent", - "Invalid Giraffe Configuration", - "Set 0111 for openpilot. 1011 for stock", - AlertStatus.normal, AlertSize.mid, - Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), - Alert( "invalidGiraffeToyotaPermanent", "Unsupported Giraffe Configuration", @@ -697,8 +704,15 @@ ALERTS = [ Alert( "internetConnectivityNeededPermanent", - "Internet Connectivity Needed", - "Check for Updates to Be Able to Engage", + "Please connect to Internet", + "An Update Check Is Required to Engage", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + + Alert( + "communityFeatureDisallowedPermanent", + "Community Feature Detected", + "Enable Community Features in Developer Settings", AlertStatus.normal, AlertSize.mid, Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), @@ -716,10 +730,25 @@ ALERTS = [ AlertStatus.normal, AlertSize.mid, Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Alert( + "lowMemoryPermanent", + "RAM Memory Critically Low", + "Reboot your EON", + AlertStatus.normal, AlertSize.mid, + Priority.LOW_LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2), + Alert( "vehicleModelInvalid", "Vehicle Parameter Identification Failed", "", AlertStatus.normal, AlertSize.small, Priority.LOWEST, VisualAlert.steerRequired, AudibleAlert.none, .0, .0, .1), + + # offroad alerts + Alert( + "ldwPermanent", + "TAKE CONTROL", + "Lane Departure Detected", + AlertStatus.userPrompt, AlertSize.mid, + Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1., 2., 3.), ] diff --git a/selfdrive/controls/lib/alerts_offroad.json b/selfdrive/controls/lib/alerts_offroad.json index 92a41ac854..9713ff71ac 100644 --- a/selfdrive/controls/lib/alerts_offroad.json +++ b/selfdrive/controls/lib/alerts_offroad.json @@ -1,6 +1,6 @@ { "Offroad_ChargeDisabled": { - "text": "EON charging disabled after car being off for more than 3 days or car voltage too low. Turn ignition on to start charging again.", + "text": "EON charging disabled after car being off for more than 30 hours. Turn ignition on to start charging again.", "severity": 0 }, "Offroad_TemperatureTooHigh": { @@ -8,12 +8,12 @@ "severity": 1 }, "Offroad_ConnectivityNeededPrompt": { - "text": "Connect to internet to check for updates. openpilot won't engage in ", + "text": "Immediately connect to the internet to check for updates. If you do not connect to the internet, openpilot won't engage in ", "severity": 0, "_comment": "Append the number of days at the end of the text" }, "Offroad_ConnectivityNeeded": { - "text": "Connect to internet to check for updates. openpilot won't engage.", + "text": "Connect to internet to check for updates. openpilot won't engage until it connects to internet to check for updates.", "severity": 1 }, "Offroad_PandaFirmwareMismatch": { @@ -23,5 +23,9 @@ "Offroad_InvalidTime": { "text": "Invalid date and time settings, system won't start. Connect to internet to set time.", "severity": 1 + }, + "Offroad_IsTakingSnapshot": { + "text": "Taking camera snapshots. System won't start until finished.", + "severity": 0 } } diff --git a/selfdrive/controls/lib/cluster/Makefile b/selfdrive/controls/lib/cluster/Makefile deleted file mode 100644 index ca4a23da59..0000000000 --- a/selfdrive/controls/lib/cluster/Makefile +++ /dev/null @@ -1,32 +0,0 @@ -CC = clang -CXX = clang++ - -CXXFLAGS = -Wall -g -fPIC -std=c++11 -O2 - -ifeq ($(ARCH),aarch64) -CXXFLAGS += -mcpu=cortex-a57 -endif - -OBJS = fastcluster.o test.o -DEPS := $(OBJS:.o=.d) - -all: libfastcluster.so - -test: libfastcluster.so test.o - $(CXX) -g -L. -lfastcluster -o $@ $+ - -valgrind: test - valgrind --leak-check=full ./test - - -libfastcluster.so: fastcluster.o - $(CXX) -g -shared -o $@ $+ - -%.o: %.cpp - $(CXX) $(CXXFLAGS) -MMD -c $*.cpp - -clean: - rm -f $(OBJS) $(DEPS) libfastcluster.so test - - --include $(DEPS) diff --git a/selfdrive/controls/lib/cluster/SConscript b/selfdrive/controls/lib/cluster/SConscript new file mode 100644 index 0000000000..97eb4300d4 --- /dev/null +++ b/selfdrive/controls/lib/cluster/SConscript @@ -0,0 +1,8 @@ +Import('env') + +fc = env.SharedLibrary("fastcluster", "fastcluster.cpp") + +# TODO: how do I gate on test +#env.Program("test", ["test.cpp"], LIBS=[fc]) +#valgrind --leak-check=full ./test + diff --git a/selfdrive/controls/lib/cluster/fastcluster_py.py b/selfdrive/controls/lib/cluster/fastcluster_py.py index 371804ce5e..12419a48ef 100644 --- a/selfdrive/controls/lib/cluster/fastcluster_py.py +++ b/selfdrive/controls/lib/cluster/fastcluster_py.py @@ -2,11 +2,8 @@ import os import numpy as np from cffi import FFI -import subprocess cluster_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) -subprocess.check_call(["make", "-j4"], cwd=cluster_dir) - cluster_fn = os.path.join(cluster_dir, "libfastcluster.so") ffi = FFI() diff --git a/selfdrive/controls/lib/driver_monitor.py b/selfdrive/controls/lib/driver_monitor.py index 9e94c4fe62..13a8130b7c 100644 --- a/selfdrive/controls/lib/driver_monitor.py +++ b/selfdrive/controls/lib/driver_monitor.py @@ -12,10 +12,14 @@ _DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. _DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. _FACE_THRESHOLD = 0.4 -_EYE_THRESHOLD = 0.4 +_EYE_THRESHOLD = 0.6 _BLINK_THRESHOLD = 0.5 # 0.225 +_BLINK_THRESHOLD_SLACK = 0.65 +_BLINK_THRESHOLD_STRICT = 0.5 _PITCH_WEIGHT = 1.35 # 1.5 # pitch matters a lot more _METRIC_THRESHOLD = 0.4 +_METRIC_THRESHOLD_SLACK = 0.55 +_METRIC_THRESHOLD_STRICT = 0.4 _PITCH_POS_ALLOWANCE = 0.04 # 0.08 # rad, to not be too sensitive on positive pitch _PITCH_NATURAL_OFFSET = 0.12 # 0.1 # people don't seem to look straight when they drive relaxed, rather a bit up _YAW_NATURAL_OFFSET = 0.08 # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car) @@ -30,6 +34,7 @@ _RECOVERY_FACTOR_MAX = 5. # relative to minus step change _RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change MAX_TERMINAL_ALERTS = 3 # not allowed to engage after 3 terminal alerts +MAX_TERMINAL_DURATION = 3000 # 30s # model output refers to center of cropped image, so need to apply the x displacement offset RESIZED_FOCAL = 320.0 @@ -40,21 +45,21 @@ class DistractedType(): BAD_POSE = 1 BAD_BLINK = 2 -def head_orientation_from_descriptor(angles_desc, pos_desc, rpy_calib): +def face_orientation_from_net(angles_desc, pos_desc, rpy_calib): # the output of these angles are in device frame # so from driver's perspective, pitch is up and yaw is right - pitch_prnet = angles_desc[0] - yaw_prnet = angles_desc[1] - roll_prnet = angles_desc[2] + pitch_net = angles_desc[0] + yaw_net = angles_desc[1] + roll_net = angles_desc[2] face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H) yaw_focal_angle = np.arctan2(face_pixel_position[0] - FULL_W//2, RESIZED_FOCAL) pitch_focal_angle = np.arctan2(face_pixel_position[1] - H//2, RESIZED_FOCAL) - roll = roll_prnet - pitch = pitch_prnet + pitch_focal_angle - yaw = -yaw_prnet + yaw_focal_angle + roll = roll_net + pitch = pitch_net + pitch_focal_angle + yaw = -yaw_net + yaw_focal_angle # no calib for roll pitch -= rpy_calib[1] @@ -68,11 +73,13 @@ class DriverPose(): self.roll = 0. self.pitch_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT) self.yaw_offseter = RunningStatFilter(max_trackable=_POSE_OFFSET_MAX_COUNT) + self.cfactor = 1. class DriverBlink(): def __init__(self): self.left_blink = 0. self.right_blink = 0. + self.cfactor = 1. class DriverStatus(): def __init__(self): @@ -87,6 +94,7 @@ class DriverStatus(): self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, DT_DMON) self.face_detected = False self.terminal_alert_cnt = 0 + self.terminal_time = 0 self.step_change = 0. self.active_monitoring_mode = True self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME @@ -140,22 +148,29 @@ class DriverStatus(): pitch_error *= _PITCH_WEIGHT pose_metric = np.sqrt(yaw_error**2 + pitch_error**2) - if pose_metric > _METRIC_THRESHOLD: + if pose_metric > _METRIC_THRESHOLD*pose.cfactor: return DistractedType.BAD_POSE - elif (blink.left_blink + blink.right_blink)*0.5 > _BLINK_THRESHOLD: + elif (blink.left_blink + blink.right_blink)*0.5 > _BLINK_THRESHOLD*blink.cfactor: return DistractedType.BAD_BLINK else: return DistractedType.NOT_DISTRACTED + def set_policy(self, model_data): + ep = min(model_data.meta.engagedProb, 0.8) / 0.8 + self.pose.cfactor = np.interp(ep, [0, 0.5, 1], [_METRIC_THRESHOLD_STRICT, _METRIC_THRESHOLD, _METRIC_THRESHOLD_SLACK])/_METRIC_THRESHOLD + self.blink.cfactor = np.interp(ep, [0, 0.5, 1], [_BLINK_THRESHOLD_STRICT, _BLINK_THRESHOLD, _BLINK_THRESHOLD_SLACK])/_BLINK_THRESHOLD + def get_pose(self, driver_monitoring, cal_rpy, car_speed, op_engaged): # 10 Hz if len(driver_monitoring.faceOrientation) == 0 or len(driver_monitoring.facePosition) == 0: return - self.pose.roll, self.pose.pitch, self.pose.yaw = head_orientation_from_descriptor(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy) + self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_monitoring.faceOrientation, driver_monitoring.facePosition, cal_rpy) self.blink.left_blink = driver_monitoring.leftBlinkProb * (driver_monitoring.leftEyeProb>_EYE_THRESHOLD) self.blink.right_blink = driver_monitoring.rightBlinkProb * (driver_monitoring.rightEyeProb>_EYE_THRESHOLD) - self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD and not self.is_rhd_region + self.face_detected = driver_monitoring.faceProb > _FACE_THRESHOLD and \ + abs(driver_monitoring.facePosition[0]) <= 0.4 and abs(driver_monitoring.facePosition[1]) <= 0.45 and \ + not self.is_rhd_region self.driver_distracted = self._is_driver_distracted(self.pose, self.blink)>0 # first order filters @@ -163,7 +178,7 @@ class DriverStatus(): # update offseter # only update when driver is actively driving the car above a certain speed - if self.face_detected and car_speed>_POSE_CALIB_MIN_SPEED and not op_engaged: + if self.face_detected and car_speed>_POSE_CALIB_MIN_SPEED and (not op_engaged or not self.driver_distracted): self.pose.pitch_offseter.push_and_update(self.pose.pitch) self.pose.yaw_offseter.push_and_update(self.pose.yaw) @@ -201,6 +216,7 @@ class DriverStatus(): if self.awareness <= 0.: # terminal red alert: disengagement required alert = 'driverDistracted' if self.active_monitoring_mode else 'driverUnresponsive' + self.terminal_time += 1 if awareness_prev > 0.: self.terminal_alert_cnt += 1 elif self.awareness <= self.threshold_prompt: diff --git a/selfdrive/controls/lib/fcw.py b/selfdrive/controls/lib/fcw.py index 93570a41db..b7e53b9ccf 100644 --- a/selfdrive/controls/lib/fcw.py +++ b/selfdrive/controls/lib/fcw.py @@ -10,6 +10,7 @@ _FCW_A_ACT_BP = [0., 30.] class FCWChecker(): def __init__(self): self.reset_lead(0.0) + self.common_counters = defaultdict(lambda: 0) def reset_lead(self, cur_time): self.last_fcw_a = 0.0 @@ -49,21 +50,23 @@ class FCWChecker(): self.last_min_a = min(mpc_solution_a) self.v_lead_max = max(self.v_lead_max, v_lead) + self.common_counters['blinkers'] = self.common_counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0 + self.common_counters['v_ego'] = self.common_counters['v_ego'] + 1 if v_ego > 5.0 else 0 + if (fcw_lead > 0.99): ttc = self.calc_ttc(v_ego, a_ego, x_lead, v_lead, a_lead) - self.counters['v_ego'] = self.counters['v_ego'] + 1 if v_ego > 5.0 else 0 self.counters['ttc'] = self.counters['ttc'] + 1 if ttc < 2.5 else 0 self.counters['v_lead_max'] = self.counters['v_lead_max'] + 1 if self.v_lead_max > 2.5 else 0 self.counters['v_ego_lead'] = self.counters['v_ego_lead'] + 1 if v_ego > v_lead else 0 self.counters['lead_seen'] = self.counters['lead_seen'] + 0.33 self.counters['y_lead'] = self.counters['y_lead'] + 1 if abs(y_lead) < 1.0 else 0 self.counters['vlat_lead'] = self.counters['vlat_lead'] + 1 if abs(vlat_lead) < 0.4 else 0 - self.counters['blinkers'] = self.counters['blinkers'] + 10.0 / (20 * 3.0) if not blinkers else 0 a_thr = interp(v_lead, _FCW_A_ACT_BP, _FCW_A_ACT_V) a_delta = min(mpc_solution_a[:15]) - min(0.0, a_ego) future_fcw_allowed = all(c >= 10 for c in self.counters.values()) + future_fcw_allowed = future_fcw_allowed and all(c >= 10 for c in self.common_counters.values()) future_fcw = (self.last_min_a < -3.0 or a_delta < a_thr) and future_fcw_allowed if future_fcw and (self.last_fcw_time + 5.0 < cur_time): diff --git a/selfdrive/controls/lib/gps_helpers.py b/selfdrive/controls/lib/gps_helpers.py index 6ac27202d8..984a450f8b 100755 --- a/selfdrive/controls/lib/gps_helpers.py +++ b/selfdrive/controls/lib/gps_helpers.py @@ -5,6 +5,7 @@ _RHD_REGION_MAP = [ ['AUS', -54.76, -9.23, 112.91, 159.11], \ ['JP1', 32.66, 45.52, 137.27, 146.02], \ ['JP2', 32.79, 37.60, 131.41, 137.28], \ ['JP3', 24.04, 34.78, 122.93, 131.42], \ + ['MY', 0.86, 7.36, 99.64, 119.27], \ ['NZ', -52.61, -29.24, 166, 178.84], \ ['SF', -35.14, -22.13, 16.07, 33.21], \ ['UK', 49.9, 60.84, -8.62, 1.77] ] diff --git a/selfdrive/controls/lib/lane_planner.py b/selfdrive/controls/lib/lane_planner.py index 7dde666c9e..0e84ad8b9f 100644 --- a/selfdrive/controls/lib/lane_planner.py +++ b/selfdrive/controls/lib/lane_planner.py @@ -1,5 +1,6 @@ from common.numpy_fast import interp import numpy as np +from cereal import log CAMERA_OFFSET = 0.06 # m from center car to camera @@ -46,6 +47,9 @@ class LanePlanner(): self.l_prob = 0. self.r_prob = 0. + self.l_lane_change_prob = 0. + self.r_lane_change_prob = 0. + self._path_pinv = compute_path_pinv() self.x_points = np.arange(50) @@ -61,7 +65,11 @@ class LanePlanner(): self.l_prob = md.leftLane.prob # left line prob self.r_prob = md.rightLane.prob # right line prob - def update_lane(self, v_ego): + if len(md.meta.desirePrediction): + self.l_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeLeft - 1] + self.r_lane_change_prob = md.meta.desirePrediction[log.PathPlan.Desire.laneChangeRight - 1] + + def update_d_poly(self, v_ego): # only offset left and right lane lines; offsetting p_poly does not make sense self.l_poly[3] += CAMERA_OFFSET self.r_poly[3] += CAMERA_OFFSET @@ -78,4 +86,4 @@ class LanePlanner(): def update(self, v_ego, md): self.parse_model(md) - self.update_lane(v_ego) + self.update_d_poly(v_ego) diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index d98b2efd8e..568241ee3f 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -40,14 +40,29 @@ class LatControlINDI(): self.inner_loop_gain = CP.lateralTuning.indi.innerLoopGain self.alpha = 1. - DT_CTRL / (self.RC + DT_CTRL) + self.sat_count_rate = 1.0 * DT_CTRL + self.sat_limit = CP.steerLimitTimer + self.reset() def reset(self): self.delayed_output = 0. self.output_steer = 0. - self.counter = 0 + self.sat_count = 0.0 + + def _check_saturation(self, control, check_saturation, limit): + saturated = abs(control) == limit + + if saturated and check_saturation: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate - def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan): + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): # Update Kalman filter y = np.matrix([[math.radians(angle_steers)], [math.radians(angle_steers_rate)]]) self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y) @@ -101,5 +116,7 @@ class LatControlINDI(): indi_log.delta = float(delta_u) indi_log.output = float(self.output_steer) - self.sat_flag = False + check_saturation = (v_ego > 10.) and not rate_limited and not steer_override + indi_log.saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) + return float(self.output_steer), float(self.angle_steers_des), indi_log diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index 1badf2d268..a23eafb5cf 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -1,16 +1,15 @@ import numpy as np from selfdrive.controls.lib.drive_helpers import get_steer_max from common.numpy_fast import clip +from common.realtime import DT_CTRL from cereal import log class LatControlLQR(): - def __init__(self, CP, rate=100): - self.sat_flag = False + def __init__(self, CP): self.scale = CP.lateralTuning.lqr.scale self.ki = CP.lateralTuning.lqr.ki - self.A = np.array(CP.lateralTuning.lqr.a).reshape((2,2)) self.B = np.array(CP.lateralTuning.lqr.b).reshape((2,1)) self.C = np.array(CP.lateralTuning.lqr.c).reshape((1,2)) @@ -19,17 +18,32 @@ class LatControlLQR(): self.dc_gain = CP.lateralTuning.lqr.dcGain self.x_hat = np.array([[0], [0]]) - self.i_unwind_rate = 0.3 / rate - self.i_rate = 1.0 / rate + self.i_unwind_rate = 0.3 * DT_CTRL + self.i_rate = 1.0 * DT_CTRL + self.sat_count_rate = 1.0 * DT_CTRL + self.sat_limit = CP.steerLimitTimer self.reset() def reset(self): self.i_lqr = 0.0 self.output_steer = 0.0 + self.sat_count = 0.0 + + def _check_saturation(self, control, check_saturation, limit): + saturated = abs(control) == limit + + if saturated and check_saturation: + self.sat_count += self.sat_count_rate + else: + self.sat_count -= self.sat_count_rate - def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan): + self.sat_count = clip(self.sat_count, 0.0, 1.0) + + return self.sat_count > self.sat_limit + + def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): lqr_log = log.ControlsState.LateralLQRState.new_message() steers_max = get_steer_max(CP, v_ego) @@ -70,8 +84,12 @@ class LatControlLQR(): self.output_steer = lqr_output + self.i_lqr self.output_steer = clip(self.output_steer, -steers_max, steers_max) + check_saturation = (v_ego > 10) and not rate_limited and not steer_override + saturated = self._check_saturation(self.output_steer, check_saturation, steers_max) + lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset lqr_log.i = self.i_lqr lqr_log.output = self.output_steer lqr_log.lqrOutput = lqr_output + lqr_log.saturated = saturated return self.output_steer, float(self.angle_steers_des), lqr_log diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 461203dec5..6a59565509 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -8,13 +8,13 @@ class LatControlPID(): def __init__(self, CP): self.pid = PIController((CP.lateralTuning.pid.kpBP, CP.lateralTuning.pid.kpV), (CP.lateralTuning.pid.kiBP, CP.lateralTuning.pid.kiV), - k_f=CP.lateralTuning.pid.kf, pos_limit=1.0) + k_f=CP.lateralTuning.pid.kf, pos_limit=1.0, sat_limit=CP.steerLimitTimer) self.angle_steers_des = 0. def reset(self): self.pid.reset() - def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, CP, path_plan): + def update(self, active, v_ego, angle_steers, angle_steers_rate, eps_torque, steer_override, rate_limited, CP, path_plan): pid_log = log.ControlsState.LateralPIDState.new_message() pid_log.steerAngle = float(angle_steers) pid_log.steerRate = float(angle_steers_rate) @@ -35,7 +35,9 @@ class LatControlPID(): steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque steer_feedforward *= v_ego**2 # proportional to realigning tire momentum (~ lateral accel) deadzone = 0.0 - output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=(v_ego > 10), override=steer_override, + + check_saturation = (v_ego > 10) and not rate_limited and not steer_override + output_steer = self.pid.update(self.angle_steers_des, angle_steers, check_saturation=check_saturation, override=steer_override, feedforward=steer_feedforward, speed=v_ego, deadzone=deadzone) pid_log.active = True pid_log.p = self.pid.p @@ -44,5 +46,4 @@ class LatControlPID(): pid_log.output = output_steer pid_log.saturated = bool(self.pid.saturated) - self.sat_flag = self.pid.saturated return output_steer, float(self.angle_steers_des), pid_log diff --git a/selfdrive/controls/lib/lateral_mpc/.gitignore b/selfdrive/controls/lib/lateral_mpc/.gitignore new file mode 100644 index 0000000000..f61a939cda --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/.gitignore @@ -0,0 +1,2 @@ +generator +lib_qp/ diff --git a/selfdrive/controls/lib/lateral_mpc/Makefile b/selfdrive/controls/lib/lateral_mpc/Makefile deleted file mode 100644 index 42970efd66..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/Makefile +++ /dev/null @@ -1,89 +0,0 @@ - -CC = clang -CXX = clang++ - -PHONELIBS = ../../../../phonelibs - -UNAME_S := $(shell uname -s) -UNAME_M := $(shell uname -m) - -CFLAGS = -O3 -fPIC -I. -CXXFLAGS = -O3 -fPIC -I. - -QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC - -ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado - -ifeq ($(UNAME_S),Darwin) -ACADO_LIBS := -lacado_toolkit_s -else ifeq ($(UNAME_M),aarch64) -ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 -else -ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a -endif - -OBJS = \ - lib_qp/Bounds.o \ - lib_qp/Constraints.o \ - lib_qp/CyclingManager.o \ - lib_qp/Indexlist.o \ - lib_qp/MessageHandling.o \ - lib_qp/QProblem.o \ - lib_qp/QProblemB.o \ - lib_qp/SubjectTo.o \ - lib_qp/Utils.o \ - lib_qp/EXTRAS/SolutionAnalysis.o \ - lib_mpc_export/acado_qpoases_interface.o \ - lib_mpc_export/acado_integrator.o \ - lib_mpc_export/acado_solver.o \ - lib_mpc_export/acado_auxiliary_functions.o \ - lateral_mpc.o - -DEPS := $(OBJS:.o=.d) - -.PHONY: all -all: libmpc.so - -libmpc.so: $(OBJS) - $(CXX) -shared -o '$@' $^ -lm - -lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp - @echo "[ CXX ] $@" - mkdir -p lib_qp/EXTRAS - $(CXX) $(CXXFLAGS) -MMD \ - -I lib_mpc_export/ \ - $(QPOASES_FLAGS) \ - -c -o '$@' '$<' - -%.o: %.cpp - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) -MMD \ - -I lib_mpc_export/ \ - $(QPOASES_FLAGS) \ - -c -o '$@' '$<' - -%.o: %.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -MMD \ - -I lib_mpc_export/ \ - $(QPOASES_FLAGS) \ - -c -o '$@' '$<' - -generator: generator.cpp - $(CXX) -v -Wall -std=c++11 \ - generator.cpp \ - -o generator \ - $(ACADO_FLAGS) \ - $(ACADO_LIBS) - -.PHONY: generate -generate: generator - ./generator - -.PHONY: clean -clean: - rm -f *.so generator $(OBJS) $(DEPS) - --include $(DEPS) diff --git a/selfdrive/controls/lib/lateral_mpc/SConscript b/selfdrive/controls/lib/lateral_mpc/SConscript new file mode 100644 index 0000000000..6cdc22d17d --- /dev/null +++ b/selfdrive/controls/lib/lateral_mpc/SConscript @@ -0,0 +1,29 @@ +Import('env', 'arch') + +cpp_path = [ + "#phonelibs/acado/include", + "#phonelibs/acado/include/acado", + "#phonelibs/qpoases/INCLUDE", + "#phonelibs/qpoases/INCLUDE/EXTRAS", + "#phonelibs/qpoases/SRC/", + "#phonelibs/qpoases", + "lib_mpc_export" + +] + +mpc_files = [ + "lateral_mpc.c", + Glob("lib_mpc_export/*.c"), + Glob("lib_mpc_export/*.cpp"), +] + +interface_dir = Dir('lib_mpc_export') + +SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) + +env.SharedLibrary('mpc', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) +# if arch != "aarch64": +# acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"), +# File("#phonelibs/acado/x64/lib/libacado_casadi.a"), +# File("#phonelibs/acado/x64/lib/libacado_csparse.a")] +# env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path) diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c index d8d29cc61c..4972dbc3ff 100644 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c +++ b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.c @@ -30,22 +30,10 @@ typedef struct { double cost; } log_t; -void init(double pathCost, double laneCost, double headingCost, double steerRateCost){ - acado_initializeSolver(); +void init_weights(double pathCost, double laneCost, double headingCost, double steerRateCost){ int i; const int STEP_MULTIPLIER = 3; - /* Initialize the states and controls. */ - for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; - for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.1; - - /* Initialize the measurements/reference. */ - for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; - for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; - - /* MPC: initialize the current state feedback. */ - for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; - for (i = 0; i < N; i++) { int f = 1; if (i > 4){ @@ -64,6 +52,24 @@ void init(double pathCost, double laneCost, double headingCost, double steerRate acadoVariables.WN[(NYN+1)*3] = headingCost * STEP_MULTIPLIER; } +void init(double pathCost, double laneCost, double headingCost, double steerRateCost){ + acado_initializeSolver(); + int i; + + /* Initialize the states and controls. */ + for (i = 0; i < NX * (N + 1); ++i) acadoVariables.x[ i ] = 0.0; + for (i = 0; i < NU * N; ++i) acadoVariables.u[ i ] = 0.1; + + /* Initialize the measurements/reference. */ + for (i = 0; i < NY * N; ++i) acadoVariables.y[ i ] = 0.0; + for (i = 0; i < NYN; ++i) acadoVariables.yN[ i ] = 0.0; + + /* MPC: initialize the current state feedback. */ + for (i = 0; i < NX; ++i) acadoVariables.x0[ i ] = 0.0; + + init_weights(pathCost, laneCost, headingCost, steerRateCost); +} + int run_mpc(state_t * x0, log_t * solution, double l_poly[4], double r_poly[4], double d_poly[4], double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width){ diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.d b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.d deleted file mode 100644 index 6cac7c2e14..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.d +++ /dev/null @@ -1,3 +0,0 @@ -lateral_mpc.o: lateral_mpc.c lib_mpc_export/acado_common.h \ - lib_mpc_export/acado_qpoases_interface.hpp \ - lib_mpc_export/acado_auxiliary_functions.h diff --git a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o b/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o deleted file mode 100644 index 0e19c553f7..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/lateral_mpc.o +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:79bd540099942480deacefb07bcc3077244ff5716f1f03351a685faf166f5508 -size 2008 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d deleted file mode 100644 index 0d376ee632..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.d +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9c5571edf4541f5966032a6964cdb3f1c3c26ac20c7fd8149c0ab6dd03c08c1f -size 219 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o deleted file mode 100644 index c56a6350a3..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_auxiliary_functions.o +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:171419af40cd4d2dd983caba560f2a0b62cc1de1e91c7c0b90aee82c7b65f828 -size 5192 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d deleted file mode 100644 index 87537719b2..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.d +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:28e31ac55feb4c787b02ce8c7eee799b1b9f95a9ec9b25e395fa4d4ce5807e6c -size 150 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o deleted file mode 100644 index 8b0c674f41..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_integrator.o +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:c93a23d4ce9f810746e08284a34476dc4ea9d51a53f0228b5edfd547e4aa05a9 -size 5496 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d deleted file mode 100644 index ae2e81c22c..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.d +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6480142ecf8a67001cfe5904fdabdbf05715901ae9e4d040673d55dc31d401ac -size 1260 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o deleted file mode 100644 index c413b8788b..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_qpoases_interface.o +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4e35ce1faf130e036cd0523e9422e046389665b98e62b48c1f7288e8dc39c75e -size 3008 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d deleted file mode 100644 index 8ae0395257..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.d +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2fa7bde92a2d7d20c459eac2c10dadc0811492cd4cd45495ae78fa05843a9b73 -size 142 diff --git a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o deleted file mode 100644 index a0fc95932b..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/lib_mpc_export/acado_solver.o +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:42e5a47c99cdbe9c34beec0208d2b9158e146c3722fd94ae9f50692b931a583c -size 272376 diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc.so b/selfdrive/controls/lib/lateral_mpc/libmpc.so deleted file mode 100755 index 2a9e16fe36..0000000000 --- a/selfdrive/controls/lib/lateral_mpc/libmpc.so +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:88415f9187ccd260f520eec417d16b801239813d5a28a6c55ecd978374c844d5 -size 531640 diff --git a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py index 92c0df8da4..d85eaecf8d 100644 --- a/selfdrive/controls/lib/lateral_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/lateral_mpc/libmpc_py.py @@ -1,11 +1,9 @@ import os -import subprocess from cffi import FFI mpc_dir = os.path.dirname(os.path.abspath(__file__)) libmpc_fn = os.path.join(mpc_dir, "libmpc.so") -subprocess.check_call(["make", "-j4"], cwd=mpc_dir) ffi = FFI() ffi.cdef(""" @@ -23,6 +21,7 @@ typedef struct { } log_t; void init(double pathCost, double laneCost, double headingCost, double steerRateCost); +void init_weights(double pathCost, double laneCost, double headingCost, double steerRateCost); int run_mpc(state_t * x0, log_t * solution, double l_poly[4], double r_poly[4], double d_poly[4], double l_prob, double r_prob, double curvature_factor, double v_ref, double lane_width); diff --git a/selfdrive/controls/lib/long_mpc.py b/selfdrive/controls/lib/long_mpc.py index 4c71683096..f694af3771 100644 --- a/selfdrive/controls/lib/long_mpc.py +++ b/selfdrive/controls/lib/long_mpc.py @@ -1,7 +1,7 @@ import os import math -import selfdrive.messaging as messaging +import cereal.messaging as messaging from selfdrive.swaglog import cloudlog from common.realtime import sec_since_boot from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU diff --git a/selfdrive/controls/lib/longitudinal_mpc/.gitignore b/selfdrive/controls/lib/longitudinal_mpc/.gitignore new file mode 100644 index 0000000000..f61a939cda --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/.gitignore @@ -0,0 +1,2 @@ +generator +lib_qp/ diff --git a/selfdrive/controls/lib/longitudinal_mpc/Makefile b/selfdrive/controls/lib/longitudinal_mpc/Makefile deleted file mode 100644 index e00914c214..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/Makefile +++ /dev/null @@ -1,91 +0,0 @@ -CC = clang -CXX = clang++ - -PHONELIBS = ../../../../phonelibs - -UNAME_S := $(shell uname -s) -UNAME_M := $(shell uname -m) - -CFLAGS = -O3 -fPIC -I. -CXXFLAGS = -O3 -fPIC -I. - -QPOASES_FLAGS = -I$(PHONELIBS)/qpoases -I$(PHONELIBS)/qpoases/INCLUDE -I$(PHONELIBS)/qpoases/SRC - -ACADO_FLAGS = -I$(PHONELIBS)/acado/include -I$(PHONELIBS)/acado/include/acado - -ifeq ($(UNAME_S),Darwin) -ACADO_LIBS := -lacado_toolkit_s -else ifeq ($(UNAME_M),aarch64) -ACADO_LIBS := -L $(PHONELIBS)/acado/aarch64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 -else -ACADO_LIBS := -L $(PHONELIBS)/acado/x64/lib -l:libacado_toolkit.a -l:libacado_casadi.a -l:libacado_csparse.a -endif - -OBJS = \ - lib_qp/Bounds.o \ - lib_qp/Constraints.o \ - lib_qp/CyclingManager.o \ - lib_qp/Indexlist.o \ - lib_qp/MessageHandling.o \ - lib_qp/QProblem.o \ - lib_qp/QProblemB.o \ - lib_qp/SubjectTo.o \ - lib_qp/Utils.o \ - lib_qp/EXTRAS/SolutionAnalysis.o \ - lib_mpc_export/acado_qpoases_interface.o \ - lib_mpc_export/acado_integrator.o \ - lib_mpc_export/acado_solver.o \ - lib_mpc_export/acado_auxiliary_functions.o \ - longitudinal_mpc.o - -DEPS := $(OBJS:.o=.d) - -.PHONY: all -all: libmpc1.so libmpc2.so - -libmpc1.so: $(OBJS) - $(CXX) -shared -o '$@' $^ -lm - -libmpc2.so: libmpc1.so - cp libmpc1.so libmpc2.so - -lib_qp/%.o: $(PHONELIBS)/qpoases/SRC/%.cpp - @echo "[ CXX ] $@" - mkdir -p lib_qp/EXTRAS - $(CXX) $(CXXFLAGS) -MMD \ - -I lib_mpc_export/ \ - $(QPOASES_FLAGS) \ - -c -o '$@' '$<' - -%.o: %.cpp - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) -MMD \ - -I lib_mpc_export/ \ - $(QPOASES_FLAGS) \ - -c -o '$@' '$<' - -%.o: %.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -MMD \ - -I lib_mpc_export/ \ - $(QPOASES_FLAGS) \ - -c -o '$@' '$<' - -generator: generator.cpp - $(CXX) -Wall -std=c++11 \ - generator.cpp \ - -o generator \ - $(ACADO_FLAGS) \ - $(ACADO_LIBS) - -.PHONY: generate -generate: generator - ./generator - -.PHONY: clean -clean: - rm -f *.so generator $(OBJS) $(DEPS) - --include $(DEPS) diff --git a/selfdrive/controls/lib/longitudinal_mpc/SConscript b/selfdrive/controls/lib/longitudinal_mpc/SConscript new file mode 100644 index 0000000000..072ff646ae --- /dev/null +++ b/selfdrive/controls/lib/longitudinal_mpc/SConscript @@ -0,0 +1,32 @@ +Import('env', 'arch') + + +cpp_path = [ + "#phonelibs/acado/include", + "#phonelibs/acado/include/acado", + "#phonelibs/qpoases/INCLUDE", + "#phonelibs/qpoases/INCLUDE/EXTRAS", + "#phonelibs/qpoases/SRC/", + "#phonelibs/qpoases", + "lib_mpc_export" + +] + +mpc_files = [ + "longitudinal_mpc.c", + Glob("lib_mpc_export/*.c"), + Glob("lib_mpc_export/*.cpp"), +] + +interface_dir = Dir('lib_mpc_export') + +SConscript(['#phonelibs/qpoases/SConscript'], variant_dir='lib_qp', exports=['interface_dir']) + +env.SharedLibrary('mpc1', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) +env.SharedLibrary('mpc2', mpc_files, LIBS=['m', 'qpoases'], LIBPATH=['lib_qp'], CPPPATH=cpp_path) + +# if arch != "aarch64": +# acado_libs = [File("#phonelibs/acado/x64/lib/libacado_toolkit.a"), +# File("#phonelibs/acado/x64/lib/libacado_casadi.a"), +# File("#phonelibs/acado/x64/lib/libacado_csparse.a")] +# env.Program('generator', 'generator.cpp', LIBS=acado_libs, CPPPATH=cpp_path) diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d deleted file mode 100644 index 0d376ee632..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.d +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:9c5571edf4541f5966032a6964cdb3f1c3c26ac20c7fd8149c0ab6dd03c08c1f -size 219 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o deleted file mode 100644 index 139912bfa4..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_auxiliary_functions.o +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:5a2f0c68a20c7c3a2eaff0010583389887c971be334bfb334c8cdc58a8f33a62 -size 5136 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d deleted file mode 100644 index 87537719b2..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.d +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:28e31ac55feb4c787b02ce8c7eee799b1b9f95a9ec9b25e395fa4d4ce5807e6c -size 150 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o deleted file mode 100644 index 9e445fed65..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_integrator.o +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:bcdecad1a69b573be3c70bb5271d7cf928592c0a82e002e515dfebd899c336ba -size 3576 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.d deleted file mode 100644 index ae2e81c22c..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.d +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:6480142ecf8a67001cfe5904fdabdbf05715901ae9e4d040673d55dc31d401ac -size 1260 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o deleted file mode 100644 index 4b3c2303d1..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_qpoases_interface.o +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:7bfecf197fc15a276103a268f4448b293472af7534adb168be6fcc1385aa2747 -size 3008 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d deleted file mode 100644 index 8ae0395257..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.d +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:2fa7bde92a2d7d20c459eac2c10dadc0811492cd4cd45495ae78fa05843a9b73 -size 142 diff --git a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o b/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o deleted file mode 100644 index 217a707edb..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/lib_mpc_export/acado_solver.o +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:52a0fb9d6feeb8182644a27c04821a5e1502c8e5a97b743f507d779461a692af -size 180712 diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so b/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so deleted file mode 100755 index 5665ad1ee2..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc1.so +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:3a649d81814d4476522e809c2ebfeefad7ce0bc84af5a525b6f1d5f96bd6e97c -size 437384 diff --git a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py index 95e9135a81..b43773593f 100644 --- a/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py +++ b/selfdrive/controls/lib/longitudinal_mpc/libmpc_py.py @@ -1,20 +1,9 @@ import os -import platform -import subprocess from cffi import FFI mpc_dir = os.path.join(os.path.dirname(os.path.abspath(__file__))) -if platform.machine() == "x86_64": - try: - FFI().dlopen(os.path.join(mpc_dir, "libmpc1.so")) - except OSError: - # libmpc1.so is likely built for aarch64. cleaning... - subprocess.check_call(["make", "clean"], cwd=mpc_dir) - -subprocess.check_call(["make", "-j4"], cwd=mpc_dir) - def _get_libmpc(mpc_id): libmpc_fn = os.path.join(mpc_dir, "libmpc%d.so" % mpc_id) diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.d b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.d deleted file mode 100644 index 34a2a0d20c..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.d +++ /dev/null @@ -1,3 +0,0 @@ -longitudinal_mpc.o: longitudinal_mpc.c lib_mpc_export/acado_common.h \ - lib_mpc_export/acado_qpoases_interface.hpp \ - lib_mpc_export/acado_auxiliary_functions.h diff --git a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o b/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o deleted file mode 100644 index ee5edddf2b..0000000000 --- a/selfdrive/controls/lib/longitudinal_mpc/longitudinal_mpc.o +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:4b204da3d4e4a763fc289cf2c539700b97260f7798df5a6ba707f58d4d12da60 -size 3152 diff --git a/selfdrive/controls/lib/pathplanner.py b/selfdrive/controls/lib/pathplanner.py index 5678cf05f9..87e3c520fc 100644 --- a/selfdrive/controls/lib/pathplanner.py +++ b/selfdrive/controls/lib/pathplanner.py @@ -1,14 +1,39 @@ import os import math -from common.realtime import sec_since_boot +from common.realtime import sec_since_boot, DT_MDL from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.lateral_mpc import libmpc_py from selfdrive.controls.lib.drive_helpers import MPC_COST_LAT from selfdrive.controls.lib.lane_planner import LanePlanner -import selfdrive.messaging as messaging +from selfdrive.config import Conversions as CV +import cereal.messaging as messaging +from cereal import log + +LaneChangeState = log.PathPlan.LaneChangeState +LaneChangeDirection = log.PathPlan.LaneChangeDirection LOG_MPC = os.environ.get('LOG_MPC', False) +DESIRES = { + LaneChangeDirection.none: { + LaneChangeState.off: log.PathPlan.Desire.none, + LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.none, + LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.none, + }, + LaneChangeDirection.left: { + LaneChangeState.off: log.PathPlan.Desire.none, + LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeLeft, + LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeLeft, + }, + LaneChangeDirection.right: { + LaneChangeState.off: log.PathPlan.Desire.none, + LaneChangeState.preLaneChange: log.PathPlan.Desire.none, + LaneChangeState.laneChangeStarting: log.PathPlan.Desire.laneChangeRight, + LaneChangeState.laneChangeFinishing: log.PathPlan.Desire.laneChangeRight, + }, +} def calc_states_after_delay(states, v_ego, steer_angle, curvature_factor, steer_ratio, delay): states[0].x = v_ego * delay @@ -21,14 +46,18 @@ class PathPlanner(): self.LP = LanePlanner() self.last_cloudlog_t = 0 + self.steer_rate_cost = CP.steerRateCost - self.setup_mpc(CP.steerRateCost) + self.setup_mpc() self.solution_invalid_cnt = 0 self.path_offset_i = 0.0 + self.lane_change_state = LaneChangeState.off + self.lane_change_timer = 0.0 + self.prev_one_blinker = False - def setup_mpc(self, steer_rate_cost): + def setup_mpc(self): self.libmpc = libmpc_py.libmpc - self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, steer_rate_cost) + self.libmpc.init(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) self.mpc_solution = libmpc_py.ffi.new("log_t *") self.cur_state = libmpc_py.ffi.new("state_t *") @@ -49,13 +78,75 @@ class PathPlanner(): angle_offset = sm['liveParameters'].angleOffset - self.LP.update(v_ego, sm['model']) - # Run MPC self.angle_steers_des_prev = self.angle_steers_des_mpc VM.update_params(sm['liveParameters'].stiffnessFactor, sm['liveParameters'].steerRatio) curvature_factor = VM.curvature_factor(v_ego) + self.LP.parse_model(sm['model']) + + # Lane change logic + lane_change_direction = LaneChangeDirection.none + one_blinker = sm['carState'].leftBlinker != sm['carState'].rightBlinker + + if not active or self.lane_change_timer > 10.0: + self.lane_change_state = LaneChangeState.off + else: + if sm['carState'].leftBlinker: + lane_change_direction = LaneChangeDirection.left + elif sm['carState'].rightBlinker: + lane_change_direction = LaneChangeDirection.right + + if lane_change_direction == LaneChangeDirection.left: + torque_applied = sm['carState'].steeringTorque > 0 and sm['carState'].steeringPressed + else: + torque_applied = sm['carState'].steeringTorque < 0 and sm['carState'].steeringPressed + + lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob + + # State transitions + # off + if False: # self.lane_change_state == LaneChangeState.off and one_blinker and not self.prev_one_blinker: + self.lane_change_state = LaneChangeState.preLaneChange + + # pre + elif self.lane_change_state == LaneChangeState.preLaneChange and not one_blinker: + self.lane_change_state = LaneChangeState.off + elif self.lane_change_state == LaneChangeState.preLaneChange and torque_applied: + self.lane_change_state = LaneChangeState.laneChangeStarting + + # starting + elif self.lane_change_state == LaneChangeState.laneChangeStarting and lane_change_prob > 0.5: + self.lane_change_state = LaneChangeState.laneChangeFinishing + + # finishing + elif self.lane_change_state == LaneChangeState.laneChangeFinishing and lane_change_prob < 0.2: + self.lane_change_state = LaneChangeState.preLaneChange + + # Don't allow starting lane change below 45 mph + if (v_ego < 45 * CV.MPH_TO_MS) and (self.lane_change_state == LaneChangeState.preLaneChange): + self.lane_change_state = LaneChangeState.off + + if self.lane_change_state in [LaneChangeState.off, LaneChangeState.preLaneChange]: + self.lane_change_timer = 0.0 + else: + self.lane_change_timer += DT_MDL + + self.prev_one_blinker = one_blinker + + desire = DESIRES[lane_change_direction][self.lane_change_state] + + # Turn off lanes during lane change + if desire == log.PathPlan.Desire.laneChangeRight or desire == log.PathPlan.Desire.laneChangeLeft: + self.LP.l_prob = 0. + self.LP.r_prob = 0. + self.libmpc.init_weights(MPC_COST_LAT.PATH / 10.0, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) + else: + self.libmpc.init_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.LANE, MPC_COST_LAT.HEADING, self.steer_rate_cost) + + self.LP.update_d_poly(v_ego) + + # TODO: Check for active, override, and saturation # if active: # self.path_offset_i += self.LP.d_poly[3] / (60.0 * 20.0) @@ -119,6 +210,10 @@ class PathPlanner(): plan_send.pathPlan.sensorValid = bool(sm['liveParameters'].sensorValid) plan_send.pathPlan.posenetValid = bool(sm['liveParameters'].posenetValid) + plan_send.pathPlan.desire = desire + plan_send.pathPlan.laneChangeState = self.lane_change_state + plan_send.pathPlan.laneChangeDirection = lane_change_direction + pm.send('pathPlan', plan_send) if LOG_MPC: diff --git a/selfdrive/controls/lib/pid.py b/selfdrive/controls/lib/pid.py index ce2d34b359..fbd3885304 100644 --- a/selfdrive/controls/lib/pid.py +++ b/selfdrive/controls/lib/pid.py @@ -35,10 +35,10 @@ class PIController(): def k_i(self): return interp(self.speed, self._k_i[0], self._k_i[1]) - def _check_saturation(self, control, override, error): + def _check_saturation(self, control, check_saturation, error): saturated = (control < self.neg_limit) or (control > self.pos_limit) - if saturated and not override and abs(error) > 0.1: + if saturated and check_saturation and abs(error) > 0.1: self.sat_count += self.sat_count_rate else: self.sat_count -= self.sat_count_rate @@ -82,10 +82,7 @@ class PIController(): if self.convert is not None: control = self.convert(control, speed=self.speed) - if check_saturation: - self.saturated = self._check_saturation(control, override, error) - else: - self.saturated = False + self.saturated = self._check_saturation(control, check_saturation, error) self.control = clip(control, self.neg_limit, self.pos_limit) return self.control diff --git a/selfdrive/controls/lib/planner.py b/selfdrive/controls/lib/planner.py index db3fe33217..6646e13454 100755 --- a/selfdrive/controls/lib/planner.py +++ b/selfdrive/controls/lib/planner.py @@ -4,9 +4,9 @@ import numpy as np from common.params import Params from common.numpy_fast import interp -import selfdrive.messaging as messaging +import cereal.messaging as messaging from cereal import car -from common.realtime import sec_since_boot, DT_PLAN +from common.realtime import sec_since_boot from selfdrive.swaglog import cloudlog from selfdrive.config import Conversions as CV from selfdrive.controls.lib.speed_smoother import speed_smoother @@ -34,13 +34,6 @@ _A_CRUISE_MAX_BP = [0., 6.4, 22.5, 40.] _A_TOTAL_MAX_V = [1.7, 3.2] _A_TOTAL_MAX_BP = [20., 40.] - -# Model speed kalman stuff -_MODEL_V_A = [[1.0, DT_PLAN], [0.0, 1.0]] -_MODEL_V_C = [1.0, 0] -# calculated with observation std of 2m/s and accel proc noise of 2m/s**2 -_MODEL_V_K = [[0.07068858], [0.04826294]] - # 75th percentile SPEED_PERCENTILE_IDX = 7 @@ -245,7 +238,7 @@ class Planner(): pm.send('plan', plan_send) # Interpolate 0.05 seconds and save as starting point for next iteration - a_acc_sol = self.a_acc_start + (DT_PLAN / LON_MPC_STEP) * (self.a_acc - self.a_acc_start) - v_acc_sol = self.v_acc_start + DT_PLAN * (a_acc_sol + self.a_acc_start) / 2.0 + a_acc_sol = self.a_acc_start + (CP.radarTimeStep / LON_MPC_STEP) * (self.a_acc - self.a_acc_start) + v_acc_sol = self.v_acc_start + CP.radarTimeStep * (a_acc_sol + self.a_acc_start) / 2.0 self.v_acc_start = v_acc_sol self.a_acc_start = a_acc_sol diff --git a/selfdrive/controls/lib/radar_helpers.py b/selfdrive/controls/lib/radar_helpers.py index 557679e5dc..8187399679 100644 --- a/selfdrive/controls/lib/radar_helpers.py +++ b/selfdrive/controls/lib/radar_helpers.py @@ -1,6 +1,5 @@ -from common.realtime import DT_MDL from common.kalman.simple_kalman import KF1D -from selfdrive.config import RADAR_TO_CENTER +from selfdrive.config import RADAR_TO_CAMERA # the longer lead decels, the more likely it will keep decelerating @@ -13,38 +12,28 @@ SPEED, ACCEL = 0, 1 # Kalman filter states enum # stationary qualification parameters v_ego_stationary = 4. # no stationary object flag below this speed -# Lead Kalman Filter params -_VLEAD_A = [[1.0, DT_MDL], [0.0, 1.0]] -_VLEAD_C = [1.0, 0.0] -#_VLEAD_Q = np.matrix([[10., 0.0], [0.0, 100.]]) -#_VLEAD_R = 1e3 -#_VLEAD_K = np.matrix([[ 0.05705578], [ 0.03073241]]) -_VLEAD_K = [[0.1988689], [0.28555364]] - class Track(): - def __init__(self): - self.ekf = None + def __init__(self, v_lead, kalman_params): self.cnt = 0 self.aLeadTau = _LEAD_ACCEL_TAU + self.K_A = kalman_params.A + self.K_C = kalman_params.C + self.K_K = kalman_params.K + self.kf = KF1D([[v_lead], [0.0]], self.K_A, self.K_C, self.K_K) - def update(self, d_rel, y_rel, v_rel, v_ego_t_aligned, measured): + def update(self, d_rel, y_rel, v_rel, v_lead, measured): # relative values, copy self.dRel = d_rel # LONG_DIST self.yRel = y_rel # -LAT_DIST self.vRel = v_rel # REL_SPEED + self.vLead = v_lead self.measured = measured # measured or estimate # computed velocity and accelerations - self.vLead = self.vRel + v_ego_t_aligned - - if self.cnt == 0: - self.kf = KF1D([[self.vLead], [0.0]], _VLEAD_A, _VLEAD_C, _VLEAD_K) - else: + if self.cnt > 0: self.kf.update(self.vLead) - self.cnt += 1 - self.vLeadK = float(self.kf.x[SPEED][0]) self.aLeadK = float(self.kf.x[ACCEL][0]) @@ -54,12 +43,14 @@ class Track(): else: self.aLeadTau *= 0.9 + self.cnt += 1 + def get_key_for_cluster(self): # Weigh y higher since radar is inaccurate in this dimension return [self.dRel, self.yRel*2, self.vRel] def reset_a_lead(self, aLeadK, aLeadTau): - self.kf = KF1D([[self.vLead], [aLeadK]], _VLEAD_A, _VLEAD_C, _VLEAD_K) + self.kf = KF1D([[self.vLead], [aLeadK]], self.K_A, self.K_C, self.K_K) self.aLeadK = aLeadK self.aLeadTau = aLeadTau @@ -143,7 +134,7 @@ class Cluster(): def get_RadarState_from_vision(self, lead_msg, v_ego): return { - "dRel": float(lead_msg.dist - RADAR_TO_CENTER), + "dRel": float(lead_msg.dist - RADAR_TO_CAMERA), "yRel": float(lead_msg.relY), "vRel": float(lead_msg.relVel), "vLead": float(v_ego + lead_msg.relVel), diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py index 59fe51aaca..21ea32a51a 100755 --- a/selfdrive/controls/plannerd.py +++ b/selfdrive/controls/plannerd.py @@ -8,7 +8,7 @@ from selfdrive.swaglog import cloudlog from selfdrive.controls.lib.planner import Planner from selfdrive.controls.lib.vehicle_model import VehicleModel from selfdrive.controls.lib.pathplanner import PathPlanner -import selfdrive.messaging as messaging +import cereal.messaging as messaging def plannerd_thread(sm=None, pm=None): diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index 4969ba3357..a99a0bdfbf 100755 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -3,25 +3,31 @@ import importlib import math from collections import defaultdict, deque -import selfdrive.messaging as messaging +import cereal.messaging as messaging from cereal import car +from common.numpy_fast import interp from common.params import Params -from common.realtime import DT_RDR, Ratekeeper, set_realtime_priority +from common.realtime import Ratekeeper, set_realtime_priority from selfdrive.config import RADAR_TO_CAMERA -from selfdrive.controls.lib.cluster.fastcluster_py import \ - cluster_points_centroid +from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid from selfdrive.controls.lib.radar_helpers import Cluster, Track from selfdrive.swaglog import cloudlog -DEBUG = False -#vision point -DIMSV = 2 -XV, SPEEDV = 0, 1 -VISION_POINT = -1 - -# Time-alignment -v_len = 20 # how many speed data points to remember for t alignment with rdr data +class KalmanParams(): + def __init__(self, dt): + # Lead Kalman Filter params, calculating K from A, C, Q, R requires the control library. + # hardcoding a lookup table to compute K for values of radar_ts between 0.1s and 1.0s + assert dt > .01 and dt < .1, "Radar time step must be between .01s and 0.1s" + self.A = [[1.0, dt], [0.0, 1.0]] + self.C = [1.0, 0.0] + #Q = np.matrix([[10., 0.0], [0.0, 100.]]) + #R = 1e3 + #K = np.matrix([[ 0.05705578], [ 0.03073241]]) + dts = [dt * 0.01 for dt in range(1, 11)] + K0 = [0.12288, 0.14557, 0.16523, 0.18282, 0.19887, 0.21372, 0.22761, 0.24069, 0.2531, 0.26491] + K1 = [0.29666, 0.29331, 0.29043, 0.28787, 0.28555, 0.28342, 0.28144, 0.27958, 0.27783, 0.27617] + self.K = [[interp(dt, dts, K0)], [interp(dt, dts, K1)]] def laplacian_cdf(x, mu, b): @@ -79,11 +85,11 @@ def get_lead(v_ego, ready, clusters, lead_msg, low_speed_override=True): class RadarD(): - def __init__(self, mocked, delay=0): + def __init__(self, radar_ts, delay=0): self.current_time = 0 - self.mocked = mocked self.tracks = defaultdict(dict) + self.kalman_params = KalmanParams(radar_ts) self.last_md_ts = 0 self.last_controls_state_ts = 0 @@ -94,7 +100,6 @@ class RadarD(): self.v_ego = 0. self.v_ego_hist = deque([0], maxlen=delay+1) - self.v_ego_t_aligned = 0. self.ready = False def update(self, frame, sm, rr, has_radar): @@ -121,12 +126,12 @@ class RadarD(): rpt = ar_pts[ids] # align v_ego by a fixed time to align it with the radar measurement - self.v_ego_t_aligned = self.v_ego_hist[0] + v_lead = rpt[2] + self.v_ego_hist[0] # create the track if it doesn't exist or it's a new track if ids not in self.tracks: - self.tracks[ids] = Track() - self.tracks[ids].update(rpt[0], rpt[1], rpt[2], self.v_ego_t_aligned, rpt[3]) + self.tracks[ids] = Track(v_lead, self.kalman_params) + self.tracks[ids].update(rpt[0], rpt[1], rpt[2], v_lead, rpt[3]) idens = list(sorted(self.tracks.keys())) track_pts = list([self.tracks[iden].get_key_for_cluster() for iden in idens]) @@ -179,7 +184,6 @@ def radard_thread(sm=None, pm=None, can_sock=None): # wait for stats about the car to come in from controls cloudlog.info("radard is waiting for CarParams") CP = car.CarParams.from_bytes(Params().get("CarParams", block=True)) - mocked = CP.carName == "mock" cloudlog.info("radard got CarParams") # import the radar from the fingerprint @@ -198,8 +202,8 @@ def radard_thread(sm=None, pm=None, can_sock=None): RI = RadarInterface(CP) - rk = Ratekeeper(1.0 / DT_RDR, print_delay_threshold=None) - RD = RadarD(mocked, RI.delay) + rk = Ratekeeper(1.0 / CP.radarTimeStep, print_delay_threshold=None) + RD = RadarD(CP.radarTimeStep, RI.delay) has_radar = not CP.radarOffCan diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py index 331cb14eac..cc70bb4d38 100644 --- a/selfdrive/controls/tests/test_following_distance.py +++ b/selfdrive/controls/tests/test_following_distance.py @@ -2,7 +2,7 @@ import unittest import numpy as np from cereal import log -import selfdrive.messaging as messaging +import cereal.messaging as messaging from selfdrive.config import Conversions as CV from selfdrive.controls.lib.planner import calc_cruise_accel_limits from selfdrive.controls.lib.speed_smoother import speed_smoother diff --git a/selfdrive/can/tests/__init__.py b/selfdrive/debug/__init__.py similarity index 100% rename from selfdrive/can/tests/__init__.py rename to selfdrive/debug/__init__.py diff --git a/selfdrive/debug/can_printer.py b/selfdrive/debug/can_printer.py index 9a72b29fa2..f06db79545 100755 --- a/selfdrive/debug/can_printer.py +++ b/selfdrive/debug/can_printer.py @@ -4,7 +4,7 @@ import os import sys from collections import defaultdict -import selfdrive.messaging as messaging +import cereal.messaging as messaging from common.realtime import sec_since_boot diff --git a/selfdrive/debug/check_freq.py b/selfdrive/debug/check_freq.py new file mode 100755 index 0000000000..75c0b8f89a --- /dev/null +++ b/selfdrive/debug/check_freq.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +import argparse +import numpy as np +from collections import defaultdict, deque +from common.realtime import sec_since_boot +import cereal.messaging as messaging + + +if __name__ == "__main__": + context = messaging.Context() + poller = messaging.Poller() + + parser = argparse.ArgumentParser() + parser.add_argument("socket", type=str, nargs='*', help="socket name") + args = parser.parse_args() + + socket_names = args.socket + sockets = {} + + rcv_times = defaultdict(lambda: deque(maxlen=100)) + + t = sec_since_boot() + for name in socket_names: + sock = messaging.sub_sock(name, poller=poller) + sockets[sock] = name + + prev_print = t + while True: + for socket in poller.poll(100): + msg = messaging.recv_one(socket) + name = msg.which() + + t = sec_since_boot() + rcv_times[name].append(msg.logMonoTime / 1e9) + + if t - prev_print > 1: + print() + for name in socket_names: + dts = np.diff(rcv_times[name]) + mean = np.mean(dts) + print("%s: Freq %.2f Hz, Min %.2f%%, Max %.2f%%" % (name, 1.0 / mean, np.min(dts) / mean * 100, np.max(dts) / mean * 100)) + + prev_print = t diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py index e1a6ac35d4..160f4f9d55 100755 --- a/selfdrive/debug/cpu_usage_stat.py +++ b/selfdrive/debug/cpu_usage_stat.py @@ -30,7 +30,7 @@ SLEEP_INTERVAL = 0.2 monitored_proc_names = [ 'ubloxd', 'thermald', 'uploader', 'deleter', 'controlsd', 'plannerd', 'radard', 'mapd', 'loggerd' , 'logmessaged', 'tombstoned', - 'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'calibrationd', 'params_learner', 'visiond', 'sensord', 'updated', 'gpsd', 'athena'] + 'logcatd', 'proclogd', 'boardd', 'pandad', './ui', 'ui', 'calibrationd', 'params_learner', 'modeld', 'monitoringd', 'camerad', 'sensord', 'updated', 'gpsd', 'athena'] cpu_time_names = ['user', 'system', 'children_user', 'children_system'] timer = getattr(time, 'monotonic', time.time) diff --git a/selfdrive/debug/dump.py b/selfdrive/debug/dump.py index b8fc0582e8..26ce350530 100755 --- a/selfdrive/debug/dump.py +++ b/selfdrive/debug/dump.py @@ -1,15 +1,15 @@ #!/usr/bin/env python3 +import os import sys import argparse import json from hexdump import hexdump from cereal import log -import selfdrive.messaging as messaging -from selfdrive.services import service_list +import cereal.messaging as messaging +from cereal.services import service_list if __name__ == "__main__": - poller = messaging.Poller() parser = argparse.ArgumentParser(description='Sniff a communcation socket') parser.add_argument('--pipe', action='store_true') @@ -22,6 +22,11 @@ if __name__ == "__main__": parser.add_argument("socket", type=str, nargs='*', help="socket name") args = parser.parse_args() + if args.addr != "127.0.0.1": + os.environ["ZMQ"] = "1" + messaging.context = messaging.Context() + + poller = messaging.Poller() for m in args.socket if len(args.socket) > 0 else service_list: sock = messaging.sub_sock(m, poller, addr=args.addr) diff --git a/selfdrive/debug/get_fingerprint.py b/selfdrive/debug/get_fingerprint.py index e2111e1914..9ece7d7268 100755 --- a/selfdrive/debug/get_fingerprint.py +++ b/selfdrive/debug/get_fingerprint.py @@ -11,7 +11,7 @@ # - since some messages are published at low frequency, keep this script running for at least 30s, # until all messages are received at least once -import selfdrive.messaging as messaging +import cereal.messaging as messaging logcan = messaging.sub_sock('can') msgs = {} diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py index 0cc62ef7c5..7286841a84 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import numpy as np -from selfdrive.messaging import SubMaster +from cereal.messaging import SubMaster def cputime_total(ct): return ct.user + ct.nice + ct.system + ct.idle + ct.iowait + ct.irq + ct.softirq diff --git a/selfdrive/debug/mpc/live_lateral_mpc.py b/selfdrive/debug/mpc/live_lateral_mpc.py index 5d7bc2bb82..48be5be9c6 100755 --- a/selfdrive/debug/mpc/live_lateral_mpc.py +++ b/selfdrive/debug/mpc/live_lateral_mpc.py @@ -3,7 +3,7 @@ import matplotlib matplotlib.use('TkAgg') import sys -import selfdrive.messaging as messaging +import cereal.messaging as messaging import numpy as np import matplotlib.pyplot as plt diff --git a/selfdrive/debug/mpc/live_longitudinal_mpc.py b/selfdrive/debug/mpc/live_longitudinal_mpc.py index c2d1143dd1..b5b4361e13 100755 --- a/selfdrive/debug/mpc/live_longitudinal_mpc.py +++ b/selfdrive/debug/mpc/live_longitudinal_mpc.py @@ -1,10 +1,7 @@ #!/usr/bin/env python3 -import matplotlib -matplotlib.use('TkAgg') - import sys -import selfdrive.messaging as messaging +import cereal.messaging as messaging import numpy as np import matplotlib.pyplot as plt diff --git a/selfdrive/debug/show_matching_cars.py b/selfdrive/debug/show_matching_cars.py index 424e89887d..df2b703635 100755 --- a/selfdrive/debug/show_matching_cars.py +++ b/selfdrive/debug/show_matching_cars.py @@ -1,6 +1,6 @@ #!/usr/bin/env python3 from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_known_cars -import selfdrive.messaging as messaging +import cereal.messaging as messaging # Prius and Leuxs es 300H diff --git a/selfdrive/launcher.py b/selfdrive/launcher.py new file mode 100644 index 0000000000..8253347d6b --- /dev/null +++ b/selfdrive/launcher.py @@ -0,0 +1,27 @@ +import importlib +from setproctitle import setproctitle #pylint: disable=no-name-in-module + +import cereal.messaging as messaging +import selfdrive.crash as crash +from selfdrive.swaglog import cloudlog + +def launcher(proc): + try: + # import the process + mod = importlib.import_module(proc) + + # rename the process + setproctitle(proc) + + # create new context since we forked + messaging.context = messaging.Context() + + # exec the process + mod.main() + except KeyboardInterrupt: + cloudlog.warning("child %s got SIGINT" % proc) + except Exception: + # can't install the crash handler becuase sys.excepthook doesn't play nice + # with threads, so catch it here. + crash.capture_exception() + raise diff --git a/selfdrive/locationd/Makefile b/selfdrive/locationd/Makefile deleted file mode 100644 index 9953293346..0000000000 --- a/selfdrive/locationd/Makefile +++ /dev/null @@ -1,115 +0,0 @@ -CC = clang -CXX = clang++ - -ARCH := $(shell uname -m) -OS := $(shell uname -o) - -BASEDIR = ../.. -PHONELIBS = ../../phonelibs - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args - -CFLAGS = -std=gnu11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS) -Wall -CXXFLAGS = -std=c++11 -g -fPIC -I../ -I../../ -O2 $(WARN_FLAGS) -Wall - -MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging -MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a - -EXTRA_LIBS = -lpthread - -ifeq ($(ARCH),aarch64) -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 -EXTRA_LIBS += -llog -luuid -lgnustl_shared -endif - - -JSON_FLAGS = -I$(PHONELIBS)/json/src -JSON11_FLAGS = -I$(PHONELIBS)/json11 - - -ifeq ($(ARCH),x86_64) -ZMQ_FLAGS = -I$(BASEDIR)/phonelibs/zmq/x64/include -endif - -.PHONY: all -all: ubloxd paramsd - -include ../common/cereal.mk - -LOC_OBJS = locationd_yawrate.o params_learner.o paramsd.o \ - ../common/swaglog.o \ - ../common/params.o \ - ../common/util.o \ - $(PHONELIBS)/json11/json11.o \ - $(PHONELIBS)/json/src/json.o \ - $(CEREAL_OBJS) - -LOC_DEPS := $(LOC_OBJS:.o=.d) - -OBJS = ublox_msg.o \ - ubloxd_main.o \ - ../common/swaglog.o \ - ../common/params.o \ - ../common/util.o \ - $(PHONELIBS)/json/src/json.o \ - $(CEREAL_OBJS) - -DEPS := $(OBJS:.o=.d) ubloxd.d ubloxd_test.d - -liblocationd.so: $(LOC_OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -shared -o '$@' $^ \ - $(CEREAL_LIBS) \ - $(EXTRA_LIBS) - -paramsd: $(LOC_OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(CEREAL_LIBS) \ - $(EXTRA_LIBS) - -ubloxd: ubloxd.o $(OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(CEREAL_LIBS) \ - $(EXTRA_LIBS) - -ubloxd_test: ubloxd_test.o $(OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(CEREAL_LIBS) \ - $(EXTRA_LIBS) - -%.o: %.cc - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) -MMD \ - -Iinclude -I.. -I../.. \ - $(CEREAL_CXXFLAGS) \ - $(ZMQ_FLAGS) \ - $(MESSAGING_FLAGS) \ - $(JSON11_FLAGS) \ - $(JSON_FLAGS) \ - -I../ \ - -I../../ \ - -c -o '$@' '$<' - -%.o: %.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -MMD \ - -Iinclude -I.. -I../.. \ - $(CEREAL_CFLAGS) \ - $(ZMQ_FLAGS) \ - $(JSON_FLAGS) \ - -c -o '$@' '$<' - -.PHONY: clean -clean: - rm -f ubloxd paramsd liblocationd.so ubloxd.d ubloxd.o ubloxd_test ubloxd_test.o ubloxd_test.d $(OBJS) $(LOC_OBJS) $(DEPS) - --include $(DEPS) --include $(LOC_DEPS) diff --git a/selfdrive/locationd/SConscript b/selfdrive/locationd/SConscript new file mode 100644 index 0000000000..fca12d1324 --- /dev/null +++ b/selfdrive/locationd/SConscript @@ -0,0 +1,25 @@ +Import('env', 'common', 'messaging') +loc_objs = [ + "locationd_yawrate.cc", + "params_learner.cc", + "paramsd.cc"] +loc_libs = [messaging, 'zmq', common, 'capnp', 'kj', 'json', 'json11', 'pthread'] + +env.Program("paramsd", loc_objs, LIBS=loc_libs) +env.SharedLibrary("locationd", loc_objs, LIBS=loc_libs) + +env.Program("ubloxd", [ + "ubloxd.cc", + "ublox_msg.cc", + "ubloxd_main.cc"], + LIBS=loc_libs) + +env.Program("ubloxd_test", [ + "ubloxd_test.cc", + "ublox_msg.cc", + "ubloxd_main.cc"], + LIBS=loc_libs) + + + + diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 50cc9d0157..3811d24f8c 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -4,7 +4,7 @@ import os import copy import json import numpy as np -import selfdrive.messaging as messaging +import cereal.messaging as messaging from selfdrive.locationd.calibration_helpers import Calibration from selfdrive.swaglog import cloudlog from common.params import Params, put_nonblocking diff --git a/selfdrive/locationd/get_vp.c b/selfdrive/locationd/get_vp.c deleted file mode 100644 index 8a48c88150..0000000000 --- a/selfdrive/locationd/get_vp.c +++ /dev/null @@ -1,41 +0,0 @@ -int get_intersections(double *lines, double *intersections, long long n) { - double D, Dx, Dy; - double x, y; - double *L1, *L2; - int k = 0; - for (int i=0; i < n; i++) { - for (int j=0; j < n; j++) { - L1 = lines + i*3; - L2 = lines + j*3; - D = L1[0] * L2[1] - L1[1] * L2[0]; - Dx = L1[2] * L2[1] - L1[1] * L2[2]; - Dy = L1[0] * L2[2] - L1[2] * L2[0]; - // only intersect lines from different quadrants and only left-right crossing - if ((D != 0) && (L1[0]*L2[0]*L1[1]*L2[1] < 0) && (L1[1]*L2[1] < 0)){ - x = Dx / D; - y = Dy / D; - if ((0 < x) && - (x < W) && - (0 < y) && - (y < H)){ - intersections[k*2 + 0] = x; - intersections[k*2 + 1] = y; - k++; - } - } - } - } - return k; -} - -void increment_grid(double *grid, double *lines, long long n) { - double *intersections = (double*) malloc(n*n*2*sizeof(double)); - int y, x, k; - k = get_intersections(lines, intersections, n); - for (int i=0; i < k; i++) { - x = (int) (intersections[i*2 + 0] + 0.5); - y = (int) (intersections[i*2 + 1] + 0.5); - grid[y*(W+1) + x] += 1.; - } - free(intersections); -} diff --git a/selfdrive/locationd/paramsd.cc b/selfdrive/locationd/paramsd.cc index a38fc403ab..6109c271c3 100644 --- a/selfdrive/locationd/paramsd.cc +++ b/selfdrive/locationd/paramsd.cc @@ -34,6 +34,12 @@ int main(int argc, char *argv[]) { SubSocket * sensor_events_sock = SubSocket::create(c, "sensorEvents"); SubSocket * camera_odometry_sock = SubSocket::create(c, "cameraOdometry"); PubSocket * live_parameters_sock = PubSocket::create(c, "liveParameters"); + + assert(controls_state_sock != NULL); + assert(sensor_events_sock != NULL); + assert(camera_odometry_sock != NULL); + assert(live_parameters_sock != NULL); + Poller * poller = Poller::create({controls_state_sock, sensor_events_sock, camera_odometry_sock}); Localizer localizer; @@ -97,7 +103,7 @@ int main(int argc, char *argv[]) { // Main loop int save_counter = 0; while (true){ - for (auto s : poller->poll(-1)){ + for (auto s : poller->poll(100)){ Message * msg = s->receive(); auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); diff --git a/selfdrive/locationd/test/ci_test.py b/selfdrive/locationd/test/ci_test.py index 847dcf635f..5b23e8326a 100755 --- a/selfdrive/locationd/test/ci_test.py +++ b/selfdrive/locationd/test/ci_test.py @@ -38,9 +38,6 @@ def main(args): print('Extract file failed') sys.exit(-3) - print('Compiling test app...') - subprocess.check_call(["make", "ubloxd_test"], cwd=ubloxd_dir) - print('Run regression test - CC parser...') if args.valgrind: subprocess.check_call(["valgrind", "--leak-check=full", os.path.join(ubloxd_dir, 'ubloxd_test'), stream_file_path, cc_output_dir]) diff --git a/selfdrive/locationd/test/test_params_learner.py b/selfdrive/locationd/test/test_params_learner.py index c1b75c5af9..a3c8487858 100755 --- a/selfdrive/locationd/test/test_params_learner.py +++ b/selfdrive/locationd/test/test_params_learner.py @@ -6,7 +6,7 @@ import unittest from selfdrive.car.honda.interface import CarInterface from selfdrive.car.honda.values import CAR from selfdrive.controls.lib.vehicle_model import VehicleModel -from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable=no-name-in-module, import-error +from selfdrive.locationd.liblocationd_py import liblocationd # pylint: disable=no-name-in-module, import-error class TestParamsLearner(unittest.TestCase): diff --git a/selfdrive/locationd/test/ublox.py b/selfdrive/locationd/test/ublox.py index c7a2adf08e..496207d401 100644 --- a/selfdrive/locationd/test/ublox.py +++ b/selfdrive/locationd/test/ublox.py @@ -720,7 +720,7 @@ class UBlox: self.dev = PandaSerial(self.panda, 1, self.baudrate) elif grey: - import selfdrive.messaging as messaging + import cereal.messaging as messaging class BoarddSerial(): def __init__(self): diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index 91c5711958..ff60f84dd0 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -9,7 +9,7 @@ import struct import sys from cereal import log from common import realtime -import selfdrive.messaging as messaging +import cereal.messaging as messaging from selfdrive.locationd.test.ephemeris import EphemerisData, GET_FIELD_U panda = os.getenv("PANDA") is not None # panda directly connected diff --git a/selfdrive/locationd/test/ubloxd_easy.py b/selfdrive/locationd/test/ubloxd_easy.py index 0753789fba..a2fa753e78 100755 --- a/selfdrive/locationd/test/ubloxd_easy.py +++ b/selfdrive/locationd/test/ubloxd_easy.py @@ -5,7 +5,7 @@ from selfdrive.locationd.test import ublox from common import realtime from selfdrive.locationd.test.ubloxd import gen_raw, gen_solution import zmq -import selfdrive.messaging as messaging +import cereal.messaging as messaging unlogger = os.getenv("UNLOGGER") is not None # debug prints diff --git a/selfdrive/locationd/ubloxd_main.cc b/selfdrive/locationd/ubloxd_main.cc index d87942d8a1..497b69820c 100644 --- a/selfdrive/locationd/ubloxd_main.cc +++ b/selfdrive/locationd/ubloxd_main.cc @@ -44,6 +44,11 @@ int ubloxd_main(poll_ubloxraw_msg_func poll_func, send_gps_event_func send_func) PubSocket * gpsLocationExternal = PubSocket::create(c, "gpsLocationExternal"); PubSocket * ubloxGnss = PubSocket::create(c, "ubloxGnss"); SubSocket * ubloxRaw = SubSocket::create(c, "ubloxRaw"); + + assert(gpsLocationExternal != NULL); + assert(ubloxGnss != NULL); + assert(ubloxRaw != NULL); + Poller * poller = Poller::create({ubloxRaw}); diff --git a/selfdrive/logcatd/Makefile b/selfdrive/logcatd/Makefile deleted file mode 100644 index c1a3ef979e..0000000000 --- a/selfdrive/logcatd/Makefile +++ /dev/null @@ -1,59 +0,0 @@ -CC = clang -CXX = clang++ - -ARCH := $(shell uname -m) - -PHONELIBS = ../../phonelibs -BASEDIR = ../.. - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args - -CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) - -MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging -MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a - -ifeq ($(ARCH),aarch64) -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 -EXTRA_LIBS += -lgnustl_shared -endif - - -.PHONY: all -all: logcatd - -include ../common/cereal.mk - -OBJS = logcatd.o \ - $(CEREAL_OBJS) - -DEPS := $(OBJS:.o=.d) - -logcatd: $(OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(CEREAL_LIBS) \ - $(EXTRA_LIBS) \ - -llog - -%.o: %.cc - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) \ - -I$(PHONELIBS)/android_system_core/include \ - $(CEREAL_CXXFLAGS) \ - $(MESSAGING_FLAGS) \ - -I../ \ - -I../../ \ - -c -o '$@' '$<' - -.PHONY: clean -clean: - rm -f logcatd $(OBJS) $(DEPS) - --include $(DEPS) diff --git a/selfdrive/logcatd/SConscript b/selfdrive/logcatd/SConscript new file mode 100644 index 0000000000..1040c3af70 --- /dev/null +++ b/selfdrive/logcatd/SConscript @@ -0,0 +1,2 @@ +Import('env', 'messaging') +env.Program('logcatd.cc', LIBS=[messaging, 'cutils', 'zmq', 'czmq', 'capnp', 'kj']) diff --git a/selfdrive/logcatd/logcatd.cc b/selfdrive/logcatd/logcatd.cc index 9e5707c29f..4baa976778 100644 --- a/selfdrive/logcatd/logcatd.cc +++ b/selfdrive/logcatd/logcatd.cc @@ -30,6 +30,7 @@ int main() { Context * c = Context::create(); PubSocket * androidLog = PubSocket::create(c, "androidLog"); + assert(androidLog != NULL); while (1) { log_msg log_msg; diff --git a/selfdrive/loggerd/Makefile b/selfdrive/loggerd/Makefile deleted file mode 100644 index bd3038ad7a..0000000000 --- a/selfdrive/loggerd/Makefile +++ /dev/null @@ -1,4 +0,0 @@ --include build_from_src.mk - -release: - @echo "loggerd: this is a release" diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript new file mode 100644 index 0000000000..6a392d15d6 --- /dev/null +++ b/selfdrive/loggerd/SConscript @@ -0,0 +1,6 @@ +Import('env', 'messaging', 'common', 'visionipc') +env.Program(['loggerd.cc', 'logger.c', 'raw_logger.cc', 'encoder.c'], LIBS=[ + 'zmq', 'czmq', 'capnp', 'kj', 'yaml-cpp', 'z', + 'avformat', 'avcodec', 'swscale', 'avutil', + 'OmxVenc', 'OmxCore', 'yuv', + 'bz2', 'cutils', common, 'json', messaging, visionipc]) diff --git a/selfdrive/loggerd/build_from_src.mk b/selfdrive/loggerd/build_from_src.mk deleted file mode 100644 index 90db118bc9..0000000000 --- a/selfdrive/loggerd/build_from_src.mk +++ /dev/null @@ -1,136 +0,0 @@ -CC = clang -CXX = clang++ - -ARCH := $(shell uname -m) - -PHONELIBS = ../../phonelibs -BASEDIR = ../.. - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args \ - -Wno-deprecated-declarations - -CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) \ - -I$(PHONELIBS)/android_frameworks_native/include \ - -I$(PHONELIBS)/android_system_core/include \ - -I$(PHONELIBS)/android_hardware_libhardware/include -CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) \ - -I$(PHONELIBS)/android_frameworks_native/include \ - -I$(PHONELIBS)/android_system_core/include \ - -I$(PHONELIBS)/android_hardware_libhardware/include - -ZMQ_LIBS = -l:libczmq.a -l:libzmq.a - -MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging -MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a - -ifeq ($(ARCH),aarch64) -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 -ZMQ_LIBS += -lgnustl_shared -endif - - -BZIP_FLAGS = -I$(PHONELIBS)/bzip2/ -BZIP_LIBS = -L$(PHONELIBS)/bzip2/ \ - -l:libbz2.a - -# todo: dont use system ffmpeg libs -FFMPEG_LIBS = -lavformat \ - -lavcodec \ - -lswscale \ - -lavutil \ - -lz - -LIBYUV_FLAGS = -I$(PHONELIBS)/libyuv/include -LIBYUV_LIBS = $(PHONELIBS)/libyuv/lib/libyuv.a - -OPENMAX_FLAGS = -I$(PHONELIBS)/openmax/include -OPENMAX_LIBS = -lOmxVenc -lOmxCore - -JSON_FLAGS = -I$(PHONELIBS)/json/src - -YAML_FLAGS = -I$(PHONELIBS)/yaml-cpp/include -YAML_LIBS = $(PHONELIBS)/yaml-cpp/lib/libyaml-cpp.a - -.PHONY: all -all: loggerd - -include ../common/cereal.mk - -OBJS += loggerd.o \ - logger.o \ - ../common/util.o \ - ../common/params.o \ - ../common/cqueue.o \ - ../common/swaglog.o \ - ../common/visionipc.o \ - ../common/ipc.o \ - $(PHONELIBS)/json/src/json.o - -ifeq ($(ARCH),x86_64) -CXXFLAGS += "-DDISABLE_ENCODER" -ZMQ_LIBS = -L$(BASEDIR)/external/zmq/lib/ \ - -l:libczmq.a -l:libzmq.a -EXTRA_LIBS = -lpthread -OPENMAX_LIBS = "" -YAML_LIBS = $(PHONELIBS)/yaml-cpp/x64/lib/libyaml-cpp.a -else -OBJS += encoder.o \ - raw_logger.o -EXTRA_LIBS = -lcutils -llog -lgnustl_shared -endif - -DEPS := $(OBJS:.o=.d) - -loggerd: $(OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(LIBYUV_LIBS) \ - $(CEREAL_LIBS) \ - $(ZMQ_LIBS) \ - -L/usr/lib \ - $(FFMPEG_LIBS) \ - -L/system/vendor/lib64 \ - $(OPENMAX_LIBS) \ - $(YAML_LIBS) \ - $(EXTRA_LIBS) \ - $(BZIP_LIBS) \ - -lm - -%.o: %.cc - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) -MMD \ - $(CEREAL_CXXFLAGS) \ - $(LIBYUV_FLAGS) \ - $(ZMQ_FLAGS) \ - $(MESSAGING_FLAGS) \ - $(OPENMAX_FLAGS) \ - $(YAML_FLAGS) \ - $(BZIP_FLAGS) \ - -Iinclude \ - -I../ \ - -I../../ \ - -c -o '$@' '$<' - -%.o: %.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -MMD \ - $(LIBYUV_FLAGS) \ - $(ZMQ_FLAGS) \ - $(OPENMAX_FLAGS) \ - $(JSON_FLAGS) \ - $(BZIP_FLAGS) \ - -Iinclude \ - -I../ \ - -I../../ \ - -c -o '$@' '$<' - -.PHONY: clean -clean: - rm -f loggerd $(OBJS) $(DEPS) - --include $(DEPS) diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index 18996353ea..4334af0f3a 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -109,6 +109,7 @@ void encoder_thread(bool is_streaming, bool raw_clips, bool front) { int cnt = 0; PubSocket *idx_sock = PubSocket::create(s.ctx, front ? "frontEncodeIdx" : "encodeIdx"); + assert(idx_sock != NULL); LoggerHandle *lh = NULL; @@ -567,7 +568,7 @@ int main(int argc, char** argv) { Poller * poller = Poller::create(); std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe")); - std::string service_list_path = exe_dir + "/../service_list.yaml"; + std::string service_list_path = exe_dir + "/../../cereal/service_list.yaml"; // subscribe to all services @@ -585,6 +586,8 @@ int main(int argc, char** argv) { if (should_log) { SubSocket * sock = SubSocket::create(s.ctx, name); + assert(sock != NULL); + poller->registerSocket(sock); socks.push_back(sock); diff --git a/selfdrive/loggerd/tests/loggerd_tests_common.py b/selfdrive/loggerd/tests/loggerd_tests_common.py index 9db75a7f59..f36ef2175e 100644 --- a/selfdrive/loggerd/tests/loggerd_tests_common.py +++ b/selfdrive/loggerd/tests/loggerd_tests_common.py @@ -37,7 +37,7 @@ class MockApi(): def get(self, *args, **kwargs): return MockResponse('{"url": "http://localhost/does/not/exist", "headers": {}}') - + def get_token(self): return "fake-token" @@ -46,7 +46,6 @@ class MockParams(): self.params = { "DongleId": b"0000000000000000", "IsUploadRawEnabled": b"1", - "IsUploadVideoOverCellularEnabled": b"1" } def get(self, k): @@ -62,7 +61,7 @@ class UploaderTestCase(unittest.TestCase): uploader.Params = MockParams uploader.fake_upload = 1 uploader.is_on_hotspot = lambda *args: False - uploader.is_on_wifi = lambda *args: False + uploader.is_on_wifi = lambda *args: True self.seg_num = random.randint(1, 300) self.seg_format = "2019-04-18--12-52-54--{}" self.seg_format2 = "2019-05-18--11-22-33--{}" diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 0951ad815a..bea384b543 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -14,6 +14,7 @@ import subprocess from selfdrive.swaglog import cloudlog from selfdrive.loggerd.config import ROOT +from common import android from common.params import Params from common.api import Api @@ -66,20 +67,14 @@ def clear_locks(root): def is_on_wifi(): # ConnectivityManager.getActiveNetworkInfo() try: - result = subprocess.check_output(["service", "call", "connectivity", "2"], encoding='utf8').strip().split("\n") # pylint: disable=unexpected-keyword-arg - except subprocess.CalledProcessError: + result = android.parse_service_call_string(["connectivity", "2"]) + return 'WIFI' in result + except AttributeError: return False - # Concatenate all ascii parts - r = "" - for line in result[1:]: - r += line[51:67] - - return "W.I.F.I" in r - def is_on_hotspot(): try: - result = subprocess.check_output(["ifconfig", "wlan0"], encoding='utf8') # pylint: disable=unexpected-keyword-arg + result = subprocess.check_output(["ifconfig", "wlan0"], encoding='utf8') result = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0] is_android = result.startswith('192.168.43.') @@ -242,10 +237,9 @@ def uploader_fn(exit_event): backoff = 0.1 while True: allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0") - allow_cellular = (params.get("IsUploadVideoOverCellularEnabled") != b"0") on_hotspot = is_on_hotspot() on_wifi = is_on_wifi() - should_upload = allow_cellular or (on_wifi and not on_hotspot) + should_upload = on_wifi and not on_hotspot if exit_event.is_set(): return @@ -257,7 +251,7 @@ def uploader_fn(exit_event): key, fn = d - cloudlog.event("uploader_netcheck", allow_cellular=allow_cellular, is_on_hotspot=on_hotspot, is_on_wifi=on_wifi) + cloudlog.event("uploader_netcheck", is_on_hotspot=on_hotspot, is_on_wifi=on_wifi) cloudlog.info("to upload %r", d) success = uploader.upload(key, fn) if success: diff --git a/selfdrive/logmessaged.py b/selfdrive/logmessaged.py index 265152be5d..3362b5baac 100755 --- a/selfdrive/logmessaged.py +++ b/selfdrive/logmessaged.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import zmq from logentries import LogentriesHandler -import selfdrive.messaging as messaging +import cereal.messaging as messaging def main(gctx=None): # setup logentries. we forward log messages to it diff --git a/selfdrive/manager.py b/selfdrive/manager.py index fbb37c51d2..b603bf3d41 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -7,12 +7,23 @@ import errno import signal import subprocess import datetime -from common.spinner import Spinner from common.basedir import BASEDIR sys.path.append(os.path.join(BASEDIR, "pyextra")) os.environ['BASEDIR'] = BASEDIR +TOTAL_SCONS_NODES = 1170 +prebuilt = os.path.exists(os.path.join(BASEDIR, 'prebuilt')) + +# Create folders needed for msgq +try: + os.mkdir("/dev/shm") +except FileExistsError: + pass + +if os.path.isfile('/EON'): + os.chmod("/dev/shm", 0o777) + def unblock_stdout(): # get a non-blocking stdout child_pid, child_pty = os.forkpty() @@ -38,34 +49,74 @@ def unblock_stdout(): try: sys.stdout.write(dat.decode('utf8')) - except (OSError, IOError): + except (OSError, IOError, UnicodeDecodeError): pass os._exit(os.wait()[1]) + if __name__ == "__main__": unblock_stdout() + from common.spinner import Spinner +else: + from common.spinner import FakeSpinner as Spinner -import glob -import shutil -import hashlib import importlib import traceback from multiprocessing import Process -from setproctitle import setproctitle #pylint: disable=no-name-in-module +# Run scons +spinner = Spinner() +spinner.update("0") + +if not prebuilt: + for retry in [True, False]: + # run scons + env = os.environ.copy() + env['SCONS_PROGRESS'] = "1" + scons = subprocess.Popen(["scons", "-j4"], cwd=BASEDIR, env=env, stderr=subprocess.PIPE) + + # Read progress from stderr and update spinner + while scons.poll() is None: + try: + line = scons.stderr.readline() + if line is None: + continue + + line = line.rstrip() + prefix = b'progress: ' + if line.startswith(prefix): + i = int(line[len(prefix):]) + if spinner is not None: + spinner.update("%d" % (50.0 * (i / TOTAL_SCONS_NODES))) + elif len(line): + print(line.decode('utf8')) + except Exception: + pass + + if scons.returncode != 0: + if retry: + print("scons build failed, make clean") + subprocess.check_call(["scons", "-c"], cwd=BASEDIR, env=env) + else: + raise RuntimeError("scons build failed") + else: + break -from common.params import Params import cereal -ThermalStatus = cereal.log.ThermalData.ThermalStatus +import cereal.messaging as messaging +from common.params import Params +import selfdrive.crash as crash from selfdrive.swaglog import cloudlog -import selfdrive.messaging as messaging from selfdrive.registration import register from selfdrive.version import version, dirty -import selfdrive.crash as crash - from selfdrive.loggerd.config import ROOT +from selfdrive.launcher import launcher +from common import android +from common.apk import update_apks, pm_apply_packages, start_frame + +ThermalStatus = cereal.log.ThermalData.ThermalStatus # comment out anything you don't want to run managed_processes = { @@ -83,25 +134,26 @@ managed_processes = { "proclogd": ("selfdrive/proclogd", ["./proclogd"]), "boardd": ("selfdrive/boardd", ["./boardd"]), # not used directly "pandad": "selfdrive.pandad", - "ui": ("selfdrive/ui", ["./start.py"]), + "ui": ("selfdrive/ui", ["./ui"]), "calibrationd": "selfdrive.locationd.calibrationd", "paramsd": ("selfdrive/locationd", ["./paramsd"]), - "visiond": ("selfdrive/visiond", ["./start.py"]), - "sensord": ("selfdrive/sensord", ["./start_sensord.py"]), - "gpsd": ("selfdrive/sensord", ["./start_gpsd.py"]), + "camerad": ("selfdrive/camerad", ["./camerad"]), + "sensord": ("selfdrive/sensord", ["./sensord"]), + "gpsd": ("selfdrive/sensord", ["./gpsd"]), "updated": "selfdrive.updated", + "monitoringd": ("selfdrive/modeld", ["./monitoringd"]), + "modeld": ("selfdrive/modeld", ["./modeld"]), } daemon_processes = { "manage_athenad": ("selfdrive.athena.manage_athenad", "AthenadPid"), } -android_packages = ("ai.comma.plus.offroad", "ai.comma.plus.frame") running = {} def get_running(): return running -# due to qualcomm kernel bugs SIGKILLing visiond sometimes causes page table corruption -unkillable_processes = ['visiond'] +# due to qualcomm kernel bugs SIGKILLing camerad sometimes causes page table corruption +unkillable_processes = ['camerad'] # processes to end with SIGINT instead of SIGTERM interrupt_processes = [] @@ -127,7 +179,9 @@ car_started_processes = [ 'radard', 'calibrationd', 'paramsd', - 'visiond', + 'camerad', + 'modeld', + 'monitoringd', 'proclogd', 'ubloxd', 'gpsd', @@ -144,27 +198,6 @@ def register_managed_process(name, desc, car_started=False): persistent_processes.append(name) # ****************** process management functions ****************** -def launcher(proc): - try: - # import the process - mod = importlib.import_module(proc) - - # rename the process - setproctitle(proc) - - # create now context since we forked - messaging.context = messaging.Context() - - # exec the process - mod.main() - except KeyboardInterrupt: - cloudlog.warning("child %s got SIGINT" % proc) - except Exception: - # can't install the crash handler becuase sys.excepthook doesn't play nice - # with threads, so catch it here. - crash.capture_exception() - raise - def nativelauncher(pargs, cwd): # exec the process os.chdir(cwd) @@ -188,7 +221,8 @@ def start_managed_process(name): running[name] = Process(name=name, target=nativelauncher, args=(pargs, cwd)) running[name].start() -def start_daemon_process(name, params): +def start_daemon_process(name): + params = Params() proc, pid_param = daemon_processes[name] pid = params.get(pid_param) @@ -203,7 +237,7 @@ def start_daemon_process(name, params): cloudlog.info("starting daemon %s" % name) proc = subprocess.Popen(['python', '-m', proc], - cwd='/', + stdin=open('/dev/null', 'r'), stdout=open('/dev/null', 'w'), stderr=open('/dev/null', 'w'), preexec_fn=os.setpgrp) @@ -216,7 +250,7 @@ def prepare_managed_process(p): # import this python cloudlog.info("preimporting %s" % proc) importlib.import_module(proc) - else: + elif os.path.isfile(os.path.join(BASEDIR, proc[0], "Makefile")): # build this process cloudlog.info("building %s" % (proc,)) try: @@ -265,9 +299,6 @@ def kill_managed_process(name): cloudlog.info("%s is dead with %d" % (name, running[name].exitcode)) del running[name] -def pm_apply_packages(cmd): - for p in android_packages: - system("pm %s %s" % (cmd, p)) def cleanup_all_processes(signal, frame): cloudlog.info("caught ctrl-c %s %s" % (signal, frame)) @@ -278,7 +309,6 @@ def cleanup_all_processes(signal, frame): kill_managed_process(name) cloudlog.info("everything is dead") - # ****************** run loop ****************** def manager_init(should_register=True): @@ -309,16 +339,10 @@ def manager_init(should_register=True): except OSError: pass -def system(cmd): - try: - cloudlog.info("running %s" % cmd) - subprocess.check_output(cmd, stderr=subprocess.STDOUT, shell=True) - except subprocess.CalledProcessError as e: - cloudlog.event("running failed", - cmd=e.cmd, - output=e.output[-1024:], - returncode=e.returncode) - + # ensure shared libraries are readable by apks + os.chmod(BASEDIR, 0o755) + os.chmod(os.path.join(BASEDIR, "cereal"), 0o755) + os.chmod(os.path.join(BASEDIR, "cereal", "libmessaging_shared.so"), 0o755) def manager_thread(): # now loop @@ -334,7 +358,7 @@ def manager_thread(): # start daemon processes for p in daemon_processes: - start_daemon_process(p, params) + start_daemon_process(p) # start persistent processes for p in persistent_processes: @@ -342,8 +366,7 @@ def manager_thread(): # start frame pm_apply_packages('enable') - system("LD_LIBRARY_PATH= appops set ai.comma.plus.offroad SU allow") - system("am start -n ai.comma.plus.frame/.MainActivity") + start_frame() if os.getenv("NOBOARD") is None: start_managed_process("pandad") @@ -370,88 +393,27 @@ def manager_thread(): start_managed_process(p) else: logger_dead = False - for p in car_started_processes: + for p in reversed(car_started_processes): kill_managed_process(p) # check the status of all processes, did any of them die? - running_list = [" running %s %s" % (p, running[p]) for p in running] - cloudlog.debug('\n'.join(running_list)) + running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running] + cloudlog.debug(' '.join(running_list)) # Exit main loop when uninstall is needed if params.get("DoUninstall", encoding='utf8') == "1": break -def get_installed_apks(): - dat = subprocess.check_output(["pm", "list", "packages", "-f"], encoding='utf8').strip().split("\n") # pylint: disable=unexpected-keyword-arg - ret = {} - for x in dat: - if x.startswith("package:"): - v,k = x.split("package:")[1].split("=") - ret[k] = v - return ret - -def install_apk(path): - # can only install from world readable path - install_path = "/sdcard/%s" % os.path.basename(path) - shutil.copyfile(path, install_path) - - ret = subprocess.call(["pm", "install", "-r", install_path]) - os.remove(install_path) - return ret == 0 - -def update_apks(): - # install apks - installed = get_installed_apks() - - install_apks = glob.glob(os.path.join(BASEDIR, "apk/*.apk")) - for apk in install_apks: - app = os.path.basename(apk)[:-4] - if app not in installed: - installed[app] = None - - cloudlog.info("installed apks %s" % (str(installed), )) - - for app in installed.keys(): - - apk_path = os.path.join(BASEDIR, "apk/"+app+".apk") - if not os.path.exists(apk_path): - continue - - h1 = hashlib.sha1(open(apk_path, 'rb').read()).hexdigest() - h2 = None - if installed[app] is not None: - h2 = hashlib.sha1(open(installed[app], 'rb').read()).hexdigest() - cloudlog.info("comparing version of %s %s vs %s" % (app, h1, h2)) - - if h2 is None or h1 != h2: - cloudlog.info("installing %s" % app) - - success = install_apk(apk_path) - if not success: - cloudlog.info("needing to uninstall %s" % app) - system("pm uninstall %s" % app) - success = install_apk(apk_path) - - assert success - -def manager_update(): - update_apks() - - uninstall = [app for app in get_installed_apks().keys() if app in ("com.spotify.music", "com.waze")] - for app in uninstall: - cloudlog.info("uninstalling %s" % app) - os.system("pm uninstall % s" % app) - def manager_prepare(spinner=None): - # build cereal first - subprocess.check_call(["make", "-j4"], cwd=os.path.join(BASEDIR, "cereal")) - # build all processes os.chdir(os.path.dirname(os.path.abspath(__file__))) + # Spinner has to start from 70 here + total = 100.0 if prebuilt else 50.0 + for i, p in enumerate(managed_processes): if spinner is not None: - spinner.update("%d" % (100.0 * (i + 1) / len(managed_processes),)) + spinner.update("%d" % ((100.0 - total) + total * (i + 1) / len(managed_processes),)) prepare_managed_process(p) def uninstall(): @@ -459,7 +421,7 @@ def uninstall(): with open('/cache/recovery/command', 'w') as f: f.write('--wipe_data\n') # IPowerManager.reboot(confirm=false, reason="recovery", wait=true) - os.system("service call power 16 i32 0 s16 recovery i32 1") + android.reboot(reason="recovery") def main(): # the flippening! @@ -468,29 +430,10 @@ def main(): # disable bluetooth os.system('service call bluetooth_manager 8') - if os.getenv("NOLOG") is not None: - del managed_processes['loggerd'] - del managed_processes['tombstoned'] - if os.getenv("NOUPLOAD") is not None: - del managed_processes['uploader'] - if os.getenv("NOVISION") is not None: - del managed_processes['visiond'] - if os.getenv("LEAN") is not None: - del managed_processes['uploader'] - del managed_processes['loggerd'] - del managed_processes['logmessaged'] - del managed_processes['logcatd'] - del managed_processes['tombstoned'] - del managed_processes['proclogd'] - if os.getenv("NOCONTROL") is not None: - del managed_processes['controlsd'] - del managed_processes['plannerd'] - del managed_processes['radard'] - # support additional internal only extensions try: import selfdrive.manager_extensions - selfdrive.manager_extensions.register(register_managed_process) # pylint: disable=no-member + selfdrive.manager_extensions.register(register_managed_process) # pylint: disable=no-member except ImportError: pass @@ -498,6 +441,8 @@ def main(): params.manager_start() # set unset params + if params.get("CommunityFeaturesToggle") is None: + params.put("CommunityFeaturesToggle", "0") if params.get("CompletedTrainingVersion") is None: params.put("CompletedTrainingVersion", "0") if params.get("IsMetric") is None: @@ -510,8 +455,8 @@ def main(): params.put("HasCompletedSetup", "0") if params.get("IsUploadRawEnabled") is None: params.put("IsUploadRawEnabled", "1") - if params.get("IsUploadVideoOverCellularEnabled") is None: - params.put("IsUploadVideoOverCellularEnabled", "1") + if params.get("IsLdwEnabled") is None: + params.put("IsLdwEnabled", "1") if params.get("IsGeofenceEnabled") is None: params.put("IsGeofenceEnabled", "-1") if params.get("SpeedLimitOffset") is None: @@ -535,11 +480,10 @@ def main(): if params.get("Passive") is None: raise Exception("Passive must be set to continue") - with Spinner() as spinner: - spinner.update("0") # Show progress bar - manager_update() - manager_init() - manager_prepare(spinner) + update_apks() + manager_init() + manager_prepare(spinner) + spinner.close() if os.getenv("PREPAREONLY") is not None: return diff --git a/selfdrive/messaging/.gitignore b/selfdrive/messaging/.gitignore deleted file mode 100644 index 1549b67ca5..0000000000 --- a/selfdrive/messaging/.gitignore +++ /dev/null @@ -1 +0,0 @@ -demo diff --git a/selfdrive/messaging/Makefile b/selfdrive/messaging/Makefile deleted file mode 100644 index d53e5ece19..0000000000 --- a/selfdrive/messaging/Makefile +++ /dev/null @@ -1,63 +0,0 @@ -CXX := clang++ -CC := clang - -BASEDIR = ../.. -PHONELIBS = ../../phonelibs - -CXXFLAGS := -g -O3 -fPIC -std=c++11 -Wall -Wextra -Wshadow -Weffc++ -Wstrict-aliasing -Wpedantic -Werror -MMD -I$(BASEDIR)/selfdrive - -LDLIBS=-lm -lstdc++ -lrt -lpthread - -UNAME_M := $(shell uname -m) - -YAML_FLAGS = -I$(PHONELIBS)/yaml-cpp/include -YAML_LIB = $(abspath $(PHONELIBS)/yaml-cpp/lib/libyaml-cpp.a) - -ifeq ($(UNAME_M),aarch64) - LDFLAGS += -llog -lgnustl_shared - ZMQ_LIBS = /usr/lib/libzmq.a -endif -ifeq ($(UNAME_M),x86_64) - ZMQ_FLAGS = -I$(BASEDIR)/phonelibs/zmq/x64/include - ZMQ_LIBS = $(abspath $(PHONELIBS)/zmq/x64/lib/libzmq.a) - YAML_DIR = $(PHONELIBS)/yaml-cpp/x64/lib/ - YAML_LIB = $(abspath $(PHONELIBS)/yaml-cpp/x64/lib/libyaml-cpp.a) -endif - -ifdef ASAN - CXXFLAGS += -fsanitize=address -fno-omit-frame-pointer - LDFLAGS += -fsanitize=address -endif - -CXXFLAGS += $(ZMQ_FLAGS) $(YAML_FLAGS) - -OBJS := messaging.o impl_zmq.o -DEPS=$(OBJS:.o=.d) - -.PRECIOUS: $(OBJS) -.PHONY: all clean -all: messaging.a messaging_pyx.so - -demo: messaging.a demo.o - $(CC) $(LDFLAGS) $^ $(LDLIBS) -L. -l:messaging.a -o '$@' - -messaging_pyx.so: messaging.a messaging_pyx_setup.py messaging_pyx.pyx messaging.pxd - python3 messaging_pyx_setup.py build_ext --inplace - rm -rf build - rm -f messaging_pyx.cpp - -%.a: $(OBJS) - @echo "[ LINK ] $@" - mkdir -p libs; \ - cd libs; \ - ar -x $(ZMQ_LIBS); \ - ar -x $(YAML_LIB); - - ar rcsD '$@' $^ libs/*.o - rm -r libs - -clean: - @echo "[ CLEAN ]" - rm -rf *.so *.a demo libs $(OBJS) $(DEPS) - --include $(DEPS) diff --git a/selfdrive/messaging/__init__.py b/selfdrive/messaging/__init__.py deleted file mode 100644 index e637756bd5..0000000000 --- a/selfdrive/messaging/__init__.py +++ /dev/null @@ -1,214 +0,0 @@ -import os -import subprocess - -can_dir = os.path.dirname(os.path.abspath(__file__)) -subprocess.check_call(["make"], cwd=can_dir) -from .messaging_pyx import Context, Poller, SubSocket, PubSocket # pylint: disable=no-name-in-module, import-error - -from cereal import log -from common.realtime import sec_since_boot -from selfdrive.services import service_list - - -context = Context() - -def new_message(): - dat = log.Event.new_message() - dat.logMonoTime = int(sec_since_boot() * 1e9) - dat.valid = True - return dat - -def pub_sock(endpoint): - sock = PubSocket() - sock.connect(context, endpoint) - return sock - -def sub_sock(endpoint, poller=None, addr="127.0.0.1", conflate=False, timeout=None): - sock = SubSocket() - addr = addr.encode('utf8') - sock.connect(context, endpoint, addr, conflate) - - if timeout is not None: - sock.setTimeout(timeout) - - if poller is not None: - poller.registerSocket(sock) - return sock - - -def drain_sock_raw(sock, wait_for_one=False): - """Receive all message currently available on the queue""" - ret = [] - while 1: - if wait_for_one and len(ret) == 0: - dat = sock.receive() - else: - dat = sock.receive(non_blocking=True) - - if dat is None: - break - - ret.append(dat) - - return ret - -def drain_sock(sock, wait_for_one=False): - """Receive all message currently available on the queue""" - ret = [] - while 1: - if wait_for_one and len(ret) == 0: - dat = sock.receive() - else: - dat = sock.receive(non_blocking=True) - - if dat is None: # Timeout hit - break - - dat = log.Event.from_bytes(dat) - ret.append(dat) - - return ret - - -# TODO: print when we drop packets? -def recv_sock(sock, wait=False): - """Same as drain sock, but only returns latest message. Consider using conflate instead.""" - dat = None - - while 1: - if wait and dat is None: - rcv = sock.receive() - else: - rcv = sock.receive(non_blocking=True) - - if rcv is None: # Timeout hit - break - - dat = rcv - - if dat is not None: - dat = log.Event.from_bytes(dat) - - return dat - -def recv_one(sock): - dat = sock.receive() - if dat is not None: - dat = log.Event.from_bytes(dat) - return dat - -def recv_one_or_none(sock): - dat = sock.receive(non_blocking=True) - if dat is not None: - dat = log.Event.from_bytes(dat) - return dat - -def recv_one_retry(sock): - """Keep receiving until we get a message""" - while True: - dat = sock.receive() - if dat is not None: - return log.Event.from_bytes(dat) - -def get_one_can(logcan): - while True: - can = recv_one_retry(logcan) - if len(can.can) > 0: - return can - -class SubMaster(): - def __init__(self, services, ignore_alive=None, addr="127.0.0.1"): - self.poller = Poller() - self.frame = -1 - self.updated = {s : False for s in services} - self.rcv_time = {s : 0. for s in services} - self.rcv_frame = {s : 0 for s in services} - self.alive = {s : False for s in services} - self.sock = {} - self.freq = {} - self.data = {} - self.logMonoTime = {} - self.valid = {} - - if ignore_alive is not None: - self.ignore_alive = ignore_alive - else: - self.ignore_alive = [] - - for s in services: - # TODO: get address automatically from service_list - if addr is not None: - self.sock[s] = sub_sock(s, poller=self.poller, addr=addr, conflate=True) - self.freq[s] = service_list[s].frequency - - data = new_message() - if s in ['can', 'sensorEvents', 'liveTracks', 'sendCan', - 'ethernetData', 'cellInfo', 'wifiScan', - 'trafficEvents', 'orbObservation', 'carEvents']: - data.init(s, 0) - else: - data.init(s) - self.data[s] = getattr(data, s) - self.logMonoTime[s] = 0 - self.valid[s] = data.valid - - def __getitem__(self, s): - return self.data[s] - - def update(self, timeout=-1): - msgs = [] - for sock in self.poller.poll(timeout): - msgs.append(recv_one(sock)) - self.update_msgs(sec_since_boot(), msgs) - - def update_msgs(self, cur_time, msgs): - # TODO: add optional input that specify the service to wait for - self.frame += 1 - self.updated = dict.fromkeys(self.updated, False) - for msg in msgs: - if msg is None: - continue - - s = msg.which() - self.updated[s] = True - self.rcv_time[s] = cur_time - self.rcv_frame[s] = self.frame - self.data[s] = getattr(msg, s) - self.logMonoTime[s] = msg.logMonoTime - self.valid[s] = msg.valid - - for s in self.data: - # arbitrary small number to avoid float comparison. If freq is 0, we can skip the check - if self.freq[s] > 1e-5: - # alive if delay is within 10x the expected frequency - self.alive[s] = (cur_time - self.rcv_time[s]) < (10. / self.freq[s]) - else: - self.alive[s] = True - - def all_alive(self, service_list=None): - if service_list is None: # check all - service_list = self.alive.keys() - return all(self.alive[s] for s in service_list if s not in self.ignore_alive) - - def all_valid(self, service_list=None): - if service_list is None: # check all - service_list = self.valid.keys() - return all(self.valid[s] for s in service_list) - - def all_alive_and_valid(self, service_list=None): - if service_list is None: # check all - service_list = self.alive.keys() - return self.all_alive(service_list=service_list) and self.all_valid(service_list=service_list) - - -class PubMaster(): - def __init__(self, services): - self.sock = {} - for s in services: - self.sock[s] = pub_sock(s) - - def send(self, s, dat): - # accept either bytes or capnp builder - if not isinstance(dat, bytes): - dat = dat.to_bytes() - self.sock[s].send(dat) diff --git a/selfdrive/messaging/demo.cc b/selfdrive/messaging/demo.cc deleted file mode 100644 index cfdf422094..0000000000 --- a/selfdrive/messaging/demo.cc +++ /dev/null @@ -1,50 +0,0 @@ -#include -#include -#include -#include -#include - -#include "messaging.hpp" -#include "impl_zmq.hpp" - -#define MSGS 1e5 - -int main() { - Context * c = Context::create(); - SubSocket * sub_sock = SubSocket::create(c, "controlsState"); - PubSocket * pub_sock = PubSocket::create(c, "controlsState"); - - char data[8]; - - Poller * poller = Poller::create({sub_sock}); - - auto start = std::chrono::steady_clock::now(); - - for (uint64_t i = 0; i < MSGS; i++){ - *(uint64_t*)data = i; - pub_sock->send(data, 8); - - auto r = poller->poll(100); - - for (auto p : r){ - Message * m = p->receive(); - uint64_t ii = *(uint64_t*)m->getData(); - assert(i == ii); - delete m; - } - } - - - auto end = std::chrono::steady_clock::now(); - double elapsed = std::chrono::duration_cast(end - start).count() / 1e9; - double throughput = ((double) MSGS / (double) elapsed); - std::cout << throughput << " msg/s" << std::endl; - - delete poller; - delete sub_sock; - delete pub_sock; - delete c; - - - return 0; -} diff --git a/selfdrive/messaging/demo.py b/selfdrive/messaging/demo.py deleted file mode 100644 index 7906a41e20..0000000000 --- a/selfdrive/messaging/demo.py +++ /dev/null @@ -1,30 +0,0 @@ -import time - -from messaging_pyx import Context, Poller, SubSocket, PubSocket # pylint: disable=no-name-in-module, import-error - -MSGS = 1e5 - -if __name__ == "__main__": - c = Context() - sub_sock = SubSocket() - pub_sock = PubSocket() - - sub_sock.connect(c, "controlsState") - pub_sock.connect(c, "controlsState") - - - poller = Poller() - poller.registerSocket(sub_sock) - - t = time.time() - for i in range(int(MSGS)): - bts = i.to_bytes(4, 'little') - pub_sock.send(bts) - - for s in poller.poll(100): - dat = s.receive() - ii = int.from_bytes(dat, 'little') - assert(i == ii) - - dt = time.time() - t - print("%.1f msg/s" % (MSGS / dt)) diff --git a/selfdrive/messaging/impl_zmq.cc b/selfdrive/messaging/impl_zmq.cc deleted file mode 100644 index 93223e863a..0000000000 --- a/selfdrive/messaging/impl_zmq.cc +++ /dev/null @@ -1,173 +0,0 @@ -#include -#include -#include -#include - -#include - -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wshadow" -#include -#pragma GCC diagnostic pop - -#include "impl_zmq.hpp" - -static int get_port(std::string endpoint) { - char * base_dir_ptr = std::getenv("BASEDIR"); - - if (base_dir_ptr == NULL){ - base_dir_ptr = std::getenv("PYTHONPATH"); - } - - assert(base_dir_ptr); - std::string base_dir = base_dir_ptr; - std::string service_list_path = base_dir + "/selfdrive/service_list.yaml"; - YAML::Node service_list = YAML::LoadFile(service_list_path); - - int port = -1; - for (const auto& it : service_list) { - auto name = it.first.as(); - - if (name == endpoint){ - port = it.second[0].as(); - break; - } - } - - if (port == -1){ - std::cout << "Service " << endpoint << " not found" << std::endl; - } - - assert(port >= 0); - return port; -} - -ZMQContext::ZMQContext() { - context = zmq_ctx_new(); -} - -ZMQContext::~ZMQContext() { - zmq_ctx_term(context); -} - -void ZMQMessage::init(size_t sz) { - size = sz; - data = new char[size]; -} - -void ZMQMessage::init(char * d, size_t sz) { - size = sz; - data = new char[size]; - memcpy(data, d, size); -} - -void ZMQMessage::close() { - if (size > 0){ - delete[] data; - } - size = 0; -} - -ZMQMessage::~ZMQMessage() { - this->close(); -} - - -void ZMQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate){ - sock = zmq_socket(context->getRawContext(), ZMQ_SUB); - assert(sock); - - zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0); - - if (conflate){ - int arg = 1; - zmq_setsockopt(sock, ZMQ_CONFLATE, &arg, sizeof(int)); - } - - int reconnect_ivl = 500; - zmq_setsockopt(sock, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl, sizeof(reconnect_ivl)); - - full_endpoint = "tcp://" + address + ":"; - full_endpoint += std::to_string(get_port(endpoint)); - - std::cout << "ZMQ SUB: " << full_endpoint << std::endl; - - assert(zmq_connect(sock, full_endpoint.c_str()) == 0); -} - - -Message * ZMQSubSocket::receive(bool non_blocking){ - zmq_msg_t msg; - assert(zmq_msg_init(&msg) == 0); - - int flags = non_blocking ? ZMQ_DONTWAIT : 0; - int rc = zmq_msg_recv(&msg, sock, flags); - Message *r = NULL; - - if (rc >= 0){ - // Make a copy to ensure the data is aligned - r = new ZMQMessage; - r->init((char*)zmq_msg_data(&msg), zmq_msg_size(&msg)); - } - - zmq_msg_close(&msg); - return r; -} - -void ZMQSubSocket::setTimeout(int timeout){ - zmq_setsockopt(sock, ZMQ_RCVTIMEO, &timeout, sizeof(int)); -} - -ZMQSubSocket::~ZMQSubSocket(){ - zmq_close(sock); -} - -void ZMQPubSocket::connect(Context *context, std::string endpoint){ - sock = zmq_socket(context->getRawContext(), ZMQ_PUB); - - full_endpoint = "tcp://*:"; - full_endpoint += std::to_string(get_port(endpoint)); - - std::cout << "ZMQ PUB: " << full_endpoint << std::endl; - - assert(zmq_bind(sock, full_endpoint.c_str()) == 0); -} - -int ZMQPubSocket::sendMessage(Message *message){ - return zmq_send(sock, message->getData(), message->getSize(), ZMQ_DONTWAIT); -} - -int ZMQPubSocket::send(char *data, size_t size){ - return zmq_send(sock, data, size, ZMQ_DONTWAIT); -} - -ZMQPubSocket::~ZMQPubSocket(){ - zmq_close(sock); -} - - -void ZMQPoller::registerSocket(SubSocket * socket){ - assert(num_polls + 1 < MAX_POLLERS); - polls[num_polls].socket = socket->getRawSocket(); - polls[num_polls].events = ZMQ_POLLIN; - - sockets.push_back(socket); - num_polls++; -} - -std::vector ZMQPoller::poll(int timeout){ - std::vector r; - - int rc = zmq_poll(polls, num_polls, timeout); - if (rc < 0){ - return r; - } - - for (size_t i = 0; i < num_polls; i++){ - if (polls[i].revents){ - r.push_back(sockets[i]); - } - } - - return r; -} diff --git a/selfdrive/messaging/impl_zmq.hpp b/selfdrive/messaging/impl_zmq.hpp deleted file mode 100644 index a40d623e3a..0000000000 --- a/selfdrive/messaging/impl_zmq.hpp +++ /dev/null @@ -1,63 +0,0 @@ -#pragma once -#include "messaging.hpp" -#include -#include - -#define MAX_POLLERS 128 - -class ZMQContext : public Context { -private: - void * context = NULL; -public: - ZMQContext(); - void * getRawContext() {return context;} - ~ZMQContext(); -}; - -class ZMQMessage : public Message { -private: - char * data; - size_t size; -public: - void init(size_t size); - void init(char *data, size_t size); - size_t getSize(){return size;} - char * getData(){return data;} - void close(); - ~ZMQMessage(); -}; - -class ZMQSubSocket : public SubSocket { -private: - void * sock; - std::string full_endpoint; -public: - void connect(Context *context, std::string endpoint, std::string address, bool conflate=false); - void setTimeout(int timeout); - void * getRawSocket() {return sock;} - Message *receive(bool non_blocking=false); - ~ZMQSubSocket(); -}; - -class ZMQPubSocket : public PubSocket { -private: - void * sock; - std::string full_endpoint; -public: - void connect(Context *context, std::string endpoint); - int sendMessage(Message *message); - int send(char *data, size_t size); - ~ZMQPubSocket(); -}; - -class ZMQPoller : public Poller { -private: - std::vector sockets; - zmq_pollitem_t polls[MAX_POLLERS]; - size_t num_polls = 0; - -public: - void registerSocket(SubSocket *socket); - std::vector poll(int timeout); - ~ZMQPoller(){}; -}; diff --git a/selfdrive/messaging/messaging.cc b/selfdrive/messaging/messaging.cc deleted file mode 100644 index a1178a5108..0000000000 --- a/selfdrive/messaging/messaging.cc +++ /dev/null @@ -1,51 +0,0 @@ -#include "messaging.hpp" -#include "impl_zmq.hpp" - -Context * Context::create(){ - Context * c = new ZMQContext(); - return c; -} - -SubSocket * SubSocket::create(){ - SubSocket * s = new ZMQSubSocket(); - return s; -} - -SubSocket * SubSocket::create(Context * context, std::string endpoint){ - SubSocket *s = SubSocket::create(); - s->connect(context, endpoint, "127.0.0.1"); - - return s; -} - -SubSocket * SubSocket::create(Context * context, std::string endpoint, std::string address){ - SubSocket *s = SubSocket::create(); - s->connect(context, endpoint, address); - - return s; -} - -PubSocket * PubSocket::create(){ - PubSocket * s = new ZMQPubSocket(); - return s; -} - -PubSocket * PubSocket::create(Context * context, std::string endpoint){ - PubSocket *s = PubSocket::create(); - s->connect(context, endpoint); - return s; -} - -Poller * Poller::create(){ - Poller * p = new ZMQPoller(); - return p; -} - -Poller * Poller::create(std::vector sockets){ - Poller * p = Poller::create(); - - for (auto s : sockets){ - p->registerSocket(s); - } - return p; -} diff --git a/selfdrive/messaging/messaging.hpp b/selfdrive/messaging/messaging.hpp deleted file mode 100644 index 95e34c0d67..0000000000 --- a/selfdrive/messaging/messaging.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#pragma once -#include -#include -#include - -class Context { -public: - virtual void * getRawContext() = 0; - static Context * create(); - virtual ~Context(){}; -}; - -class Message { -public: - virtual void init(size_t size) = 0; - virtual void init(char * data, size_t size) = 0; - virtual void close() = 0; - virtual size_t getSize() = 0; - virtual char * getData() = 0; - virtual ~Message(){}; -}; - - -class SubSocket { -public: - virtual void connect(Context *context, std::string endpoint, std::string address, bool conflate=false) = 0; - virtual void setTimeout(int timeout) = 0; - virtual Message *receive(bool non_blocking=false) = 0; - virtual void * getRawSocket() = 0; - static SubSocket * create(); - static SubSocket * create(Context * context, std::string endpoint); - static SubSocket * create(Context * context, std::string endpoint, std::string address); - virtual ~SubSocket(){}; -}; - -class PubSocket { -public: - virtual void connect(Context *context, std::string endpoint) = 0; - virtual int sendMessage(Message *message) = 0; - virtual int send(char *data, size_t size) = 0; - static PubSocket * create(); - static PubSocket * create(Context * context, std::string endpoint); - virtual ~PubSocket(){}; -}; - -class Poller { -public: - virtual void registerSocket(SubSocket *socket) = 0; - virtual std::vector poll(int timeout) = 0; - static Poller * create(); - static Poller * create(std::vector sockets); - virtual ~Poller(){}; -}; diff --git a/selfdrive/messaging/messaging.pxd b/selfdrive/messaging/messaging.pxd deleted file mode 100644 index 5e3da72415..0000000000 --- a/selfdrive/messaging/messaging.pxd +++ /dev/null @@ -1,42 +0,0 @@ -# distutils: language = c++ -#cython: language_level=3 - -from libcpp.string cimport string -from libcpp.vector cimport vector -from libcpp cimport bool - - -cdef extern from "messaging.hpp": - cdef cppclass Context: - @staticmethod - Context * create() - - cdef cppclass Message: - void init(size_t) - void init(char *, size_t) - void close() - size_t getSize() - char *getData() - - - - cdef cppclass SubSocket: - @staticmethod - SubSocket * create() - void connect(Context *, string, string, bool) - Message * receive(bool) - void setTimeout(int) - - - cdef cppclass PubSocket: - @staticmethod - PubSocket * create() - void connect(Context *, string) - int sendMessage(Message *) - int send(char *, size_t) - - cdef cppclass Poller: - @staticmethod - Poller * create() - void registerSocket(SubSocket *) - vector[SubSocket*] poll(int) diff --git a/selfdrive/messaging/messaging_pyx.pyx b/selfdrive/messaging/messaging_pyx.pyx deleted file mode 100644 index ff55c31075..0000000000 --- a/selfdrive/messaging/messaging_pyx.pyx +++ /dev/null @@ -1,104 +0,0 @@ -# distutils: language = c++ -# cython: c_string_encoding=ascii, language_level=3 - -from libcpp.string cimport string -from libcpp cimport bool - - -from messaging cimport Context as cppContext -from messaging cimport SubSocket as cppSubSocket -from messaging cimport PubSocket as cppPubSocket -from messaging cimport Poller as cppPoller -from messaging cimport Message as cppMessage - - -cdef class Context: - cdef cppContext * context - def __cinit__(self): - self.context = cppContext.create() - - def __dealloc__(self): - pass - # Deleting the context will hang if sockets are still active - # TODO: Figure out a way to make sure the context is closed last - # del self.context - - -cdef class Poller: - cdef cppPoller * poller - cdef list sub_sockets - - def __cinit__(self): - self.sub_sockets = [] - self.poller = cppPoller.create() - - def __dealloc__(self): - del self.poller - - def registerSocket(self, SubSocket socket): - self.sub_sockets.append(socket) - self.poller.registerSocket(socket.socket) - - def poll(self, timeout): - sockets = [] - - result = self.poller.poll(timeout) - for s in result: - socket = SubSocket() - socket.setPtr(s) - sockets.append(socket) - - return sockets - -cdef class SubSocket: - cdef cppSubSocket * socket - cdef bool is_owner - - def __cinit__(self): - self.socket = cppSubSocket.create() - self.is_owner = True - - def __dealloc__(self): - if self.is_owner: - del self.socket - - cdef setPtr(self, cppSubSocket * ptr): - if self.is_owner: - del self.socket - - self.is_owner = False - self.socket = ptr - - def connect(self, Context context, string endpoint, string address=b"127.0.0.1", bool conflate=False): - self.socket.connect(context.context, endpoint, address, conflate) - - def setTimeout(self, int timeout): - self.socket.setTimeout(timeout) - - - def receive(self, bool non_blocking=False): - msg = self.socket.receive(non_blocking) - - if msg == NULL: - return None - else: - sz = msg.getSize() - m = msg.getData()[:sz] - del msg - - return m - - -cdef class PubSocket: - cdef cppPubSocket * socket - def __cinit__(self): - self.socket = cppPubSocket.create() - - def __dealloc__(self): - del self.socket - - def connect(self, Context context, string endpoint): - self.socket.connect(context.context, endpoint) - - def send(self, string data): - return self.socket.send(data.c_str(), len(data)) diff --git a/selfdrive/messaging/messaging_pyx_setup.py b/selfdrive/messaging/messaging_pyx_setup.py deleted file mode 100644 index 53cd73c7d7..0000000000 --- a/selfdrive/messaging/messaging_pyx_setup.py +++ /dev/null @@ -1,34 +0,0 @@ -import os -import subprocess -from distutils.core import Extension, setup # pylint: disable=import-error,no-name-in-module - -from Cython.Build import cythonize - -from common.basedir import BASEDIR -from common.cython_hacks import BuildExtWithoutPlatformSuffix - -sourcefiles = ['messaging_pyx.pyx'] -extra_compile_args = ["-std=c++11"] -libraries = [] -ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg - -if ARCH == "aarch64": - extra_compile_args += ["-Wno-deprecated-register"] - libraries += ['gnustl_shared'] - -setup(name='CAN parser', - cmdclass={'build_ext': BuildExtWithoutPlatformSuffix}, - ext_modules=cythonize( - Extension( - "messaging_pyx", - language="c++", - sources=sourcefiles, - extra_compile_args=extra_compile_args, - libraries=libraries, - extra_objects=[ - os.path.join(BASEDIR, 'selfdrive', 'messaging', 'messaging.a'), - ] - ) - ), - nthreads=4, -) diff --git a/selfdrive/modeld/SConscript b/selfdrive/modeld/SConscript new file mode 100644 index 0000000000..b752bb3ade --- /dev/null +++ b/selfdrive/modeld/SConscript @@ -0,0 +1,25 @@ +Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc') + +libs = [messaging, common, 'OpenCL', 'SNPE', 'capnp', 'zmq', 'kj', 'yuv', gpucommon, visionipc] + +if arch == "aarch64": + libs += ['gsl', 'CB', 'gnustl_shared'] +else: + libs += ['symphony-cpu', 'pthread'] + +common = env.Object([ + "models/commonmodel.c", + "runners/snpemodel.cc", + "transforms/loadyuv.c", + "transforms/transform.c"]) + +env.Program('_monitoringd', [ + "monitoringd.cc", + "models/monitoring.cc", + ]+common, LIBS=libs) + +env.Program('_modeld', [ + "modeld.cc", + "models/driving.cc", + "models/posenet.cc", + ]+common, LIBS=libs) diff --git a/selfdrive/visiond/constants.py b/selfdrive/modeld/constants.py similarity index 59% rename from selfdrive/visiond/constants.py rename to selfdrive/modeld/constants.py index 56c4b76a3a..2bf48574f8 100644 --- a/selfdrive/visiond/constants.py +++ b/selfdrive/modeld/constants.py @@ -1,3 +1,6 @@ MAX_DISTANCE = 140. LANE_OFFSET = 1.8 MAX_REL_V = 10. + +LEAD_X_SCALE = 10 +LEAD_Y_SCALE = 10 diff --git a/selfdrive/modeld/modeld b/selfdrive/modeld/modeld new file mode 100755 index 0000000000..cb7dc65e7b --- /dev/null +++ b/selfdrive/modeld/modeld @@ -0,0 +1,4 @@ +#!/bin/sh +export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/:$LD_LIBRARY_PATH" +exec ./_modeld + diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc new file mode 100644 index 0000000000..276a347925 --- /dev/null +++ b/selfdrive/modeld/modeld.cc @@ -0,0 +1,281 @@ +#include +#include + +#ifdef QCOM +#include +#else +#include +#endif + +#include "common/visionbuf.h" +#include "common/visionipc.h" +#include "common/swaglog.h" + +#include "models/driving.h" +#include "models/posenet.h" + +volatile sig_atomic_t do_exit = 0; + +static void set_do_exit(int sig) { + do_exit = 1; +} + +// globals +bool run_model; +mat3 cur_transform; +pthread_mutex_t transform_lock; + +void* live_thread(void *arg) { + int err; + set_thread_name("live"); + + Context * c = Context::create(); + SubSocket * live_calibration_sock = SubSocket::create(c, "liveCalibration"); + assert(live_calibration_sock != NULL); + + Poller * poller = Poller::create({live_calibration_sock}); + + /* + import numpy as np + from common.transformations.model import medmodel_frame_from_road_frame + medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)] + ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground) + */ + Eigen::Matrix ground_from_medmodel_frame; + ground_from_medmodel_frame << + 0.00000000e+00, 0.00000000e+00, 1.00000000e+00, + -1.09890110e-03, 0.00000000e+00, 2.81318681e-01, + -1.84808520e-20, 9.00738606e-04,-4.28751576e-02; + + Eigen::Matrix eon_intrinsics; + eon_intrinsics << + 910.0, 0.0, 582.0, + 0.0, 910.0, 437.0, + 0.0, 0.0, 1.0; + + while (!do_exit) { + for (auto sock : poller->poll(10)){ + Message * msg = sock->receive(); + + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + if (event.isLiveCalibration()) { + pthread_mutex_lock(&transform_lock); + + auto extrinsic_matrix = event.getLiveCalibration().getExtrinsicMatrix(); + Eigen::Matrix extrinsic_matrix_eigen; + for (int i = 0; i < 4*3; i++){ + extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i]; + } + + auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen; + Eigen::Matrix camera_frame_from_ground; + camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0); + camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1); + camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3); + + auto warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame; + + for (int i=0; i<3*3; i++) { + cur_transform.v[i] = warp_matrix(i / 3, i % 3); + } + + run_model = true; + pthread_mutex_unlock(&transform_lock); + } + + delete msg; + } + + } + + + return NULL; +} + +int main(int argc, char **argv) { + int err; + set_realtime_priority(1); + + // start calibration thread + pthread_t live_thread_handle; + err = pthread_create(&live_thread_handle, NULL, live_thread, NULL); + assert(err == 0); + + // messaging + Context *msg_context = Context::create(); + PubSocket *model_sock = PubSocket::create(msg_context, "model"); + PubSocket *posenet_sock = PubSocket::create(msg_context, "cameraOdometry"); + SubSocket *pathplan_sock = SubSocket::create(msg_context, "pathPlan", "127.0.0.1", true); + + assert(model_sock != NULL); + assert(posenet_sock != NULL); + assert(pathplan_sock != NULL); + + // cl init + cl_device_id device_id; + cl_context context; + cl_command_queue q; + { + // TODO: refactor this + cl_platform_id platform_id = NULL; + cl_uint num_devices; + cl_uint num_platforms; + + err = clGetPlatformIDs(1, &platform_id, &num_platforms); + assert(err == 0); + err = clGetDeviceIDs(platform_id, CL_DEVICE_TYPE_DEFAULT, 1, + &device_id, &num_devices); + assert(err == 0); + + context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err); + assert(err == 0); + + q = clCreateCommandQueue(context, device_id, 0, &err); + assert(err == 0); + } + + // init the models + ModelState model; + PosenetState posenet; + model_init(&model, device_id, context, true); + posenet_init(&posenet); + LOGW("models loaded, modeld starting"); + + // debayering does a 2x downscale + mat3 yuv_transform = transform_scale_buffer((mat3){{ + 1.0, 0.0, 0.0, + 0.0, 1.0, 0.0, + 0.0, 0.0, 1.0, + }}, 0.5); + + // loop + VisionStream stream; + while (!do_exit) { + VisionStreamBufs buf_info; + err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info); + if (err) { + printf("visionstream connect fail\n"); + usleep(100000); + continue; + } + LOGW("connected with buffer size: %d", buf_info.buf_len); + + // one frame in memory + cl_mem yuv_cl; + VisionBuf yuv_ion = visionbuf_allocate_cl(buf_info.buf_len, device_id, context, &yuv_cl); + + double last = 0; + int desire = -1; + while (!do_exit) { + VIPCBuf *buf; + VIPCBufExtra extra; + buf = visionstream_get(&stream, &extra); + if (buf == NULL) { + printf("visionstream get failed\n"); + break; + } + + pthread_mutex_lock(&transform_lock); + mat3 transform = cur_transform; + const bool run_model_this_iter = run_model; + pthread_mutex_unlock(&transform_lock); + + Message *msg = pathplan_sock->receive(true); + if (msg != NULL) { + // TODO: copy and pasted from camerad/main.cc + auto amsg = kj::heapArray((msg->getSize() / sizeof(capnp::word)) + 1); + memcpy(amsg.begin(), msg->getData(), msg->getSize()); + + capnp::FlatArrayMessageReader cmsg(amsg); + cereal::Event::Reader event = cmsg.getRoot(); + + // TODO: path planner timeout? + desire = ((int)event.getPathPlan().getDesire()) - 1; + delete msg; + } + + double mt1 = 0, mt2 = 0; + if (run_model_this_iter) { + float vec_desire[DESIRE_SIZE] = {0}; + if (desire >= 0 && desire < DESIRE_SIZE) { + vec_desire[desire] = 1.0; + } + + mat3 model_transform = matmul3(yuv_transform, transform); + + mt1 = millis_since_boot(); + + // TODO: don't make copies! + memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len); + + ModelData model_buf = + model_eval_frame(&model, q, yuv_cl, buf_info.width, buf_info.height, + model_transform, NULL, vec_desire); + mt2 = millis_since_boot(); + + model_publish(model_sock, extra.frame_id, model_buf, extra.timestamp_eof); + + LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last); + last = mt1; + } + + // push the frame to the posenet + // TODO: This doesn't always have to run + double pt1 = 0, pt2 = 0, pt3 = 0; + pt1 = millis_since_boot(); + posenet_push(&posenet, (uint8_t*)buf->addr, buf_info.width); + pt2 = millis_since_boot(); + + // posenet runs every 5 + if (extra.frame_id % 5 == 0) { + posenet_eval(&posenet); + + // send posenet event + { + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto posenetd = event.initCameraOdometry(); + kj::ArrayPtr trans_vs(&posenet.output[0], 3); + posenetd.setTrans(trans_vs); + kj::ArrayPtr rot_vs(&posenet.output[3], 3); + posenetd.setRot(rot_vs); + kj::ArrayPtr trans_std_vs(&posenet.output[6], 3); + posenetd.setTransStd(trans_std_vs); + kj::ArrayPtr rot_std_vs(&posenet.output[9], 3); + posenetd.setRotStd(rot_std_vs); + posenetd.setTimestampEof(extra.timestamp_eof); + posenetd.setFrameId(extra.frame_id); + + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + posenet_sock->send((char*)bytes.begin(), bytes.size()); + } + pt3 = millis_since_boot(); + LOGD("pre: %.2fms | posenet: %.2fms", (pt2-pt1), (pt3-pt1)); + } + + } + visionbuf_free(&yuv_ion); + } + + visionstream_destroy(&stream); + + delete model_sock; + delete posenet_sock; + + posenet_free(&posenet); + model_free(&model); + + LOG("joining live_thread"); + err = pthread_join(live_thread_handle, NULL); + assert(err == 0); + + return 0; +} diff --git a/selfdrive/visiond/models/commonmodel.c b/selfdrive/modeld/models/commonmodel.c similarity index 100% rename from selfdrive/visiond/models/commonmodel.c rename to selfdrive/modeld/models/commonmodel.c diff --git a/selfdrive/visiond/models/commonmodel.h b/selfdrive/modeld/models/commonmodel.h similarity index 100% rename from selfdrive/visiond/models/commonmodel.h rename to selfdrive/modeld/models/commonmodel.h diff --git a/selfdrive/visiond/models/driving.cc b/selfdrive/modeld/models/driving.cc similarity index 92% rename from selfdrive/visiond/models/driving.cc rename to selfdrive/modeld/models/driving.cc index bb985425f8..127e3c2e80 100644 --- a/selfdrive/visiond/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -21,7 +21,7 @@ #define SELECTION 3 //output 3 group (lead now, in 2s and 6s) #define MDN_GROUP_SIZE 11 #define SPEED_BUCKETS 100 -#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION) +#define OUTPUT_SIZE ((MODEL_PATH_DISTANCE*2) + (2*(MODEL_PATH_DISTANCE*2 + 1)) + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION + OTHER_META_SIZE + DESIRE_PRED_SIZE) #ifdef TEMPORAL #define TEMPORAL_SIZE 512 #else @@ -65,6 +65,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, float *right_lane; float *lead; float *speed; + float *meta; } net_outputs = {NULL}; #ifdef DESIRE @@ -99,6 +100,7 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, net_outputs.right_lane = &s->output[MODEL_PATH_DISTANCE*2 + MODEL_PATH_DISTANCE*2 + 1]; net_outputs.lead = &s->output[MODEL_PATH_DISTANCE*2 + (MODEL_PATH_DISTANCE*2 + 1)*2]; //net_outputs.speed = &s->output[OUTPUT_SIZE - SPEED_BUCKETS]; + net_outputs.meta = &s->output[OUTPUT_SIZE - OTHER_META_SIZE - DESIRE_PRED_SIZE]; ModelData model = {0}; @@ -111,9 +113,9 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, model.right_lane.stds[i] = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + i]); } - model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]); - model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]); - model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE + MODEL_PATH_DISTANCE/4]); + model.path.std = softplus(net_outputs.path[MODEL_PATH_DISTANCE]); + model.left_lane.std = softplus(net_outputs.left_lane[MODEL_PATH_DISTANCE]); + model.right_lane.std = softplus(net_outputs.right_lane[MODEL_PATH_DISTANCE]); model.path.prob = 1.; model.left_lane.prob = sigmoid(net_outputs.left_lane[MODEL_PATH_DISTANCE*2]); @@ -182,6 +184,10 @@ ModelData model_eval_frame(ModelState* s, cl_command_queue q, // model.speed[i-1] = model.speed[i]; // } //} + for (int i=0; i desire_pred(&meta_data[OTHER_META_SIZE], DESIRE_PRED_SIZE); + meta.setDesirePrediction(desire_pred); +} + void model_publish(PubSocket *sock, uint32_t frame_id, const ModelData data, uint64_t timestamp_eof) { // make msg @@ -272,6 +287,9 @@ void model_publish(PubSocket *sock, uint32_t frame_id, auto lead_future = framed.initLeadFuture(); fill_lead(lead_future, data.lead_future); + auto meta = framed.initMeta(); + fill_meta(meta, data.meta); + // send message auto words = capnp::messageToFlatArray(msg); diff --git a/selfdrive/visiond/models/driving.h b/selfdrive/modeld/models/driving.h similarity index 100% rename from selfdrive/visiond/models/driving.h rename to selfdrive/modeld/models/driving.h diff --git a/selfdrive/modeld/models/monitoring.cc b/selfdrive/modeld/models/monitoring.cc new file mode 100644 index 0000000000..dad8fa04e7 --- /dev/null +++ b/selfdrive/modeld/models/monitoring.cc @@ -0,0 +1,152 @@ +#include +#include "monitoring.h" +#include "common/mat.h" +#include "common/timing.h" + +// #include +#include + +#define MODEL_WIDTH 160 +#define MODEL_HEIGHT 320 +#define FULL_W 426 + +void monitoring_init(MonitoringState* s) { + s->m = new DefaultRunModel("../../models/monitoring_model_q.dlc", (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME); +} + +MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int width, int height) { + + uint8_t *raw_buf = (uint8_t*) stream_buf; + uint8_t *raw_y_buf = raw_buf; + uint8_t *raw_u_buf = raw_y_buf + (width * height); + uint8_t *raw_v_buf = raw_u_buf + ((width/2) * (height/2)); + + int cropped_width = height/2; + int cropped_height = height; + + int resized_width = MODEL_WIDTH; + int resized_height = MODEL_HEIGHT; + + uint8_t *cropped_buf = new uint8_t[cropped_width*cropped_height*3/2]; + uint8_t *cropped_y_buf = cropped_buf; + uint8_t *cropped_u_buf = cropped_y_buf + (cropped_width * cropped_height); + uint8_t *cropped_v_buf = cropped_u_buf + ((cropped_width/2) * (cropped_height/2)); + + if (true) { + for (int r = 0; r < height/2; r++) { + memcpy(cropped_y_buf + 2*r*cropped_width, raw_y_buf + 2*r*width + (width - cropped_width), cropped_width); + memcpy(cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r+1)*width + (width - cropped_width), cropped_width); + memcpy(cropped_u_buf + r*cropped_width/2, raw_u_buf + r*width/2 + ((width/2) - (cropped_width/2)), cropped_width/2); + memcpy(cropped_v_buf + r*cropped_width/2, raw_v_buf + r*width/2 + ((width/2) - (cropped_width/2)), cropped_width/2); + } + } else { + // not tested + uint8_t *premirror_cropped_buf = new uint8_t[cropped_width*cropped_height*3/2]; + uint8_t *premirror_cropped_y_buf = premirror_cropped_buf; + uint8_t *premirror_cropped_u_buf = premirror_cropped_y_buf + (cropped_width * cropped_height); + uint8_t *premirror_cropped_v_buf = premirror_cropped_u_buf + ((cropped_width/2) * (cropped_height/2)); + for (int r = 0; r < height/2; r++) { + memcpy(premirror_cropped_y_buf + 2*r*cropped_width, raw_y_buf + 2*r*width, cropped_width); + memcpy(premirror_cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r+1)*width, cropped_width); + memcpy(premirror_cropped_u_buf + r*cropped_width/2, raw_u_buf + r*width/2, cropped_width/2); + memcpy(premirror_cropped_v_buf + r*cropped_width/2, raw_v_buf + r*width/2, cropped_width/2); + } + libyuv::I420Mirror(premirror_cropped_y_buf, cropped_width, + premirror_cropped_u_buf, cropped_width/2, + premirror_cropped_v_buf, cropped_width/2, + cropped_y_buf, cropped_width, + cropped_u_buf, cropped_width/2, + cropped_v_buf, cropped_width/2, + cropped_width, cropped_height); + } + + uint8_t *resized_buf = new uint8_t[resized_width*resized_height*3/2]; + uint8_t *resized_y_buf = resized_buf; + uint8_t *resized_u_buf = resized_y_buf + (resized_width * resized_height); + uint8_t *resized_v_buf = resized_u_buf + ((resized_width/2) * (resized_height/2)); + + libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear; + libyuv::I420Scale(cropped_y_buf, cropped_width, + cropped_u_buf, cropped_width/2, + cropped_v_buf, cropped_width/2, + cropped_width, cropped_height, + resized_y_buf, resized_width, + resized_u_buf, resized_width/2, + resized_v_buf, resized_width/2, + resized_width, resized_height, + mode); + + int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v + + float *net_input_buf = new float[yuv_buf_len]; + // one shot conversion, O(n) anyway + // yuvframe2tensor, normalize + for (int r = 0; r < MODEL_HEIGHT/2; r++) { + for (int c = 0; c < MODEL_WIDTH/2; c++) { + // Y_ul + net_input_buf[(c*MODEL_HEIGHT/2) + r] = (resized_buf[(2*r*resized_width) + (2*c)] - 128.f) * 0.0078125f; + // Y_ur + net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width) + (2*c+1)] - 128.f) * 0.0078125f; + // Y_dl + net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width+1) + (2*c)] - 128.f) * 0.0078125f; + // Y_dr + net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width+1) + (2*c+1)] - 128.f) * 0.0078125f; + // U + net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c] - 128.f) * 0.0078125f; + // V + net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c] - 128.f) * 0.0078125f; + } + } + + // FILE *dump_yuv_file = fopen("/sdcard/rawdump.yuv", "wb"); + // fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file); + // fclose(dump_yuv_file); + + // FILE *dump_yuv_file2 = fopen("/sdcard/inputdump.yuv", "wb"); + // fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2); + // fclose(dump_yuv_file2); + + delete[] cropped_buf; + delete[] resized_buf; + s->m->execute(net_input_buf); + delete[] net_input_buf; + + MonitoringResult ret = {0}; + memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation); + memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position); + memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob); + memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob); + memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob); + memcpy(&ret.left_blink_prob, &s->output[31], sizeof ret.right_eye_prob); + memcpy(&ret.right_blink_prob, &s->output[32], sizeof ret.right_eye_prob); + return ret; +} + +void monitoring_publish(PubSocket* sock, uint32_t frame_id, const MonitoringResult res) { + // make msg + capnp::MallocMessageBuilder msg; + cereal::Event::Builder event = msg.initRoot(); + event.setLogMonoTime(nanos_since_boot()); + + auto framed = event.initDriverMonitoring(); + framed.setFrameId(frame_id); + + kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); + kj::ArrayPtr face_position(&res.face_position[0], ARRAYSIZE(res.face_position)); + framed.setFaceOrientation(face_orientation); + framed.setFacePosition(face_position); + framed.setFaceProb(res.face_prob); + framed.setLeftEyeProb(res.left_eye_prob); + framed.setRightEyeProb(res.right_eye_prob); + framed.setLeftBlinkProb(res.left_blink_prob); + framed.setRightBlinkProb(res.right_blink_prob); + + // send message + auto words = capnp::messageToFlatArray(msg); + auto bytes = words.asBytes(); + sock->send((char*)bytes.begin(), bytes.size()); +} + +void monitoring_free(MonitoringState* s) { + delete s->m; +} diff --git a/selfdrive/visiond/models/monitoring.h b/selfdrive/modeld/models/monitoring.h similarity index 62% rename from selfdrive/visiond/models/monitoring.h rename to selfdrive/modeld/models/monitoring.h index 4732363711..10e2cc9b8b 100644 --- a/selfdrive/visiond/models/monitoring.h +++ b/selfdrive/modeld/models/monitoring.h @@ -13,13 +13,9 @@ extern "C" { #endif -#define OUTPUT_SIZE_DEPRECATED 8 #define OUTPUT_SIZE 33 typedef struct MonitoringResult { - float descriptor_DEPRECATED[OUTPUT_SIZE_DEPRECATED - 1]; - float std_DEPRECATED; - float face_orientation[3]; float face_position[2]; float face_prob; @@ -30,18 +26,15 @@ typedef struct MonitoringResult { } MonitoringResult; typedef struct MonitoringState { - ModelInput in; RunModel *m; float output[OUTPUT_SIZE]; } MonitoringState; -void monitoring_init(MonitoringState* s, cl_device_id device_id, cl_context context); -MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q, cl_mem yuv_cl, int width, int height); -void monitoring_publish(PubSocket *sock, uint32_t frame_id, const MonitoringResult res, float ir_target); +void monitoring_init(MonitoringState* s); +MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int width, int height); +void monitoring_publish(PubSocket *sock, uint32_t frame_id, const MonitoringResult res); void monitoring_free(MonitoringState* s); -float ir_target_set(float *cur_front_gain, const MonitoringResult res); - #ifdef __cplusplus } #endif diff --git a/selfdrive/visiond/models/posenet.cc b/selfdrive/modeld/models/posenet.cc similarity index 100% rename from selfdrive/visiond/models/posenet.cc rename to selfdrive/modeld/models/posenet.cc diff --git a/selfdrive/visiond/models/posenet.h b/selfdrive/modeld/models/posenet.h similarity index 100% rename from selfdrive/visiond/models/posenet.h rename to selfdrive/modeld/models/posenet.h diff --git a/selfdrive/modeld/monitoringd b/selfdrive/modeld/monitoringd new file mode 100755 index 0000000000..63e28b12f7 --- /dev/null +++ b/selfdrive/modeld/monitoringd @@ -0,0 +1,5 @@ +#!/bin/sh +export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8:$LD_LIBRARY_PATH" +export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/" +exec ./_monitoringd + diff --git a/selfdrive/modeld/monitoringd.cc b/selfdrive/modeld/monitoringd.cc new file mode 100644 index 0000000000..98863ec9ff --- /dev/null +++ b/selfdrive/modeld/monitoringd.cc @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include + +#include "common/visionbuf.h" +#include "common/visionipc.h" +#include "common/swaglog.h" + +#include "models/monitoring.h" + +#ifndef PATH_MAX +#include +#endif + + +volatile sig_atomic_t do_exit = 0; + +static void set_do_exit(int sig) { + do_exit = 1; +} + +int main(int argc, char **argv) { + int err; + set_realtime_priority(1); + + // messaging + Context *msg_context = Context::create(); + PubSocket *monitoring_sock = PubSocket::create(msg_context, "driverMonitoring"); + + // init the models + MonitoringState monitoring; + monitoring_init(&monitoring); + + // loop + VisionStream stream; + while (!do_exit) { + VisionStreamBufs buf_info; + err = visionstream_init(&stream, VISION_STREAM_YUV_FRONT, true, &buf_info); + if (err) { + printf("visionstream connect fail\n"); + usleep(100000); + continue; + } + LOGW("connected with buffer size: %d", buf_info.buf_len); + + double last = 0; + while (!do_exit) { + VIPCBuf *buf; + VIPCBufExtra extra; + buf = visionstream_get(&stream, &extra); + if (buf == NULL) { + printf("visionstream get failed\n"); + break; + } + //printf("frame_id: %d %dx%d\n", extra.frame_id, buf_info.width, buf_info.height); + + double t1 = millis_since_boot(); + + MonitoringResult res = monitoring_eval_frame(&monitoring, buf->addr, buf_info.width, buf_info.height); + + double t2 = millis_since_boot(); + + // send dm packet + monitoring_publish(monitoring_sock, extra.frame_id, res); + + LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last); + last = t1; + } + + } + + visionstream_destroy(&stream); + + delete monitoring_sock; + monitoring_free(&monitoring); + + return 0; +} diff --git a/selfdrive/visiond/runners/run.h b/selfdrive/modeld/runners/run.h similarity index 100% rename from selfdrive/visiond/runners/run.h rename to selfdrive/modeld/runners/run.h diff --git a/selfdrive/visiond/runners/runmodel.h b/selfdrive/modeld/runners/runmodel.h similarity index 100% rename from selfdrive/visiond/runners/runmodel.h rename to selfdrive/modeld/runners/runmodel.h diff --git a/selfdrive/visiond/runners/snpemodel.cc b/selfdrive/modeld/runners/snpemodel.cc similarity index 100% rename from selfdrive/visiond/runners/snpemodel.cc rename to selfdrive/modeld/runners/snpemodel.cc diff --git a/selfdrive/visiond/runners/snpemodel.h b/selfdrive/modeld/runners/snpemodel.h similarity index 100% rename from selfdrive/visiond/runners/snpemodel.h rename to selfdrive/modeld/runners/snpemodel.h diff --git a/selfdrive/visiond/transforms/loadyuv.c b/selfdrive/modeld/transforms/loadyuv.c similarity index 100% rename from selfdrive/visiond/transforms/loadyuv.c rename to selfdrive/modeld/transforms/loadyuv.c diff --git a/selfdrive/visiond/transforms/loadyuv.cl b/selfdrive/modeld/transforms/loadyuv.cl similarity index 100% rename from selfdrive/visiond/transforms/loadyuv.cl rename to selfdrive/modeld/transforms/loadyuv.cl diff --git a/selfdrive/visiond/transforms/loadyuv.h b/selfdrive/modeld/transforms/loadyuv.h similarity index 100% rename from selfdrive/visiond/transforms/loadyuv.h rename to selfdrive/modeld/transforms/loadyuv.h diff --git a/selfdrive/visiond/transforms/transform.c b/selfdrive/modeld/transforms/transform.c similarity index 100% rename from selfdrive/visiond/transforms/transform.c rename to selfdrive/modeld/transforms/transform.c diff --git a/selfdrive/visiond/transforms/transform.cl b/selfdrive/modeld/transforms/transform.cl similarity index 100% rename from selfdrive/visiond/transforms/transform.cl rename to selfdrive/modeld/transforms/transform.cl diff --git a/selfdrive/visiond/transforms/transform.h b/selfdrive/modeld/transforms/transform.h similarity index 100% rename from selfdrive/visiond/transforms/transform.h rename to selfdrive/modeld/transforms/transform.h diff --git a/selfdrive/proclogd/Makefile b/selfdrive/proclogd/Makefile deleted file mode 100644 index 46c4ca5c46..0000000000 --- a/selfdrive/proclogd/Makefile +++ /dev/null @@ -1,58 +0,0 @@ -ARCH := $(shell uname -m) - -CC = clang -CXX = clang++ - -PHONELIBS = ../../phonelibs -BASEDIR = ../.. - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args - -CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) -CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) - -MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging -MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a - -ifeq ($(ARCH),aarch64) -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 -EXTRA_LIBS += -lgnustl_shared -endif - - -.PHONY: all -all: proclogd - -include ../common/cereal.mk - -OBJS = proclogd.o \ - $(CEREAL_OBJS) - -DEPS := $(OBJS:.o=.d) - -proclogd: $(OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(CEREAL_LIBS) \ - $(EXTRA_LIBS) \ - -llog - -%.o: %.cc - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) \ - $(CEREAL_CXXFLAGS) \ - $(MESSAGING_FLAGS) \ - -I../ \ - -I../../ \ - -c -o '$@' '$<' - -.PHONY: clean -clean: - rm -f proclogd $(OBJS) $(DEPS) - --include $(DEPS) diff --git a/selfdrive/proclogd/SConscript b/selfdrive/proclogd/SConscript new file mode 100644 index 0000000000..2b87f8d5e5 --- /dev/null +++ b/selfdrive/proclogd/SConscript @@ -0,0 +1,2 @@ +Import('env', 'messaging') +env.Program('proclogd.cc', LIBS=[messaging, 'pthread', 'zmq', 'czmq', 'capnp', 'kj']) diff --git a/selfdrive/proclogd/proclogd.cc b/selfdrive/proclogd/proclogd.cc index 50e3b4b659..ec8a9ad286 100644 --- a/selfdrive/proclogd/proclogd.cc +++ b/selfdrive/proclogd/proclogd.cc @@ -38,6 +38,7 @@ int main() { Context * c = Context::create(); PubSocket * publisher = PubSocket::create(c, "procLog"); + assert(publisher != NULL); double jiffy = sysconf(_SC_CLK_TCK); size_t page_size = sysconf(_SC_PAGE_SIZE); diff --git a/selfdrive/registration.py b/selfdrive/registration.py index 1c85e40b22..734f6f8713 100644 --- a/selfdrive/registration.py +++ b/selfdrive/registration.py @@ -1,61 +1,13 @@ import os import json -import binascii -import subprocess -import itertools from datetime import datetime, timedelta from selfdrive.swaglog import cloudlog from selfdrive.version import version, terms_version, training_version, get_git_commit, get_git_branch, get_git_remote +from common.android import get_imei, get_serial, get_subscriber_info from common.api import api_get from common.params import Params - -def get_imei(): - ret = subprocess.check_output(["getprop", "oem.device.imeicache"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg - if ret == "": - ret = "000000000000000" - return ret - - -def get_serial(): - return subprocess.check_output(["getprop", "ro.serialno"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg - - -# TODO: move this to a library -def parse_service_call(call): - ret = subprocess.check_output(call, encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg - if 'Parcel' not in ret: - return None - - try: - r = b"" - for line in ret.split("\n")[1:]: # Skip 'Parcel(' - line_hex = line[14:49].replace(' ', '') - r += binascii.unhexlify(line_hex) - - r = r[8:] # Cut off length field - r = r.decode('utf_16_be') - - # All pairs of two characters seem to be swapped. Not sure why - result = "" - for a, b, in itertools.zip_longest(r[::2], r[1::2], fillvalue='\x00'): - result += b + a - - result = result.replace('\x00', '') - - return result - except Exception: - return None - - -def get_subscriber_info(): - ret = parse_service_call(["service", "call", "iphonesubinfo", "7"]) - if ret is None or len(ret) < 8: - return "" - return ret - - def register(): params = Params() params.put("Version", version) diff --git a/selfdrive/sensord/Makefile b/selfdrive/sensord/Makefile deleted file mode 100644 index 98508d8d1a..0000000000 --- a/selfdrive/sensord/Makefile +++ /dev/null @@ -1,4 +0,0 @@ --include build_from_src.mk - -release: - @echo "sensord: this is a release" diff --git a/selfdrive/sensord/SConscript b/selfdrive/sensord/SConscript new file mode 100644 index 0000000000..b285cf9754 --- /dev/null +++ b/selfdrive/sensord/SConscript @@ -0,0 +1,5 @@ +Import('env', 'common', 'messaging') +env.Program('_sensord', 'sensors.cc', LIBS=['hardware', common, 'json', messaging, 'capnp', 'zmq', 'kj']) +lenv = env.Clone() +lenv['LIBPATH'] += ['/system/vendor/lib64'] +lenv.Program('_gpsd', ['gpsd.cc', 'rawgps.cc'], LIBS=['hardware', common, 'diag', 'time_genoff', 'json', messaging, 'capnp', 'zmq', 'kj']) diff --git a/selfdrive/sensord/build_from_src.mk b/selfdrive/sensord/build_from_src.mk deleted file mode 100644 index b51364e8dd..0000000000 --- a/selfdrive/sensord/build_from_src.mk +++ /dev/null @@ -1,90 +0,0 @@ -CC = clang -CXX = clang++ - -PHONELIBS = ../../phonelibs -BASEDIR = ../.. - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args - -CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) \ - -I$(PHONELIBS)/android_frameworks_native/include \ - -I$(PHONELIBS)/android_system_core/include \ - -I$(PHONELIBS)/android_hardware_libhardware/include -CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS) \ - -I$(PHONELIBS)/android_frameworks_native/include \ - -I$(PHONELIBS)/android_system_core/include \ - -I$(PHONELIBS)/android_hardware_libhardware/include - -MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging -MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a - -# Sensord can only be compiled for the phone -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 -EXTRA_LIBS += -lgnustl_shared - - -JSON_FLAGS = -I$(PHONELIBS)/json/src - -DIAG_LIBS = -L/system/vendor/lib64 -ldiag -ltime_genoff - -.PHONY: all -all: sensord gpsd - -include ../common/cereal.mk - -SENSORD_OBJS = sensors.o \ - ../common/swaglog.o \ - $(PHONELIBS)/json/src/json.o - -GPSD_OBJS = gpsd.o \ - rawgps.o \ - ../common/swaglog.o \ - $(PHONELIBS)/json/src/json.o - -DEPS := $(SENSORD_OBJS:.o=.d) $(GPSD_OBJS:.o=.d) - -sensord: $(SENSORD_OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(CEREAL_LIBS) \ - $(EXTRA_LIBS) \ - -lhardware - -gpsd: $(GPSD_OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(CEREAL_LIBS) \ - $(DIAG_LIBS) \ - $(EXTRA_LIBS) \ - -lhardware - -%.o: %.cc - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) \ - $(CEREAL_CXXFLAGS) \ - $(MESSAGING_FLAGS) \ - $(JSON_FLAGS) \ - -I../ \ - -I../../ \ - -c -o '$@' '$<' - - -%.o: %.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) \ - $(JSON_FLAGS) \ - $(ZMQ_FLAGS) \ - -I../ \ - -I../../ \ - -c -o '$@' '$<' - -.PHONY: clean -clean: - rm -f sensord gpsd $(OBJS) $(DEPS) - --include $(DEPS) diff --git a/selfdrive/sensord/gpsd b/selfdrive/sensord/gpsd new file mode 100755 index 0000000000..97d5a1467a --- /dev/null +++ b/selfdrive/sensord/gpsd @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:35038a572308e61aa56a483b0edffbdc64322c04911a8422669c52a6fbe76a86 +size 79 diff --git a/selfdrive/sensord/gpsd.cc b/selfdrive/sensord/gpsd.cc index d7637651f9..54eb6b4dc4 100644 --- a/selfdrive/sensord/gpsd.cc +++ b/selfdrive/sensord/gpsd.cc @@ -164,6 +164,9 @@ void gps_init() { gps_context = Context::create(); gps_publisher = PubSocket::create(gps_context, "gpsNMEA"); gps_location_publisher = PubSocket::create(gps_context, "gpsLocation"); + + assert(gps_publisher != NULL); + assert(gps_location_publisher != NULL); } void gps_destroy() { @@ -183,6 +186,7 @@ void* clock_thread(void* args) { int err = 0; PubSocket* clock_publisher = PubSocket::create(gps_context, "clocks"); + assert(clock_publisher != NULL); int timerfd = timerfd_create(CLOCK_BOOTTIME, 0); assert(timerfd >= 0); diff --git a/selfdrive/sensord/rawgps.cc b/selfdrive/sensord/rawgps.cc index 2cd4975175..3a0d1c8046 100644 --- a/selfdrive/sensord/rawgps.cc +++ b/selfdrive/sensord/rawgps.cc @@ -1108,6 +1108,7 @@ void rawgps_init() { rawgps_context = Context::create(); rawgps_publisher = PubSocket::create(rawgps_context, "qcomGnss"); + assert(rawgps_publisher != NULL); bool init_success = Diag_LSM_Init(NULL); assert(init_success); diff --git a/selfdrive/sensord/sensord b/selfdrive/sensord/sensord new file mode 100755 index 0000000000..71509d05e4 --- /dev/null +++ b/selfdrive/sensord/sensord @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:ed52dac08364c5582ae5f7ab3ab8d541014d90a9acfeddd287817695ca0260b9 +size 82 diff --git a/selfdrive/sensord/sensors.cc b/selfdrive/sensord/sensors.cc index de20bfc6c3..0f70bb66c0 100644 --- a/selfdrive/sensord/sensors.cc +++ b/selfdrive/sensord/sensors.cc @@ -55,6 +55,7 @@ void sensor_loop() { Context * c = Context::create(); PubSocket * sensor_events_sock = PubSocket::create(c, "sensorEvents"); + assert(sensor_events_sock != NULL); struct sensors_poll_device_t* device; struct sensors_module_t* module; diff --git a/selfdrive/sensord/start_gpsd.py b/selfdrive/sensord/start_gpsd.py deleted file mode 100755 index 287ead2959..0000000000 --- a/selfdrive/sensord/start_gpsd.py +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/env python3 -import os - -assert os.system("make") == 0 -os.environ['LD_LIBRARY_PATH'] = "/system/lib64:" + os.environ['LD_LIBRARY_PATH'] -os.execv("./gpsd", ["gpsd"]) diff --git a/selfdrive/sensord/start_sensord.py b/selfdrive/sensord/start_sensord.py deleted file mode 100755 index bd0cbed82b..0000000000 --- a/selfdrive/sensord/start_sensord.py +++ /dev/null @@ -1,6 +0,0 @@ -#!/usr/bin/env python3 -import os - -assert os.system("make") == 0 -os.environ['LD_LIBRARY_PATH'] = "/system/lib64:" + os.environ['LD_LIBRARY_PATH'] -os.execv("./sensord", ["sensord"]) diff --git a/selfdrive/service_list.yaml b/selfdrive/service_list.yaml deleted file mode 100644 index a38c6ad714..0000000000 --- a/selfdrive/service_list.yaml +++ /dev/null @@ -1,163 +0,0 @@ -# TODO: these port numbers are hardcoded in c, fix this - -# LogRotate: 8001 is a PUSH PULL socket between loggerd and visiond - -# all ZMQ pub sub: port, should_log, frequency, (qlog_decimation) - -# frame syncing packet -frame: [8002, true, 20., 1] -# accel, gyro, and compass -sensorEvents: [8003, true, 100., 100] -# GPS data, also global timestamp -gpsNMEA: [8004, true, 9.] # 9 msgs each sec -# CPU+MEM+GPU+BAT temps -thermal: [8005, true, 2., 1] -# List(CanData), list of can messages -can: [8006, true, 100.] -controlsState: [8007, true, 100., 100] -#liveEvent: [8008, true, 0.] -model: [8009, true, 20., 5] -features: [8010, true, 0.] -health: [8011, true, 2., 1] -radarState: [8012, true, 20.] -#liveUI: [8014, true, 0.] -encodeIdx: [8015, true, 20.] -liveTracks: [8016, true, 20.] -sendcan: [8017, true, 100.] -logMessage: [8018, true, 0.] -liveCalibration: [8019, true, 5.] -androidLog: [8020, true, 0.] -carState: [8021, true, 100., 10] -# 8022 is reserved for sshd -carControl: [8023, true, 100., 10] -plan: [8024, true, 20.] -liveLocation: [8025, true, 0.] -gpsLocation: [8026, true, 1., 1] -ethernetData: [8027, true, 0.] -navUpdate: [8028, true, 0.] -qcomGnss: [8029, true, 0.] -lidarPts: [8030, true, 0.] -procLog: [8031, true, 0.5] -gpsLocationExternal: [8032, true, 10., 1] -ubloxGnss: [8033, true, 10.] -clocks: [8034, true, 1., 1] -liveMpc: [8035, false, 20.] -liveLongitudinalMpc: [8036, false, 20.] -plusFrame: [8037, false, 0.] -navStatus: [8038, true, 0.] -gpsLocationTrimble: [8039, true, 0.] -trimbleGnss: [8041, true, 0.] -ubloxRaw: [8042, true, 20.] -gpsPlannerPoints: [8043, true, 0.] -gpsPlannerPlan: [8044, true, 0.] -applanixRaw: [8046, true, 0.] -orbLocation: [8047, true, 0.] -trafficEvents: [8048, true, 0.] -liveLocationTiming: [8049, true, 0.] -orbslamCorrection: [8050, true, 0.] -liveLocationCorrected: [8051, true, 0.] -orbObservation: [8052, true, 0.] -applanixLocation: [8053, true, 0.] -liveLocationKalman: [8054, true, 0.] -uiNavigationEvent: [8055, true, 0.] -orbOdometry: [8057, true, 0.] -orbFeatures: [8058, false, 0.] -orbKeyFrame: [8059, true, 0.] -uiLayoutState: [8060, true, 0.] -frontEncodeIdx: [8061, true, 5.] -orbFeaturesSummary: [8062, true, 0.] -driverMonitoring: [8063, true, 5., 1] -liveParameters: [8064, true, 10.] -liveMapData: [8065, true, 0.] -cameraOdometry: [8066, true, 5.] -pathPlan: [8067, true, 20.] -kalmanOdometry: [8068, true, 0.] -thumbnail: [8069, true, 0.2, 1] -carEvents: [8070, true, 1., 1] -carParams: [8071, true, 0.02, 1] - -testModel: [8040, false, 0.] -testLiveLocation: [8045, false, 0.] -testJoystick: [8056, false, 0.] - -# 8080 is reserved for slave testing daemon -# 8762 is reserved for logserver - -# manager -- base process to manage starting and stopping of all others -# subscribes: thermal - -# **** processes that communicate with the outside world **** - -# thermald -- decides when to start and stop onroad -# subscribes: health, location -# publishes: thermal - -# boardd -- communicates with the car -# subscribes: sendcan -# publishes: can, health, ubloxRaw - -# sensord -- publishes IMU and Magnetometer -# publishes: sensorEvents - -# gpsd -- publishes EON's gps -# publishes: gpsNMEA - -# visiond -- talks to the cameras, runs the model, saves the videos -# publishes: frame, model, driverMonitoring, cameraOdometry, thumbnail - -# **** stateful data transformers **** - -# plannerd -- decides where to drive the car -# subscribes: carState, model, radarState, controlsState, liveParameters -# publishes: plan, pathPlan, liveMpc, liveLongitudinalMpc - -# controlsd -- drives the car by sending CAN messages to panda -# subscribes: can, thermal, health, plan, pathPlan, driverMonitoring, liveCalibration -# publishes: carState, carControl, sendcan, controlsState, carEvents, carParams - -# radard -- processes the radar and vision data -# subscribes: can, controlsState, model, liveParameters -# publishes: radarState, liveTracks - -# params_learner -- learns vehicle params by observing the vehicle dynamics -# subscribes: controlsState, sensorEvents, cameraOdometry -# publishes: liveParameters - -# calibrationd -- reads posenet and applies a temporal filter on the frame region to look at -# subscribes: cameraOdometry -# publishes: liveCalibration - -# ubloxd -- read raw ublox data and converts them in readable format -# subscribes: ubloxRaw -# publishes: ubloxGnss - -# **** LOGGING SERVICE **** - -# loggerd -# subscribes: EVERYTHING - -# **** NON VITAL SERVICES **** - -# ui -# subscribes: thermal, model, controlsState, uiLayout, liveCalibration, radarState, liveMpc, plusFrame, liveMapData - -# uploader -# communicates through file system with loggerd - -# deleter -# communicates through file system with loggerd and uploader - -# logmessaged -- central logging service, can log to cloud -# publishes: logMessage - -# logcatd -- fetches logcat info from android -# publishes: androidLog - -# proclogd -- fetches process information -# publishes: procLog - -# tombstoned -- reports native crashes - -# athenad -- on request, open a sub socket and return the value - -# updated -- waits for network access and tries to update every hour diff --git a/selfdrive/services.py b/selfdrive/services.py deleted file mode 100644 index 24a96c4eb2..0000000000 --- a/selfdrive/services.py +++ /dev/null @@ -1,20 +0,0 @@ -import os -import yaml - -class Service(): - def __init__(self, port, should_log, frequency, decimation=None): - self.port = port - self.should_log = should_log - self.frequency = frequency - self.decimation = decimation - -service_list_path = os.path.join(os.path.dirname(__file__), "service_list.yaml") - -service_list = {} -with open(service_list_path, "r") as f: - for k, v in yaml.safe_load(f).items(): - decimation = None - if len(v) == 4: - decimation = v[3] - - service_list[k] = Service(v[0], v[1], v[2], decimation) diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index b81f5ea0e1..5dd1d0789b 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -10,22 +10,35 @@ from opendbc import DBC_PATH from common.realtime import Ratekeeper from selfdrive.config import Conversions as CV -import selfdrive.messaging as messaging +import cereal.messaging as messaging from selfdrive.car import crc8_pedal -from selfdrive.car.honda.hondacan import fix from selfdrive.car.honda.values import CAR from selfdrive.car.honda.carstate import get_can_signals -from selfdrive.boardd.boardd import can_capnp_to_can_list, can_list_to_can_capnp +from selfdrive.boardd.boardd import can_list_to_can_capnp -from selfdrive.can.plant_can_parser import CANParser +from opendbc.can.parser import CANParser from selfdrive.car.honda.interface import CarInterface -from common.dbc import dbc +from opendbc.can.dbc import dbc honda = dbc(os.path.join(DBC_PATH, "honda_civic_touring_2016_can_generated.dbc")) # Trick: set 0x201 (interceptor) in fingerprints for gas is controlled like if there was an interceptor CP = CarInterface.get_params(CAR.CIVIC, {0: {0x201: 6}, 1: {}, 2: {}, 3: {}}) +# Honda checksum +def can_cksum(mm): + s = 0 + for c in mm: + s += (c>>4) + s += c & 0xF + s = 8-s + s %= 0x10 + return s + +def fix(msg, addr): + msg2 = msg[0:-1] + (msg[-1] | can_cksum(struct.pack("I", addr)+msg)).to_bytes(1, 'little') + return msg2 + def car_plant(pos, speed, grade, gas, brake): # vehicle parameters @@ -65,7 +78,7 @@ def car_plant(pos, speed, grade, gas, brake): return speed, acceleration def get_car_can_parser(): - dbc_f = 'honda_civic_touring_2016_can_generated.dbc' + dbc_f = 'honda_civic_touring_2016_can_generated' signals = [ ("STEER_TORQUE", 0xe4, 0), ("STEER_TORQUE_REQUEST", 0xe4, 0), @@ -78,7 +91,7 @@ def get_car_can_parser(): (0x1fa, 50), (0x200, 50), ] - return CANParser(dbc_f, signals, checks) + return CANParser(dbc_f, signals, checks, 0) def to_3_byte(x): # Convert into 12 bit value @@ -167,15 +180,13 @@ class Plant(): cks_msgs.add(0x30C) # ******** get messages sent to the car ******** - can_msgs = [] - for a in messaging.drain_sock(Plant.sendcan, wait_for_one=self.response_seen): - can_msgs.extend(can_capnp_to_can_list(a.sendcan, [0,2])) + can_strings = messaging.drain_sock_raw(Plant.sendcan, wait_for_one=self.response_seen) # After the first response the car is done fingerprinting, so we can run in lockstep with controlsd - if can_msgs: + if can_strings: self.response_seen = True - self.cp.update_can(can_msgs) + self.cp.update_strings(can_strings, sendcan=True) # ******** get controlsState messages for plotting *** controls_state_msgs = [] diff --git a/selfdrive/test/longitudinal_maneuvers/plant_ui.py b/selfdrive/test/longitudinal_maneuvers/plant_ui.py index e9dd18f372..f4b7715e38 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant_ui.py +++ b/selfdrive/test/longitudinal_maneuvers/plant_ui.py @@ -3,7 +3,7 @@ import pygame # pylint: disable=import-error from selfdrive.test.longitudinal_maneuvers.plant import Plant from selfdrive.car.honda.values import CruiseButtons import numpy as np -import selfdrive.messaging as messaging +import cereal.messaging as messaging import math CAR_WIDTH = 2.0 diff --git a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py index bfcbf67f7b..647eb3633c 100755 --- a/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py +++ b/selfdrive/test/longitudinal_maneuvers/test_longitudinal.py @@ -333,6 +333,7 @@ class LongitudinalControl(unittest.TestCase): params = Params() params.put("Passive", "1" if os.getenv("PASSIVE") else "0") params.put("OpenpilotEnabledToggle", "1") + params.put("CommunityFeaturesToggle", "1") manager.gctx = {} manager.prepare_managed_process('radard') diff --git a/selfdrive/test/openpilotci_upload.py b/selfdrive/test/openpilotci_upload.py index e2ad2d4129..177f854bfd 100755 --- a/selfdrive/test/openpilotci_upload.py +++ b/selfdrive/test/openpilotci_upload.py @@ -5,7 +5,7 @@ import subprocess def upload_file(path, name): - from azure.storage.blob import BlockBlobService # pylint: disable=no-name-in-module, import-error + from azure.storage.blob import BlockBlobService sas_token = os.getenv("TOKEN", None) if sas_token is None: sas_token = subprocess.check_output("az storage container generate-sas --account-name commadataci --name openpilotci --https-only --permissions lrw --expiry $(date -u '+%Y-%m-%dT%H:%M:%SZ' -d '+1 hour') --auth-mode login --as-user --output tsv", shell=True).decode().strip("\n") diff --git a/selfdrive/test/process_replay/compare_logs.py b/selfdrive/test/process_replay/compare_logs.py index ca550cbea4..82e701a75f 100755 --- a/selfdrive/test/process_replay/compare_logs.py +++ b/selfdrive/test/process_replay/compare_logs.py @@ -38,12 +38,14 @@ def remove_ignored_fields(msg, ignore): return msg.as_reader() def compare_logs(log1, log2, ignore=[]): - assert len(log1) == len(log2), "logs are not same length" + assert len(log1) == len(log2), "logs are not same length: " + str(len(log1)) + " VS " + str(len(log2)) ignore_fields = [k for k, v in ignore] diff = [] for msg1, msg2 in tqdm(zip(log1, log2)): - assert msg1.which() == msg2.which(), "msgs not aligned between logs" + if msg1.which() != msg2.which(): + print(msg1, msg2) + assert False, "msgs not aligned between logs" msg1_bytes = remove_ignored_fields(msg1, ignore).as_builder().to_bytes() msg2_bytes = remove_ignored_fields(msg2, ignore).as_builder().to_bytes() @@ -58,4 +60,4 @@ def compare_logs(log1, log2, ignore=[]): if __name__ == "__main__": log1 = list(LogReader(sys.argv[1])) log2 = list(LogReader(sys.argv[2])) - compare_logs(log1, log2, sys.argv[3:]) + print(compare_logs(log1, log2, sys.argv[3:])) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 718bab6dd6..fdce596e41 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -12,9 +12,9 @@ else: from cereal import car, log from selfdrive.car.car_helpers import get_car import selfdrive.manager as manager -import selfdrive.messaging as messaging +import cereal.messaging as messaging from common.params import Params -from selfdrive.services import service_list +from cereal.services import service_list from collections import namedtuple ProcessConfig = namedtuple('ProcessConfig', ['proc_name', 'pub_sub', 'ignore', 'init_callback', 'should_recv_callback']) @@ -181,7 +181,8 @@ CONFIGS = [ proc_name="controlsd", pub_sub={ "can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"], - "thermal": [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": [], "gpsLocation": [], + "thermal": [], "health": [], "liveCalibration": [], "driverMonitoring": [], "plan": [], "pathPlan": [], "gpsLocation": [], + "model": [], }, ignore=[("logMonoTime", 0), ("valid", True), ("controlsState.startMonoTime", 0), ("controlsState.cumLagMs", 0)], init_callback=fingerprint, @@ -237,6 +238,7 @@ def replay_process(cfg, lr): params.manager_start() params.put("OpenpilotEnabledToggle", "1") params.put("Passive", "0") + params.put("CommunityFeaturesToggle", "1") os.environ['NO_RADAR_SLEEP'] = "1" manager.prepare_managed_process(cfg.proc_name) diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index f00366d3f8..f603a24986 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -5fe3678541ca1254ca5c1f04231f42225d68c538 \ No newline at end of file +b2364d6239bca0a8caaf11f0433bf766c66e15a4 \ No newline at end of file diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index 9ef7ea31cf..0d18496d52 100755 --- a/selfdrive/test/test_car_models.py +++ b/selfdrive/test/test_car_models.py @@ -9,7 +9,7 @@ import requests from cereal import car import selfdrive.manager as manager -import selfdrive.messaging as messaging +import cereal.messaging as messaging from common.params import Params from common.basedir import BASEDIR from selfdrive.car.fingerprints import all_known_cars @@ -255,12 +255,12 @@ routes = { "5f5afb36036506e4|2019-05-14--02-09-54": { 'carFingerprint': TOYOTA.COROLLA_TSS2, 'enableCamera': True, - 'enableDsu': True, + 'enableDsu': False, }, "5ceff72287a5c86c|2019-10-19--10-59-02": { 'carFingerprint': TOYOTA.COROLLAH_TSS2, 'enableCamera': True, - 'enableDsu': True, + 'enableDsu': False, }, "56fb1c86a9a86404|2017-11-10--10-18-43": { 'carFingerprint': TOYOTA.PRIUS, @@ -282,12 +282,12 @@ routes = { "cdf2f7de565d40ae|2019-04-25--03-53-41": { 'carFingerprint': TOYOTA.RAV4_TSS2, 'enableCamera': True, - 'enableDsu': True, + 'enableDsu': False, }, "e6a24be49a6cd46e|2019-10-29--10-52-42": { 'carFingerprint': TOYOTA.LEXUS_ES_TSS2, 'enableCamera': True, - 'enableDsu': True, + 'enableDsu': False, }, "f49e8041283f2939|2019-05-29--13-48-33": { 'carFingerprint': TOYOTA.LEXUS_ESH_TSS2, @@ -297,7 +297,7 @@ routes = { "f49e8041283f2939|2019-05-30--11-51-51": { 'carFingerprint': TOYOTA.LEXUS_ESH_TSS2, 'enableCamera': True, - 'enableDsu': True, + 'enableDsu': False, }, "b0f5a01cf604185c|2018-02-01--21-12-28": { 'carFingerprint': TOYOTA.LEXUS_RXH, diff --git a/selfdrive/test/test_fingerprints.py b/selfdrive/test/test_fingerprints.py index 6b5e8378af..af544e1a7d 100755 --- a/selfdrive/test/test_fingerprints.py +++ b/selfdrive/test/test_fingerprints.py @@ -8,7 +8,6 @@ from common.basedir import BASEDIR CAN_IGNITION_MSGS = { 'gm': [(0x1F1, 8), (0x160, 5)], 'tesla' : [(0x348, 8)], - 'volkswagen' : [(0x3C0, 4)], } def _get_fingerprints(): diff --git a/selfdrive/test/test_openpilot.py b/selfdrive/test/test_openpilot.py index 0176a7fcd0..68e35d90bc 100644 --- a/selfdrive/test/test_openpilot.py +++ b/selfdrive/test/test_openpilot.py @@ -1,10 +1,17 @@ import os os.environ['FAKEUPLOAD'] = "1" +from common.apk import update_apks, start_frame, pm_apply_packages, android_packages +from common.params import Params from common.testing import phone_only from selfdrive.manager import manager_init, manager_prepare from selfdrive.manager import start_managed_process, kill_managed_process, get_running +from selfdrive.manager import start_daemon_process from functools import wraps +import json +import requests +import signal +import subprocess import time DID_INIT = False @@ -29,14 +36,39 @@ def with_processes(processes): assert all(get_running()[name].exitcode is None for name in processes) # call the function - func() + try: + func() + # assert processes are still started + assert all(get_running()[name].exitcode is None for name in processes) + finally: + # kill and assert all stopped + [kill_managed_process(p) for p in processes] + assert len(get_running()) == 0 + return wrap + return wrapper - # assert processes are still started - assert all(get_running()[name].exitcode is None for name in processes) +def with_apks(): + def wrapper(func): + @wraps(func) + def wrap(): + if not DID_INIT: + test_manager_prepare() + + update_apks() + pm_apply_packages('enable') + start_frame() - # kill and assert all stopped - [kill_managed_process(p) for p in processes] - assert len(get_running()) == 0 + func() + + try: + for package in android_packages: + apk_is_running = (subprocess.call(["pidof", package]) == 0) + assert apk_is_running, package + finally: + pm_apply_packages('disable') + for package in android_packages: + apk_is_not_running = (subprocess.call(["pidof", package]) == 1) + assert apk_is_not_running, package return wrap return wrapper @@ -67,7 +99,7 @@ def test_logging(): time.sleep(1.0) @phone_only -@with_processes(['visiond']) +@with_processes(['camerad', 'modeld', 'monitoringd']) def test_visiond(): print("VISIOND IS SET UP") time.sleep(5.0) @@ -91,3 +123,129 @@ def test_ui(): def test_uploader(): print("UPLOADER") time.sleep(10.0) + +@phone_only +def test_athena(): + print("ATHENA") + start_daemon_process("manage_athenad") + params = Params() + manage_athenad_pid = params.get("AthenadPid") + assert manage_athenad_pid is not None + try: + os.kill(int(manage_athenad_pid), 0) + # process is running + except OSError: + assert False, "manage_athenad is dead" + + def expect_athena_starts(timeout=30): + now = time.time() + athenad_pid = None + while athenad_pid is None: + try: + athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip() + return athenad_pid + except subprocess.CalledProcessError: + if time.time() - now > timeout: + assert False, f"Athena did not start within {timeout} seconds" + time.sleep(0.5) + + def athena_post(payload, max_retries=5): + tries = 0 + while 1: + try: + return requests.post( + "https://athena.comma.ai/" + params.get("DongleId", encoding="utf-8"), + headers={ + "Authorization": "JWT " + os.getenv("COMMA_JWT"), + "Content-Type": "application/json" + }, + data=json.dumps(payload), + timeout=30 + ) + except Exception as e: + print(e) + time.sleep(5.0) + tries += 1 + if tries == max_retries: + raise + + def expect_athena_registers(timeout=60): + now = time.time() + while 1: + resp = athena_post({ + "method": "echo", + "params": ["hello"], + "id": 0, + "jsonrpc": "2.0" + }) + resp_json = resp.json() + if resp_json.get('result') == "hello": + break + elif time.time() - now > timeout: + assert False, f"Athena did not become available within {timeout} seconds." + else: + time.sleep(5.0) + + try: + athenad_pid = expect_athena_starts() + # kill athenad and ensure it is restarted (check_output will throw if it is not) + os.kill(int(athenad_pid), signal.SIGINT) + expect_athena_starts() + + if not os.getenv('COMMA_JWT'): + print('WARNING: COMMA_JWT env not set, will not test requests to athena.comma.ai') + return + + expect_athena_registers() + + print("ATHENA: getSimInfo") + resp = athena_post({ + "method": "getSimInfo", + "id": 0, + "jsonrpc": "2.0" + }) + resp_json = resp.json() + assert resp_json.get('result'), resp_json + assert 'sim_id' in resp_json['result'], resp_json['result'] + + print("ATHENA: takeSnapshot") + resp = athena_post({ + "method": "takeSnapshot", + "id": 0, + "jsonrpc": "2.0" + }) + resp_json = resp.json() + assert resp_json.get('result'), resp_json + assert resp_json['result']['jpegBack'], resp_json['result'] + + @with_processes(["thermald"]) + def test_athena_thermal(): + print("ATHENA: getMessage(thermal)") + resp = athena_post({ + "method": "getMessage", + "params": {"service": "thermal", "timeout": 5000}, + "id": 0, + "jsonrpc": "2.0" + }) + resp_json = resp.json() + assert resp_json.get('result'), resp_json + assert resp_json['result']['thermal'], resp_json['result'] + test_athena_thermal() + finally: + try: + athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip() + except subprocess.CalledProcessError: + athenad_pid = None + + try: + os.kill(int(manage_athenad_pid), signal.SIGINT) + os.kill(int(athenad_pid), signal.SIGINT) + except (OSError, TypeError): + pass + +# TODO: re-enable when jenkins test has /data/pythonpath -> /data/openpilot +# @phone_only +# @with_apks() +# def test_apks(): +# print("APKS") +# time.sleep(14.0) diff --git a/selfdrive/thermald.py b/selfdrive/thermald.py index eeec3f3b11..d78e0011d1 100755 --- a/selfdrive/thermald.py +++ b/selfdrive/thermald.py @@ -3,6 +3,7 @@ import os import json import copy import datetime +import psutil from smbus2 import SMBus from cereal import log from common.basedir import BASEDIR @@ -12,7 +13,7 @@ from common.numpy_fast import clip, interp from common.filter_simple import FirstOrderFilter from selfdrive.version import terms_version, training_version from selfdrive.swaglog import cloudlog -import selfdrive.messaging as messaging +import cereal.messaging as messaging from selfdrive.loggerd.config import get_available_percent from selfdrive.pandad import get_expected_version @@ -27,9 +28,12 @@ DAYS_NO_CONNECTIVITY_PROMPT = 4 # send an offroad prompt after 4 days with no i with open(BASEDIR + "/selfdrive/controls/lib/alerts_offroad.json") as json_file: OFFROAD_ALERTS = json.load(json_file) -def read_tz(x): +def read_tz(x, clip=True): with open("/sys/devices/virtual/thermal/thermal_zone%d/temp" % x) as f: - ret = max(0, int(f.read())) + ret = int(f.read()) + if clip: + ret = max(0, ret) + return ret def read_thermal(): @@ -42,6 +46,7 @@ def read_thermal(): dat.thermal.mem = read_tz(2) dat.thermal.gpu = read_tz(16) dat.thermal.bat = read_tz(29) + dat.thermal.pa0 = read_tz(25) return dat LEON = False @@ -76,7 +81,9 @@ def set_eon_fan(val): # tusb320 if val == 0: bus.write_i2c_block_data(0x67, 0xa, [0]) + #bus.write_i2c_block_data(0x67, 0x45, [1<<2]) else: + #bus.write_i2c_block_data(0x67, 0x45, [0]) bus.write_i2c_block_data(0x67, 0xa, [0x20]) bus.write_i2c_block_data(0x67, 0x8, [(val-1)<<6]) else: @@ -120,12 +127,10 @@ def handle_fan_uno(max_cpu_temp, bat_temp, fan_speed): return int(interp(max_cpu_temp, [40.0, 80.0], [0, 100])) def thermald_thread(): - setup_eon_fan() - # prevent LEECO from undervoltage BATT_PERC_OFF = 10 if LEON else 3 - health_timeout = int(1000 * 2 * DT_TRML) # 2x the expected health frequency + health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency # now loop thermal_sock = messaging.pub_sock('thermal') @@ -148,6 +153,14 @@ def thermald_thread(): fw_version_match_prev = True current_connectivity_alert = None time_valid_prev = True + should_start_prev = False + + is_uno = (read_tz(29, clip=False) < -1000) + if is_uno: + handle_fan = handle_fan_uno + else: + setup_eon_fan() + handle_fan = handle_fan_eon params = Params() @@ -165,11 +178,10 @@ def thermald_thread(): if health is not None: usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client - # loggerd is gated based on free space - avail = get_available_percent() / 100.0 + msg.thermal.freeSpace = get_available_percent() / 100.0 # disk space + msg.thermal.memUsedPercent = int(round(psutil.virtual_memory().percent)) + msg.thermal.cpuPerc = int(round(psutil.cpu_percent())) - # thermal message now also includes free space - msg.thermal.freeSpace = avail with open("/sys/class/power_supply/battery/capacity") as f: msg.thermal.batteryPercent = int(f.read()) with open("/sys/class/power_supply/battery/status") as f: @@ -189,11 +201,7 @@ def thermald_thread(): max_comp_temp = max(max_cpu_temp, msg.thermal.mem / 10., msg.thermal.gpu / 10.) bat_temp = msg.thermal.bat/1000. - if health is not None and health.health.hwType == log.HealthData.HwType.uno: - fan_speed = handle_fan_uno(max_cpu_temp, bat_temp, fan_speed) - else: - fan_speed = handle_fan_eon(max_cpu_temp, bat_temp, fan_speed) - + fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed) msg.thermal.fanSpeed = fan_speed # thermal logic with hysterisis @@ -265,14 +273,11 @@ def thermald_thread(): should_start = ignition - # have we seen a panda? - passive = (params.get("Passive") == "1") - # with 2% left, we killall, otherwise the phone will take a long time to boot should_start = should_start and msg.thermal.freeSpace > 0.02 # confirm we have completed training and aren't uninstalling - should_start = should_start and accepted_terms and (passive or completed_training) and (not do_uninstall) + should_start = should_start and accepted_terms and completed_training and (not do_uninstall) # check for firmware mismatch should_start = should_start and fw_version_match @@ -280,6 +285,11 @@ def thermald_thread(): # check if system time is valid should_start = should_start and time_valid + # don't start while taking snapshot + if not should_start_prev: + is_taking_snapshot = params.get("IsTakingSnapshot") == b"1" + should_start = should_start and (not is_taking_snapshot) + if fw_version_match and not fw_version_match_prev: params.delete("Offroad_PandaFirmwareMismatch") if not fw_version_match and fw_version_match_prev: @@ -328,8 +338,9 @@ def thermald_thread(): thermal_status_prev = thermal_status usb_power_prev = usb_power fw_version_match_prev = fw_version_match + should_start_prev = should_start - print(msg) + #print(msg) # report to server once per minute if (count % int(60. / DT_TRML)) == 0: diff --git a/selfdrive/ui/Makefile b/selfdrive/ui/Makefile deleted file mode 100644 index 1ab8f00c86..0000000000 --- a/selfdrive/ui/Makefile +++ /dev/null @@ -1,127 +0,0 @@ -CC = clang -CXX = clang++ - -BASEDIR = ../.. -PHONELIBS = ../../phonelibs - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args - -CFLAGS = -std=gnu11 -fPIC -O2 $(WARN_FLAGS) -CXXFLAGS = -std=c++11 -fPIC -O2 $(WARN_FLAGS) - -ZMQ_LIBS = -l:libczmq.a - -MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging -MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a - -CEREAL_LIBS = -l:libcapn.a -CEREAL_OBJS = ../../cereal/gen/c/log.capnp.o - -NANOVG_FLAGS = -I$(PHONELIBS)/nanovg -JSON_FLAGS = -I$(PHONELIBS)/json/src - -OPENCL_FLAGS = -I$(PHONELIBS)/opencl/include -OPENCL_LIBS = -lgsl -lCB -lOpenCL - -OPENGL_LIBS = -lGLESv3 - -OPENSL_LIBS = -lOpenSLES - -UUID_LIBS = -luuid - -FRAMEBUFFER_LIBS = -lutils -lgui -lEGL - -CFLAGS += -DQCOM \ - -I$(PHONELIBS)/android_frameworks_native/include \ - -I$(PHONELIBS)/android_system_core/include \ - -I$(PHONELIBS)/android_hardware_libhardware/include -CXXFLAGS += -DQCOM \ - -I$(PHONELIBS)/android_frameworks_native/include \ - -I$(PHONELIBS)/android_system_core/include \ - -I$(PHONELIBS)/android_hardware_libhardware/include - -# ui can only be compiled for the phone -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 - -OBJS = slplay.o \ - ui.o \ - ../common/glutil.o \ - ../common/visionipc.o \ - ../common/ipc.o \ - ../common/visionimg.o \ - ../common/visionbuf_ion.o \ - ../common/framebuffer.o \ - ../common/params.o \ - ../common/util.o \ - ../common/touch.o \ - ../common/swaglog.o \ - $(PHONELIBS)/nanovg/nanovg.o \ - $(PHONELIBS)/json/src/json.o \ - $(CEREAL_OBJS) - -DEPS := $(OBJS:.o=.d) - -all: ui - -ui: $(OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(FRAMEBUFFER_LIBS) \ - $(CEREAL_LIBS) \ - -lgnustl_shared \ - -L/system/vendor/lib64 \ - -lhardware -lui \ - $(ZMQ_LIBS) \ - $(OPENGL_LIBS) \ - $(OPENCL_LIBS) \ - ${OPENSL_LIBS} \ - ${UUID_LIBS} \ - -Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib \ - -lcutils -lm -llog -lui -ladreno_utils - -slplay.o: slplay.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -fPIC \ - -I../ \ - $(OPENSL_LIBS) \ - -c -o '$@' $^ - -%.o: %.cc - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) -MMD \ - -Iinclude -I.. -I../.. \ - $(OPENCL_FLAGS) \ - $(NANOVG_FLAGS) \ - $(ZMQ_FLAGS) \ - $(MESSAGING_FLAGS) \ - $(CEREAL_CFLAGS) \ - $(JSON_FLAGS) \ - -I$(PHONELIBS)/android_frameworks_native/include \ - -I$(PHONELIBS)/android_system_core/include \ - -I$(PHONELIBS)/android_hardware_libhardware/include \ - -I$(PHONELIBS)/libgralloc/include \ - -I$(PHONELIBS)/linux/include \ - -c -o '$@' '$<' - -%.o: %.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -MMD \ - -Iinclude -I.. -I../.. \ - $(NANOVG_FLAGS) \ - $(ZMQ_FLAGS) \ - $(CEREAL_CFLAGS) \ - $(JSON_FLAGS) \ - $(OPENCL_FLAGS) \ - -I$(PHONELIBS)/linux/include \ - -c -o '$@' '$<' - -.PHONY: clean -clean: - rm -f ui $(OBJS) $(DEPS) - --include $(DEPS) diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript new file mode 100644 index 0000000000..b3e96e79af --- /dev/null +++ b/selfdrive/ui/SConscript @@ -0,0 +1,5 @@ +Import('env', 'common', 'messaging', 'gpucommon', 'visionipc', 'cereal') + +env.Program('_ui', ['ui.cc', 'slplay.c', '#phonelibs/nanovg/nanovg.c'], + LINKFLAGS=['-Wl,-rpath=/system/lib64,-rpath=/system/comma/usr/lib'], + LIBS=[common, 'zmq', 'czmq', 'capnp', 'capnp_c', 'm', 'GLESv3', 'EGL', cereal, 'gnustl_shared', 'log', 'utils', 'gui', 'hardware', 'ui', 'json', messaging, 'CB', 'OpenCL', 'gsl', 'adreno_utils', 'OpenSLES', 'cutils', 'uuid', gpucommon, visionipc]) diff --git a/selfdrive/ui/start.py b/selfdrive/ui/start.py deleted file mode 100755 index a1d00ce145..0000000000 --- a/selfdrive/ui/start.py +++ /dev/null @@ -1,7 +0,0 @@ -#!/usr/bin/env python3 -import os - -assert os.system("make") == 0 -os.environ['LD_LIBRARY_PATH'] = "/system/lib64:"+os.environ['LD_LIBRARY_PATH'] -os.execv("./ui", ["ui"]) - diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui new file mode 100755 index 0000000000..d212c92da7 --- /dev/null +++ b/selfdrive/ui/ui @@ -0,0 +1,4 @@ +#!/bin/sh +export LD_LIBRARY_PATH="/system/lib64:$LD_LIBRARY_PATH" +exec ./_ui + diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 2cb1e190d2..fb22ad790d 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -55,12 +55,12 @@ extern "C"{ #define ALERTSIZE_MID 2 #define ALERTSIZE_FULL 3 +//#define UI_60FPS + #define UI_BUF_COUNT 4 //#define SHOW_SPEEDLIMIT 1 //#define DEBUG_TURN -//#define DEBUG_FPS - const int vwp_w = 1920; const int vwp_h = 1080; const int nav_w = 640; @@ -122,7 +122,6 @@ typedef struct UIScene { int transformed_width, transformed_height; - uint64_t model_ts; ModelData model; float mpc_x[50]; @@ -165,8 +164,6 @@ typedef struct UIScene { float awareness_status; - uint64_t started_ts; - // Used to show gps planner status bool gps_planner_active; } UIScene; @@ -208,17 +205,15 @@ typedef struct UIState { // Sockets Context *ctx; - SubSocket *thermal_sock; SubSocket *model_sock; SubSocket *controlsstate_sock; SubSocket *livecalibration_sock; SubSocket *radarstate_sock; - SubSocket *plus_sock; SubSocket *map_data_sock; SubSocket *uilayout_sock; Poller * poller; - int plus_state; + int active_app; // vision state bool vision_connected; @@ -331,7 +326,7 @@ static void set_awake(UIState *s, bool awake) { static void set_volume(UIState *s, int volume) { char volume_change_cmd[64]; - sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1", volume); + sprintf(volume_change_cmd, "service call audio 3 i32 3 i32 %d i32 1 &", volume); // 5 second timeout at 60fps s->volume_timeout = 5 * UI_FREQ; @@ -503,25 +498,29 @@ static void ui_init(UIState *s) { pthread_cond_init(&s->bg_cond, NULL); s->ctx = Context::create(); - s->thermal_sock = SubSocket::create(s->ctx, "thermal"); s->model_sock = SubSocket::create(s->ctx, "model"); s->controlsstate_sock = SubSocket::create(s->ctx, "controlsState"); s->uilayout_sock = SubSocket::create(s->ctx, "uiLayoutState"); s->livecalibration_sock = SubSocket::create(s->ctx, "liveCalibration"); s->radarstate_sock = SubSocket::create(s->ctx, "radarState"); - s->plus_sock = SubSocket::create(s->ctx, "plusFrame"); + + assert(s->model_sock != NULL); + assert(s->controlsstate_sock != NULL); + assert(s->uilayout_sock != NULL); + assert(s->livecalibration_sock != NULL); + assert(s->radarstate_sock != NULL); + s->poller = Poller::create({ - s->thermal_sock, s->model_sock, s->controlsstate_sock, s->uilayout_sock, s->livecalibration_sock, - s->radarstate_sock, - s->plus_sock + s->radarstate_sock }); #ifdef SHOW_SPEEDLIMIT s->map_data_sock = SubSock::create(s->ctx, "liveMapData"); + assert(s->map_data_sock != NULL); s->poller.registerSocket(s->map_data_sock); #endif @@ -1040,10 +1039,8 @@ static void ui_draw_world(UIState *s) { return; } - if ((nanos_since_boot() - scene->model_ts) < 1000000000ULL) { - // Draw lane edges and vision/mpc tracks - ui_draw_vision_lanes(s); - } + // Draw lane edges and vision/mpc tracks + ui_draw_vision_lanes(s); if (scene->lead_status) { // Draw lead car indicator @@ -1528,7 +1525,7 @@ static void ui_draw_blank(UIState *s) { } static void ui_draw(UIState *s) { - if (s->vision_connected && s->plus_state == 0) { + if (s->vision_connected && s->active_app == cereal_UiLayoutState_App_home && s->status != STATUS_STOPPED) { ui_draw_vision(s); } else { ui_draw_blank(s); @@ -1605,7 +1602,12 @@ void handle_message(UIState *s, Message * msg) { eventp.p = capn_getp(capn_root(&ctx), 0, 1); struct cereal_Event eventd; cereal_read_Event(&eventd, eventp); - double t = millis_since_boot(); + + // Skip messages from previous run + if (nanos_since_boot() - eventd.logMonoTime > 1e9) { + return; + } + if (eventd.which == cereal_Event_controlsState) { struct cereal_ControlsState datad; cereal_read_ControlsState(&datad, eventd.controlsState); @@ -1668,14 +1670,16 @@ void handle_message(UIState *s, Message * msg) { s->alert_size = ALERTSIZE_FULL; } - if (datad.alertStatus == cereal_ControlsState_AlertStatus_userPrompt) { - update_status(s, STATUS_WARNING); - } else if (datad.alertStatus == cereal_ControlsState_AlertStatus_critical) { - update_status(s, STATUS_ALERT); - } else if (datad.enabled) { - update_status(s, STATUS_ENGAGED); - } else { - update_status(s, STATUS_DISENGAGED); + if (s->status != STATUS_STOPPED) { + if (datad.alertStatus == cereal_ControlsState_AlertStatus_userPrompt) { + update_status(s, STATUS_WARNING); + } else if (datad.alertStatus == cereal_ControlsState_AlertStatus_critical) { + update_status(s, STATUS_ALERT); + } else if (datad.enabled) { + update_status(s, STATUS_ENGAGED); + } else { + update_status(s, STATUS_DISENGAGED); + } } s->scene.alert_blinkingrate = datad.alertBlinkingRate; @@ -1717,7 +1721,6 @@ void handle_message(UIState *s, Message * msg) { capn_to_f32(capn_get32(extrinsicl, i)); } } else if (eventd.which == cereal_Event_model) { - s->scene.model_ts = eventd.logMonoTime; s->scene.model = read_model(eventd.model); s->model_changed = true; } else if (eventd.which == cereal_Event_liveMpc) { @@ -1738,21 +1741,10 @@ void handle_message(UIState *s, Message * msg) { s->scene.mpc_y[i] = capn_to_f32(capn_get32(y_list, i)); } s->livempc_or_radarstate_changed = true; - } else if (eventd.which == cereal_Event_thermal) { - struct cereal_ThermalData datad; - cereal_read_ThermalData(&datad, eventd.thermal); - - if (!datad.started) { - update_status(s, STATUS_STOPPED); - } else if (s->status == STATUS_STOPPED) { - // car is started but controls doesn't have fingerprint yet - update_status(s, STATUS_DISENGAGED); - } - - s->scene.started_ts = datad.startedTs; } else if (eventd.which == cereal_Event_uiLayoutState) { struct cereal_UiLayoutState datad; cereal_read_UiLayoutState(&datad, eventd.uiLayoutState); + s->active_app = datad.activeApp; s->scene.uilayout_sidebarcollapsed = datad.sidebarCollapsed; s->scene.uilayout_mapenabled = datad.mapEnabled; @@ -1850,20 +1842,30 @@ static void ui_update(UIState *s) { } zmq_pollitem_t polls[1] = {{0}}; - // Wait for next rgb image from visiond + // Take an rgb image from visiond if there is one while(true) { assert(s->ipc_fd >= 0); polls[0].fd = s->ipc_fd; polls[0].events = ZMQ_POLLIN; - int ret = zmq_poll(polls, 1, 1000); + #ifdef UI_60FPS + // uses more CPU in both UI and surfaceflinger + // 16% / 21% + int ret = zmq_poll(polls, 1, 1); + #else + // 9% / 13% = a 14% savings + int ret = zmq_poll(polls, 1, 1000); + #endif if (ret < 0) { + if (errno == EINTR) continue; + LOGW("poll failed (%d)", ret); close(s->ipc_fd); s->ipc_fd = -1; s->vision_connected = false; return; - } else if (ret == 0) - continue; + } else if (ret == 0) { + break; + } // vision ipc event VisionPacket rp; err = vipc_recv(s->ipc_fd, &rp); @@ -1910,31 +1912,21 @@ static void ui_update(UIState *s) { } // peek and consume all events in the zmq queue, then return. while(true) { - bool awake = false; auto polls = s->poller->poll(0); if (polls.size() == 0) return; - + for (auto sock : polls){ Message * msg = sock->receive(); + if (msg == NULL) continue; - if (sock != s->thermal_sock){ - awake = true; - } + set_awake(s, true); - if (sock == s->plus_sock){ - s->plus_state = msg->getData()[0]; - } else { - handle_message(s, msg); - } + handle_message(s, msg); delete msg; } - - if (awake){ - set_awake(s, true); - } } } @@ -2162,11 +2154,7 @@ int main(int argc, char* argv[]) { const int MAX_VOLUME = LEON ? 15 : 12; set_volume(s, MIN_VOLUME); -#ifdef DEBUG_FPS - vipc_t1 = millis_since_boot(); - double t1 = millis_since_boot(); - int draws = 0, old_draws = 0; -#endif //DEBUG_FPS + int draws = 0; while (!do_exit) { bool should_swap = false; if (!s->vision_connected) { @@ -2175,6 +2163,7 @@ int main(int argc, char* argv[]) { usleep(30 * 1000); } pthread_mutex_lock(&s->lock); + double u1 = millis_since_boot(); // light sensor is only exposed on EONs float clipped_brightness = (s->light_sensor*BRIGHTNESS_M) + BRIGHTNESS_B; @@ -2189,9 +2178,10 @@ int main(int argc, char* argv[]) { polls[0].fd = s->touch_fd; polls[0].events = ZMQ_POLLIN; int ret = zmq_poll(polls, 1, 0); - if (ret < 0) + if (ret < 0){ + if (errno == EINTR) continue; LOGW("poll failed (%d)", ret); - else if (ret > 0) { + } else if (ret > 0) { // awake on any touch int touch_x = -1, touch_y = -1; int touched = touch_read(&touch, &touch_x, &touch_y); @@ -2199,7 +2189,13 @@ int main(int argc, char* argv[]) { set_awake(s, true); } } + if (s->status != STATUS_STOPPED) { + update_status(s, STATUS_STOPPED); + } } else { + if (s->status == STATUS_STOPPED) { + update_status(s, STATUS_DISENGAGED); + } // Car started, fetch a new rgb image from ipc and peek for zmq events. ui_update(s); if(!s->vision_connected) { @@ -2209,27 +2205,19 @@ int main(int argc, char* argv[]) { should_swap = true; } } + // manage wakefulness if (s->awake_timeout > 0) { s->awake_timeout--; } else { set_awake(s, false); } + // Don't waste resources on drawing in case screen is off or car is not started. if (s->awake && s->vision_connected) { ui_draw(s); glFinish(); should_swap = true; -#ifdef DEBUG_FPS - draws++; - double t2 = millis_since_boot(); - const double interval = 30.; - if(t2 - t1 >= interval * 1000.) { - printf("ui draw fps: %.2f\n",((double)(draws - old_draws)) / interval) ; - t1 = t2; - old_draws = draws; - } -#endif } if (s->volume_timeout > 0) { @@ -2252,7 +2240,9 @@ int main(int argc, char* argv[]) { // if visiond is still running and controlsState times out, display an alert if (s->controls_seen && s->vision_connected && strcmp(s->scene.alert_text2, "Controls Unresponsive") != 0) { s->scene.alert_size = ALERTSIZE_FULL; - update_status(s, STATUS_ALERT); + if (s->status != STATUS_STOPPED) { + update_status(s, STATUS_ALERT); + } snprintf(s->scene.alert_text1, sizeof(s->scene.alert_text1), "%s", "TAKE CONTROL IMMEDIATELY"); snprintf(s->scene.alert_text2, sizeof(s->scene.alert_text2), "%s", "Controls Unresponsive"); ui_draw_vision_alert(s, s->scene.alert_size, s->status, s->scene.alert_text1, s->scene.alert_text2); @@ -2276,6 +2266,12 @@ int main(int argc, char* argv[]) { // the bg thread needs to be scheduled, so the main thread needs time without the lock // safe to do this outside the lock? if (should_swap) { + double u2 = millis_since_boot(); + if (u2-u1 > 66) { + // warn on sub 15fps + LOGW("slow frame(%d) time: %.2f", draws, u2-u1); + } + draws++; eglSwapBuffers(s->display, s->surface); } } diff --git a/selfdrive/updated.py b/selfdrive/updated.py index d05a9a8fa7..1ac216c4fe 100755 --- a/selfdrive/updated.py +++ b/selfdrive/updated.py @@ -14,9 +14,9 @@ def main(gctx=None): params = Params() while True: - # try network + time_wrong = datetime.datetime.now().year < 2019 ping_failed = subprocess.call(["ping", "-W", "4", "-c", "1", "8.8.8.8"]) - if ping_failed: + if ping_failed or time_wrong: time.sleep(60) continue diff --git a/selfdrive/version.py b/selfdrive/version.py index dd78fa4e0b..21c042923e 100644 --- a/selfdrive/version.py +++ b/selfdrive/version.py @@ -5,25 +5,25 @@ from selfdrive.swaglog import cloudlog def get_git_commit(): - return subprocess.check_output(["git", "rev-parse", "HEAD"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg + return subprocess.check_output(["git", "rev-parse", "HEAD"], encoding='utf8').strip() def get_git_branch(): - return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg + return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "HEAD"], encoding='utf8').strip() def get_git_full_branchname(): - return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg + return subprocess.check_output(["git", "rev-parse", "--abbrev-ref", "--symbolic-full-name", "@{u}"], encoding='utf8').strip() def get_git_remote(): try: - local_branch = subprocess.check_output(["git", "name-rev", "--name-only", "HEAD"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg - tracking_remote = subprocess.check_output(["git", "config", "branch." + local_branch + ".remote"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg - return subprocess.check_output(["git", "config", "remote." + tracking_remote + ".url"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg + local_branch = subprocess.check_output(["git", "name-rev", "--name-only", "HEAD"], encoding='utf8').strip() + tracking_remote = subprocess.check_output(["git", "config", "branch." + local_branch + ".remote"], encoding='utf8').strip() + return subprocess.check_output(["git", "config", "remote." + tracking_remote + ".url"], encoding='utf8').strip() except subprocess.CalledProcessError: # Not on a branch, fallback - return subprocess.check_output(["git", "config", "--get", "remote.origin.url"], encoding='utf8').strip() # pylint: disable=unexpected-keyword-arg + return subprocess.check_output(["git", "config", "--get", "remote.origin.url"], encoding='utf8').strip() with open(os.path.join(os.path.dirname(os.path.abspath(__file__)), "common", "version.h")) as _versionf: @@ -45,9 +45,9 @@ try: dirty = subprocess.call(["git", "diff-index", "--quiet", branch, "--"]) != 0 if dirty: - dirty_files = subprocess.check_output(["git", "diff-index", branch, "--"], encoding='utf8') # pylint: disable=unexpected-keyword-arg - commit = subprocess.check_output(["git", "rev-parse", "--verify", "HEAD"], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg - origin_commit = subprocess.check_output(["git", "rev-parse", "--verify", branch], encoding='utf8').rstrip() # pylint: disable=unexpected-keyword-arg + dirty_files = subprocess.check_output(["git", "diff-index", branch, "--"], encoding='utf8') + commit = subprocess.check_output(["git", "rev-parse", "--verify", "HEAD"], encoding='utf8').rstrip() + origin_commit = subprocess.check_output(["git", "rev-parse", "--verify", branch], encoding='utf8').rstrip() cloudlog.event("dirty comma branch", vesion=version, dirty=dirty, origin=origin, branch=branch, dirty_files=dirty_files, commit=commit, origin_commit=origin_commit) else: diff --git a/selfdrive/visiond/LICENSE.boringssl b/selfdrive/visiond/LICENSE.boringssl deleted file mode 100644 index 0b0b9b3292..0000000000 --- a/selfdrive/visiond/LICENSE.boringssl +++ /dev/null @@ -1,192 +0,0 @@ -BoringSSL is a fork of OpenSSL. As such, large parts of it fall under OpenSSL -licensing. Files that are completely new have a Google copyright and an ISC -license. This license is reproduced at the bottom of this file. - -Contributors to BoringSSL are required to follow the CLA rules for Chromium: -https://cla.developers.google.com/clas - -Some files from Intel are under yet another license, which is also included -underneath. - -The OpenSSL toolkit stays under a dual license, i.e. both the conditions of the -OpenSSL License and the original SSLeay license apply to the toolkit. See below -for the actual license texts. Actually both licenses are BSD-style Open Source -licenses. In case of any license issues related to OpenSSL please contact -openssl-core@openssl.org. - -The following are Google-internal bug numbers where explicit permission from -some authors is recorded for use of their work. (This is purely for our own -record keeping.) - 27287199 - 27287880 - 27287883 - - OpenSSL License - --------------- - -/* ==================================================================== - * Copyright (c) 1998-2011 The OpenSSL Project. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * - * 3. All advertising materials mentioning features or use of this - * software must display the following acknowledgment: - * "This product includes software developed by the OpenSSL Project - * for use in the OpenSSL Toolkit. (http://www.openssl.org/)" - * - * 4. The names "OpenSSL Toolkit" and "OpenSSL Project" must not be used to - * endorse or promote products derived from this software without - * prior written permission. For written permission, please contact - * openssl-core@openssl.org. - * - * 5. Products derived from this software may not be called "OpenSSL" - * nor may "OpenSSL" appear in their names without prior written - * permission of the OpenSSL Project. - * - * 6. Redistributions of any form whatsoever must retain the following - * acknowledgment: - * "This product includes software developed by the OpenSSL Project - * for use in the OpenSSL Toolkit (http://www.openssl.org/)" - * - * THIS SOFTWARE IS PROVIDED BY THE OpenSSL PROJECT ``AS IS'' AND ANY - * EXPRESSED OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR - * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE OpenSSL PROJECT OR - * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, - * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT - * NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, - * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) - * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED - * OF THE POSSIBILITY OF SUCH DAMAGE. - * ==================================================================== - * - * This product includes cryptographic software written by Eric Young - * (eay@cryptsoft.com). This product includes software written by Tim - * Hudson (tjh@cryptsoft.com). - * - */ - - Original SSLeay License - ----------------------- - -/* Copyright (C) 1995-1998 Eric Young (eay@cryptsoft.com) - * All rights reserved. - * - * This package is an SSL implementation written - * by Eric Young (eay@cryptsoft.com). - * The implementation was written so as to conform with Netscapes SSL. - * - * This library is free for commercial and non-commercial use as long as - * the following conditions are aheared to. The following conditions - * apply to all code found in this distribution, be it the RC4, RSA, - * lhash, DES, etc., code; not just the SSL code. The SSL documentation - * included with this distribution is covered by the same copyright terms - * except that the holder is Tim Hudson (tjh@cryptsoft.com). - * - * Copyright remains Eric Young's, and as such any Copyright notices in - * the code are not to be removed. - * If this package is used in a product, Eric Young should be given attribution - * as the author of the parts of the library used. - * This can be in the form of a textual message at program startup or - * in documentation (online or textual) provided with the package. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * 1. Redistributions of source code must retain the copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in the - * documentation and/or other materials provided with the distribution. - * 3. All advertising materials mentioning features or use of this software - * must display the following acknowledgement: - * "This product includes cryptographic software written by - * Eric Young (eay@cryptsoft.com)" - * The word 'cryptographic' can be left out if the rouines from the library - * being used are not cryptographic related :-). - * 4. If you include any Windows specific code (or a derivative thereof) from - * the apps directory (application code) you must include an acknowledgement: - * "This product includes software written by Tim Hudson (tjh@cryptsoft.com)" - * - * THIS SOFTWARE IS PROVIDED BY ERIC YOUNG ``AS IS'' AND - * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS - * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) - * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY - * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF - * SUCH DAMAGE. - * - * The licence and distribution terms for any publically available version or - * derivative of this code cannot be changed. i.e. this code cannot simply be - * copied and put under another distribution licence - * [including the GNU Public Licence.] - */ - - -ISC license used for completely new code in BoringSSL: - -/* Copyright (c) 2015, Google Inc. - * - * Permission to use, copy, modify, and/or distribute this software for any - * purpose with or without fee is hereby granted, provided that the above - * copyright notice and this permission notice appear in all copies. - * - * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES - * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF - * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY - * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES - * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION - * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN - * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. */ - - -Some files from Intel carry the following license: - -# Copyright (c) 2012, Intel Corporation -# -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are -# met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the -# distribution. -# -# * Neither the name of the Intel Corporation nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# -# THIS SOFTWARE IS PROVIDED BY INTEL CORPORATION ""AS IS"" AND ANY -# EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR -# PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL INTEL CORPORATION OR -# CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, -# EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, -# PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR -# PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF -# LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING -# NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS -# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/selfdrive/visiond/LICENSE.libyuv b/selfdrive/visiond/LICENSE.libyuv deleted file mode 100644 index c911747a6b..0000000000 --- a/selfdrive/visiond/LICENSE.libyuv +++ /dev/null @@ -1,29 +0,0 @@ -Copyright 2011 The LibYuv Project Authors. All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are -met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in - the documentation and/or other materials provided with the - distribution. - - * Neither the name of Google nor the names of its contributors may - be used to endorse or promote products derived from this software - without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR -A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT -HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, -SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT -LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, -DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY -THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT -(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/selfdrive/visiond/LICENSE.opencv b/selfdrive/visiond/LICENSE.opencv deleted file mode 100644 index fce70d7135..0000000000 --- a/selfdrive/visiond/LICENSE.opencv +++ /dev/null @@ -1,41 +0,0 @@ -By downloading, copying, installing or using the software you agree to this license. -If you do not agree to this license, do not download, install, -copy or use the software. - - - License Agreement - For Open Source Computer Vision Library - (3-clause BSD License) - -Copyright (C) 2000-2016, Intel Corporation, all rights reserved. -Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved. -Copyright (C) 2009-2016, NVIDIA Corporation, all rights reserved. -Copyright (C) 2010-2013, Advanced Micro Devices, Inc., all rights reserved. -Copyright (C) 2015-2016, OpenCV Foundation, all rights reserved. -Copyright (C) 2015-2016, Itseez Inc., all rights reserved. -Third party copyrights are property of their respective owners. - -Redistribution and use in source and binary forms, with or without modification, -are permitted provided that the following conditions are met: - - * Redistributions of source code must retain the above copyright notice, - this list of conditions and the following disclaimer. - - * Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - - * Neither the names of the copyright holders nor the names of the contributors - may be used to endorse or promote products derived from this software - without specific prior written permission. - -This software is provided by the copyright holders and contributors "as is" and -any express or implied warranties, including, but not limited to, the implied -warranties of merchantability and fitness for a particular purpose are disclaimed. -In no event shall copyright holders or contributors be liable for any direct, -indirect, incidental, special, exemplary, or consequential damages -(including, but not limited to, procurement of substitute goods or services; -loss of use, data, or profits; or business interruption) however caused -and on any theory of liability, whether in contract, strict liability, -or tort (including negligence or otherwise) arising in any way out of -the use of this software, even if advised of the possibility of such damage. diff --git a/selfdrive/visiond/Makefile b/selfdrive/visiond/Makefile deleted file mode 100644 index 753c5e41ca..0000000000 --- a/selfdrive/visiond/Makefile +++ /dev/null @@ -1,4 +0,0 @@ --include build_from_src.mk - -release: - @echo "visiond: this is a release" diff --git a/selfdrive/visiond/README b/selfdrive/visiond/README deleted file mode 100644 index 1b612afcec..0000000000 --- a/selfdrive/visiond/README +++ /dev/null @@ -1 +0,0 @@ -visiond runs the openpilot/chffrplus vision pipeline. Everything running between the camera hardware and model outputs lives here. diff --git a/selfdrive/visiond/__init__.py b/selfdrive/visiond/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/selfdrive/visiond/build_from_src.mk b/selfdrive/visiond/build_from_src.mk deleted file mode 100644 index 725b345fa4..0000000000 --- a/selfdrive/visiond/build_from_src.mk +++ /dev/null @@ -1,226 +0,0 @@ -CC = clang -CXX = clang++ - -BASEDIR = ../.. -EXTERNAL = ../../external -PHONELIBS = ../../phonelibs - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args \ - -Wno-deprecated-declarations - -CFLAGS = -I. -std=gnu11 -fPIC -O2 $(WARN_FLAGS) -CXXFLAGS = -I. -std=c++14 -fPIC -O2 $(WARN_FLAGS) - -MESSAGING_FLAGS = -I$(BASEDIR)/selfdrive/messaging -MESSAGING_LIBS = $(BASEDIR)/selfdrive/messaging/messaging.a - -ifeq ($(ARCH),aarch64) -CFLAGS += -mcpu=cortex-a57 -CXXFLAGS += -mcpu=cortex-a57 -endif - -JSON_FLAGS = -I$(PHONELIBS)/json/src -JSON11_FLAGS = -I$(PHONELIBS)/json11/ -EIGEN_FLAGS = -I$(PHONELIBS)/eigen - -UNAME_M := $(shell uname -m) -UNAME_S := $(shell uname -s) - -ifeq ($(UNAME_M),x86_64) - -ifeq ($(UNAME_S),Darwin) - LIBYUV_FLAGS = -I$(PHONELIBS)/libyuv/include - LIBYUV_LIBS = $(PHONELIBS)/libyuv/mac/lib/libyuv.a - - ZMQ_FLAGS = -I$(EXTERNAL)/zmq/include - ZMQ_LIBS = $(PHONELIBS)/zmq/mac/lib/libczmq.a \ - $(PHONELIBS)/zmq/mac/lib/libzmq.a - - OPENCL_LIBS = -framework OpenCL - - PLATFORM_OBJS = cameras/camera_fake.o \ - ../common/visionbuf_cl.o -else - # assume x86_64 linux - LIBYUV_FLAGS = -I$(PHONELIBS)/libyuv/include - LIBYUV_LIBS = $(PHONELIBS)/libyuv/x64/lib/libyuv.a - - ZMQ_FLAGS = -I$(PHONELIBS)/zmq/x64/include - ZMQ_LIBS = -L$(PHONELIBS)/zmq/x64/lib/ -l:libczmq.a -l:libzmq.a - - OPENCL_LIBS = -lOpenCL - - #TF_FLAGS = -I$(EXTERNAL)/tensorflow/include - #TF_LIBS = -L$(EXTERNAL)/tensorflow/lib -ltensorflow \ - # -Wl,-rpath $(EXTERNAL)/tensorflow/lib - - SNPE_FLAGS = -I$(PHONELIBS)/snpe/include/ - SNPE_LIBS = -L$(PHONELIBS)/snpe/x86_64-linux-clang/ \ - -lSNPE -lsymphony-cpu \ - -Wl,-rpath $(PHONELIBS)/snpe/x86_64-linux-clang/ - - CFLAGS += -g - CXXFLAGS += -g -I../common - - PLATFORM_OBJS = cameras/camera_frame_stream.o \ - ../common/visionbuf_cl.o \ - ../common/visionimg.o \ - # runners/tfmodel.o -endif - - SSL_FLAGS = -I/usr/include/openssl/ - SSL_LIBS = -lssl -lcrypto - - OTHER_LIBS = -lz -lm -lpthread - - CFLAGS += -D_GNU_SOURCE \ - -DCLU_NO_CACHE - OBJS = visiond.o -else - # assume phone - - LIBYUV_FLAGS = -I$(PHONELIBS)/libyuv/include - LIBYUV_LIBS = $(PHONELIBS)/libyuv/lib/libyuv.a - - ZMQ_LIBS = -l:libczmq.a -l:libzmq.a -lgnustl_shared - - CURL_FLAGS = -I$(PHONELIBS)/curl/include - CURL_LIBS = $(PHONELIBS)/curl/lib/libcurl.a \ - $(PHONELIBS)/zlib/lib/libz.a - - SSL_FLAGS = -I$(PHONELIBS)/boringssl/include - SSL_LIBS = $(PHONELIBS)/boringssl/lib/libssl_static.a \ - $(PHONELIBS)/boringssl/lib/libcrypto_static.a - - OPENCL_FLAGS = -I$(PHONELIBS)/opencl/include - OPENCL_LIBS = -lgsl -lCB -lOpenCL - - OPENGL_LIBS = -lGLESv3 -lEGL - UUID_LIBS = -luuid - - SNPE_FLAGS = -I$(PHONELIBS)/snpe/include/ - SNPE_LIBS = -lSNPE -lsymphony-cpu -lsymphonypower - - OTHER_LIBS = -lz -lcutils -lm -llog -lui -ladreno_utils - - PLATFORM_OBJS = cameras/camera_qcom.o \ - ../common/visionbuf_ion.o \ - ../common/visionimg.o - - CFLAGS += -DQCOM \ - -I$(PHONELIBS)/android_system_core/include \ - -I$(PHONELIBS)/android_frameworks_native/include \ - -I$(PHONELIBS)/android_hardware_libhardware/include \ - -I$(PHONELIBS)/linux/include - CXXFLAGS += -DQCOM \ - -I$(PHONELIBS)/android_system_core/include \ - -I$(PHONELIBS)/android_frameworks_native/include \ - -I$(PHONELIBS)/android_hardware_libhardware/include \ - -I$(PHONELIBS)/linux/include - OBJS = visiond.o -endif - -OUTPUT = visiond - -.PHONY: all -all: $(OUTPUT) - -include ../common/cereal.mk - -OBJS += $(PLATFORM_OBJS) \ - ../common/swaglog.o \ - ../common/ipc.o \ - ../common/visionipc.o \ - ../common/util.o \ - ../common/params.o \ - ../common/efd.o \ - ../common/buffering.o \ - transforms/transform.o \ - transforms/loadyuv.o \ - transforms/rgb_to_yuv.o \ - models/commonmodel.o \ - runners/snpemodel.o \ - models/posenet.o \ - models/monitoring.o \ - models/driving.o \ - clutil.o \ - $(PHONELIBS)/json/src/json.o \ - $(PHONELIBS)/json11/json11.o \ - $(CEREAL_OBJS) - -DEPS := $(OBJS:.o=.d) - -rgb_to_yuv_test: transforms/rgb_to_yuv_test.o clutil.o transforms/rgb_to_yuv.o ../common/util.o - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(LIBYUV_LIBS) \ - $(LDFLAGS) \ - -L/usr/lib \ - -L/system/vendor/lib64 \ - $(OPENCL_LIBS) \ - - -$(OUTPUT): $(OBJS) $(MESSAGING_LIBS) - @echo "[ LINK ] $@" - $(CXX) -fPIC -o '$@' $^ \ - $(LDFLAGS) \ - $(LIBYUV_LIBS) \ - $(OPENGL_LIBS) \ - $(CEREAL_LIBS) \ - $(ZMQ_LIBS) \ - -ljpeg \ - -L/usr/lib \ - -L/system/vendor/lib64 \ - $(OPENCL_LIBS) \ - $(CURL_LIBS) \ - $(SSL_LIBS) \ - $(TF_LIBS) \ - $(SNPE_LIBS) \ - $(UUID_LIBS) \ - $(OTHER_LIBS) - -$(MODEL_OBJS): %.o: %.dlc - @echo "[ bin2o ] $@" - cd '$(dir $<)' && ld -r -b binary '$(notdir $<)' -o '$(abspath $@)' - -%.o: %.cc - @echo "[ CXX ] $@" - $(CXX) $(CXXFLAGS) -MMD \ - -Iinclude -I.. -I../.. \ - $(EIGEN_FLAGS) \ - $(ZMQ_FLAGS) \ - $(MESSAGING_FLAGS) \ - $(CEREAL_CXXFLAGS) \ - $(OPENCL_FLAGS) \ - $(LIBYUV_FLAGS) \ - $(TF_FLAGS) \ - $(SNPE_FLAGS) \ - $(JSON_FLAGS) \ - $(JSON11_FLAGS) $(CURL_FLAGS) \ - -I$(PHONELIBS)/libgralloc/include \ - -I$(PHONELIBS)/linux/include \ - -c -o '$@' '$<' - -%.o: %.c - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -MMD \ - -Iinclude -I.. -I../.. \ - $(ZMQ_FLAGS) \ - $(MESSAGING_FLAGS) \ - $(CEREAL_CFLAGS) \ - $(OPENCL_FLAGS) \ - $(LIBYUV_FLAGS) \ - $(JSON_FLAGS) \ - -I$(PHONELIBS)/libgralloc/include \ - -I$(PHONELIBS)/linux/include \ - -c -o '$@' '$<' - -.PHONY: clean -clean: - rm -f visiond rgb_to_yuv_test rgb_to_yuv_test.o $(OBJS) $(DEPS) - --include $(DEPS) diff --git a/selfdrive/visiond/models/monitoring.cc b/selfdrive/visiond/models/monitoring.cc deleted file mode 100644 index 30a6f56c1a..0000000000 --- a/selfdrive/visiond/models/monitoring.cc +++ /dev/null @@ -1,108 +0,0 @@ -#include -#include "monitoring.h" -#include "common/mat.h" -#include "common/timing.h" - -#define MODEL_WIDTH 320 -#define MODEL_HEIGHT 160 - -#define MAX_IR_POWER 0.5f -#define MIN_IR_POWER 0.0f -#define CUTOFF_GAIN 0.015625f // iso400 -#define SATURATE_GAIN 0.0625f // iso1600 - -// match driver_monitor.py -#define FACE_THRESH 0.4f -#define EYE_THRESH 0.4f - -void monitoring_init(MonitoringState* s, cl_device_id device_id, cl_context context) { - model_input_init(&s->in, MODEL_WIDTH, MODEL_HEIGHT, device_id, context); - s->m = new DefaultRunModel("../../models/monitoring_model.dlc", (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME); -} - -MonitoringResult monitoring_eval_frame(MonitoringState* s, cl_command_queue q, - cl_mem yuv_cl, int width, int height) { - const mat3 front_frame_from_scaled_frame = (mat3){{ - width/426.0f, 0.0, 0.0, - 0.0,height/320.0f, 0.0, - 0.0, 0.0, 1.0, - }}; - - const mat3 scaled_frame_from_cropped_frame = (mat3){{ - 1.0, 0.0, 426.0-160.0, - 0.0, 1.0, 0.0, - 0.0, 0.0, 1.0, - }}; - - const mat3 transpose = (mat3){{ - 0.0, 1.0, 0.0, - 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, - }}; - - const mat3 front_frame_from_cropped_frame = matmul3(front_frame_from_scaled_frame, scaled_frame_from_cropped_frame); - const mat3 front_frame_from_monitoring_frame = matmul3(front_frame_from_cropped_frame, transpose); - - float *net_input_buf = model_input_prepare(&s->in, q, yuv_cl, width, height, front_frame_from_monitoring_frame); - s->m->execute(net_input_buf); - - MonitoringResult ret = {0}; - memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation); - memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position); - memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob); - memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob); - memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob); - memcpy(&ret.left_blink_prob, &s->output[31], sizeof ret.right_eye_prob); - memcpy(&ret.right_blink_prob, &s->output[32], sizeof ret.right_eye_prob); - return ret; -} - -void monitoring_publish(PubSocket* sock, uint32_t frame_id, const MonitoringResult res, float ir_target) { - // make msg - capnp::MallocMessageBuilder msg; - cereal::Event::Builder event = msg.initRoot(); - event.setLogMonoTime(nanos_since_boot()); - - auto framed = event.initDriverMonitoring(); - framed.setFrameId(frame_id); - - kj::ArrayPtr face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation)); - kj::ArrayPtr face_position(&res.face_position[0], ARRAYSIZE(res.face_position)); - framed.setFaceOrientation(face_orientation); - framed.setFacePosition(face_position); - framed.setFaceProb(res.face_prob); - framed.setLeftEyeProb(res.left_eye_prob); - framed.setRightEyeProb(res.right_eye_prob); - framed.setLeftBlinkProb(res.left_blink_prob); - framed.setRightBlinkProb(res.right_blink_prob); - framed.setIrPwr(ir_target); - - // send message - auto words = capnp::messageToFlatArray(msg); - auto bytes = words.asBytes(); - sock->send((char*)bytes.begin(), bytes.size()); - } - -void monitoring_free(MonitoringState* s) { - model_input_free(&s->in); - delete s->m; -} - -float ir_target_set(float *cur_front_gain, const MonitoringResult res) { - bool face_detected = res.face_prob > FACE_THRESH; - bool eyes_detected = (res.left_eye_prob > EYE_THRESH) && (res.right_eye_prob > EYE_THRESH); - static float set_point = 0.5; - - if ((*cur_front_gain <= CUTOFF_GAIN) && !face_detected) { - set_point = MIN_IR_POWER; - } else if (face_detected && eyes_detected) { - if (*cur_front_gain > SATURATE_GAIN) { - set_point = MAX_IR_POWER; - } else { - set_point = MIN_IR_POWER + ((*cur_front_gain - CUTOFF_GAIN) * (MAX_IR_POWER - MIN_IR_POWER) / (SATURATE_GAIN - CUTOFF_GAIN)); - } - } else { - set_point = (set_point*1.1 > MAX_IR_POWER) ? MAX_IR_POWER : set_point*1.1; - } - return set_point; -} \ No newline at end of file diff --git a/selfdrive/visiond/snapshot/Makefile b/selfdrive/visiond/snapshot/Makefile deleted file mode 100644 index 6edde2f822..0000000000 --- a/selfdrive/visiond/snapshot/Makefile +++ /dev/null @@ -1,32 +0,0 @@ -CC = clang -CXX = clang++ - -WARN_FLAGS = -Werror=implicit-function-declaration \ - -Werror=incompatible-pointer-types \ - -Werror=int-conversion \ - -Werror=return-type \ - -Werror=format-extra-args - -CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS) - -.PHONY: all -all: libvisionipc.so - -visionipc.o: ../../common/visionipc.c ../../common/visionipc.h - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -MMD \ - -I../.. -I../../.. \ - -c -o '$@' ../../common/visionipc.c - -ipc.o: ../../common/ipc.c ../../common/ipc.h - @echo "[ CC ] $@" - $(CC) $(CFLAGS) -MMD \ - -I../.. -I../../.. \ - -c -o '$@' ../../common/ipc.c - -libvisionipc.so: visionipc.o ipc.o - $(CC) -shared -fPIC -o '$@' visionipc.o ipc.o - -.PHONY: clean -clean: - rm visionipc.o ipc.o libvisionipc.so diff --git a/selfdrive/visiond/snapshot/__init__.py b/selfdrive/visiond/snapshot/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/selfdrive/visiond/snapshot/snapshot.py b/selfdrive/visiond/snapshot/snapshot.py deleted file mode 100755 index affc635b81..0000000000 --- a/selfdrive/visiond/snapshot/snapshot.py +++ /dev/null @@ -1,52 +0,0 @@ -#!/usr/bin/env python3 -import os -import subprocess -from PIL import Image -from common.params import Params -import time -import signal - -from selfdrive.visiond.snapshot.visionipc import VisionIPC - -def jpeg_write(fn, dat): - img = Image.fromarray(dat) - img.save(fn, "JPEG") - -def snapshot(): - # note: super sketch race condition if you start the car at the same time at this - # TODO: lock car starting? - ps = subprocess.Popen("ps | grep visiond", shell=True, stdout=subprocess.PIPE) - ret = list(filter(lambda x: 'grep ' not in x, ps.communicate()[0].decode('utf-8').strip().split("\n"))) - if len(ret) > 0: - return None - - front_camera_allowed = int(Params().get("RecordFront")) - - proc = subprocess.Popen(os.path.join(os.getenv("HOME"), "one/selfdrive/visiond/start.py"), cwd=os.path.join(os.getenv("HOME"), "one/selfdrive/visiond")) - time.sleep(6.0) - - ret = None - try: - ipc = VisionIPC() - pic = ipc.get() - del ipc - - if front_camera_allowed: - ipc_front = VisionIPC(front=True) - fpic = ipc_front.get() - del ipc_front - else: - fpic = None - - ret = pic, fpic - finally: - proc.send_signal(signal.SIGINT) - proc.communicate() - - return ret - -if __name__ == "__main__": - pic, fpic = snapshot() - jpeg_write("/tmp/back.jpg", pic) - jpeg_write("/tmp/front.jpg", fpic) - diff --git a/selfdrive/visiond/start.py b/selfdrive/visiond/start.py deleted file mode 100755 index 5556ac2997..0000000000 --- a/selfdrive/visiond/start.py +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python3 -import os - -assert os.system("make") == 0 -from common.basedir import BASEDIR - -os.environ["ADSP_LIBRARY_PATH"] = os.path.join(BASEDIR, "selfdrive/visiond/dsp") -os.execv("./visiond", ["visiond"])