mirror of
https://github.com/sunnypilot/sunnypilot.git
synced 2026-07-21 19:02:12 +08:00
Compare commits
2 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
 github-actions[bot]
|
7207274595 | ||
|
github-actions[bot]
|
2f96614632 |
@@ -1,3 +1,20 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
IFS=$'\n\t'
|
||||
|
||||
# On any failure, run the fallback launcher
|
||||
trap 'exec ./launch_chffrplus.sh' ERR
|
||||
C3_LAUNCH_SH="./openpilot/sunnypilot/system/hardware/c3/launch_chffrplus.sh"
|
||||
|
||||
MODEL="$(tr -d '\0' < "/sys/firmware/devicetree/base/model")"
|
||||
export MODEL
|
||||
|
||||
if [ "$MODEL" = "comma tici" ]; then
|
||||
# Force a failure if the launcher doesn't exist
|
||||
[ -x "$C3_LAUNCH_SH" ] || false
|
||||
|
||||
# If it exists, run it
|
||||
exec "$C3_LAUNCH_SH"
|
||||
fi
|
||||
|
||||
exec ./launch_chffrplus.sh
|
||||
|
||||
Binary file not shown.
@@ -0,0 +1,524 @@
|
||||
{
|
||||
"acados_include_path": "/usr/local/venv/lib/python3.12/site-packages/acados/install/include",
|
||||
"acados_lib_path": "/usr/local/venv/lib/python3.12/site-packages/acados/install/lib",
|
||||
"code_export_directory": "/data/openpilot/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/c_generated_code",
|
||||
"constraints": {
|
||||
"C": [],
|
||||
"C_e": [],
|
||||
"D": [],
|
||||
"constr_type": "BGH",
|
||||
"constr_type_e": "BGH",
|
||||
"idxbu": [],
|
||||
"idxbx": [],
|
||||
"idxbx_0": [
|
||||
0,
|
||||
1,
|
||||
2
|
||||
],
|
||||
"idxbx_e": [],
|
||||
"idxbxe_0": [
|
||||
0,
|
||||
1,
|
||||
2
|
||||
],
|
||||
"idxsbu": [],
|
||||
"idxsbx": [],
|
||||
"idxsbx_e": [],
|
||||
"idxsg": [],
|
||||
"idxsg_e": [],
|
||||
"idxsh": [
|
||||
0,
|
||||
1,
|
||||
2,
|
||||
3
|
||||
],
|
||||
"idxsh_e": [],
|
||||
"idxsphi": [],
|
||||
"idxsphi_e": [],
|
||||
"lbu": [],
|
||||
"lbx": [],
|
||||
"lbx_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"lbx_e": [],
|
||||
"lg": [],
|
||||
"lg_e": [],
|
||||
"lh": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"lh_e": [],
|
||||
"lphi": [],
|
||||
"lphi_e": [],
|
||||
"lsbu": [],
|
||||
"lsbx": [],
|
||||
"lsbx_e": [],
|
||||
"lsg": [],
|
||||
"lsg_e": [],
|
||||
"lsh": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"lsh_e": [],
|
||||
"lsphi": [],
|
||||
"lsphi_e": [],
|
||||
"ubu": [],
|
||||
"ubx": [],
|
||||
"ubx_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"ubx_e": [],
|
||||
"ug": [],
|
||||
"ug_e": [],
|
||||
"uh": [
|
||||
10000.0,
|
||||
10000.0,
|
||||
10000.0,
|
||||
10000.0
|
||||
],
|
||||
"uh_e": [],
|
||||
"uphi": [],
|
||||
"uphi_e": [],
|
||||
"usbu": [],
|
||||
"usbx": [],
|
||||
"usbx_e": [],
|
||||
"usg": [],
|
||||
"usg_e": [],
|
||||
"ush": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"ush_e": [],
|
||||
"usphi": [],
|
||||
"usphi_e": []
|
||||
},
|
||||
"cost": {
|
||||
"Vu": [],
|
||||
"Vu_0": [],
|
||||
"Vx": [],
|
||||
"Vx_0": [],
|
||||
"Vx_e": [],
|
||||
"Vz": [],
|
||||
"Vz_0": [],
|
||||
"W": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"W_0": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"W_e": [
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
]
|
||||
],
|
||||
"Zl": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"Zl_e": [],
|
||||
"Zu": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"Zu_e": [],
|
||||
"cost_ext_fun_type": "casadi",
|
||||
"cost_ext_fun_type_0": "casadi",
|
||||
"cost_ext_fun_type_e": "casadi",
|
||||
"cost_type": "NONLINEAR_LS",
|
||||
"cost_type_0": "NONLINEAR_LS",
|
||||
"cost_type_e": "NONLINEAR_LS",
|
||||
"yref": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"yref_0": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"yref_e": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zl": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zl_e": [],
|
||||
"zu": [
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
0.0
|
||||
],
|
||||
"zu_e": []
|
||||
},
|
||||
"cython_include_dirs": [
|
||||
"/usr/local/venv/lib/python3.12/site-packages/numpy/_core/include",
|
||||
"/usr/include/python3.12"
|
||||
],
|
||||
"dims": {
|
||||
"N": 12,
|
||||
"nbu": 0,
|
||||
"nbx": 0,
|
||||
"nbx_0": 3,
|
||||
"nbx_e": 0,
|
||||
"nbxe_0": 3,
|
||||
"ng": 0,
|
||||
"ng_e": 0,
|
||||
"nh": 4,
|
||||
"nh_e": 0,
|
||||
"np": 6,
|
||||
"nphi": 0,
|
||||
"nphi_e": 0,
|
||||
"nr": 0,
|
||||
"nr_e": 0,
|
||||
"ns": 4,
|
||||
"ns_e": 0,
|
||||
"nsbu": 0,
|
||||
"nsbx": 0,
|
||||
"nsbx_e": 0,
|
||||
"nsg": 0,
|
||||
"nsg_e": 0,
|
||||
"nsh": 4,
|
||||
"nsh_e": 0,
|
||||
"nsphi": 0,
|
||||
"nsphi_e": 0,
|
||||
"nu": 1,
|
||||
"nx": 3,
|
||||
"ny": 6,
|
||||
"ny_0": 6,
|
||||
"ny_e": 5,
|
||||
"nz": 0
|
||||
},
|
||||
"json_file": "/data/openpilot/openpilot/selfdrive/controls/lib/longitudinal_mpc_lib/acados_ocp_long.json",
|
||||
"model": {
|
||||
"con_h_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegiaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihchcaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgchegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhchbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaachbaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaa",
|
||||
"con_h_expr_e": null,
|
||||
"con_phi_expr": null,
|
||||
"con_phi_expr_e": null,
|
||||
"con_r_expr": null,
|
||||
"con_r_expr_e": null,
|
||||
"con_r_in_phi": null,
|
||||
"con_r_in_phi_e": null,
|
||||
"cost_conl_custom_outer_hess": null,
|
||||
"cost_conl_custom_outer_hess_0": null,
|
||||
"cost_conl_custom_outer_hess_e": null,
|
||||
"cost_expr_ext_cost": null,
|
||||
"cost_expr_ext_cost_0": null,
|
||||
"cost_expr_ext_cost_custom_hess": null,
|
||||
"cost_expr_ext_cost_custom_hess_0": null,
|
||||
"cost_expr_ext_cost_custom_hess_e": null,
|
||||
"cost_expr_ext_cost_e": null,
|
||||
"cost_psi_expr": null,
|
||||
"cost_psi_expr_0": null,
|
||||
"cost_psi_expr_e": null,
|
||||
"cost_r_in_psi_expr": null,
|
||||
"cost_r_in_psi_expr_0": null,
|
||||
"cost_r_in_psi_expr_e": null,
|
||||
"cost_y_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfgghegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
|
||||
"cost_y_expr_0": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfgghegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
|
||||
"cost_y_expr_e": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegjaaaaaaaaaaaaaaafaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegbaaaaaaaaaaaaaaaegbaaaaaaaaaaaaaaaegeaaaaaaaaaaaaaaaeglaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegmcaaaaaaaaaaaaaajgfaaaaaaaegdaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhcheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajggaaaaaaaegbaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegmcaaaaaaaaaaaaaajgkaaaaaaachcaaaaaaaaaaaaaaacheaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaachbbaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfggh",
|
||||
"disc_dyn_expr": null,
|
||||
"dyn_disc_fun": null,
|
||||
"dyn_disc_fun_jac": null,
|
||||
"dyn_disc_fun_jac_hess": null,
|
||||
"dyn_ext_fun_type": "casadi",
|
||||
"dyn_generic_source": null,
|
||||
"f_expl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
|
||||
"f_impl_expr": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpgegcaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgehegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
|
||||
"gnsf": {
|
||||
"nontrivial_f_LO": 1,
|
||||
"purely_linear": 0
|
||||
},
|
||||
"name": "long",
|
||||
"p": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegkaaaaaaaaaaaaaaagaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaeaaaaaaaaaaaaaaafaaaaaaaaaaaaaaagaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaabgpfngjgogegpcaaaaaaaaaaaaaafaaaaaaabgpfngbgihegpcaaaaaaaaaaaaaakaaaaaaaihpfpgcgdhehbgdgmgfgegpcaaaaaaaaaaaaaagaaaaaaabgpfahchfgghegpcaaaaaaaaaaaaaanaaaaaaamgfgbgegpfehpfggpgmgmgpghhegpcaaaaaaaaaaaaaacbaaaaaamgfgbgegpfegbgoghgfgchpfggbgdgehpgch",
|
||||
"u": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegfaaaaaaaaaaaaaaabaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaakgpffghgpg",
|
||||
"x": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaafaaaaaaaihpffghgpgegpcaaaaaaaaaaaaaafaaaaaaaghpffghgpgegpcaaaaaaaaaaaaaafaaaaaaabgpffghgpg",
|
||||
"xdot": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaeghaaaaaaaaaaaaaaadaaaaaaaaaaaaaaabaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaabaaaaaaaaaaaaaaacaaaaaaaaaaaaaaadaaaaaaaaaaaaaaaegpcaaaaaaaaaaaaaajaaaaaaaihpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaaghpffghgpgpfegpgehegpcaaaaaaaaaaaaaajaaaaaaabgpffghgpgpfegpgeh",
|
||||
"z": "jhpnnagiieahaaaadaaaaaaaaaaaaaaaaaegdaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa"
|
||||
},
|
||||
"parameter_values": [
|
||||
-1.2,
|
||||
1.2,
|
||||
0.0,
|
||||
0.0,
|
||||
1.45,
|
||||
0.75
|
||||
],
|
||||
"problem_class": "OCP",
|
||||
"shared_lib_ext": ".so",
|
||||
"solver_options": {
|
||||
"Tsim": 0.06944444444444445,
|
||||
"alpha_min": 0.05,
|
||||
"alpha_reduction": 0.7,
|
||||
"collocation_type": "GAUSS_LEGENDRE",
|
||||
"custom_templates": [],
|
||||
"custom_update_copy": true,
|
||||
"custom_update_filename": "",
|
||||
"custom_update_header_filename": "",
|
||||
"eps_sufficient_descent": 0.0001,
|
||||
"exact_hess_constr": 1,
|
||||
"exact_hess_cost": 1,
|
||||
"exact_hess_dyn": 1,
|
||||
"ext_cost_num_hess": 0,
|
||||
"ext_fun_compile_flags": "-O2",
|
||||
"full_step_dual": 0,
|
||||
"globalization": "FIXED_STEP",
|
||||
"globalization_use_SOC": 0,
|
||||
"hessian_approx": "GAUSS_NEWTON",
|
||||
"hpipm_mode": "BALANCE",
|
||||
"initialize_t_slacks": 0,
|
||||
"integrator_type": "ERK",
|
||||
"levenberg_marquardt": 0.0,
|
||||
"line_search_use_sufficient_descent": 0,
|
||||
"model_external_shared_lib_dir": null,
|
||||
"model_external_shared_lib_name": null,
|
||||
"nlp_solver_ext_qp_res": 0,
|
||||
"nlp_solver_max_iter": 100,
|
||||
"nlp_solver_step_length": 1.0,
|
||||
"nlp_solver_tol_comp": 1e-06,
|
||||
"nlp_solver_tol_eq": 1e-06,
|
||||
"nlp_solver_tol_ineq": 1e-06,
|
||||
"nlp_solver_tol_stat": 1e-06,
|
||||
"nlp_solver_type": "SQP_RTI",
|
||||
"print_level": 0,
|
||||
"qp_solver": "PARTIAL_CONDENSING_HPIPM",
|
||||
"qp_solver_cond_N": 1,
|
||||
"qp_solver_cond_ric_alg": 1,
|
||||
"qp_solver_iter_max": 10,
|
||||
"qp_solver_ric_alg": 1,
|
||||
"qp_solver_tol_comp": 0.001,
|
||||
"qp_solver_tol_eq": 0.001,
|
||||
"qp_solver_tol_ineq": 0.001,
|
||||
"qp_solver_tol_stat": 0.001,
|
||||
"qp_solver_warm_start": 0,
|
||||
"regularize_method": null,
|
||||
"shooting_nodes": [
|
||||
0.0,
|
||||
0.06944444444444445,
|
||||
0.2777777777777778,
|
||||
0.625,
|
||||
1.1111111111111112,
|
||||
1.7361111111111114,
|
||||
2.5,
|
||||
3.4027777777777786,
|
||||
4.444444444444445,
|
||||
5.625,
|
||||
6.9444444444444455,
|
||||
8.402777777777777,
|
||||
10.0
|
||||
],
|
||||
"sim_method_jac_reuse": [
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
0
|
||||
],
|
||||
"sim_method_newton_iter": 3,
|
||||
"sim_method_newton_tol": 0.0,
|
||||
"sim_method_num_stages": [
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4,
|
||||
4
|
||||
],
|
||||
"sim_method_num_steps": [
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1,
|
||||
1
|
||||
],
|
||||
"tf": 10.0,
|
||||
"time_steps": [
|
||||
0.06944444444444445,
|
||||
0.20833333333333334,
|
||||
0.3472222222222222,
|
||||
0.48611111111111116,
|
||||
0.6250000000000002,
|
||||
0.7638888888888886,
|
||||
0.9027777777777786,
|
||||
1.041666666666666,
|
||||
1.1805555555555554,
|
||||
1.3194444444444455,
|
||||
1.4583333333333313,
|
||||
1.5972222222222232
|
||||
]
|
||||
}
|
||||
}
|
||||
@@ -23,14 +23,13 @@ def main():
|
||||
cloudlog.info("plannerd got CarParamsSP")
|
||||
|
||||
gps_location_service = get_gps_location_service(params)
|
||||
ignore_services = ["liveMapDataSP", gps_location_service]
|
||||
|
||||
ldw = LaneDepartureWarning()
|
||||
longitudinal_planner = LongitudinalPlanner(CP, CP_SP)
|
||||
pm = messaging.PubMaster(['longitudinalPlan', 'driverAssistance', 'longitudinalPlanSP'])
|
||||
sm = messaging.SubMaster(['carControl', 'carState', 'controlsState', 'liveParameters', 'radarState', 'modelV2', 'selfdriveState',
|
||||
'liveMapDataSP', 'carStateSP', gps_location_service],
|
||||
poll='carState', ignore_alive=ignore_services, ignore_avg_freq=ignore_services, ignore_valid=ignore_services)
|
||||
poll='carState')
|
||||
|
||||
while True:
|
||||
sm.update()
|
||||
|
||||
@@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227;
|
||||
* *
|
||||
* This file is part of 'ekf' *
|
||||
******************************************************************************/
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_3010729618614624127) {
|
||||
out_3010729618614624127[0] = delta_x[0] + nom_x[0];
|
||||
out_3010729618614624127[1] = delta_x[1] + nom_x[1];
|
||||
out_3010729618614624127[2] = delta_x[2] + nom_x[2];
|
||||
out_3010729618614624127[3] = delta_x[3] + nom_x[3];
|
||||
out_3010729618614624127[4] = delta_x[4] + nom_x[4];
|
||||
out_3010729618614624127[5] = delta_x[5] + nom_x[5];
|
||||
out_3010729618614624127[6] = delta_x[6] + nom_x[6];
|
||||
out_3010729618614624127[7] = delta_x[7] + nom_x[7];
|
||||
out_3010729618614624127[8] = delta_x[8] + nom_x[8];
|
||||
void err_fun(double *nom_x, double *delta_x, double *out_4503132331689013745) {
|
||||
out_4503132331689013745[0] = delta_x[0] + nom_x[0];
|
||||
out_4503132331689013745[1] = delta_x[1] + nom_x[1];
|
||||
out_4503132331689013745[2] = delta_x[2] + nom_x[2];
|
||||
out_4503132331689013745[3] = delta_x[3] + nom_x[3];
|
||||
out_4503132331689013745[4] = delta_x[4] + nom_x[4];
|
||||
out_4503132331689013745[5] = delta_x[5] + nom_x[5];
|
||||
out_4503132331689013745[6] = delta_x[6] + nom_x[6];
|
||||
out_4503132331689013745[7] = delta_x[7] + nom_x[7];
|
||||
out_4503132331689013745[8] = delta_x[8] + nom_x[8];
|
||||
}
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_3985552318855446440) {
|
||||
out_3985552318855446440[0] = -nom_x[0] + true_x[0];
|
||||
out_3985552318855446440[1] = -nom_x[1] + true_x[1];
|
||||
out_3985552318855446440[2] = -nom_x[2] + true_x[2];
|
||||
out_3985552318855446440[3] = -nom_x[3] + true_x[3];
|
||||
out_3985552318855446440[4] = -nom_x[4] + true_x[4];
|
||||
out_3985552318855446440[5] = -nom_x[5] + true_x[5];
|
||||
out_3985552318855446440[6] = -nom_x[6] + true_x[6];
|
||||
out_3985552318855446440[7] = -nom_x[7] + true_x[7];
|
||||
out_3985552318855446440[8] = -nom_x[8] + true_x[8];
|
||||
void inv_err_fun(double *nom_x, double *true_x, double *out_5519222819843562437) {
|
||||
out_5519222819843562437[0] = -nom_x[0] + true_x[0];
|
||||
out_5519222819843562437[1] = -nom_x[1] + true_x[1];
|
||||
out_5519222819843562437[2] = -nom_x[2] + true_x[2];
|
||||
out_5519222819843562437[3] = -nom_x[3] + true_x[3];
|
||||
out_5519222819843562437[4] = -nom_x[4] + true_x[4];
|
||||
out_5519222819843562437[5] = -nom_x[5] + true_x[5];
|
||||
out_5519222819843562437[6] = -nom_x[6] + true_x[6];
|
||||
out_5519222819843562437[7] = -nom_x[7] + true_x[7];
|
||||
out_5519222819843562437[8] = -nom_x[8] + true_x[8];
|
||||
}
|
||||
void H_mod_fun(double *state, double *out_4306310304618992202) {
|
||||
out_4306310304618992202[0] = 1.0;
|
||||
out_4306310304618992202[1] = 0.0;
|
||||
out_4306310304618992202[2] = 0.0;
|
||||
out_4306310304618992202[3] = 0.0;
|
||||
out_4306310304618992202[4] = 0.0;
|
||||
out_4306310304618992202[5] = 0.0;
|
||||
out_4306310304618992202[6] = 0.0;
|
||||
out_4306310304618992202[7] = 0.0;
|
||||
out_4306310304618992202[8] = 0.0;
|
||||
out_4306310304618992202[9] = 0.0;
|
||||
out_4306310304618992202[10] = 1.0;
|
||||
out_4306310304618992202[11] = 0.0;
|
||||
out_4306310304618992202[12] = 0.0;
|
||||
out_4306310304618992202[13] = 0.0;
|
||||
out_4306310304618992202[14] = 0.0;
|
||||
out_4306310304618992202[15] = 0.0;
|
||||
out_4306310304618992202[16] = 0.0;
|
||||
out_4306310304618992202[17] = 0.0;
|
||||
out_4306310304618992202[18] = 0.0;
|
||||
out_4306310304618992202[19] = 0.0;
|
||||
out_4306310304618992202[20] = 1.0;
|
||||
out_4306310304618992202[21] = 0.0;
|
||||
out_4306310304618992202[22] = 0.0;
|
||||
out_4306310304618992202[23] = 0.0;
|
||||
out_4306310304618992202[24] = 0.0;
|
||||
out_4306310304618992202[25] = 0.0;
|
||||
out_4306310304618992202[26] = 0.0;
|
||||
out_4306310304618992202[27] = 0.0;
|
||||
out_4306310304618992202[28] = 0.0;
|
||||
out_4306310304618992202[29] = 0.0;
|
||||
out_4306310304618992202[30] = 1.0;
|
||||
out_4306310304618992202[31] = 0.0;
|
||||
out_4306310304618992202[32] = 0.0;
|
||||
out_4306310304618992202[33] = 0.0;
|
||||
out_4306310304618992202[34] = 0.0;
|
||||
out_4306310304618992202[35] = 0.0;
|
||||
out_4306310304618992202[36] = 0.0;
|
||||
out_4306310304618992202[37] = 0.0;
|
||||
out_4306310304618992202[38] = 0.0;
|
||||
out_4306310304618992202[39] = 0.0;
|
||||
out_4306310304618992202[40] = 1.0;
|
||||
out_4306310304618992202[41] = 0.0;
|
||||
out_4306310304618992202[42] = 0.0;
|
||||
out_4306310304618992202[43] = 0.0;
|
||||
out_4306310304618992202[44] = 0.0;
|
||||
out_4306310304618992202[45] = 0.0;
|
||||
out_4306310304618992202[46] = 0.0;
|
||||
out_4306310304618992202[47] = 0.0;
|
||||
out_4306310304618992202[48] = 0.0;
|
||||
out_4306310304618992202[49] = 0.0;
|
||||
out_4306310304618992202[50] = 1.0;
|
||||
out_4306310304618992202[51] = 0.0;
|
||||
out_4306310304618992202[52] = 0.0;
|
||||
out_4306310304618992202[53] = 0.0;
|
||||
out_4306310304618992202[54] = 0.0;
|
||||
out_4306310304618992202[55] = 0.0;
|
||||
out_4306310304618992202[56] = 0.0;
|
||||
out_4306310304618992202[57] = 0.0;
|
||||
out_4306310304618992202[58] = 0.0;
|
||||
out_4306310304618992202[59] = 0.0;
|
||||
out_4306310304618992202[60] = 1.0;
|
||||
out_4306310304618992202[61] = 0.0;
|
||||
out_4306310304618992202[62] = 0.0;
|
||||
out_4306310304618992202[63] = 0.0;
|
||||
out_4306310304618992202[64] = 0.0;
|
||||
out_4306310304618992202[65] = 0.0;
|
||||
out_4306310304618992202[66] = 0.0;
|
||||
out_4306310304618992202[67] = 0.0;
|
||||
out_4306310304618992202[68] = 0.0;
|
||||
out_4306310304618992202[69] = 0.0;
|
||||
out_4306310304618992202[70] = 1.0;
|
||||
out_4306310304618992202[71] = 0.0;
|
||||
out_4306310304618992202[72] = 0.0;
|
||||
out_4306310304618992202[73] = 0.0;
|
||||
out_4306310304618992202[74] = 0.0;
|
||||
out_4306310304618992202[75] = 0.0;
|
||||
out_4306310304618992202[76] = 0.0;
|
||||
out_4306310304618992202[77] = 0.0;
|
||||
out_4306310304618992202[78] = 0.0;
|
||||
out_4306310304618992202[79] = 0.0;
|
||||
out_4306310304618992202[80] = 1.0;
|
||||
void H_mod_fun(double *state, double *out_99880963271144866) {
|
||||
out_99880963271144866[0] = 1.0;
|
||||
out_99880963271144866[1] = 0.0;
|
||||
out_99880963271144866[2] = 0.0;
|
||||
out_99880963271144866[3] = 0.0;
|
||||
out_99880963271144866[4] = 0.0;
|
||||
out_99880963271144866[5] = 0.0;
|
||||
out_99880963271144866[6] = 0.0;
|
||||
out_99880963271144866[7] = 0.0;
|
||||
out_99880963271144866[8] = 0.0;
|
||||
out_99880963271144866[9] = 0.0;
|
||||
out_99880963271144866[10] = 1.0;
|
||||
out_99880963271144866[11] = 0.0;
|
||||
out_99880963271144866[12] = 0.0;
|
||||
out_99880963271144866[13] = 0.0;
|
||||
out_99880963271144866[14] = 0.0;
|
||||
out_99880963271144866[15] = 0.0;
|
||||
out_99880963271144866[16] = 0.0;
|
||||
out_99880963271144866[17] = 0.0;
|
||||
out_99880963271144866[18] = 0.0;
|
||||
out_99880963271144866[19] = 0.0;
|
||||
out_99880963271144866[20] = 1.0;
|
||||
out_99880963271144866[21] = 0.0;
|
||||
out_99880963271144866[22] = 0.0;
|
||||
out_99880963271144866[23] = 0.0;
|
||||
out_99880963271144866[24] = 0.0;
|
||||
out_99880963271144866[25] = 0.0;
|
||||
out_99880963271144866[26] = 0.0;
|
||||
out_99880963271144866[27] = 0.0;
|
||||
out_99880963271144866[28] = 0.0;
|
||||
out_99880963271144866[29] = 0.0;
|
||||
out_99880963271144866[30] = 1.0;
|
||||
out_99880963271144866[31] = 0.0;
|
||||
out_99880963271144866[32] = 0.0;
|
||||
out_99880963271144866[33] = 0.0;
|
||||
out_99880963271144866[34] = 0.0;
|
||||
out_99880963271144866[35] = 0.0;
|
||||
out_99880963271144866[36] = 0.0;
|
||||
out_99880963271144866[37] = 0.0;
|
||||
out_99880963271144866[38] = 0.0;
|
||||
out_99880963271144866[39] = 0.0;
|
||||
out_99880963271144866[40] = 1.0;
|
||||
out_99880963271144866[41] = 0.0;
|
||||
out_99880963271144866[42] = 0.0;
|
||||
out_99880963271144866[43] = 0.0;
|
||||
out_99880963271144866[44] = 0.0;
|
||||
out_99880963271144866[45] = 0.0;
|
||||
out_99880963271144866[46] = 0.0;
|
||||
out_99880963271144866[47] = 0.0;
|
||||
out_99880963271144866[48] = 0.0;
|
||||
out_99880963271144866[49] = 0.0;
|
||||
out_99880963271144866[50] = 1.0;
|
||||
out_99880963271144866[51] = 0.0;
|
||||
out_99880963271144866[52] = 0.0;
|
||||
out_99880963271144866[53] = 0.0;
|
||||
out_99880963271144866[54] = 0.0;
|
||||
out_99880963271144866[55] = 0.0;
|
||||
out_99880963271144866[56] = 0.0;
|
||||
out_99880963271144866[57] = 0.0;
|
||||
out_99880963271144866[58] = 0.0;
|
||||
out_99880963271144866[59] = 0.0;
|
||||
out_99880963271144866[60] = 1.0;
|
||||
out_99880963271144866[61] = 0.0;
|
||||
out_99880963271144866[62] = 0.0;
|
||||
out_99880963271144866[63] = 0.0;
|
||||
out_99880963271144866[64] = 0.0;
|
||||
out_99880963271144866[65] = 0.0;
|
||||
out_99880963271144866[66] = 0.0;
|
||||
out_99880963271144866[67] = 0.0;
|
||||
out_99880963271144866[68] = 0.0;
|
||||
out_99880963271144866[69] = 0.0;
|
||||
out_99880963271144866[70] = 1.0;
|
||||
out_99880963271144866[71] = 0.0;
|
||||
out_99880963271144866[72] = 0.0;
|
||||
out_99880963271144866[73] = 0.0;
|
||||
out_99880963271144866[74] = 0.0;
|
||||
out_99880963271144866[75] = 0.0;
|
||||
out_99880963271144866[76] = 0.0;
|
||||
out_99880963271144866[77] = 0.0;
|
||||
out_99880963271144866[78] = 0.0;
|
||||
out_99880963271144866[79] = 0.0;
|
||||
out_99880963271144866[80] = 1.0;
|
||||
}
|
||||
void f_fun(double *state, double dt, double *out_841039768515563673) {
|
||||
out_841039768515563673[0] = state[0];
|
||||
out_841039768515563673[1] = state[1];
|
||||
out_841039768515563673[2] = state[2];
|
||||
out_841039768515563673[3] = state[3];
|
||||
out_841039768515563673[4] = state[4];
|
||||
out_841039768515563673[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_841039768515563673[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_841039768515563673[7] = state[7];
|
||||
out_841039768515563673[8] = state[8];
|
||||
void f_fun(double *state, double dt, double *out_4490574509847198287) {
|
||||
out_4490574509847198287[0] = state[0];
|
||||
out_4490574509847198287[1] = state[1];
|
||||
out_4490574509847198287[2] = state[2];
|
||||
out_4490574509847198287[3] = state[3];
|
||||
out_4490574509847198287[4] = state[4];
|
||||
out_4490574509847198287[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5];
|
||||
out_4490574509847198287[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6];
|
||||
out_4490574509847198287[7] = state[7];
|
||||
out_4490574509847198287[8] = state[8];
|
||||
}
|
||||
void F_fun(double *state, double dt, double *out_3325406224632462615) {
|
||||
out_3325406224632462615[0] = 1;
|
||||
out_3325406224632462615[1] = 0;
|
||||
out_3325406224632462615[2] = 0;
|
||||
out_3325406224632462615[3] = 0;
|
||||
out_3325406224632462615[4] = 0;
|
||||
out_3325406224632462615[5] = 0;
|
||||
out_3325406224632462615[6] = 0;
|
||||
out_3325406224632462615[7] = 0;
|
||||
out_3325406224632462615[8] = 0;
|
||||
out_3325406224632462615[9] = 0;
|
||||
out_3325406224632462615[10] = 1;
|
||||
out_3325406224632462615[11] = 0;
|
||||
out_3325406224632462615[12] = 0;
|
||||
out_3325406224632462615[13] = 0;
|
||||
out_3325406224632462615[14] = 0;
|
||||
out_3325406224632462615[15] = 0;
|
||||
out_3325406224632462615[16] = 0;
|
||||
out_3325406224632462615[17] = 0;
|
||||
out_3325406224632462615[18] = 0;
|
||||
out_3325406224632462615[19] = 0;
|
||||
out_3325406224632462615[20] = 1;
|
||||
out_3325406224632462615[21] = 0;
|
||||
out_3325406224632462615[22] = 0;
|
||||
out_3325406224632462615[23] = 0;
|
||||
out_3325406224632462615[24] = 0;
|
||||
out_3325406224632462615[25] = 0;
|
||||
out_3325406224632462615[26] = 0;
|
||||
out_3325406224632462615[27] = 0;
|
||||
out_3325406224632462615[28] = 0;
|
||||
out_3325406224632462615[29] = 0;
|
||||
out_3325406224632462615[30] = 1;
|
||||
out_3325406224632462615[31] = 0;
|
||||
out_3325406224632462615[32] = 0;
|
||||
out_3325406224632462615[33] = 0;
|
||||
out_3325406224632462615[34] = 0;
|
||||
out_3325406224632462615[35] = 0;
|
||||
out_3325406224632462615[36] = 0;
|
||||
out_3325406224632462615[37] = 0;
|
||||
out_3325406224632462615[38] = 0;
|
||||
out_3325406224632462615[39] = 0;
|
||||
out_3325406224632462615[40] = 1;
|
||||
out_3325406224632462615[41] = 0;
|
||||
out_3325406224632462615[42] = 0;
|
||||
out_3325406224632462615[43] = 0;
|
||||
out_3325406224632462615[44] = 0;
|
||||
out_3325406224632462615[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_3325406224632462615[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_3325406224632462615[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3325406224632462615[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3325406224632462615[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_3325406224632462615[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_3325406224632462615[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_3325406224632462615[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_3325406224632462615[53] = -9.8100000000000005*dt;
|
||||
out_3325406224632462615[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_3325406224632462615[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_3325406224632462615[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3325406224632462615[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3325406224632462615[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_3325406224632462615[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_3325406224632462615[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_3325406224632462615[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_3325406224632462615[62] = 0;
|
||||
out_3325406224632462615[63] = 0;
|
||||
out_3325406224632462615[64] = 0;
|
||||
out_3325406224632462615[65] = 0;
|
||||
out_3325406224632462615[66] = 0;
|
||||
out_3325406224632462615[67] = 0;
|
||||
out_3325406224632462615[68] = 0;
|
||||
out_3325406224632462615[69] = 0;
|
||||
out_3325406224632462615[70] = 1;
|
||||
out_3325406224632462615[71] = 0;
|
||||
out_3325406224632462615[72] = 0;
|
||||
out_3325406224632462615[73] = 0;
|
||||
out_3325406224632462615[74] = 0;
|
||||
out_3325406224632462615[75] = 0;
|
||||
out_3325406224632462615[76] = 0;
|
||||
out_3325406224632462615[77] = 0;
|
||||
out_3325406224632462615[78] = 0;
|
||||
out_3325406224632462615[79] = 0;
|
||||
out_3325406224632462615[80] = 1;
|
||||
void F_fun(double *state, double dt, double *out_5588273031121660281) {
|
||||
out_5588273031121660281[0] = 1;
|
||||
out_5588273031121660281[1] = 0;
|
||||
out_5588273031121660281[2] = 0;
|
||||
out_5588273031121660281[3] = 0;
|
||||
out_5588273031121660281[4] = 0;
|
||||
out_5588273031121660281[5] = 0;
|
||||
out_5588273031121660281[6] = 0;
|
||||
out_5588273031121660281[7] = 0;
|
||||
out_5588273031121660281[8] = 0;
|
||||
out_5588273031121660281[9] = 0;
|
||||
out_5588273031121660281[10] = 1;
|
||||
out_5588273031121660281[11] = 0;
|
||||
out_5588273031121660281[12] = 0;
|
||||
out_5588273031121660281[13] = 0;
|
||||
out_5588273031121660281[14] = 0;
|
||||
out_5588273031121660281[15] = 0;
|
||||
out_5588273031121660281[16] = 0;
|
||||
out_5588273031121660281[17] = 0;
|
||||
out_5588273031121660281[18] = 0;
|
||||
out_5588273031121660281[19] = 0;
|
||||
out_5588273031121660281[20] = 1;
|
||||
out_5588273031121660281[21] = 0;
|
||||
out_5588273031121660281[22] = 0;
|
||||
out_5588273031121660281[23] = 0;
|
||||
out_5588273031121660281[24] = 0;
|
||||
out_5588273031121660281[25] = 0;
|
||||
out_5588273031121660281[26] = 0;
|
||||
out_5588273031121660281[27] = 0;
|
||||
out_5588273031121660281[28] = 0;
|
||||
out_5588273031121660281[29] = 0;
|
||||
out_5588273031121660281[30] = 1;
|
||||
out_5588273031121660281[31] = 0;
|
||||
out_5588273031121660281[32] = 0;
|
||||
out_5588273031121660281[33] = 0;
|
||||
out_5588273031121660281[34] = 0;
|
||||
out_5588273031121660281[35] = 0;
|
||||
out_5588273031121660281[36] = 0;
|
||||
out_5588273031121660281[37] = 0;
|
||||
out_5588273031121660281[38] = 0;
|
||||
out_5588273031121660281[39] = 0;
|
||||
out_5588273031121660281[40] = 1;
|
||||
out_5588273031121660281[41] = 0;
|
||||
out_5588273031121660281[42] = 0;
|
||||
out_5588273031121660281[43] = 0;
|
||||
out_5588273031121660281[44] = 0;
|
||||
out_5588273031121660281[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4]));
|
||||
out_5588273031121660281[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2));
|
||||
out_5588273031121660281[47] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5588273031121660281[48] = -dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5588273031121660281[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2)));
|
||||
out_5588273031121660281[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1;
|
||||
out_5588273031121660281[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]));
|
||||
out_5588273031121660281[52] = dt*stiffness_front*state[0]/(mass*state[1]);
|
||||
out_5588273031121660281[53] = -9.8100000000000005*dt;
|
||||
out_5588273031121660281[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4]));
|
||||
out_5588273031121660281[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2));
|
||||
out_5588273031121660281[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5588273031121660281[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5588273031121660281[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2)));
|
||||
out_5588273031121660281[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]);
|
||||
out_5588273031121660281[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1;
|
||||
out_5588273031121660281[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]);
|
||||
out_5588273031121660281[62] = 0;
|
||||
out_5588273031121660281[63] = 0;
|
||||
out_5588273031121660281[64] = 0;
|
||||
out_5588273031121660281[65] = 0;
|
||||
out_5588273031121660281[66] = 0;
|
||||
out_5588273031121660281[67] = 0;
|
||||
out_5588273031121660281[68] = 0;
|
||||
out_5588273031121660281[69] = 0;
|
||||
out_5588273031121660281[70] = 1;
|
||||
out_5588273031121660281[71] = 0;
|
||||
out_5588273031121660281[72] = 0;
|
||||
out_5588273031121660281[73] = 0;
|
||||
out_5588273031121660281[74] = 0;
|
||||
out_5588273031121660281[75] = 0;
|
||||
out_5588273031121660281[76] = 0;
|
||||
out_5588273031121660281[77] = 0;
|
||||
out_5588273031121660281[78] = 0;
|
||||
out_5588273031121660281[79] = 0;
|
||||
out_5588273031121660281[80] = 1;
|
||||
}
|
||||
void h_25(double *state, double *unused, double *out_8501492422434664386) {
|
||||
out_8501492422434664386[0] = state[6];
|
||||
void h_25(double *state, double *unused, double *out_4633670035125740311) {
|
||||
out_4633670035125740311[0] = state[6];
|
||||
}
|
||||
void H_25(double *state, double *unused, double *out_7217655417465113750) {
|
||||
out_7217655417465113750[0] = 0;
|
||||
out_7217655417465113750[1] = 0;
|
||||
out_7217655417465113750[2] = 0;
|
||||
out_7217655417465113750[3] = 0;
|
||||
out_7217655417465113750[4] = 0;
|
||||
out_7217655417465113750[5] = 0;
|
||||
out_7217655417465113750[6] = 1;
|
||||
out_7217655417465113750[7] = 0;
|
||||
out_7217655417465113750[8] = 0;
|
||||
void H_25(double *state, double *unused, double *out_9153674884526169513) {
|
||||
out_9153674884526169513[0] = 0;
|
||||
out_9153674884526169513[1] = 0;
|
||||
out_9153674884526169513[2] = 0;
|
||||
out_9153674884526169513[3] = 0;
|
||||
out_9153674884526169513[4] = 0;
|
||||
out_9153674884526169513[5] = 0;
|
||||
out_9153674884526169513[6] = 1;
|
||||
out_9153674884526169513[7] = 0;
|
||||
out_9153674884526169513[8] = 0;
|
||||
}
|
||||
void h_24(double *state, double *unused, double *out_2528371558444461081) {
|
||||
out_2528371558444461081[0] = state[4];
|
||||
out_2528371558444461081[1] = state[5];
|
||||
void h_24(double *state, double *unused, double *out_4147879785077922233) {
|
||||
out_4147879785077922233[0] = state[4];
|
||||
out_4147879785077922233[1] = state[5];
|
||||
}
|
||||
void H_24(double *state, double *unused, double *out_7688112899508452474) {
|
||||
out_7688112899508452474[0] = 0;
|
||||
out_7688112899508452474[1] = 0;
|
||||
out_7688112899508452474[2] = 0;
|
||||
out_7688112899508452474[3] = 0;
|
||||
out_7688112899508452474[4] = 1;
|
||||
out_7688112899508452474[5] = 0;
|
||||
out_7688112899508452474[6] = 0;
|
||||
out_7688112899508452474[7] = 0;
|
||||
out_7688112899508452474[8] = 0;
|
||||
out_7688112899508452474[9] = 0;
|
||||
out_7688112899508452474[10] = 0;
|
||||
out_7688112899508452474[11] = 0;
|
||||
out_7688112899508452474[12] = 0;
|
||||
out_7688112899508452474[13] = 0;
|
||||
out_7688112899508452474[14] = 1;
|
||||
out_7688112899508452474[15] = 0;
|
||||
out_7688112899508452474[16] = 0;
|
||||
out_7688112899508452474[17] = 0;
|
||||
void H_24(double *state, double *unused, double *out_7067361405204513541) {
|
||||
out_7067361405204513541[0] = 0;
|
||||
out_7067361405204513541[1] = 0;
|
||||
out_7067361405204513541[2] = 0;
|
||||
out_7067361405204513541[3] = 0;
|
||||
out_7067361405204513541[4] = 1;
|
||||
out_7067361405204513541[5] = 0;
|
||||
out_7067361405204513541[6] = 0;
|
||||
out_7067361405204513541[7] = 0;
|
||||
out_7067361405204513541[8] = 0;
|
||||
out_7067361405204513541[9] = 0;
|
||||
out_7067361405204513541[10] = 0;
|
||||
out_7067361405204513541[11] = 0;
|
||||
out_7067361405204513541[12] = 0;
|
||||
out_7067361405204513541[13] = 0;
|
||||
out_7067361405204513541[14] = 1;
|
||||
out_7067361405204513541[15] = 0;
|
||||
out_7067361405204513541[16] = 0;
|
||||
out_7067361405204513541[17] = 0;
|
||||
}
|
||||
void h_30(double *state, double *unused, double *out_8776686484719170275) {
|
||||
out_8776686484719170275[0] = state[4];
|
||||
void h_30(double *state, double *unused, double *out_8271119563379762661) {
|
||||
out_8271119563379762661[0] = state[4];
|
||||
}
|
||||
void H_30(double *state, double *unused, double *out_6701392326116829668) {
|
||||
out_6701392326116829668[0] = 0;
|
||||
out_6701392326116829668[1] = 0;
|
||||
out_6701392326116829668[2] = 0;
|
||||
out_6701392326116829668[3] = 0;
|
||||
out_6701392326116829668[4] = 1;
|
||||
out_6701392326116829668[5] = 0;
|
||||
out_6701392326116829668[6] = 0;
|
||||
out_6701392326116829668[7] = 0;
|
||||
out_6701392326116829668[8] = 0;
|
||||
void H_30(double *state, double *unused, double *out_2376378847691765348) {
|
||||
out_2376378847691765348[0] = 0;
|
||||
out_2376378847691765348[1] = 0;
|
||||
out_2376378847691765348[2] = 0;
|
||||
out_2376378847691765348[3] = 0;
|
||||
out_2376378847691765348[4] = 1;
|
||||
out_2376378847691765348[5] = 0;
|
||||
out_2376378847691765348[6] = 0;
|
||||
out_2376378847691765348[7] = 0;
|
||||
out_2376378847691765348[8] = 0;
|
||||
}
|
||||
void h_26(double *state, double *unused, double *out_8173370600042692758) {
|
||||
out_8173370600042692758[0] = state[7];
|
||||
void h_26(double *state, double *unused, double *out_749423849060713781) {
|
||||
out_749423849060713781[0] = state[7];
|
||||
}
|
||||
void H_26(double *state, double *unused, double *out_7487585337370381642) {
|
||||
out_7487585337370381642[0] = 0;
|
||||
out_7487585337370381642[1] = 0;
|
||||
out_7487585337370381642[2] = 0;
|
||||
out_7487585337370381642[3] = 0;
|
||||
out_7487585337370381642[4] = 0;
|
||||
out_7487585337370381642[5] = 0;
|
||||
out_7487585337370381642[6] = 0;
|
||||
out_7487585337370381642[7] = 1;
|
||||
out_7487585337370381642[8] = 0;
|
||||
void H_26(double *state, double *unused, double *out_5412171565652113289) {
|
||||
out_5412171565652113289[0] = 0;
|
||||
out_5412171565652113289[1] = 0;
|
||||
out_5412171565652113289[2] = 0;
|
||||
out_5412171565652113289[3] = 0;
|
||||
out_5412171565652113289[4] = 0;
|
||||
out_5412171565652113289[5] = 0;
|
||||
out_5412171565652113289[6] = 0;
|
||||
out_5412171565652113289[7] = 1;
|
||||
out_5412171565652113289[8] = 0;
|
||||
}
|
||||
void h_27(double *state, double *unused, double *out_4943578383614041585) {
|
||||
out_4943578383614041585[0] = state[3];
|
||||
void h_27(double *state, double *unused, double *out_2801473399302960502) {
|
||||
out_2801473399302960502[0] = state[3];
|
||||
}
|
||||
void H_27(double *state, double *unused, double *out_8924986397300772885) {
|
||||
out_8924986397300772885[0] = 0;
|
||||
out_8924986397300772885[1] = 0;
|
||||
out_8924986397300772885[2] = 0;
|
||||
out_8924986397300772885[3] = 1;
|
||||
out_8924986397300772885[4] = 0;
|
||||
out_8924986397300772885[5] = 0;
|
||||
out_8924986397300772885[6] = 0;
|
||||
out_8924986397300772885[7] = 0;
|
||||
out_8924986397300772885[8] = 0;
|
||||
void H_27(double *state, double *unused, double *out_6849572625582504532) {
|
||||
out_6849572625582504532[0] = 0;
|
||||
out_6849572625582504532[1] = 0;
|
||||
out_6849572625582504532[2] = 0;
|
||||
out_6849572625582504532[3] = 1;
|
||||
out_6849572625582504532[4] = 0;
|
||||
out_6849572625582504532[5] = 0;
|
||||
out_6849572625582504532[6] = 0;
|
||||
out_6849572625582504532[7] = 0;
|
||||
out_6849572625582504532[8] = 0;
|
||||
}
|
||||
void h_29(double *state, double *unused, double *out_4668384321329535696) {
|
||||
out_4668384321329535696[0] = state[1];
|
||||
void h_29(double *state, double *unused, double *out_5173951242668943310) {
|
||||
out_5173951242668943310[0] = state[1];
|
||||
}
|
||||
void H_29(double *state, double *unused, double *out_7211623670431221852) {
|
||||
out_7211623670431221852[0] = 0;
|
||||
out_7211623670431221852[1] = 1;
|
||||
out_7211623670431221852[2] = 0;
|
||||
out_7211623670431221852[3] = 0;
|
||||
out_7211623670431221852[4] = 0;
|
||||
out_7211623670431221852[5] = 0;
|
||||
out_7211623670431221852[6] = 0;
|
||||
out_7211623670431221852[7] = 0;
|
||||
out_7211623670431221852[8] = 0;
|
||||
void H_29(double *state, double *unused, double *out_8912176792012229989) {
|
||||
out_8912176792012229989[0] = 0;
|
||||
out_8912176792012229989[1] = 1;
|
||||
out_8912176792012229989[2] = 0;
|
||||
out_8912176792012229989[3] = 0;
|
||||
out_8912176792012229989[4] = 0;
|
||||
out_8912176792012229989[5] = 0;
|
||||
out_8912176792012229989[6] = 0;
|
||||
out_8912176792012229989[7] = 0;
|
||||
out_8912176792012229989[8] = 0;
|
||||
}
|
||||
void h_28(double *state, double *unused, double *out_6247021973002058322) {
|
||||
out_6247021973002058322[0] = state[0];
|
||||
void h_28(double *state, double *unused, double *out_5994836064963036638) {
|
||||
out_5994836064963036638[0] = state[0];
|
||||
}
|
||||
void H_28(double *state, double *unused, double *out_9175253941996548103) {
|
||||
out_9175253941996548103[0] = 1;
|
||||
out_9175253941996548103[1] = 0;
|
||||
out_9175253941996548103[2] = 0;
|
||||
out_9175253941996548103[3] = 0;
|
||||
out_9175253941996548103[4] = 0;
|
||||
out_9175253941996548103[5] = 0;
|
||||
out_9175253941996548103[6] = 0;
|
||||
out_9175253941996548103[7] = 0;
|
||||
out_9175253941996548103[8] = 0;
|
||||
void H_28(double *state, double *unused, double *out_6948546520446903738) {
|
||||
out_6948546520446903738[0] = 1;
|
||||
out_6948546520446903738[1] = 0;
|
||||
out_6948546520446903738[2] = 0;
|
||||
out_6948546520446903738[3] = 0;
|
||||
out_6948546520446903738[4] = 0;
|
||||
out_6948546520446903738[5] = 0;
|
||||
out_6948546520446903738[6] = 0;
|
||||
out_6948546520446903738[7] = 0;
|
||||
out_6948546520446903738[8] = 0;
|
||||
}
|
||||
void h_31(double *state, double *unused, double *out_1448590970011147238) {
|
||||
out_1448590970011147238[0] = state[8];
|
||||
void h_31(double *state, double *unused, double *out_6352515274334922980) {
|
||||
out_6352515274334922980[0] = state[8];
|
||||
}
|
||||
void H_31(double *state, double *unused, double *out_7187009455588153322) {
|
||||
out_7187009455588153322[0] = 0;
|
||||
out_7187009455588153322[1] = 0;
|
||||
out_7187009455588153322[2] = 0;
|
||||
out_7187009455588153322[3] = 0;
|
||||
out_7187009455588153322[4] = 0;
|
||||
out_7187009455588153322[5] = 0;
|
||||
out_7187009455588153322[6] = 0;
|
||||
out_7187009455588153322[7] = 0;
|
||||
out_7187009455588153322[8] = 1;
|
||||
void H_31(double *state, double *unused, double *out_9184320846403129941) {
|
||||
out_9184320846403129941[0] = 0;
|
||||
out_9184320846403129941[1] = 0;
|
||||
out_9184320846403129941[2] = 0;
|
||||
out_9184320846403129941[3] = 0;
|
||||
out_9184320846403129941[4] = 0;
|
||||
out_9184320846403129941[5] = 0;
|
||||
out_9184320846403129941[6] = 0;
|
||||
out_9184320846403129941[7] = 0;
|
||||
out_9184320846403129941[8] = 1;
|
||||
}
|
||||
#include <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
@@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) {
|
||||
update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31);
|
||||
}
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_3010729618614624127) {
|
||||
err_fun(nom_x, delta_x, out_3010729618614624127);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_4503132331689013745) {
|
||||
err_fun(nom_x, delta_x, out_4503132331689013745);
|
||||
}
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3985552318855446440) {
|
||||
inv_err_fun(nom_x, true_x, out_3985552318855446440);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5519222819843562437) {
|
||||
inv_err_fun(nom_x, true_x, out_5519222819843562437);
|
||||
}
|
||||
void car_H_mod_fun(double *state, double *out_4306310304618992202) {
|
||||
H_mod_fun(state, out_4306310304618992202);
|
||||
void car_H_mod_fun(double *state, double *out_99880963271144866) {
|
||||
H_mod_fun(state, out_99880963271144866);
|
||||
}
|
||||
void car_f_fun(double *state, double dt, double *out_841039768515563673) {
|
||||
f_fun(state, dt, out_841039768515563673);
|
||||
void car_f_fun(double *state, double dt, double *out_4490574509847198287) {
|
||||
f_fun(state, dt, out_4490574509847198287);
|
||||
}
|
||||
void car_F_fun(double *state, double dt, double *out_3325406224632462615) {
|
||||
F_fun(state, dt, out_3325406224632462615);
|
||||
void car_F_fun(double *state, double dt, double *out_5588273031121660281) {
|
||||
F_fun(state, dt, out_5588273031121660281);
|
||||
}
|
||||
void car_h_25(double *state, double *unused, double *out_8501492422434664386) {
|
||||
h_25(state, unused, out_8501492422434664386);
|
||||
void car_h_25(double *state, double *unused, double *out_4633670035125740311) {
|
||||
h_25(state, unused, out_4633670035125740311);
|
||||
}
|
||||
void car_H_25(double *state, double *unused, double *out_7217655417465113750) {
|
||||
H_25(state, unused, out_7217655417465113750);
|
||||
void car_H_25(double *state, double *unused, double *out_9153674884526169513) {
|
||||
H_25(state, unused, out_9153674884526169513);
|
||||
}
|
||||
void car_h_24(double *state, double *unused, double *out_2528371558444461081) {
|
||||
h_24(state, unused, out_2528371558444461081);
|
||||
void car_h_24(double *state, double *unused, double *out_4147879785077922233) {
|
||||
h_24(state, unused, out_4147879785077922233);
|
||||
}
|
||||
void car_H_24(double *state, double *unused, double *out_7688112899508452474) {
|
||||
H_24(state, unused, out_7688112899508452474);
|
||||
void car_H_24(double *state, double *unused, double *out_7067361405204513541) {
|
||||
H_24(state, unused, out_7067361405204513541);
|
||||
}
|
||||
void car_h_30(double *state, double *unused, double *out_8776686484719170275) {
|
||||
h_30(state, unused, out_8776686484719170275);
|
||||
void car_h_30(double *state, double *unused, double *out_8271119563379762661) {
|
||||
h_30(state, unused, out_8271119563379762661);
|
||||
}
|
||||
void car_H_30(double *state, double *unused, double *out_6701392326116829668) {
|
||||
H_30(state, unused, out_6701392326116829668);
|
||||
void car_H_30(double *state, double *unused, double *out_2376378847691765348) {
|
||||
H_30(state, unused, out_2376378847691765348);
|
||||
}
|
||||
void car_h_26(double *state, double *unused, double *out_8173370600042692758) {
|
||||
h_26(state, unused, out_8173370600042692758);
|
||||
void car_h_26(double *state, double *unused, double *out_749423849060713781) {
|
||||
h_26(state, unused, out_749423849060713781);
|
||||
}
|
||||
void car_H_26(double *state, double *unused, double *out_7487585337370381642) {
|
||||
H_26(state, unused, out_7487585337370381642);
|
||||
void car_H_26(double *state, double *unused, double *out_5412171565652113289) {
|
||||
H_26(state, unused, out_5412171565652113289);
|
||||
}
|
||||
void car_h_27(double *state, double *unused, double *out_4943578383614041585) {
|
||||
h_27(state, unused, out_4943578383614041585);
|
||||
void car_h_27(double *state, double *unused, double *out_2801473399302960502) {
|
||||
h_27(state, unused, out_2801473399302960502);
|
||||
}
|
||||
void car_H_27(double *state, double *unused, double *out_8924986397300772885) {
|
||||
H_27(state, unused, out_8924986397300772885);
|
||||
void car_H_27(double *state, double *unused, double *out_6849572625582504532) {
|
||||
H_27(state, unused, out_6849572625582504532);
|
||||
}
|
||||
void car_h_29(double *state, double *unused, double *out_4668384321329535696) {
|
||||
h_29(state, unused, out_4668384321329535696);
|
||||
void car_h_29(double *state, double *unused, double *out_5173951242668943310) {
|
||||
h_29(state, unused, out_5173951242668943310);
|
||||
}
|
||||
void car_H_29(double *state, double *unused, double *out_7211623670431221852) {
|
||||
H_29(state, unused, out_7211623670431221852);
|
||||
void car_H_29(double *state, double *unused, double *out_8912176792012229989) {
|
||||
H_29(state, unused, out_8912176792012229989);
|
||||
}
|
||||
void car_h_28(double *state, double *unused, double *out_6247021973002058322) {
|
||||
h_28(state, unused, out_6247021973002058322);
|
||||
void car_h_28(double *state, double *unused, double *out_5994836064963036638) {
|
||||
h_28(state, unused, out_5994836064963036638);
|
||||
}
|
||||
void car_H_28(double *state, double *unused, double *out_9175253941996548103) {
|
||||
H_28(state, unused, out_9175253941996548103);
|
||||
void car_H_28(double *state, double *unused, double *out_6948546520446903738) {
|
||||
H_28(state, unused, out_6948546520446903738);
|
||||
}
|
||||
void car_h_31(double *state, double *unused, double *out_1448590970011147238) {
|
||||
h_31(state, unused, out_1448590970011147238);
|
||||
void car_h_31(double *state, double *unused, double *out_6352515274334922980) {
|
||||
h_31(state, unused, out_6352515274334922980);
|
||||
}
|
||||
void car_H_31(double *state, double *unused, double *out_7187009455588153322) {
|
||||
H_31(state, unused, out_7187009455588153322);
|
||||
void car_H_31(double *state, double *unused, double *out_9184320846403129941) {
|
||||
H_31(state, unused, out_9184320846403129941);
|
||||
}
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt) {
|
||||
predict(in_x, in_P, in_Q, dt);
|
||||
|
||||
@@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_3010729618614624127);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_3985552318855446440);
|
||||
void car_H_mod_fun(double *state, double *out_4306310304618992202);
|
||||
void car_f_fun(double *state, double dt, double *out_841039768515563673);
|
||||
void car_F_fun(double *state, double dt, double *out_3325406224632462615);
|
||||
void car_h_25(double *state, double *unused, double *out_8501492422434664386);
|
||||
void car_H_25(double *state, double *unused, double *out_7217655417465113750);
|
||||
void car_h_24(double *state, double *unused, double *out_2528371558444461081);
|
||||
void car_H_24(double *state, double *unused, double *out_7688112899508452474);
|
||||
void car_h_30(double *state, double *unused, double *out_8776686484719170275);
|
||||
void car_H_30(double *state, double *unused, double *out_6701392326116829668);
|
||||
void car_h_26(double *state, double *unused, double *out_8173370600042692758);
|
||||
void car_H_26(double *state, double *unused, double *out_7487585337370381642);
|
||||
void car_h_27(double *state, double *unused, double *out_4943578383614041585);
|
||||
void car_H_27(double *state, double *unused, double *out_8924986397300772885);
|
||||
void car_h_29(double *state, double *unused, double *out_4668384321329535696);
|
||||
void car_H_29(double *state, double *unused, double *out_7211623670431221852);
|
||||
void car_h_28(double *state, double *unused, double *out_6247021973002058322);
|
||||
void car_H_28(double *state, double *unused, double *out_9175253941996548103);
|
||||
void car_h_31(double *state, double *unused, double *out_1448590970011147238);
|
||||
void car_H_31(double *state, double *unused, double *out_7187009455588153322);
|
||||
void car_err_fun(double *nom_x, double *delta_x, double *out_4503132331689013745);
|
||||
void car_inv_err_fun(double *nom_x, double *true_x, double *out_5519222819843562437);
|
||||
void car_H_mod_fun(double *state, double *out_99880963271144866);
|
||||
void car_f_fun(double *state, double dt, double *out_4490574509847198287);
|
||||
void car_F_fun(double *state, double dt, double *out_5588273031121660281);
|
||||
void car_h_25(double *state, double *unused, double *out_4633670035125740311);
|
||||
void car_H_25(double *state, double *unused, double *out_9153674884526169513);
|
||||
void car_h_24(double *state, double *unused, double *out_4147879785077922233);
|
||||
void car_H_24(double *state, double *unused, double *out_7067361405204513541);
|
||||
void car_h_30(double *state, double *unused, double *out_8271119563379762661);
|
||||
void car_H_30(double *state, double *unused, double *out_2376378847691765348);
|
||||
void car_h_26(double *state, double *unused, double *out_749423849060713781);
|
||||
void car_H_26(double *state, double *unused, double *out_5412171565652113289);
|
||||
void car_h_27(double *state, double *unused, double *out_2801473399302960502);
|
||||
void car_H_27(double *state, double *unused, double *out_6849572625582504532);
|
||||
void car_h_29(double *state, double *unused, double *out_5173951242668943310);
|
||||
void car_H_29(double *state, double *unused, double *out_8912176792012229989);
|
||||
void car_h_28(double *state, double *unused, double *out_5994836064963036638);
|
||||
void car_H_28(double *state, double *unused, double *out_6948546520446903738);
|
||||
void car_h_31(double *state, double *unused, double *out_6352515274334922980);
|
||||
void car_H_31(double *state, double *unused, double *out_9184320846403129941);
|
||||
void car_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
void car_set_mass(double x);
|
||||
void car_set_rotational_inertia(double x);
|
||||
|
||||
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl
|
||||
void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_342973137176612241);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_4414887224430184744);
|
||||
void pose_H_mod_fun(double *state, double *out_824354775938542426);
|
||||
void pose_f_fun(double *state, double dt, double *out_432270031952541382);
|
||||
void pose_F_fun(double *state, double dt, double *out_6844361959189325516);
|
||||
void pose_h_4(double *state, double *unused, double *out_948512472116913193);
|
||||
void pose_H_4(double *state, double *unused, double *out_4098576854729491679);
|
||||
void pose_h_10(double *state, double *unused, double *out_8780443367168833320);
|
||||
void pose_H_10(double *state, double *unused, double *out_7022073385523406729);
|
||||
void pose_h_13(double *state, double *unused, double *out_3854432803415109726);
|
||||
void pose_H_13(double *state, double *unused, double *out_3512054353587209250);
|
||||
void pose_h_14(double *state, double *unused, double *out_3383842427869284419);
|
||||
void pose_H_14(double *state, double *unused, double *out_135335998390007150);
|
||||
void pose_err_fun(double *nom_x, double *delta_x, double *out_8799966904452395012);
|
||||
void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1661397671847398073);
|
||||
void pose_H_mod_fun(double *state, double *out_2131008325054536789);
|
||||
void pose_f_fun(double *state, double dt, double *out_884777716370448931);
|
||||
void pose_F_fun(double *state, double dt, double *out_6755798908020795646);
|
||||
void pose_h_4(double *state, double *unused, double *out_4561759232303120700);
|
||||
void pose_H_4(double *state, double *unused, double *out_5331418226156006813);
|
||||
void pose_h_10(double *state, double *unused, double *out_8855709713594721197);
|
||||
void pose_H_10(double *state, double *unused, double *out_3758101988975610904);
|
||||
void pose_h_13(double *state, double *unused, double *out_6255148116667054704);
|
||||
void pose_H_13(double *state, double *unused, double *out_2279212982160694116);
|
||||
void pose_h_14(double *state, double *unused, double *out_5422638595254952120);
|
||||
void pose_H_14(double *state, double *unused, double *out_8414206658451379109);
|
||||
void pose_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
#define SUNNYPILOT_VERSION "2026.07.20-4580"
|
||||
#define SUNNYPILOT_VERSION "2026.07.20-4574"
|
||||
|
||||
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -10,29 +10,29 @@ void live_update_32(double *in_x, double *in_P, double *in_z, double *in_R, doub
|
||||
void live_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_update_33(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea);
|
||||
void live_H(double *in_vec, double *out_6352154163192849580);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_4071252314637571743);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_7579279316636236983);
|
||||
void live_H_mod_fun(double *state, double *out_2183030463631189817);
|
||||
void live_f_fun(double *state, double dt, double *out_5889449294370966489);
|
||||
void live_F_fun(double *state, double dt, double *out_4590604826483372195);
|
||||
void live_h_4(double *state, double *unused, double *out_8188582832631334816);
|
||||
void live_H_4(double *state, double *unused, double *out_7947231586507101220);
|
||||
void live_h_9(double *state, double *unused, double *out_3642341249638579952);
|
||||
void live_H_9(double *state, double *unused, double *out_3212293551938002926);
|
||||
void live_h_10(double *state, double *unused, double *out_2827509996430913912);
|
||||
void live_H_10(double *state, double *unused, double *out_5851187932200043394);
|
||||
void live_h_12(double *state, double *unused, double *out_8787305242966137389);
|
||||
void live_H_12(double *state, double *unused, double *out_5480056079170488601);
|
||||
void live_h_35(double *state, double *unused, double *out_5904086788787176377);
|
||||
void live_H_35(double *state, double *unused, double *out_7132850429829843020);
|
||||
void live_h_32(double *state, double *unused, double *out_4350922156484078074);
|
||||
void live_H_32(double *state, double *unused, double *out_7095194406207799526);
|
||||
void live_h_13(double *state, double *unused, double *out_3453680179466203195);
|
||||
void live_H_13(double *state, double *unused, double *out_2170845890615723155);
|
||||
void live_h_14(double *state, double *unused, double *out_3642341249638579952);
|
||||
void live_H_14(double *state, double *unused, double *out_3212293551938002926);
|
||||
void live_h_33(double *state, double *unused, double *out_8251223619274971083);
|
||||
void live_H_33(double *state, double *unused, double *out_3982293425190985416);
|
||||
void live_H(double *in_vec, double *out_285400171142782786);
|
||||
void live_err_fun(double *nom_x, double *delta_x, double *out_2436896259464335423);
|
||||
void live_inv_err_fun(double *nom_x, double *true_x, double *out_8871984062805370648);
|
||||
void live_H_mod_fun(double *state, double *out_8228926511208625476);
|
||||
void live_f_fun(double *state, double dt, double *out_4538780046133425309);
|
||||
void live_F_fun(double *state, double dt, double *out_417010570473098233);
|
||||
void live_h_4(double *state, double *unused, double *out_2309729505615761905);
|
||||
void live_H_4(double *state, double *unused, double *out_8886506882666231613);
|
||||
void live_h_9(double *state, double *unused, double *out_1348765263594904281);
|
||||
void live_H_9(double *state, double *unused, double *out_8645317236036640968);
|
||||
void live_h_10(double *state, double *unused, double *out_7251990229046891328);
|
||||
void live_H_10(double *state, double *unused, double *out_2812694342253054382);
|
||||
void live_h_12(double *state, double *unused, double *out_1162751087914723431);
|
||||
void live_H_12(double *state, double *unused, double *out_3867050474634269818);
|
||||
void live_h_35(double *state, double *unused, double *out_1137082034917540827);
|
||||
void live_H_35(double *state, double *unused, double *out_5519844825293624237);
|
||||
void live_h_32(double *state, double *unused, double *out_7169859216053261212);
|
||||
void live_H_32(double *state, double *unused, double *out_7164684335004523839);
|
||||
void live_h_13(double *state, double *unused, double *out_4721203745943767592);
|
||||
void live_H_13(double *state, double *unused, double *out_6086407052825232130);
|
||||
void live_h_14(double *state, double *unused, double *out_1348765263594904281);
|
||||
void live_H_14(double *state, double *unused, double *out_8645317236036640968);
|
||||
void live_h_33(double *state, double *unused, double *out_859896650697914554);
|
||||
void live_H_33(double *state, double *unused, double *out_2369287820654766633);
|
||||
void live_predict(double *in_x, double *in_P, double *in_Q, double dt);
|
||||
}
|
||||
@@ -0,0 +1,3 @@
|
||||
# C3 specific hardware code
|
||||
|
||||
`c3` is known as `tici` and comma three by comma. Not to confuse it with `c3x` which is known as `tizi`.
|
||||
@@ -0,0 +1,84 @@
|
||||
[
|
||||
{
|
||||
"name": "xbl",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/xbl-effa23294138e2297b85a5b482a885184c437b5ab25d74f2a62d4fce4e68f63b.img.xz",
|
||||
"hash": "effa23294138e2297b85a5b482a885184c437b5ab25d74f2a62d4fce4e68f63b",
|
||||
"hash_raw": "effa23294138e2297b85a5b482a885184c437b5ab25d74f2a62d4fce4e68f63b",
|
||||
"size": 3282256,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "ed61a650bea0c56652dd0fc68465d8fc722a4e6489dc8f257630c42c6adcdc89"
|
||||
},
|
||||
{
|
||||
"name": "xbl_config",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/xbl_config-63d019efed684601f145ef37628e62c8da73f5053a8e51d7de09e72b8b11f97c.img.xz",
|
||||
"hash": "63d019efed684601f145ef37628e62c8da73f5053a8e51d7de09e72b8b11f97c",
|
||||
"hash_raw": "63d019efed684601f145ef37628e62c8da73f5053a8e51d7de09e72b8b11f97c",
|
||||
"size": 98124,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "b12801ffaa81e58e3cef914488d3b447e35483ba549b28c6cd9deb4814c3265f"
|
||||
},
|
||||
{
|
||||
"name": "abl",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/abl-32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6.img.xz",
|
||||
"hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
|
||||
"hash_raw": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6",
|
||||
"size": 274432,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "32a2174b5f764e95dfc54cf358ba01752943b1b3b90e626149c3da7d5f1830b6"
|
||||
},
|
||||
{
|
||||
"name": "aop",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/aop-21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9.img.xz",
|
||||
"hash": "21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9",
|
||||
"hash_raw": "21370172e590bd4ea907a558bcd6df20dc7a6c7d38b8e62fdde18f4a512ba9e9",
|
||||
"size": 184364,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "c1be2f4aac5b3af49b904b027faec418d05efd7bd5144eb4fdfcba602bcf2180"
|
||||
},
|
||||
{
|
||||
"name": "devcfg",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/devcfg-d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620.img.xz",
|
||||
"hash": "d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620",
|
||||
"hash_raw": "d7d7e52963bbedbbf8a7e66847579ca106a0a729ce2cf60f4b8d8ea4b535d620",
|
||||
"size": 40336,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "17b229668b20305ff8fa3cd5f94716a3aaa1e5bf9d1c24117eff7f2f81ae719f"
|
||||
},
|
||||
{
|
||||
"name": "boot",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/boot-0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4.img.xz",
|
||||
"hash": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
|
||||
"hash_raw": "0191529aa97d90d1fa04b472d80230b777606459e1e1e9e2323c9519839827b4",
|
||||
"size": 18515968,
|
||||
"sparse": false,
|
||||
"full_check": true,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "492ae27f569e8db457c79d0e358a7a6297d1a1c685c2b1ae6deba7315d3a6cb0"
|
||||
},
|
||||
{
|
||||
"name": "system",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img.xz",
|
||||
"hash": "1468d50b7ad0fda0f04074755d21e786e3b1b6ca5dd5b17eb2608202025e6126",
|
||||
"hash_raw": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
|
||||
"size": 5368709120,
|
||||
"sparse": true,
|
||||
"full_check": false,
|
||||
"has_ab": true,
|
||||
"ondevice_hash": "242aa5adad1c04e1398e00e2440d1babf962022eb12b89adf2e60ee3068946e7",
|
||||
"alt": {
|
||||
"hash": "e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087",
|
||||
"url": "https://commadist.azureedge.net/agnosupdate/system-e0007afa5d1026671c1943d44bb7f7ad26259f673392dd00a03073a2870df087.img",
|
||||
"size": 5368709120
|
||||
}
|
||||
}
|
||||
]
|
||||
+100
@@ -0,0 +1,100 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
SP_C3_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
|
||||
DIR="$( cd "$SP_C3_DIR/../../../../.." >/dev/null 2>&1 && pwd )"
|
||||
|
||||
source "$SP_C3_DIR/launch_env.sh"
|
||||
|
||||
function agnos_init {
|
||||
# TODO: move this to agnos
|
||||
sudo rm -f /data/etc/NetworkManager/system-connections/*.nmmeta
|
||||
|
||||
# set success flag for current boot slot
|
||||
sudo abctl --set_success
|
||||
|
||||
# TODO: do this without udev in AGNOS
|
||||
# udev does this, but sometimes we startup faster
|
||||
sudo chgrp gpu /dev/adsprpc-smd /dev/ion /dev/kgsl-3d0
|
||||
sudo chmod 660 /dev/adsprpc-smd /dev/ion /dev/kgsl-3d0
|
||||
|
||||
|
||||
if [ $(< /VERSION) != "$AGNOS_VERSION" ]; then
|
||||
AGNOS_PY="$DIR/openpilot/common/hardware/tici/agnos.py"
|
||||
MANIFEST="$SP_C3_DIR/agnos.json"
|
||||
if $AGNOS_PY --verify $MANIFEST; then
|
||||
sudo reboot
|
||||
fi
|
||||
$DIR/openpilot/common/hardware/tici/updater $AGNOS_PY $MANIFEST
|
||||
fi
|
||||
}
|
||||
|
||||
function launch {
|
||||
# Remove orphaned git lock if it exists on boot
|
||||
[ -f "$DIR/.git/index.lock" ] && rm -f $DIR/.git/index.lock
|
||||
|
||||
# Check to see if there's a valid overlay-based update available. Conditions
|
||||
# are as follows:
|
||||
#
|
||||
# 1. The DIR init file has to exist, with a newer modtime than anything in
|
||||
# the DIR Git repo. This checks for local development work or the user
|
||||
# switching branches/forks, which should not be overwritten.
|
||||
# 2. The FINALIZED consistent file has to exist, indicating there's an update
|
||||
# that completed successfully and synced to disk.
|
||||
|
||||
if [ -f "${DIR}/.overlay_init" ]; then
|
||||
find ${DIR}/.git -newer ${DIR}/.overlay_init | grep -q '.' 2> /dev/null
|
||||
if [ $? -eq 0 ]; then
|
||||
echo "${DIR} has been modified, skipping overlay update installation"
|
||||
else
|
||||
if [ -f "${STAGING_ROOT}/finalized/.overlay_consistent" ]; then
|
||||
if [ ! -d /data/safe_staging/old_openpilot ]; then
|
||||
echo "Valid overlay update found, installing"
|
||||
LAUNCHER_LOCATION="${BASH_SOURCE[0]}"
|
||||
|
||||
mv $DIR /data/safe_staging/old_openpilot
|
||||
mv "${STAGING_ROOT}/finalized" $DIR
|
||||
cd $DIR
|
||||
|
||||
echo "Restarting launch script ${LAUNCHER_LOCATION}"
|
||||
unset AGNOS_VERSION
|
||||
exec "${LAUNCHER_LOCATION}"
|
||||
else
|
||||
echo "openpilot backup found, not updating"
|
||||
# TODO: restore backup? This means the updater didn't start after swapping
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
fi
|
||||
|
||||
# handle pythonpath
|
||||
ln -sfn $(pwd) /data/pythonpath
|
||||
export PYTHONPATH="$PWD"
|
||||
|
||||
# submodule package symlinks for PYTHONPATH imports on device.
|
||||
# on PC these come from editable installs via pyproject.toml / uv.
|
||||
ln -sfn msgq_repo/msgq msgq
|
||||
ln -sfn opendbc_repo/opendbc opendbc
|
||||
ln -sfn rednose_repo/rednose rednose
|
||||
ln -sfn teleoprtc_repo/teleoprtc teleoprtc
|
||||
ln -sfn tinygrad_repo/tinygrad tinygrad
|
||||
|
||||
# hardware specific init
|
||||
if [ -f /AGNOS ]; then
|
||||
agnos_init
|
||||
fi
|
||||
|
||||
# write tmux scrollback to a file
|
||||
tmux capture-pane -pq -S-1000 > /tmp/launch_log
|
||||
|
||||
# start manager
|
||||
cd $DIR/openpilot/system/manager
|
||||
if [ ! -f $DIR/prebuilt ]; then
|
||||
./build.py
|
||||
fi
|
||||
./manager.py
|
||||
|
||||
# if broken, keep on screen error
|
||||
while true; do sleep 1; done
|
||||
}
|
||||
|
||||
launch
|
||||
+13
@@ -0,0 +1,13 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
export OMP_NUM_THREADS=1
|
||||
export MKL_NUM_THREADS=1
|
||||
export NUMEXPR_NUM_THREADS=1
|
||||
export OPENBLAS_NUM_THREADS=1
|
||||
export VECLIB_MAXIMUM_THREADS=1
|
||||
|
||||
if [ -z "$AGNOS_VERSION" ]; then
|
||||
export AGNOS_VERSION="12.8"
|
||||
fi
|
||||
|
||||
export STAGING_ROOT="/data/safe_staging"
|
||||
Reference in New Issue
Block a user