mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-09-14 19:43:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'devel-staging' of https://github.com/commaai/openpilot into devel

Browse Source
This commit is contained in:
dragonpilot
2020-02-18 12:29:25 +10:00
parent 89dfdd961f 13fb670ba0
commit 74dbd89e3e
94 changed files with 3175 additions and 1472 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/params.py
+2
View File
@@ -81,6 +81,7 @@ keys = {
"LiveParameters": [TxType.PERSISTENT],
"LongitudinalControl": [TxType.PERSISTENT],
"OpenpilotEnabledToggle": [TxType.PERSISTENT],
"LaneChangeEnabled": [TxType.PERSISTENT],
"PandaFirmware": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
"PandaFirmwareHex": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
"PandaDongleId": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
@@ -93,6 +94,7 @@ keys = {
"TermsVersion": [TxType.PERSISTENT],
"TrainingVersion": [TxType.PERSISTENT],
"UpdateAvailable": [TxType.CLEAR_ON_MANAGER_START],
"UpdateFailedCount": [TxType.CLEAR_ON_MANAGER_START],
"Version": [TxType.PERSISTENT],
"Offroad_ChargeDisabled": [TxType.CLEAR_ON_MANAGER_START, TxType.CLEAR_ON_PANDA_DISCONNECT],
"Offroad_ConnectivityNeeded": [TxType.CLEAR_ON_MANAGER_START],
common/sympy_helpers.py
-81
View File
@@ -1,81 +0,0 @@
#!/usr/bin/env python3
import sympy as sp
import numpy as np
def cross(x):
ret = sp.Matrix(np.zeros((3,3)))
ret[0,1], ret[0,2] = -x[2], x[1]
ret[1,0], ret[1,2] = x[2], -x[0]
ret[2,0], ret[2,1] = -x[1], x[0]
return ret
def euler_rotate(roll, pitch, yaw):
# make symbolic rotation matrix from eulers
matrix_roll = sp.Matrix([[1, 0, 0],
[0, sp.cos(roll), -sp.sin(roll)],
[0, sp.sin(roll), sp.cos(roll)]])
matrix_pitch = sp.Matrix([[sp.cos(pitch), 0, sp.sin(pitch)],
[0, 1, 0],
[-sp.sin(pitch), 0, sp.cos(pitch)]])
matrix_yaw = sp.Matrix([[sp.cos(yaw), -sp.sin(yaw), 0],
[sp.sin(yaw), sp.cos(yaw), 0],
[0, 0, 1]])
return matrix_yaw*matrix_pitch*matrix_roll
def quat_rotate(q0, q1, q2, q3):
# make symbolic rotation matrix from quat
return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2*(q1*q2 + q0*q3), 2*(q1*q3 - q0*q2)],
[2*(q1*q2 - q0*q3), q0**2 - q1**2 + q2**2 - q3**2, 2*(q2*q3 + q0*q1)],
[2*(q1*q3 + q0*q2), 2*(q2*q3 - q0*q1), q0**2 - q1**2 - q2**2 + q3**2]]).T
def quat_matrix_l(p):
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
[p[1], p[0], -p[3], p[2]],
[p[2], p[3], p[0], -p[1]],
[p[3], -p[2], p[1], p[0]]])
def quat_matrix_r(p):
return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
[p[1], p[0], p[3], -p[2]],
[p[2], -p[3], p[0], p[1]],
[p[3], p[2], -p[1], p[0]]])
def sympy_into_c(sympy_functions):
from sympy.utilities import codegen
routines = []
for name, expr, args in sympy_functions:
r = codegen.make_routine(name, expr, language="C99")
# argument ordering input to sympy is broken with function with output arguments
nargs = []
# reorder the input arguments
for aa in args:
if aa is None:
nargs.append(codegen.InputArgument(sp.Symbol('unused'), dimensions=[1,1]))
continue
found = False
for a in r.arguments:
if str(aa.name) == str(a.name):
nargs.append(a)
found = True
break
if not found:
# [1,1] is a hack for Matrices
nargs.append(codegen.InputArgument(aa, dimensions=[1,1]))
# add the output arguments
for a in r.arguments:
if type(a) == codegen.OutputArgument:
nargs.append(a)
#assert len(r.arguments) == len(args)+1
r.arguments = nargs
# add routine to list
routines.append(r)
[(c_name, c_code), (h_name, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf")
c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#')
c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
return c_header, c_code